/*
 * Copyright (C) 2018, Hogeschool Rotterdam, Harry Broeders
 * All rights reserved.
 */

#include <stdint.h>
#include <stddef.h>
#include <stdio.h>
#include <NoRTOS.h>

#include <ti/devices/cc32xx/inc/hw_memmap.h>
#include <ti/devices/cc32xx/inc/hw_types.h>
#include <ti/devices/cc32xx/driverlib/prcm.h>
#include <ti/devices/cc32xx/driverlib/i2s.h>
#include <ti/drivers/I2C.h>

#include "ti_drivers_config.h"
#include "config.h"

// You can select the sample rate here:
#define SAMPLINGFREQUENCY 36000
#if SAMPLINGFREQUENCY < 8000 || SAMPLINGFREQUENCY > 48000 || SAMPLINGFREQUENCY % 4000 != 0
#error Sampling Frequency must be between 8 kHz and 48 kHz (included) and must be a multiple of 4 kHz.
#endif

//const int AL = 4;
//const int16_t A[4] = {
//      519,   1558,   1558,    519
//};
//const int BL = 4;
//const int16_t B[4] = {
//    16384, -23905,  14915,  -3241
//};

//const int NL = 4;
//const float NUM[4] = {
//    0.03169250488,  0.09507369995,  0.09507369995,  0.03169250488
//};
//const int DL = 4;
//const float DEN[4] = {
//                1,   -1.458984375,   0.9103393555,  -0.1978149414
//};

const int NL = 11;
const float NUM[11] = {
  1.105590945372e-05,0.0001105590999941,0.0004975159536116, 0.001326709170826,
   0.002321741078049, 0.002786089200526, 0.002321741078049, 0.001326709170826,
  0.0004975159536116,0.0001105590999941,1.105590945372e-05
};
const int DL = 11;
const float DEN[11] = {
                   1,   -4.986985206604,    11.93643665314,   -17.74237251282,
      17.97322845459,   -12.88624191284,    6.593202114105,   -2.369091749191,
     0.5706326961517, -0.08301767706871, 0.005529714748263
};

float buffer_N[NL];
float buffer_D[DL];

int main(void)
{
    // Init CC3220S LAUNCHXL board.
    Board_initGeneral();
    // Prepare to use TI drivers without operating system.
    NoRTOS_start();

    // Configure an I2C connection which is used to configure the audio codec.
    I2C_Handle i2cHandle = ConfigureI2C(CONFIG_I2C_0, I2C_400kHz);
    // Configure the audio codec.
    ConfigureAudioCodec(i2cHandle, SAMPLINGFREQUENCY);

    // Configure an I2S connection which is use to send/receive samples to/from the codec.
    ConfigureI2S(CONFIG_I2S_0, I2S_BASE, SAMPLINGFREQUENCY);
    int16_t dataLeft, dataRight;
    int k;
    while (1)
    {
        float out;
        I2SDataGet(I2S_BASE, I2S_DATA_LINE_1, &dataLeft);
        I2SDataPut(I2S_BASE, I2S_DATA_LINE_0,(int16_t) 0);
        I2SDataGet(I2S_BASE, I2S_DATA_LINE_1, &dataRight);

        buffer_N[0] = (float)dataRight;
        out = 0;
        for (k=0; k < NL; k++){
            out += NUM[k] * buffer_N[k];
        }
        for(k=NL-1; k >= 1; k--){
            buffer_N[k] = buffer_N[k-1];
        }
        for (k=1; k < DL; k++){
            out -= DEN[k] * buffer_D[k];
        }
        out /= DEN[0];
        buffer_D[0] = out;

        for(k=DL-1; k >= 1; k--){
            buffer_D[k] = buffer_D[k-1];
        }

        I2SDataPut(I2S_BASE, I2S_DATA_LINE_0, out);
    }
    return 0;
}