#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_ints.h"
#include "inc/hw_gpio.h"
#include "driverlib/adc.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/rom.h"
#include "driverlib/debug.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"
#include "driverlib/pin_map.h"

#define sampFreq 10000 //Sampling freq
#define sizeSample 2500 //Sample size
#define tLag 300 //Acorr length

int endCatch = 0, endSpin = 0, start = 0, state = 0, debounceOk = 1; //Flags
int peak = 0, peak1 = 0, peak2 = 0, ori = 3; //variables
float Sample[sizeSample] = { 0 }; //Stores the sample signal
float aCorr[tLag + 1] = { 0 }; //Stores the autocorrelation
int sampleNumber = 0; //Sample counter
uint32_t s = 0; //ADC value

void press(void);
void depress(void);

void sampling(void) //Sample record ISR
{
    ADCIntClear(ADC0_BASE, 3);
    if (sampleNumber < sizeSample)
    {
        ADCSequenceDataGet(ADC0_BASE, 3, &s);
        Sample[sampleNumber] = s;
        sampleNumber++;
    }
    else
    {
        TimerDisable(TIMER0_BASE, TIMER_A);
        TimerLoadSet(TIMER0_BASE, TIMER_A, (SysCtlClockGet() / sampFreq) - 1);
        endCatch = 1;
    }
}

void motorTimeout(void) //Stop timed motor spin ISR
{
    TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
    TimerDisable(TIMER1_BASE, TIMER_A);
    endSpin = 1;
}

void press(void) //pressed button ISR
{
    if (debounceOk == 0)
    {
        GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_0);
        GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_1);
        GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_2);
    }
    if (debounceOk == 1)
    {
        state = 1; //pressed
        debounceOk = 0;
        TimerEnable(TIMER2_BASE, TIMER_A);
        if (GPIOIntStatus(GPIO_PORTB_BASE, false) & GPIO_PIN_0) //button 1 pressed
        {
            GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_0);
            ori = 2; //ccw
        }
        else if (GPIOIntStatus(GPIO_PORTB_BASE, false) & GPIO_PIN_1) //button 2 pressed
        {
            GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_1);
            ori = 1; //cw
        }
        else if (GPIOIntStatus(GPIO_PORTB_BASE, false) & GPIO_PIN_2) //button 3 pressed
        {
            GPIOIntClear(GPIO_PORTB_BASE, GPIO_INT_PIN_2);
            start = 1;
        }
    }
}

void depress(void) //de-pressed button ISR
{
    GPIOIntClear(GPIO_PORTB_BASE,
    GPIO_INT_PIN_0 | GPIO_INT_PIN_1 | GPIO_INT_PIN_2);
    if (debounceOk == 1)
    {
        state = 0; //de pressed
        debounceOk = 0;
        TimerEnable(TIMER2_BASE, TIMER_A);
        ori = 0;
    }
}

void debounce(void) //Debounce button ISR
{
    GPIOIntClear(GPIO_PORTB_BASE,
    GPIO_INT_PIN_0 | GPIO_INT_PIN_1 | GPIO_INT_PIN_2);
    TimerDisable(TIMER2_BASE, TIMER_A);
    TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
    TimerLoadSet(TIMER2_BASE, TIMER_A, (SysCtlClockGet() / 1000) - 1); //1ms debounce
    debounceOk = 1;
    if (state == 0) //De-pressed button
    {
        GPIOIntRegister(GPIO_PORTB_BASE, press);
        GPIOIntTypeSet(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2,
        GPIO_FALLING_EDGE);
    }
    else if (state == 1) //Pressed button
    {
        GPIOIntRegister(GPIO_PORTB_BASE, depress);
        GPIOIntTypeSet(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2,
        GPIO_RISING_EDGE);
    }
}

int sigPro(float samp[], float Corr[], int lLim, int hLim) //Signal processing function
{
    int k = 0, i = 0, t = 0, n = 0;
    float mean = 0;
    float c0 = 0;
    float temp = 0, stdDeviation = 0;
    for (i = 0; i < sizeSample; i++) //find average of array
    {
        mean = mean + samp[i];
    }
    mean = mean / (sizeSample);
    for (n = 0; n < sizeSample; n++) //std deviation
    {
        stdDeviation = pow(samp[n] - mean, 2) + stdDeviation;
    }
    stdDeviation = sqrt(stdDeviation / (float) (sizeSample - 1));
    if (stdDeviation > 30) //Use of std deviation to discard open string samples
    {
        for (k = 0; k < tLag + 1; k++) //calculate autocorrelation
        {
            temp = 0;
            for (t = 0; t < sizeSample - k; t++)
            {
                temp = (samp[t] - mean) * (samp[t + k] - mean) + temp;
            }
            if (k == 0)
            {
                c0 = temp / (sizeSample);
                Corr[k] = 1;
            }
            else
            {
                Corr[k] = temp / (sizeSample * c0);
            }
        }
        int peak = lLim, n = 0;
        for (n = lLim; n < hLim; n++)
        {
            if (Corr[peak] < Corr[n])
            {
                peak = n;
            }
        }
        return peak;
    }
    else
    {
        peak = 0;
        return peak;
    }
}

void spMot(int orientation, float secs) //Control motor(CW-CCW-Stop) function
{
    if (orientation == 0)  //stop
    {
        GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, 0);
        GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, 0);
    }
    else
    {
        if (orientation == 1)  //cw
        {
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, 0);
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, GPIO_PIN_5);
        }
        else if (orientation == 2)  //ccw
        {
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, GPIO_PIN_4);
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, 0);
        }
        //timer 1 setup #2 (Enable)
        TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet() * secs);
        TimerEnable(TIMER1_BASE, TIMER_A);
    }
}

int inTuneAcorr(int strNum) //Find aCorr in tune value for the respective string plucked function (To be completed)
{
    int aCorrVal = 0;
    switch (strNum)
    {
    case 1:  //High E
        aCorrVal = 121;
        break;
    case 2:  //a
        break;
    case 3:  //d
        break;
    case 4:  //g
        break;
    case 5:  //b
        break;
    case 6:  //Low e
        break;
    }
    return aCorrVal;
}

int strPkFind(int strNum) //Peak value finder(from aCorr) function
{
    int peakResult = 0;
    switch (strNum)
    {
    case 1:  //High E
        peakResult = sigPro(Sample, aCorr, 60, 160);
        break;
    case 2:  //a
        break;
    case 3:  //d
        break;
    case 4:  //g
        break;
    case 5:  //b
        break;
    case 6:  //Low e
        break;
    }
    return peakResult;
}

float spnTmFind(int strNum, int ori) //Find required time to spin the motors function
{
    float sT = 0;
    switch (strNum)
    {
    case 1:  //High E
        if (ori == 0)  //ccw spin
        {
            sT = (0.0546 * peak) - 6.6039;
            //sT = (0.0512 * peak) - 6.1316;
        }
        else if (ori == 1) //cw spin
        {
            sT = 6.2936 - (0.052 * peak);
            //sT =5.5915 - (0.045 * peak);
        }
        break;
    case 2:  //a
        break;
    case 3:  //d
        break;
    case 4:  //g
        break;
    case 5:  //b
        break;
    case 6:  //Low e
        break;
    }
    return sT;
}

void autoTune(int strNum) //General auto-tune function
{
    TimerEnable(TIMER0_BASE, TIMER_A);
    int move = 0;
    while (move <= 1)
    {
        if (endCatch == 1)    //Sample recorded and processing signal
        {
            sampleNumber = 0;
            endCatch = 0;
            peak = strPkFind(strNum);
            if (peak > inTuneAcorr(strNum))    //sharp(ccw spin needed)
            {
                spMot(2, spnTmFind(strNum, 0));
            }
            else if (peak < inTuneAcorr(strNum)) //flat(cw spin needed)
            {
                spMot(1, spnTmFind(strNum, 1));
            }
            else
            {
                endSpin = 1;
            }
        }
        else if (endSpin == 1) //Motor spin stop
        {
            spMot(0, 0); //Stop motor
            endSpin = 0;
            move++;
            TimerEnable(TIMER0_BASE, TIMER_A);
        }
    }
}

int main(void)
{
//general setup
    SysCtlClockSet(
    SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); //40Mhz clock
//buttons setup
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
    GPIOPinTypeGPIOInput(GPIO_PORTB_BASE,
    GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2);
    GPIOPadConfigSet(GPIO_PORTB_BASE,
    GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2,
                     GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);
    GPIOIntDisable(GPIO_PORTB_BASE,
    GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2);
    GPIOIntClear(GPIO_PORTB_BASE,
    GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2);
    GPIOIntRegister(GPIO_PORTB_BASE, press);
    GPIOIntTypeSet(GPIO_PORTB_BASE,
    GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2,
                   GPIO_FALLING_EDGE);
    GPIOIntEnable(
    GPIO_PORTB_BASE,
                  GPIO_INT_PIN_0 | GPIO_INT_PIN_1 | GPIO_INT_PIN_2);
//timer 0 setup (Sampling)
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);    //sampling
    TimerConfigure(TIMER0_BASE, TIMER_CFG_A_PERIODIC);
    TimerLoadSet(TIMER0_BASE, TIMER_A, (SysCtlClockGet() / sampFreq) - 1);
    TimerControlTrigger(TIMER0_BASE, TIMER_A, true);
//ADC setup(Sampling)
    SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
    GPIOPinTypeADC(GPIO_PORTD_BASE, GPIO_PIN_0);
    GPIOPadConfigSet(GPIO_PORTD_BASE, GPIO_PIN_0, GPIO_STRENGTH_2MA,
    GPIO_PIN_TYPE_STD_WPD);
    ADCSequenceConfigure(ADC0_BASE, 3, ADC_TRIGGER_TIMER, 1);
    ADCSequenceStepConfigure(ADC0_BASE, 3, 0,
    ADC_CTL_CH7 | ADC_CTL_IE | ADC_CTL_END);
    ADCIntRegister(ADC0_BASE, 3, sampling);
    ADCSequenceEnable(ADC0_BASE, 3);
    ADCIntEnable(ADC0_BASE, 3);
    IntEnable(INT_ADC0SS3);
//GPIO setup (Motor)
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_4 | GPIO_PIN_5);
//timer 1 setup #1 (Motor timer)
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
    TimerConfigure(TIMER1_BASE, TIMER_CFG_ONE_SHOT);
    TimerIntRegister(TIMER1_BASE, TIMER_A, motorTimeout);
    IntEnable(INT_TIMER1A);
    TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
//timer 2 setup (Debounce)
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);    //debounce
    TimerConfigure(TIMER2_BASE, TIMER_CFG_ONE_SHOT);
    TimerLoadSet(TIMER2_BASE, TIMER_A, (SysCtlClockGet() / 1000) - 1);
    TimerIntRegister(TIMER2_BASE, TIMER_A, debounce);
    IntEnable(INT_TIMER2A);
    TimerIntEnable(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
//Master int setup
    IntMasterEnable();
    while (1)
    {

        if (start == 1)
        {
            autoTune(1);
            start = 0;
        }
        else if (ori == 0)    // stop motor
        {
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, 0);
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, 0);
            ori = 3;
        }
        else if (ori == 1)     //Manual spin cw
        {
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, 0);
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, GPIO_PIN_5);
            ori = 3;
        }
        else if (ori == 2)     //Manual spin ccw
        {
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_4, GPIO_PIN_4);
            GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_5, 0);
            ori = 3;
        }
    }
}