/*
 * Copyright 2019-2020 NXP
 * This software is owned or controlled by NXP and may only be used strictly
 * in accordance with the applicable license terms.  By expressly accepting
 * such terms or by downloading, installing, activating and/or otherwise using
 * the software, you are agreeing that you have read, and that you agree to
 * comply with and are bound by, such license terms.  If you do not agree to
 * be bound by the applicable license terms, then you may not retain, install,
 * activate or otherwise use the software.
 */

#include <stdint.h>
#include "usb_device_config.h"
#if ((defined(USB_CHAT_SPEAKER_ENABLE)) && (USB_CHAT_SPEAKER_ENABLE > 0U))

#include "usb_feedback.h"
#include "fsl_sai.h"
#include "fsl_gpio.h"
#include "fsl_port.h"
#include "audio_internal.h"
#include "audio_tx.h"

/* ---------------------------------------------------------------------------- */
/* Defines                                                                      */
/* ---------------------------------------------------------------------------- */

/* Set this macro to 1 to have extended feedback debug prints. */
#define DEBUG_FEEDBACK  (0)
/* Calculates the specified buffer upper limit */
#define BUFFER_UPPER_LIMIT(x)  (((x) * BUFFER_UPPER_LIMIT_PERCENTAGE) / 100)
/* Calculates the specified buffer lower limit */
#define BUFFER_LOWER_LIMIT(x)  (((x) * BUFFER_LOWER_LIMIT_PERCENTAGE) / 100)
/*
 * Defines how many sub samples we can correct the feedback value per usb frame (millisecond).
 * One sub sample is 1/(2^14) = 1/16384 sample.
 * 1638 sub samples is 1/10 sample.
 * So this corrects 1/10 sample * 2 bytes/sample * 2 chan = 0.4B per frame.
 * So for a 64 ms interval, this corrects 25.6B between subsequent refreshes.
 */
#define SUBSAMPLES_STEP_PER_REFRESH  (1638)

/* Convert a number to 10.14 format (Q14 for short) */
#define SAMPLES_TO_Q14(x)     ((x) << 14)
#define SUBSAMPLES_TO_Q14(x)  (x)                       /* One sub sample is 1/(2^14) = 1/16384 sample. */
#define Q14_TO_SAMPLES(x)     ((x) >> 14)
#define Q14_TO_SUBSAMPLES(x)  (((x) << 18) >> 18)

#ifdef FREEMASTER_DEBUG
/* Global to make it available to Freemaster */
volatile uint32_t Feedback_SpaceAvailable;
#endif

extern UsbFeedback_t g_UsbChatSpeaker_FeedbackSettings;
volatile uint32_t g_i2sSampleRate = 48;
uint8_t g_I2sBclkSwitchFlag;
uint8_t g_I2sFormatIndex = 1;
extern volatile uint32_t g_audioTxISRBusy;
/* -------------------------------------------------------------------------
 * Local functions
 * ------------------------------------------------------------------------- */



/*!
 * @brief Calculates if more or less samples are required.
 *
 * @param BufferSettings Pointer to the buffer settings.
 * @param BufferSpaceInUse Space in use on the buffer.
 * @return void.
 */

void USB_FeedbackCalculate(UsbFeedback_t *BufferSettings, uint32_t BufferSpaceInUse)
{
    uint32_t bytesLeftInXfer = 0;
    bytesLeftInXfer = audio_GetBytesLeftInTransfer();
    uint8_t audioTxSampleSizeBytes = audio_GetTxSampleSizeBytes();
    if(g_audioTxISRBusy)
    {
        BufferSpaceInUse =  BufferSpaceInUse - TX_AUDIO_DMA_TRANSFER_SIZE(audioTxSampleSizeBytes) + bytesLeftInXfer; 
    }
    else
    {
        BufferSpaceInUse =  BufferSpaceInUse + bytesLeftInXfer; 
    }
    if (BufferSpaceInUse > BufferSettings->BufferUpperLimit) {

        if(g_I2sFormatIndex != 2)
        {
            g_I2sBclkSwitchFlag = 1;
            g_I2sFormatIndex = 2;
            g_i2sSampleRate = 48387;
        }
    } else if (BufferSpaceInUse < BufferSettings->BufferLowerLimit) {

        if(g_I2sFormatIndex != 0)
        {
            g_I2sBclkSwitchFlag = 1;
            g_I2sFormatIndex = 0;
            g_i2sSampleRate = 47619;
        }

    } else {
        
        if( BufferSpaceInUse <= (BufferSettings->BufferSize/2 + 50) && BufferSpaceInUse >= (BufferSettings->BufferSize/2 - 50))
        {
            if(g_I2sFormatIndex != 1)
            {
                g_I2sBclkSwitchFlag = 1;
                g_I2sFormatIndex = 1;
                g_i2sSampleRate = 48000;
            }
        }

    }

#ifdef FREEMASTER_DEBUG
    Feedback_SpaceAvailable = BufferSpaceInUse;
#endif

#if DEBUG_FEEDBACK
    PDBG("F%d", BufferSpaceInUse);
    PDBG("%dS%dSS", Q14_TO_SAMPLES(BufferSettings->Feedback), Q14_TO_SUBSAMPLES(BufferSettings->Feedback));
#endif
}

static void FeedbackDpc(void *ctxt)
{
    UsbFeedback_t *BufferSettings = (UsbFeedback_t *)ctxt;

    uint32_t currentBufferSpaceInUse = BufferSettings->Cb();
    USB_FeedbackCalculate(BufferSettings, currentBufferSpaceInUse);
}


/* -------------------------------------------------------------------------
 * Public functions
 * ------------------------------------------------------------------------- */

void USB_FeedbackInit(UsbFeedback_t *BufferSettings, uint32_t BufferSize, uint8_t SampleRateInKhz, uint8_t RefreshRateInMs,
                      usb_feedback_GetFillingCb Cb)
{
    q14 MinStep;


    BufferSettings->BufferSize = BufferSize;
    BufferSettings->SampleRate = SampleRateInKhz;

    BufferSettings->BufferUpperLimit = BUFFER_UPPER_LIMIT(BufferSize);
    BufferSettings->BufferLowerLimit = BUFFER_LOWER_LIMIT(BufferSize);

    /* the feedback uses Q14 format */
    BufferSettings->BufferSamples = SAMPLES_TO_Q14(SampleRateInKhz);

#if 0
    /*
     * Divide the number of samples by the RefreshRate because the step is added to each
     * frame and the author wants to keep the total compensation in bytes between refreshes constant.
     * This leads to very slow correction for big RefreshRates.
     * E.g. RefreshRate = 64 ms gives a total correction of 1/64 sample/ms * 64 ms * 2 bytes/sample * 2 chan = 4 bytes per 64 ms.
     * E.g. RefreshRate = 1 ms  gives a total correction of 1/1  sample/ms *  1 ms * 2 bytes/sample * 2 chan = 4 bytes per 1 ms.
     * Therefore, we will not use this formula.
     */
    MinStep = (SAMPLES_TO_Q14(1000 / RefreshRateInMs)) / 1000;
#else
    /*
     * We take a different approach: make the total compensation in bytes between refreshes
     * independent from the RefreshRate.
     * So for bigger RefreshRates, we have a linearly bigger total compensation in bytes.
     */
    MinStep = SUBSAMPLES_TO_Q14(SUBSAMPLES_STEP_PER_REFRESH);
    (void)RefreshRateInMs;
#endif

    BufferSettings->BufferMoreSamples = BufferSettings->BufferSamples + MinStep;
    BufferSettings->BufferLessSamples = BufferSettings->BufferSamples - MinStep;

    BufferSettings->Cb = Cb;
    //FRAMEWORK_InitTimer(&BufferSettings->Timer, USB_FeedbackTimerTick, BufferSettings);

#if DEBUG_FEEDBACK
    PDBG("F init, minstep = %d (%dS%dSS)", MinStep, Q14_TO_SAMPLES(MinStep), Q14_TO_SUBSAMPLES(MinStep));
    PDBG("nominal= %d (%dS%dSS)",
         BufferSettings->BufferSamples,
         Q14_TO_SAMPLES(BufferSettings->BufferSamples),
         Q14_TO_SUBSAMPLES(BufferSettings->BufferSamples));
    PDBG("more= %d (%dS%dSS)",
         BufferSettings->BufferMoreSamples,
         Q14_TO_SAMPLES(BufferSettings->BufferMoreSamples),
         Q14_TO_SUBSAMPLES(BufferSettings->BufferMoreSamples));
    PDBG("less= %d (%dS%dSS)",
         BufferSettings->BufferLessSamples,
         Q14_TO_SAMPLES(BufferSettings->BufferLessSamples),
         Q14_TO_SUBSAMPLES(BufferSettings->BufferLessSamples));
#endif

    BufferSettings->Feedback = BufferSettings->BufferSamples;

}

void SysTick_Handler(void)
{
    //FeedbackDpc((void *)&g_UsbChatSpeaker_FeedbackSettings);
}


void USB_FeedbackStart(UsbFeedback_t *BufferSettings, uint32_t IntervalUs)
{
    BufferSettings->IntervalUs = IntervalUs;
    /* start 2ms systick */
    //SysTick_Config(SystemCoreClock / 500U);
}

void USB_FeedbackStop(UsbFeedback_t *BufferSettings)
{
    //FRAMEWORK_StopTimer(&BufferSettings->Timer);
}

void USB_FeedbackReset(UsbFeedback_t *BufferSettings)
{
    BufferSettings->Feedback = BufferSettings->BufferSamples;
}

#endif
