/* DriverLib Includes */
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>

/* Standard Includes */
#include <stdint.h>
#include <math.h>
#include <descrambler.h>

#define SINE_SIZE 50
#define FS 50000

// Coefficients for bandstop filter
//const double B1[] = {0.7755, -2.5985, 6.3671, -9.6188, 11.5650, -9.6188, 6.3671, -2.5985, 0.7755};
//const double A1[] = {1.0000, -3.1835, 7.4065, -10.6464, 12.1861, -9.6677, 6.1083, -2.3849, 0.6809};
const float B1[] = {0.9208, 0.1952, 0.9208};
const float A1[] = {1.0000, 0.1952, 0.8415};
//const float A1[] = {1.0000, -1.0457, 0.9510};
const float A2[] = {1.0000, -1.5610, 0.6414};
// Coefficients for lowpass filter
//const float B2[] = {0.0000, 0.0001, 0.0005, 0.0025, 0.0093, 0.0260, 0.0564, 0.0967, 0.1330, 0.1477, 0.1330, 0.0967, 0.0564, 0.0260, 0.0093, 0.0025, 0.0005, 0.0001, 0.0000};
//const float A2[] = {0.0001, -0.0014, 0.0096, -0.0423, 0.1340, -0.3250, 0.6243, -0.9708, 1.2386, -1.3057, 1.1399, -0.8221, 0.4862, -0.2326, 0.0881, -0.0255, 0.0053, -0.0007, 0.0000};
// Time delays for both filters

static float Z1[2][3];
static float Z2[2][3];

float sine[SINE_SIZE];
unsigned int sine_index = 0;

int main(void){
    /* Halting the Watchdog */
    MAP_WDT_A_holdTimer();
    
    for(;sine_index<=SINE_SIZE;sine_index++){
//        sine[sine_index] = 2.0 * sin(6.2831853071795862 * 7000.0 * sine_index * 20e-6);
//        sine[sine_index] = 1024 * sin(sine_index * 1.4661); // for 30kHz sampling
        sine[sine_index] = (1+sin(sine_index * 0.8796f))/2.0f;
//        sine[sine_index] = sin(6.2831853071795862 * 7000 * sine_index * 20e-6);
    }/* Fill sine array */
    //![Simple CS Config]
    /* Configuring pins for peripheral/crystal usage */
    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ, GPIO_PIN3 | GPIO_PIN2, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Configure P5.6 and P5.7 to their analogue functions to output VREF */
    P5SEL0 |= BIT6 | BIT7;
    P5SEL1 |= BIT6 | BIT7;

    REFCTL0 |= REFON;                     // Turn on reference module
    REFCTL0 |= REFOUT;                    // Output reference voltage to a pin

     /* Output VREF = 1.2V */
    REFCTL0 &= ~(REFVSEL_3);              // Clear existing VREF voltage level setting
    REFCTL0 |= REFVSEL_0;                 // Set VREF = 1.2V
    while (REFCTL0 & REFGENBUSY);       // Wait until the reference generation is settled


    /* Configuring pins for HFXT crystal */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_PJ,
            GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Set P6.0 as output */
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P6, GPIO_PIN0);

    /* Set all 8 pins of P4 as output */
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P4, GPIO_PIN0 | GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7);

    /* Configuring GPIOs (4.3 MCLK) */
//    MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P4, GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Setting the external clock frequency. This API is optional, but will
     * come in handy if the user ever wants to use the getMCLK/getACLK/etc
     * functions
     */
    CS_setExternalClockSourceFrequency(32000,48000000);

    /* Starting HFXT in non-bypass mode without a timeout. Before we start
     * we have to change VCORE to 1 to support the 48MHz frequency */
    MAP_PCM_setCoreVoltageLevel(PCM_VCORE1);
    MAP_FlashCtl_setWaitState(FLASH_BANK0, 2);
    MAP_FlashCtl_setWaitState(FLASH_BANK1, 2);
    CS_startHFXT(false);

    /* Initialising MCLK to HFXT (effectively 48MHz) */
    MAP_CS_initClockSignal(CS_MCLK, CS_HFXTCLK_SELECT, CS_CLOCK_DIVIDER_1);

    /* Set SysTick to 48MHz/1200=40kHz*/
    MAP_SysTick_enableModule();
    MAP_SysTick_setPeriod(960); // 960 for 50kHz
    MAP_Interrupt_enableSleepOnIsrExit();
    MAP_SysTick_enableInterrupt();

    /* Initialising ADC (MCLK/1//1) */
    MAP_ADC14_enableModule();
    MAP_ADC14_initModule(ADC_CLOCKSOURCE_MCLK, ADC_PREDIVIDER_1, ADC_DIVIDER_1, 0);

    /* Configuring GPIOs (P5.0 (A5) as the ADC input pin) */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P5, GPIO_PIN0, GPIO_TERTIARY_MODULE_FUNCTION);

    /* Configure ADC Resolution */
    ADC14_setResolution(ADC_10BIT);

    /* Configuring ADC Memory */
    MAP_ADC14_configureSingleSampleMode(ADC_MEM0, true);
    MAP_ADC14_configureConversionMemory(ADC_MEM0, ADC_VREFPOS_INTBUF_VREFNEG_VSS, ADC_INPUT_A5, false);

    /* Configuring Sample Timer */
    MAP_ADC14_enableSampleTimer(ADC_MANUAL_ITERATION);


     /* Enabling MASTER interrupts */
    MAP_Interrupt_enableMaster();

    while (1){
        //MAP_PCM_gotoLPM0();
    }
}

float bandstop_filter(float value){
//    Z1[0] = Z1[1];
//    Z1[], Z1[2], value};
//    Z2[] = {Z2[1], Z2[2], (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[1]) + (  0.959f * Z2[2])};
    Z1[0][0] = Z1[0][1];
    Z1[0][1] = Z1[0][2];
    Z1[0][2] = value;
    Z1[1][0] = Z1[1][1];
    Z1[1][1] = Z1[1][2];
//    Z2[2] =   (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[0]) + (  0.959f * Z2[1]);
    Z1[1][2] = (Z1[0][0] + Z1[0][2]) - A1[0] * Z1[0][1] + (-1*A1[1] * Z1[1][0]) + (A1[2] * Z1[1][1]);
    return Z1[1][2];
}

//float bandstop_filter2(float value){
////    Z1[0] = Z1[1];
////    Z1[], Z1[2], value};
////    Z2[] = {Z2[1], Z2[2], (Z1[0] + Z1[2]) -   1.080f * Z1[1] + (-0.775f * Z2[1]) + (  0.959f * Z2[2])};
//    Z2[0][0] = Z2[0][1];
//    Z2[0][1] = Z2[0][2];
//    Z2[0][2] = value;
//    Z2[1][0] = Z2[1][1];
//    Z2[1][1] = Z2[1][2];
////    Z2[2] =   (Z2[0] + Z2[2]) -   1.080f * Z2[1] + (-0.775f * Z2[0]) + (  0.959f * Z2[1]);
//    Z2[1][2] =   (Z2[0][0] + Z2[0][2]) - A2[0] * Z2[0][1] + (-1*A2[1] * Z2[1][0]) + (A2[2] * Z2[1][1]);
//    return Z2[1][2];
//}
float lowpass_filter(float value) {
    Z2[0][0] = Z2[0][1];
    Z2[0][1] = Z2[0][2];
    Z2[0][2] = value / 3.589405582f;
    Z2[1][0] = Z2[1][1];
    Z2[1][1] = Z2[1][2];
//    Z2[1][2] = (Z2[0][0] + Z2[0][2]) + 2 * Z2[0][1]  + ( -0.4918122372 * Z2[1][0]) + (  1.3072850288 * Z2[1][1]);;
    Z2[1][2] = (Z2[0][0] + Z2[0][2]) + 2 * Z2[0][1] + ( -0.5869195081f * Z2[1][0]) + (  1.4754804436f * Z2[1][1]);

//            (Z2[0][0] + Z2[0][2]) -   1.4547966009f * Z2[0][1] + ( -0.2905268567f * Z2[1][0]) + (  0.9387270423f * Z2[1][1]);
    return Z2[1][2];
}


void SysTick_Handler(void)
{
    P6OUT |= BIT0;
    float buffer = ADC14_getResult(ADC_MEM0);  //Get the conversion result.


    buffer = bandstop_filter(buffer);// * sine[sine_index];
//    buffer -= 512;
    buffer = buffer * sine[sine_index];
//    buffer += 512;
    buffer = lowpass_filter(buffer);

//    float buffer = 256*(1+sine[sine_index]);

    P4OUT = buffer / 4;  //We do this because the ADC is set to use 10 bits but P2OUT is only 8 bits.

    /* calculating sine index */
    sine_index++;
    if (sine_index >= SINE_SIZE) sine_index = 0;

    /* Enabling/Toggling Conversion */
    MAP_ADC14_enableConversion();
    MAP_ADC14_toggleConversionTrigger();
    MAP_ADC14_toggleConversionTrigger();
    P6OUT &= ~BIT0;
}

void ADC14_IRQHandler(void)
{
    uint64_t status = MAP_ADC14_getEnabledInterruptStatus();
    MAP_ADC14_clearInterruptFlag(status);
}