/*
 * Copyright (c) 2013 - 2014, Freescale Semiconductor, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*******************************************************************************
 * Standard C Included Files
 ******************************************************************************/
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
/*******************************************************************************
 * SDK Included Files
 ******************************************************************************/
#include "fsl_soundcard.h"
#include "fsl_sai_driver.h"
#include "fsl_sai_features.h"
#include "fsl_sgtl5000_driver.h"
/*******************************************************************************
 * Application Included Files
 ******************************************************************************/
#include "modulator.h"
#include "terminal_menu.h" 
/*******************************************************************************
 * Include CMSIS-DSP library
 ******************************************************************************/
#include "arm_math.h" 
/*******************************************************************************
 * Global Variables
 ******************************************************************************/
sound_card_t g_soundCard;
static sai_data_format_t *format;
static sai_user_config_t tx_config;
bool firstCopy;
/*******************************************************************************
 * Function Definitions
 ******************************************************************************/   
void modulator_init(void)
{
    /* Configure the audio format */
    format = (sai_data_format_t *)mem_allocate_zero(sizeof(sai_data_format_t));
    format->bits = 16;
    format->sample_rate = 44100;
    format->mclk = 384 * format->sample_rate;
    format->words = 2;
    format->watermark = 4;

    /* SAI configuration */
    tx_config.bus_type = kSaiBusI2SLeft;
    tx_config.channel = 0;
    tx_config.slave_master = kSaiMaster;
    tx_config.sync_mode = kSaiModeAsync;
    tx_config.bclk_source = kSaiBclkSourceMclkDiv;
    tx_config.mclk_source = kSaiMclkSourceSysclk;
    tx_config.mclk_divide_enable = true;
    
    sai_handler_t *tx_handler = (sai_handler_t *)mem_allocate_zero(sizeof(sai_handler_t));
    g_soundCard.controller.handler = tx_handler;
    tx_handler->direction = AUDIO_TX;
    tx_handler->instance = 0;
    tx_handler->fifo_channel = 0;
    g_soundCard.controller.ops = &g_sai_ops;
#if USEDMA
    g_soundCard.controller.dma_source = kDmaRequestMux0I2S0Tx;
#endif
    sgtl_handler_t *codec_handler = (sgtl_handler_t *)mem_allocate_zero(sizeof(sgtl_handler_t));
    g_soundCard.codec.handler = codec_handler;
    g_soundCard.codec.ops = &g_sgtl_ops;
    g_soundCard.direction = AUDIO_TX;
    edma_init();
    /* Initialize sound card */
    snd_init(&g_soundCard, &tx_config, NULL);
    
}

snd_status_t get_wav_data(wave_file_t *waveFile)
{
    uint8_t *dataTemp = (uint8_t *)waveFile->data;
  
    /* check for RIFF */
    memcpy(waveFile->header.riff, dataTemp, 4);
    dataTemp += 4;
    if( memcmp( (uint8_t*)waveFile->header.riff, "RIFF", 4) )
    {
        return kStatus_SND_Fail;
    }
    
    /* Get size */
    memcpy(&waveFile->header.size, dataTemp, 4);
    dataTemp += 4;
    
    /* .wav file flag */
    memcpy(waveFile->header.waveFlag, dataTemp, 4);
    dataTemp += 4;
    if( memcmp( (uint8_t*)waveFile->header.waveFlag, "WAVE", 4) )
    {
        return kStatus_SND_Fail;
    }
    
    /* fmt */
    memcpy(waveFile->header.fmt, dataTemp, 4);
    dataTemp += 4;
    if( memcmp( (uint8_t*)waveFile->header.fmt, "fmt ", 4) )
    {
        return kStatus_SND_Fail;
    }
    
    /* fmt length */
    memcpy(&waveFile->header.fmtLen, dataTemp, 4);
    dataTemp += 4;
    
    /* Tag: PCM or not */
    memcpy(&waveFile->header.tag, dataTemp, 4);
    dataTemp += 2;

    /* Channels */
    memcpy(&waveFile->header.channels, dataTemp, 4);
    dataTemp += 2;
    
    /* Sample Rate in Hz */
    memcpy(&waveFile->header.sampFreq, dataTemp, 4);
    dataTemp += 4;
    memcpy(&waveFile->header.byteRate, dataTemp, 4);
    dataTemp += 4;
    
    /* quantize bytes for per samp point */
    memcpy(&waveFile->header.blockAlign, dataTemp, 4);
    dataTemp += 2;
    memcpy(&waveFile->header.bitSamp, dataTemp, 4);
    dataTemp += 2;
    
    /* Data */
    memcpy(waveFile->header.dataFlag, dataTemp, 4);
    dataTemp += 4;
    if( memcmp( (uint8_t*)waveFile->header.dataFlag, "data ", 4) )
    {
        return kStatus_SND_Fail;
    }
    memcpy(&waveFile->header.length, dataTemp, 4);
    dataTemp += 4;
    
    return kStatus_SND_Success;
}

snd_status_t play_wav(uint32_t *pcmBuffer, uint8_t divider)
{
    uint32_t bytesToRead    = 0;
    uint32_t bytesRead      = 0;
    uint8_t statusCheck;
      
    wave_file_t newWav;
    newWav.data = (uint32_t *)pcmBuffer;
    
    /* Get header data and check status of read */
    statusCheck = get_wav_data(&newWav);
    if(statusCheck == kStatus_SND_Fail)
    {
        return kStatus_SND_Fail;
    }
    
    /* Reformat sound card to play .wav file */
    format->bits = newWav.header.bitSamp;
    format->sample_rate = newWav.header.sampFreq;
    format->words = newWav.header.channels;
    format->mclk = 384 * format->sample_rate;
    snd_data_format_configure(&g_soundCard, format);
    
    /* Remove header size from byte count */
    bytesToRead = (newWav.header.length - WAVE_FILE_HEADER_SIZE) / divider; /* Adjust note duration by divider value, wav tables in pcm_data.h are 200ms by default */
    bytesRead = WAVE_FILE_HEADER_SIZE;
    
    firstCopy = true;
    
    /* Send .wav data */
    while (bytesRead < bytesToRead)
    {
        send_wav((uint8_t *)newWav.data, bytesToRead, format);
        bytesRead += bytesToRead;
    }    

    if(bytesRead >= bytesToRead)
    {
        return kStatus_SND_Success;
    }
    else
    {
        return kStatus_SND_Fail;
    }
}

static void send_wav(uint8_t *dataBuffer, uint32_t length, sai_data_format_t *dataFormat)
{
    uint32_t index = 0;
    uint32_t count = 0;	
    uint8_t *pData = dataBuffer;	
    snd_state_t tx_status;
    //As the sync can not initialized to be non-zero value, application have to copy a period of data firstly
    if(firstCopy)
    {
        firstCopy = false;
        snd_wait_event(&g_soundCard);
        snd_get_status(&tx_status, &g_soundCard);
        for(index = 0; index < tx_status.size; index++)
        {
            tx_status.input_address[index] = *pData;
            pData++; 
        }
        count += index;
        snd_update_tx_status(&g_soundCard, tx_status.size);
        snd_start_tx(&g_soundCard);
    }
    while(count < length)
    {
        snd_wait_event(&g_soundCard);
        snd_get_status(&tx_status, &g_soundCard);
	// Memcopy to sai buffer
	for(index = 0; index < tx_status.size; index++)
	{
            tx_status.input_address[index] = *pData;
            pData++;
	}
        count += index;
        snd_update_tx_status(&g_soundCard, tx_status.size);
    }  
}


snd_status_t play_mod_wav(uint16_t *pcmBuffer, uint16_t *modPointer, float32_t *fftData, float32_t *fftResult, uint8_t divider, uint8_t modulation, uint32_t srcSizeBytes, uint32_t sampleSize)
{       
    snd_status_t statusCheck;
    uint8_t count = 0;
    uint32_t currIndex = 0;
    uint32_t totalData;
    uint8_t loopsNeeded;

    totalData = srcSizeBytes / 2;          /* Total 16-bit integers needed. */
    loopsNeeded = totalData / sampleSize;  /* How many loops are needed to fill modBuffer. */
    
    printf("\r\nProcessing...\r\n");
    
    /* Perform waveform modulation over the duration of the wav table. */
    while(count < loopsNeeded)
    {
       currIndex = mod_wav_data(pcmBuffer, modPointer, fftData, fftResult, currIndex, sampleSize, modulation);
       count++;
    }
    printf("\r\nCompleted!\r\n");
    
    /* Play wav data. */
    statusCheck = play_wav((uint32_t *)modPointer, divider);
      
    return statusCheck;
}

float32_t get_wav_Hz(uint16_t *pcmBuffer, float32_t *fftData, float32_t *fftResult, uint32_t sampleSize)
{
    /* Counter variable for navigating buffers */
    uint16_t counter;
  
    /* Return value for wav frequency in hertz */
    float32_t wavFreqHz;
  
    /* CMSIS status & FFT instance */
    arm_status status;                  /* ARM status variable */
    arm_cfft_radix2_instance_f32 fft;   /* ARM FFT instance */
    
    /* Frequency analysis variables */
    float32_t maxValue;                    /* max value for greatest FFT bin amplitude */         
    uint32_t testIndex = 0;               /* value for storing the bin location with maxValue*/
    uint32_t complexBuffSize = sampleSize * 2;
    uint32_t fftSize = sampleSize;        /* FFT bin size  */
    uint32_t ifftFlag = 0;                /* Flag for the selection of CFFT/CIFFT */
    uint32_t doBitReverse = 1;            /* Flag for selection of normal order or bit reversed order */
    uint32_t sampleRate = format->sample_rate;      /* Get sample rate from current format */
    float32_t hzPerBin = ((float32_t)sampleRate/(float32_t)fftSize);   /* Calculate hz per FFT bin */
    
    /* Wav data variables */
    uint16_t *temp = pcmBuffer;             /* Point to wav data */ 
    int16_t tempData = 0;
    
    /* Move past wav header */
    for(counter = 0; counter < WAVE_FILE_HEADER_SIZE/2; counter++)
    {
        temp++;
    }  
    
    /* Copy wav data to fft input array */
    for(counter = 0; counter < complexBuffSize; counter++)
    {
        if(counter % 2 == 0)
        {
            tempData = (int16_t)*temp;
            fftData[counter] = (float)tempData;
            temp++;
        }
        else
        {
            fftData[counter] = 0.0;
        }
    }
    
    /* Set status as success */
    status = ARM_MATH_SUCCESS;
    
    /* Set instance for Real FFT */
    status = arm_cfft_radix2_init_f32(&fft, fftSize, ifftFlag, doBitReverse);
    
    /* Perform Real FFT on fftData */
    arm_cfft_radix2_f32(&fft, fftData);
    
    /* Populate FFT bins */
    arm_cmplx_mag_f32(fftData, fftResult, fftSize);
    
    /* Find max bin and location of max (first half of bins as this is the only valid section) */
    arm_max_f32(fftResult, fftSize, &maxValue, &testIndex);
  
    if(status != ARM_MATH_SUCCESS)
    {
        wavFreqHz = 0;     /* If an error has occured set frequency of wav data to 0Hz*/
    }
    else
    {
        wavFreqHz = testIndex * hzPerBin;  /* Set wavFreqHz to bin location of max amplitude multiplied by the hz per bin */
    }
    
    return wavFreqHz;
}

uint32_t mod_wav_data(uint16_t *pcmBuffer, uint16_t *modBuffer, float32_t *fftData, float32_t *fftResult, uint16_t startIndex, uint32_t sampleSize, uint8_t modType)
{
    /* Counter variable for navigating buffers */
    uint16_t counter = 0;
    uint16_t index = startIndex;
    
    /* StatusCheck variable */
    volatile uint16_t statusCheck;
    
    /* Return value for wav frequency in hertz */
    float32_t fundFreqHz;
    
    /* Define array to store necessary harmonics to modulate waveform based on modulation type */
    uint8_t  numOfHarmonics;
    switch(modType)
    {
        case kNoModulation:
            return kStatus_SND_Success;  /* Exit function with abort status as there is no modulation required */
            break;
        case kSquareWave:
            numOfHarmonics = 7;    
            break;
        case kSawWave:
            numOfHarmonics = 10;
            break;
        case kTriangleWave:
            numOfHarmonics = 7;
            break;
        default:
            return kStatus_SND_Fail; /* Exit function as waveform type is not supported. */
            break;
    }   
    
    /* CMSIS status & FFT instance */
    arm_status status;                  /* ARM status variable */
    arm_cfft_radix2_instance_f32 fft;   /* ARM FFT instance */
    
    /* Frequency analysis variables */
    float32_t maxValue;                          /* max value for greatest FFT bin amplitude */         
    uint32_t testIndex = 0;                      /* value for storing the bin location with maxValue*/
    uint32_t complexBuffSize = sampleSize * 2;   /* Complex buffer size is twice the sample size */
    uint32_t fftSize = sampleSize;               /* FFT bin size is sample size divided by 2 */
    uint32_t ifftFlag = 0;                       /* Flag for the selection of CFFT/CIFFT */
    uint32_t doBitReverse = 1;                   /* Flag for selection of normal order or bit reversed order */
    uint32_t sampleRate = format->sample_rate;                         /* Get sample rate from current format */
    float32_t hzPerBin = ((float32_t)sampleRate/(float32_t)fftSize);   /* Calculate hz per FFT bin */
    
    /* Wav data variables */
    uint16_t *tempIn = pcmBuffer;             
    int16_t tempData = 0;
    
    /* Check to see if a header is present, in other words "is this the start of the wav file?" */
    if((pcmBuffer[index] == 0x4952) && (index == 0))
    {
        /* Move past wav header and store header data */
        for(counter = 0; counter < WAVE_FILE_HEADER_SIZE/2; counter++)
        {
            modBuffer[counter] = *tempIn;
            tempIn++;
        }
        index += WAVE_FILE_HEADER_SIZE/2;
    } 
    
    
    /* Copy wav data to fft input array */
    for(counter = 0; counter < complexBuffSize; counter++)
    {
        if(counter % 2 == 0)
        {
            tempData = (int16_t)*tempIn;
            fftData[counter] = (float32_t)tempData;
            tempIn++;
        }
        else
        {
            fftData[counter] = 0.0;
        }
    } 
   
    /* Set status as success */
    status = ARM_MATH_SUCCESS;
    
    /* Set instance for Complex FFT */
    status = arm_cfft_radix2_init_f32(&fft, fftSize, ifftFlag, doBitReverse);
    
    /* Perform Complex FFT on fftData */
    arm_cfft_radix2_f32(&fft, fftData);
    
    /* Populate FFT bins */
    arm_cmplx_mag_f32(fftData, fftResult, fftSize);
    
    /* Find max bin and location of max (first half of bins as this is the only valid section) */
    arm_max_f32(fftResult, fftSize, &maxValue, &testIndex);
  
    if(status != ARM_MATH_SUCCESS)
    {
        return kStatus_SND_Fail;     /* If an error has occured return fail status */
    }
    else
    {
        fundFreqHz = testIndex * hzPerBin;  /* Set wavFreqHz to bin location of max amplitude multiplied by the hz per bin */
    }
    
  
    /* Modulate waveform */
    float32_t harmonicHz;
    uint32_t rFftBinIndex;
    float32_t orderOfHarmonic;
    switch(modType)
    {
        case kSquareWave:
           for(counter = 0; counter < numOfHarmonics; counter++)
           {
               orderOfHarmonic = (3.0 + (2.0 * counter));
               harmonicHz = fundFreqHz * orderOfHarmonic;
               rFftBinIndex = (uint16_t)(2 * (harmonicHz / hzPerBin));
               fftData[rFftBinIndex] = maxValue / orderOfHarmonic;
           }
           break;
        case kSawWave:
           for(counter = 0; counter < numOfHarmonics; counter++)
           {
               orderOfHarmonic = (2.0 + counter);
               harmonicHz = fundFreqHz * orderOfHarmonic;
               rFftBinIndex = (uint16_t)(2 * (harmonicHz / hzPerBin));
               fftData[rFftBinIndex] = maxValue / orderOfHarmonic;
           }
           break;
        case kTriangleWave:
           for(counter = 0; counter < numOfHarmonics; counter++)
           {
               orderOfHarmonic = (3.0 + (2.0 * counter));
               harmonicHz = fundFreqHz * orderOfHarmonic;
               rFftBinIndex = (uint16_t)(2 * (harmonicHz / hzPerBin));
               fftData[rFftBinIndex] = maxValue / (orderOfHarmonic * orderOfHarmonic);
           }
           break;           
    }
    /* Re-build waveform from frequency spectrum */
    ifftFlag = 1;
    status = arm_cfft_radix2_init_f32(&fft, fftSize, ifftFlag, doBitReverse);
    arm_cfft_radix2_f32(&fft, fftData);
    
    /* Populate  output buffer */ 
    for(counter = 0; counter < complexBuffSize; counter++)
    {
        if(counter % 2 == 0)
        {
             tempData = (int16_t)fftData[counter];
             modBuffer[index] = (uint16_t)tempData;
             index++;
        }
    }
    
    return index;
}

void play_wav_arpeggio(uint32_t *arpeggioBuffer, uint32_t length, uint8_t divider)
{
    uint32_t counter; 

    for(counter = 0; counter < length; counter++)
    {
        play_wav((uint32_t *)arpeggioBuffer[counter], divider);
    }
   
}

/*******************************************************************************
 * EOF                                           
 *******************************************************************************/
