/*
 * ECE 4760 Final Project
 * Fall 2015 (11/16-12/8)
 * Daniel Eddowes (dme67) and Manish Patel (mkp53)
 * Custom Filter Design and Automatic Signal Transformations
 * Using Fast Fourier Transform and Parks McClellan Algorithms
 */

#define	SYS_FREQ    40000000            //System Operating Frequency
#define _SUPPRESS_PLIB_WARNING 1

//Include Device, Threading, TFT, Touch Screen, FFT, and C Dependent Libraries
#include "config.h"
#include "pt_cornell_TFT.h"
#include "tft_master.h"
#include "tft_gfx.h"
#include "touch_master.h"
#include "filter.h"
#include "dsplib_dsp.h"
#include "fftc.h"
#include <math.h>
#include <stdlib.h>
#include <GenericTypeDefs.h>

//UINT32 init_time;
//#define tic() {init_time = PT_GET_TIME();}
//#define toc() (PT_GET_TIME() - init_time)

//Touch Screen Constants
volatile UINT16 XMIN=123, YMIN=101, XMAX=810, YMAX=865, x_span=687, y_span=764;

//State Machine Definitions
static UINT8 state = 0, FFT_MODE = 0;
#define STATE_START         0
#define FFT_SELECT_STATE    1
#define AMP_FREQ            2
#define FFT_DRAW_STATE      3
#define PM_INSTRUCTION      4
#define FFT_INSTRUCTION     5
#define PROCESS_INPUT       6
#define FFT_COMPUTE         7
#define PM_COMPUTE          8
#define OUTPUT              9
#define FWT_MENU_STATE   	10

//Protothread Definitions
static struct pt pt_touch, pt_state;

//debugging function, prints touch coordinates to screen
void debug(void){
#define PRINT_POINT

#ifdef PRINT_POINT
	sprintf(buffer, "%d, %d", touch_point.X, touch_point.Y);
	tft_setTextColor(ILI9340_GREEN);
	tft_setTextSize(2);
	tft_fillRect(0,250, 230, 20, BLACK);
	tft_setCursor(0, 150);
	tft_writeString(buffer);
#endif
}

//protothread to read touch input
static PT_THREAD (protothread_touch(struct pt *pt))
{
    PT_BEGIN(pt);
	
    do{
        PT_YIELD_TIME_msec(5);  // yield time 5 milliseconds
        touch_getPoint();

        if (touch_point.DONE && touch_point.Z) {    //valid input
            //debug();
            if (touch_point.X >= XMIN && touch_point.X <= XMAX) //valid domain
                touch_point.X = touch_point.X /*- XMIN*/;
            else
                touch_point.Z = 0;	// make it an invalid touch

            if (touch_point.Y >= YMIN && touch_point.Y <= YMAX) //valid range
                touch_point.Y = touch_point.Y /*- YMIN*/;
            else
                touch_point.Z = 0;	// make it an invalid touch
        }
    } while (touch_point.DONE==0 || touch_point.Z==0);

    PT_EXIT(pt);
	PT_END(pt);
}


//protothread to handel state machine
static PT_THREAD (protothread_state(struct pt *pt))
{
    PT_BEGIN(pt);
	
	state = STATE_START;    //initialize state
    k = 0;
    u = 0;
	
	while (1){
		PT_YIELD_TIME_msec(50);
        
        //********************* START_STATE ********************//
		if (state == STATE_START) {
        //********************* START_STATE ********************//
            drawState_Start();  //draw state graphics

            //request input from user
            touch_reset();
            PT_SPAWN(pt, &pt_touch, protothread_touch(&pt_touch));

            /*drawing for next state (mostly) always done in previous state
             * and not in that state b/c next state would continually refresh 
             * graphics w/0 use of annoying flags  */
            if(touch_point.Y < YMAX/2){ //touch in top half of screen
                state = FFT_SELECT_STATE;
                drawFFT_Select_State(); //draw FFT selection state
            }
            else {  //bottom half
                state = FWT_MENU_STATE;
                drawFWT_Menu_State();   //draw FWT menu state
            }
		}
        

        //********************* FFT_SELECT_STATE ********************//
        else if (state == FFT_SELECT_STATE) {
        //********************* FFT_SELECT_STATE ********************//
            //request input from user
            touch_reset();
			PT_SPAWN(pt, &pt_touch, protothread_touch(&pt_touch));            
            
            if (touch_point.Y < 180 && touch_point.X < 280) {   //press back button
                state = STATE_START;
            }
            else if(touch_point.Y < YMAX/2){    //press top half of screen
                state = AMP_FREQ;
                drawAmp_Freq();
                FFT_MODE = 1;   //signifies standard FFT filter design
            }
            else {
                state = AMP_FREQ;   //press bottom half of screen
                drawAmp_Freq();
                FFT_MODE = 2;   //PM filter design
            }
        }

        
        //********************* AMP_FREQ ********************//
        else if (state == AMP_FREQ) {
        //********************* AMP_FREQ ********************//
            //request input from user
            touch_reset();
			PT_SPAWN(pt, &pt_touch, protothread_touch(&pt_touch));

            for (j=1;j<6;j++){	
                /*screen divided into 5 regions below y pixel value 360 
                 allows user to specify freq and amp values*/
                if(touch_point.Y >= (360+93*(j-1)) && touch_point.Y < (453+93*(j-1))) {
                    if (touch_point.X < 500) {  //left half of screen, freq
                        prev_freqsel = max_freqmag; //get prev val to change txt color
                        max_freqmag = j;
                        max_freqval = freqTbl[j];   //find freq value

                        tft_setTextColor(BLUE);
                        tft_setTextSize(2);
                        updateFreqText((char)(max_freqmag+64)); //fill buffer w/ selected txt
                        centerTxt(140+36*(j-1),2,0);    //center and print text
                        
                        if (prev_freqsel != 0) {    //change prev selection back
                            tft_setTextColor(WHITE);
                            updateFreqText((char)(prev_freqsel+64));
                            centerTxt(140+36*(prev_freqsel-1),2,0);
                        }
                        k++;
                    }
                    else {  //right half, amp
                        prev_ampsel = max_ampval;
                        max_ampval = j; //get amp value (linear, nice!)

                        tft_setTextColor(BLUE);
                        tft_setTextSize(2);
                        updateAmpText((char)(max_ampval+64)); //fill buffer w/ selected txt
                        centerTxt(140+36*(j-1),2,1);    //center and print
                        
                        if (prev_ampsel != 0) {     //change prev selection back
                            tft_setTextColor(WHITE);
                            updateAmpText((char)(prev_ampsel+64),1);
                            centerTxt(140+36*(prev_ampsel-1),2,1);
                        }
                        
                        u++;
                    }
                }
            }            
            
            if (touch_point.Y < 180 && touch_point.X < 280) {   //press back button
                state = FFT_SELECT_STATE;   //go to 
                drawFFT_Select_State();
            }
            else if(touch_point.Y < 180 && touch_point.X > 680) {   //go forward
                if (u >= 1 && k >= 1) { //things have been pressed
                    if (FFT_MODE == 1) { state = FFT_INSTRUCTION; }
                    else if (FFT_MODE ==2) { state = PM_INSTRUCTION; }
                }
                else {  //error checking must have two things selected
                    tft_setTextColor(BIGRED);
                    sprintf(buffer, "SELECT TWO");
                    centerTxt(10,1,0);
                    sprintf(buffer, "VALUES!");
                    centerTxt(30,1,0);
                    delayMS(250);
                    
                    resetScreen();  //reset state
                    drawAmp_Freq();
                }
            }
        }
        
        
        //********************* FFT_INSTRUCTION ********************//
        else if (state == FFT_INSTRUCTION) {
        //********************* FFT_INSTRUCTION ********************//
            //Write Instructions
            drawInstruction();
            sprintf(buffer, "Freq (Hz) v Amp");
            centerTxt(110,1,0);
            sprintf(buffer, "DRAW ANYTHING!");
            centerTxt(130,1,0);
            sprintf(buffer, "RESULT WILL VARY!");
            centerTxt(150,1,0);

            delayMS(500);

            //Wait for press to advance
            resetScreen();
            state = FFT_DRAW_STATE;
            drawFFT_Draw_State();
        }
        
        
        //********************* PM_INSTRUCTION ********************//
        else if (state == PM_INSTRUCTION) {
        //********************* PM_INSTRUCTION ********************//
            //Write Instructions
            drawInstruction();
            sprintf(buffer, "x (0-pi) v Amp");
            centerTxt(110,1,0);
            sprintf(buffer, "x is samp rate");
            centerTxt(130,1,0);
            sprintf(buffer, "LIMIT PASS/STOP");
            centerTxt(150,1,0);
            sprintf(buffer, "BANDS TO THREE!");
            centerTxt(170,1,0);
             
            delayMS(500);

            //Wait for press to advance
            resetScreen();
            state = FFT_DRAW_STATE;
            drawFFT_Draw_State();
        }
        
        
        //********************* FFT_DRAW_STATE ********************//
		else if (state == FFT_DRAW_STATE) {
        //********************* FFT_DRAW_STATE ********************//
			touch_reset();  //get user press
			PT_SPAWN(pt, &pt_touch, protothread_touch(&pt_touch));
            
            //Get transfer function information
            x = (320*touch_point.Y)/y_span - 40;    //scale to pixels
            y = (240*touch_point.X)/x_span - 40;
            
            if(x<40 && y > 200) {      //Go Back
                state = AMP_FREQ;
                drawAmp_Freq();
            }
            else if(x>280 && y>200) {    //Go Forward
                state = PROCESS_INPUT;
            }
            else if(x>120 && x<200 && y>200) {  //Resfresh pressed
                for(j=0;j<320;j++) { bode[j] = 0; }
                resetScreen();
                drawFFT_Draw_State();
            }
            else if(bode[x]==0) {   //fill bode array, can't write over data
                bode[x] = y;
                tft_fillCircle(x,240-y,2,BLUE); //make blue circle indicator
            }
		}
        
        
        //********************* FFT_PROCESS_INPUT ********************//
		else if (state == PROCESS_INPUT) {
        //********************* FFT_PROCESS_INPUT ********************//
            findBodeNonZero();  //Find Nonzero elements            

            filterMedian();         //5 element Median Filter
            drawFFT_Draw_State();
            for(i=0;i<numNZ;i++) {
                tft_fillCircle(xNZ[i],240-yNZ[i],2,BLUE);
            }
            delayMS(1000);
            
            linearInterpolate();    //Interpolate
            drawProcess_Input();
            filterMovAvg();         //4 element Moving Average (LP) Filter
            drawProcess_Input();    
            
            if (FFT_MODE == 2) {
                discreteDerivative(); //Discrete Derivative

                for(i=0;i<320;i++) { xNZ[i] = 0; yNZ[i] = 0; }
                //Determine Bands
                j = 0;
                for(i=0;i<80;i++) {
                    if(ddx[i]<3) {  //if there is little change value to value
                        tft_fillCircle(i*4,240-bode[i*4],2,BIGRED);
                        xNZ[j] = i*4;   //fill nonzero element arrays
                        yNZ[j] = bode[i*4];
                        j++;
                    }
                }
                delayMS(1000);
                
                j = 0;
                for(i=0;i<320;i++) {    
                    bode[i] = 0;    //reset bode array
                    if (i == xNZ[j]) {  //fill it back up with nonzero elements
                        bode[i] = yNZ[j];
                        j++;
                    }
                }
                
                //Second Iteration
                findBodeNonZero();  //Find Nonzero elements            

                filterMedian();         //5 element Median Filter
                drawFFT_Draw_State();
                for(i=0;i<numNZ;i++) {
                    tft_fillCircle(xNZ[i],240-yNZ[i],2,BLUE);
                }
                delayMS(1000);

                linearInterpolate();    //Interpolate
                drawProcess_Input();
                filterMovAvg();         //4 element Moving Average (LP) Filter
                drawProcess_Input();    
                
                discreteDerivative();
                
                for(i=0;i<320;i++) { xNZ[i] = 0; yNZ[i] = 0; }
                
                p = 0;
                for(i=0;i<80;i++) {
                    if(ddx[i]<3) {  //within threshold
                        tft_fillCircle(i*4,240-bode[i*4],2,BIGRED);
                        xNZ[p] = i*4;   //fill nonzero arrays
                        yNZ[p] = bode[i*4];
                        p++;
                    }
                }
                delayMS(1000);
                
                xband[0] = 0;       //set initial band start to zero
                yband[0] = yNZ[0];
                i = 1;
                j = 1;
                while (xNZ[i] != 0) {
                    if (abs(yNZ[i-1] - yNZ[i]) > 10) {  //large change in nonzero values
                        xband[j] = xNZ[i-1];    //fill up bands
                        xband[j+1] = xNZ[i];
                        yband[j] = yNZ[i-1];
                        yband[j+1] = yNZ[i];
                        tft_fillCircle(xband[j],240-yNZ[i-1],3,GREEN);  //draw indication
                        delayMS(500)
                        tft_fillCircle(xband[j+1],240-yNZ[i],3,GREEN);
                        delayMS(500);
                        j = j+2;
                    }
                    i++;
                }
                xband[j] = 320;     //set last band to 320 (pi)
                yband[j] = yNZ[p-1];
            }
            
            if (FFT_MODE == 2) { conditionCheck(); }    //check errors
            
            if(errorflg == 1) { //error exists
                errorflg = 0;
                for(i=0;i<320;i++) { bode[i] = 0; } //reset bode
                for(i=0;i<6;i++) { //reset bands
                    xband[i] = 0;
                    yband[i] = 0;
                }
                resetScreen();
                drawFFT_Draw_State();   //go back a state
                state = FFT_DRAW_STATE;
            }
            else if(errorflg == 0) {    //no errors
                scaleBands();   //scale bands
                if (FFT_MODE == 1) { state = FFT_COMPUTE; } //change state based on FFT mode
                else if (FFT_MODE == 2) { state = PM_COMPUTE; }
            }
		}
        
        
        //********************* FFT_COMPUTE ********************//
        else if (state == FFT_COMPUTE) {
        //********************* FFT_COMPUTE ********************//
            for(i=0;i<32;i++) { //grab points from plot
                M[i] = (float)bode[i*10]*(float)max_ampval/200.0;
//                sprintf(buffer, "%.3f", M[i]);
//                tft_setCursor(0,0);
//                tft_writeString(buffer);
//                delay_ms(1000);
//                tft_fillScreen(ILI9340_BLACK);
            }
            M[33] = (float)bode[319]*(float)max_ampval/200.0;
            filterDesign(); //FFT filter design
            
            hLength = 64;   //set filter length
            
            drawCompute();
            //startTimers();
            //start timers allowing ADC and DAC interrupts
            OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_8, ADC_PDR[max_freqmag]);
            ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
            OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_8, ADC_PDR[max_freqmag]);
            ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_2);
            SetChanADC10(ADC_CH0_POS_SAMPLEA_AN0);  //set correct ADC channel
            state = OUTPUT;
        }
        
        
        //********************* PM_COMPUTE ********************//
        else if (state == PM_COMPUTE) {
        //********************* PM_COMPUTE ********************//            
            pm(0.0, M_PI, XBAND, YBAND, numBands, h);   //Parks McClellan filter design
            
            if (numBands == 2) { hLength = 35; }    //set filter length
            else if (numBands == 3) { hLength = 59; }
            
            drawCompute();
            //startTimers();
            //start timers allowing ADC and DAC interrupts
            OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_8, ADC_PDR[max_freqmag]);
            ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
            OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_8, ADC_PDR[max_freqmag]);
            ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_2);
            SetChanADC10(ADC_CH0_POS_SAMPLEA_AN0);  //set correct ADC channel
            state = OUTPUT;
        }
        
        
        //********************* OUTPUT ********************//
        else if (state == OUTPUT) {
        //********************* OUTPUT ********************//
            ConfigINT0(EXT_INT_PRI_2 | RISING_EDGE_INT | EXT_INT_ENABLE);   //enable external interrupt
            tft_fillScreen(BLACK);
            tft_setTextSize(2); //print status of state
            sprintf(buffer, "Convolving the");
            centerTxt(40,1,0);
            sprintf(buffer, "Input Signal . . .");
            centerTxt(60,1,0);
            sprintf(buffer, "Press External");
            centerTxt(100,1,0);
            sprintf(buffer, "Button to Stop");
            centerTxt(120,1,0);
            
            int a = 0;              //convolution variable definitions
            fix16 conv = 0;
            fix16 cbuffer[hLength];
            int cIndex = 0;
            int hIndex = 0;
            fix16 min = int2fix16(5000);
            int flag = 0;           // if flag is high add abs(min) to DAC_VALUE
            
            
            // fill buffer at least once
            while(cIndex < hLength) {
                if(newValAvail == 1) {
                    cbuffer[cIndex++] = ADC_value*4;
                    newValAvail = 0;
                }
            }
            cIndex = 0;
            while(stopFilter != 1) {       // change this to while button not pressed
                if(newValAvail == 1) {
                    
                    cbuffer[cIndex++] = ADC_value*4;    //scale 10-bit to 12-bit
                    for(a=cIndex-1; a<hLength; a++) {   //do convolution
                        conv += cbuffer[a]*h[hIndex++];
                    }
                    for(a=0; a<cIndex-1; a++) {         //hIndex traversing array
                        conv += cbuffer[a]*h[hIndex++];
                    }
                    hIndex = 0;
                    
                    if(conv < min && conv < 0) {    //find min value
                        min = conv;
                    } 
                    
                    if(min < 0){    //Offset by min value, DC offset
                        DAC_VALUE = fix2int16(conv + abs(min));
                    }
                    else {
                        DAC_VALUE = fix2int16(conv);
                    }
                    //DAC_VALUE = fix2int16(conv);
                    
                    if(DAC_VALUE < 0) { //DAC boundary conditions
                        DAC_VALUE = 0;
                    }
                    else if(DAC_VALUE > 4095) {
                        DAC_VALUE = 4095;
                    }
                    
                    if(cIndex >= hLength) { //reset convolutions
                        cIndex = 0;
                    }
                    
                    conv = 0;
                    newValAvail = 0;
                    
                    }
                }
            
                if(stopFilter == 1) {   //external interrupt triggered
                    state = STATE_START;    //go back to beginning of state machine
                    ConfigINT0(EXT_INT_DISABLE);    //disable specific interrupts
                    ConfigIntTimer2(T2_INT_OFF);
                    ConfigIntTimer3(T3_INT_OFF);
                    delayMS(1);
                    stopFilter = 0;     //reset values
                    DAC_VALUE = 0;
                }
            }

       
        //********************* FWT_MENU_STATE ********************//
        else if (state == FWT_MENU_STATE) {
        //********************* FWT_MENU_STATE ********************//
            touch_reset();
			PT_SPAWN(pt, &pt_touch, protothread_touch(&pt_touch));
            
            if (touch_point.Y < 180 && touch_point.X < 280) {   //go back
                state = STATE_START;
                drawState_Start();
            }
		}
	}

	PT_END(pt);
}

void drawState_Start() {
    for(i=0;i<320;i++) {    //reset all the values
        bode[i] = 0;
        xNZ[i] = 0;
        yNZ[i] = 0;
    }
    for(i=0;i<80;i++) { ddx[i] = 0; }
    for(i=0;i<6;i++) {  //reset bands
        xband[i] = 0;
        yband[i] = 0;
        XBAND[i] = 0;
        YBAND[i] = 0;
    }
    max_freqmag = 0;    //reset freq and amp vals
    prev_freqsel = 0;
    prev_ampsel = 0;
    max_freqval = 0;
    max_ampval = 0;
    
    tft_setRotation(0); //set default rotation
    tft_fillScreen(BLACK);
    tft_fillRect(0, 155, SCREEN_WIDTH, 10, WHITE);

    tft_setTextSize(1); //write instructions
    tft_setTextColor(WHITE);
    sprintf(buffer, "Please Select a Transform");
    centerTxt(10,1,0);

    tft_setTextSize(2);     //draw options
    tft_setTextColor(BLUE);
    sprintf(buffer, "FOURIER\n");
    centerTxt(ILI9340_TFTHEIGHT/4,1,0);
    sprintf(buffer, "WAVELET\n");
    centerTxt(3*ILI9340_TFTHEIGHT/4,1,0);
}

void drawFFT_Select_State(void) {
    tft_setRotation(0); //default rotation
    tft_fillScreen(BLACK);
    tft_fillRect(0, 155, SCREEN_WIDTH, 10, WHITE);

    tft_setTextSize(1); //write instructions
    tft_setTextColor(WHITE);
    sprintf(buffer, "Please Select a FFT Variety");
    tft_setCursor(69,10);
    tft_writeString(buffer);

    tft_setTextSize(2); //write options
    tft_setTextColor(BLUE);
    sprintf(buffer, "STANDARD FFT\n");
    centerTxt(ILI9340_TFTHEIGHT/4,1,0);
    sprintf(buffer, "PARKS MCCLELLAN\n");
    centerTxt(3*ILI9340_TFTHEIGHT/4,1,0);
    tft_setTextSize(1);
    tft_setTextColor(WHITE);
    sprintf(buffer, "Results may vary, filter responsibly!");
    centerTxt(120,1,0);
    sprintf(buffer, "Follows the rules and the filter");
    centerTxt(280,1,0);
    sprintf(buffer, "never fails. . .maybe, we hope!");
    centerTxt(290,1,0);
    
    //Back Button
    tft_drawCircle(30,30,20,WHITE);
    tft_drawFastHLine(15,30,30,WHITE);
    tft_drawLine(15,30,30,15,WHITE);
    tft_drawLine(15,30,30,45,WHITE);
    
    FFT_MODE = 0;   //reset FFT_MODE
}

void drawAmp_Freq() {
    tft_fillScreen(BLACK);
    tft_setRotation(0);     //default rotation

    drawForwardBack();

    //Write Instructions and Table
    tft_setTextColor(WHITE);    //write instructions
    tft_setTextSize(1);
    sprintf(buffer, "Select the max values for amplitude and freq");
    centerTxt(60,1,0);
    sprintf(buffer, "selected freq corresponds to 1/2 Nyquist");
    centerTxt(70,1,0);
    sprintf(buffer, "min values are zero");
    centerTxt(80,1,0);
    tft_setTextSize(2);
    sprintf(buffer, "FREQ");
    tft_setCursor(42, 100);
    tft_writeString(buffer);
    sprintf(buffer, "AMP");
    tft_setCursor(162, 100);
    tft_writeString(buffer);
    tft_drawFastHLine(0,120,SCREEN_WIDTH,WHITE);
    tft_drawFastVLine(120,120,220,WHITE);

    //Draw frequencies, column 0
    tft_setTextSize(2);
    sprintf(buffer, "100 Hz");
    centerTxt(140,2,0);
    sprintf(buffer, "1 kHz");
    centerTxt(140+36,2,0);
    sprintf(buffer, "5 kHz");
    centerTxt(140+72,2,0);
    sprintf(buffer, "10 kHz");
    centerTxt(140+108,2,0);
    sprintf(buffer, "20 kHz");
    centerTxt(140+144,2,0);
    //draw amplitudes, column 1
    sprintf(buffer, "1");
    centerTxt(140,2,1);
    sprintf(buffer, "2");
    centerTxt(140+36,2,1);
    sprintf(buffer, "3");
    centerTxt(140+72,2,1);
    sprintf(buffer, "4");
    centerTxt(140+108,2,1);
    sprintf(buffer, "5");
    centerTxt(140+144,2,1);
    
    u = 0;
    k = 0;
}

void drawFFT_Draw_State() {
    tft_fillScreen(BLACK);
    tft_fillRect(0,40,320,3,WHITE);

    x_pos = 32; //get and draw dashed lines
    while (x_pos<315) {
        drawVDash(x_pos);   //draw vertical dash
        x_pos = x_pos + 32;
    }   
    y_pos = 200/max_ampval + 40;
    while (y_pos<320) {
        drawHDash(y_pos);   //draw horizontal dash
        y_pos = y_pos + 200/max_ampval;
    }
    
    //Back Button
    tft_fillRect(0,0,40,40,BLACK);
    tft_drawCircle(20,20,15,WHITE);
    tft_drawFastHLine(10,20,20,WHITE);
    tft_drawLine(10,20,20,10,WHITE);
    tft_drawLine(10,20,20,30,WHITE);

    //Forward Button
    tft_fillRect(280,0,40,40,BLACK);
    tft_drawCircle(300,20,15,WHITE);
    tft_drawFastHLine(290,20,20,WHITE);
    tft_drawLine(310,20,300,10,WHITE);
    tft_drawLine(310,20,300,30,WHITE);
    
    //Refresh Button
    tft_setTextSize(2);
    tft_setTextColor(WHITE);
    sprintf(buffer, "RERESH");
	tft_setCursor(124,10);
	tft_writeString(buffer);
}

void drawInstruction() {
    tft_setRotation(1); //90 degree CCW rotation
    tft_fillScreen(BLACK);
    tft_setTextColor(WHITE);
    tft_setTextSize(2);
    
    sprintf(buffer, "On the next screen");  //write instructions
    centerTxt(10,1,0);
    sprintf(buffer, "draw a transfer func");
    centerTxt(30,1,0);
    sprintf(buffer, "x&y linear scale w/");
    centerTxt(70,1,0);
    sprintf(buffer, "specified ranges");
    centerTxt(90,1,0);
    
    sprintf(buffer, "Press anywhere to");
    centerTxt(190,1,0);
    sprintf(buffer, "Continue");
    centerTxt(210,1,0);
    
    //Draw Axis Orientation
    tft_drawFastVLine(260,100,40,WHITE);
    tft_drawFastHLine(260,140,40,WHITE);
    tft_drawLine(260,100,255,105,WHITE);
    tft_drawLine(260,100,265,105,WHITE);
    tft_drawLine(300,140,295,145,WHITE);
    tft_drawLine(300,140,295,135,WHITE);
    sprintf(buffer, "y");
    tft_setCursor(255,80);
    tft_writeString(buffer);
    sprintf(buffer, "x");
    tft_setCursor(305,130);
    tft_writeString(buffer);  
}

void drawProcess_Input() {
    drawFFT_Draw_State();   //redraw FFT Draw State
    for(i=0;i<320;i++) {
        tft_fillCircle(i,240-bode[i],1,BLUE);   //update bode plot
    }
    delayMS(1000);
}

void drawCompute() {
    tft_setRotation(0); //default rotation
    tft_fillScreen(BLACK);
    tft_setTextSize(2);
    sprintf(buffer, "Filter Coefficients:");    //write what is going on
    centerTxt(40,1,0);

    int p = 0;
    for(p=0; p<hLength; p++) {  //print all filter coefficients
        sprintf(buffer, "%d", h[p]);
        centerTxt(60,1,0);
        delayMS(400);
        tft_fillRect(60,60,120,20,BLACK);
    }
}

void drawFWT_Menu_State() {
    tft_fillScreen(BLACK);  //Just write two lines of text for FWT
    tft_setTextColor(WHITE);
    tft_setTextSize(1);
    sprintf(buffer, "FWT");
    centerTxt(120,1,0);
    sprintf(buffer, "UNDER CONSTRUCTION");
    centerTxt(140,1,0);
    
    drawForwardBack();
}

void drawForwardBack() {    //circle and three lines
    //Back Button
    tft_drawCircle(30,30,20,WHITE);
    tft_drawFastHLine(15,30,30,WHITE);
    tft_drawLine(15,30,30,15,WHITE);
    tft_drawLine(15,30,30,45,WHITE);

    //Forward Button
    tft_drawCircle(210,30,20,WHITE);
    tft_drawFastHLine(195,30,30,WHITE);
    tft_drawLine(225,30,210,15,WHITE);
    tft_drawLine(225,30,210,45,WHITE);
}

UINT16 x_inc = 0, y_inc = 40;
void drawHDash(int pos) {
    while (x_inc < 320) {
        tft_drawFastHLine(x_inc,pos,10,WHITE);  //draw line
        x_inc = x_inc + 30; //update line starting position
    }
    x_inc = 0;
}

void drawVDash(int pos) {
    while (y_inc < 240) {
        tft_drawFastVLine(pos,y_inc,10,WHITE);  //draw line
        y_inc = y_inc + 30; //draw line starting position
    }
    y_inc = 40;
}

void updateFreqText(char p) {   //fill buffer with text that was previously touched
    switch(p) { //why did we add 64 back in the state machine?
        case 'A':   //this is why, everything is an ASCII character
            sprintf(buffer, "100 Hz");
            break;
        case 'B':
            sprintf(buffer, "1 kHz");
            break;
        case 'C':
            sprintf(buffer, "5 kHz");
            break;
        case 'D':
            sprintf(buffer, "10 kHz");
            break;
        case 'E':
            sprintf(buffer, "20 kHz");
            break;
        default:
            break;  
    }
}

void updateAmpText(char p) { //fill buffer with text that was previously touched
    switch(p) { //why did we add 64 back in the state machine?
        case 'A':   //this is why, everything is an ASCII character
            sprintf(buffer, "1"); 
            break;
        case 'B':
            sprintf(buffer, "2");    
            break;
        case 'C':
            sprintf(buffer, "3");   
            break;
        case 'D':
            sprintf(buffer, "4");   
            break;
        case 'E':
            sprintf(buffer, "5"); 
            break;
        default:
            break;  
    }
}

void findBodeNonZero() {    //find nonzero bode elements
    numNZ = 0;
    for(i=0;i<320;i++) {
        if(bode[i] != 0) {
            yNZ[numNZ] = bode[i];
            xNZ[numNZ] = i;
            numNZ++;
        }
    }
}

static UINT16 sep, offset;
static float slope, tmpslp;
void linearInterpolate() {  //linear interpolate between nonzero points
    for(p=0;p<numNZ-1;p++) {
        sep = xNZ[p+1] - xNZ[p];    //distance b/2 points

        slope = ((float)yNZ[p+1]-(float)yNZ[p])/(float)sep; //floating pt slope
        
        for(i=1;i<sep;i++) {
            offset = xNZ[p] + i;
            tmpslp = slope*((float)offset-(float)xNZ[p]);
            bode[offset] = yNZ[p] + (int)tmpslp;
        }
    }
    
    i = 0;
    while(bode[i] == 0) { i++; }    //fill in blanks at beginning of plot
    for(p=0;p<=i;p++) { bode[p] = bode[i+1]; }
    i = 0;
    while(bode[i] !=0 ) { i++; }    //fill in blanks at end of plot
    for(p=i;p<320;p++) { bode[p] = bode[i-1]; }
}

void filterMovAvg() {   //4 element moving average
    for(i=0;i<320;i++) { xNZ[i] = bode[i]; } //Quick and dirty, eliminate in place averaging error
    bode[1] = (xNZ[0] + xNZ[1])/2;
    bode[2] = (xNZ[0] + xNZ[1] + xNZ[2])/3;
    for(i=3;i<320;i++) {
        bode[i] = (xNZ[i-3] + xNZ[i-2] + xNZ[i-1] + xNZ[i])/4;  //bulk of averaging
    }
}

static UINT16 medArray[5], temp;
void filterMedian() {
    //for(i=0;i<320;i++) { xNZ[i] = bode[i]; }
    for(i=2;i<=numNZ-2;i++) {
        p = 0;
        for(j=i-2;j<=i+2;j++) { //place 5 amplitudes in median array
            medArray[p] = yNZ[j];
            p++; 
        }
        
        for(p=0;p<4;p++) {  //sort 5 elements in ascending order
            for(j=p+1;j<5;j++) {
                if(medArray[j] < medArray[p]) {
                    temp = medArray[p];     // swap elements
                    medArray[p] = medArray[j];
                    medArray[j] = temp;
                }
            }
        }
        
        yNZ[i] = medArray[2];   //change amplitude value
    }
}

void discreteDerivative() { //simple discrete derivative (first order Newton approximation, maybe :)
    j = 0;
    for(i=4;i<320;i=i+4){   //sweep through all of plot
        ddx[j] = abs(bode[i-4] - bode[i]); //>>3    //only want magnitude not value
        j++;
    }
}

//static UINT16 numBands;
void conditionCheck() {
    j = 0;
    for(i=0;i<6;i++) {  //determine number of bands
        if (xband[i] != 0) { j++; }
        numBands = (j+1)/2;
    }
    
    tft_setTextSize(2);
    tft_setTextColor(BIGRED);
    if (numBands == 2) {    //explicitly check 2 band conditions
        if (xband[2]>258) { //start after .8pi
            drawBandStartError();
        }
        else if(xband[2]-xband[1] < 32) {   //small transition regions 
            drawTransitionError();
        }
    }
    else if (numBands == 3) {   //explicitly check three band errors
        if (xband[4]>258) {     //start after .8pi
            drawBandStartError();
        }
        else if((xband[2]-xband[1]) < 32) { //small transition regions
            drawTransitionError();
        }
        else if((xband[4]-xband[3]) < 32) { //small transition regions
            drawTransitionError();
        }
    }
}

void drawTransitionError() {
    tft_fillRect(55,0,210,30,BLACK);    //draw transition region error
    sprintf(buffer, "Transition < .1pi");
    tft_setCursor(58,10);
    tft_writeString(buffer);
    delayMS(250);
    
    errorflg = 1;   //set error flag
}

void drawBandStartError() {
    tft_fillRect(55,0,210,30,BLACK);    //draw late start error
    sprintf(buffer, "Band Start > .8pi");
    tft_setCursor(58,10);
    tft_writeString(buffer);
    delayMS(250);

    errorflg = 1;   //set error flag
}

static UINT16 accum = 0, spctmp = 0;
void scaleBands() {
    XBAND[0] = 0.0;
    for(i=1;i<6;i++) {
        if (xband[i] != 0) {    //scale XBANDS from zero to pi
            XBAND[i] = (long double)(xband[i])*M_PI;
            XBAND[i] = XBAND[i]/320.0;
        }
    }
    
//    j = 0;
//    p = 0;
//    for(i=0;i<numBands;i++) {
//        while (yNZ[j] != yband[2*i + 1]) {
//            accum = accum + yNZ[j];
//            j++;
//        }
//        p = j - spctmp;
//        YBAND[2*i] = (long double)(accum)/((long double)(p));
//        YBAND[2*i] = (YBAND[2*i]*(long double)max_ampval)/200.0;
//        YBAND[2*i + 1] = YBAND[2*i];
//        accum = 0;
//        spctmp = p;
//    }
    
    for(i=0;i<numBands;i++) {   //scale YBANDS according to amp range
        YBAND[2*i] = ((long double)(yband[2*i]) + (long double)(yband[2*i+1]))/2.0;
        YBAND[2*i] = (YBAND[2*i]*(long double)max_ampval)/200.0;
        YBAND[2*i+1] = YBAND[2*i];
    }
}

#define LOG2N 6
#define N_WAVE (1 << LOG2N)
#define twiddles fft32c64

static int32c x_t[N_WAVE];
static int32c X_f[N_WAVE];
static int32c scratch[N_WAVE];


void filterDesign() {

	int x = 0;

	#define N 64
	#define np N/2+1
	float m[np];
	//float M[np];

	for(x=0; x<np; x++) {
		m[x] = x;
	}

//	for(x=0; x<np; x++) {
//		M[x] = sin(1.0*m[x]*M_PI/((float)(np)-1.0));
//	}

	float w[np];
	for(x=0; x<np; x++) {
		w[x] = (float)(m[x])*2.0*M_PI/((float)(N));
	}

	float Dreal[N];
	float Dimag[N];

	for(x=0; x<np; x++) {
		Dreal[x] = M[x]*cos(((float)(N)-1.0)/2.0*(float)(w[x]));
		Dimag[x] = M[x]*sin(((float)(N)-1.0)/2.0*(float)(w[x]));
	}

	int index = np;
	for(x=np-2; x>=1; x--) {
		Dreal[index] = Dreal[x];
		Dimag[index] = -Dimag[x];
		index++;
	}

    for(x=0; x<N; x++) {
        x_t[x].re = float2fix32(Dreal[x]);
        x_t[x].im = float2fix32(Dimag[x]);
    }
	
    mips_fft32(X_f, x_t, twiddles, scratch, LOG2N);
    
    for(x=0; x<64; x++) {
        h[x] = float2fix16(fix2float32(X_f[x].re));
    }
}

//void startTimers() {
//    // edit this to account for user's selection in frequency
//    OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_8, ADC_PDR[max_freqmag]);
//    ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
//    OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_8, ADC_PDR[max_freqmag]);
//    ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_2);
//    SetChanADC10(ADC_CH0_POS_SAMPLEA_AN0);
//}

void resetScreen(){ //reset touchscreen variable and wait for touch
    touch_reset();
    do{
        touch_getPoint();
        delayMS(10);
    } while (touch_point.DONE==0 || touch_point.Z==0);
}

UINT16 ct;
UINT8 scale;
void centerTxt(int height, int col, int colnum) {   //center text according to column on screen
    if(textsize == 2) { scale = 12; }   //set for different text sizes
    else if(textsize = 1) { scale = 6; }
    ct = (SCREEN_WIDTH/col-strlen(buffer)*scale)/2 + colnum*SCREEN_WIDTH/col;   //set x position
	tft_setCursor(ct, height);  //set cursor
	tft_writeString(buffer);    //write to touchscreen
}

void __ISR(_TIMER_2_VECTOR, ipl2) Timer2ISR(void) { //Timer 2 interrupt, ADC
    mT2ClearIntFlag();          //clear ISR flag
    ADC_value = ReadADC10(0);   //Read ADC
    newValAvail = 1;
}

#define DAC_config_chan_A 0b0011000000000000
volatile SpiChannel spiChn = SPI_CHANNEL2 ;	// the SPI channel to use
volatile int spiClkDiv = 2 ; // 20 MHz max speed for this DAC
void __ISR(_TIMER_3_VECTOR, ipl3) Timer3ISR(void) {
    mT3ClearIntFlag();
    mPORTBClearBits(BIT_9);     //CS Low
    while (TxBufFullSPI2());    
    WriteSPI2(DAC_config_chan_A | (int) DAC_VALUE); //Write to DAC
    while (SPI2STATbits.SPIBUSY);   //Wait for data transfer
    mPORTBSetBits(BIT_9); //CS High
}

void __ISR(_EXTERNAL_0_VECTOR,ipl2)INT0Interrupt() {    //external interrupt ISR
    mINT0ClearIntFlag();    //clear ISR flag
    stopFilter = 1;         //set filter stop condition
}

void main(void) {
	SYSTEMConfigPerformance(PBCLK);
	INTEnableSystemMultiVectoredInt();

    //Clear Analog Peripherals
	ANSELA = 0;
	ANSELB = 0;
	CM1CON = 0;
	CM2CON = 0;
    
    mPORTASetPinsAnalogIn(BIT_0);   //ADC
    
    mPORTBSetPinsDigitalIn(BIT_7);
    CNPDBSET = BIT_7;
    //PPSInput(2,INT3R,RPA1);
    //INT3R = 0b0000;


    //Configure Protothreads
	PT_setup();
	PT_INIT(&pt_state);

    //Initalize TFT Display
	tft_begin();
	tft_fillScreen(BLACK);
	touch_init();
    
    // init DAC Stuff
    PPSOutput(2, RPB8, SDO2);
    mPORTBSetPinsDigitalOut(BIT_9); //CS pin
    mPORTBSetBits(BIT_9);
    SpiChnOpen(spiChn, SPI_OPEN_ON | SPI_OPEN_MODE16 | SPI_OPEN_MSTEN | SPI_OPEN_CKE_REV , spiClkDiv);

    //Welcome Screen
	tft_setTextColor(ILI9340_WHITE);
	tft_setTextSize(2);

    sprintf(buffer, "Automatic Signal");        //print fantastic introduction
    centerTxt(SCREEN_HEIGHT/3-60,1,0);
    sprintf(buffer, "Transformer & Custom");
    centerTxt(SCREEN_HEIGHT/3-40,1,0);
    sprintf(buffer, "Filter Designer");
    centerTxt(SCREEN_HEIGHT/3-20,1,0);
	sprintf(buffer, "ECE 4760");
    centerTxt(SCREEN_HEIGHT/3+20,1,0);
	sprintf(buffer, "Final Project");
    centerTxt(SCREEN_HEIGHT/3+40,1,0);
    sprintf(buffer, "Daniel Eddowes");
    centerTxt(SCREEN_HEIGHT/3+60,1,0);
    sprintf(buffer, "Manish Patel");
    centerTxt(SCREEN_HEIGHT/3+80,1,0);
    sprintf(buffer, "Press to Start");
    centerTxt(SCREEN_HEIGHT/3+140,1,0);

	delayMS(500);

    //Wait for press to advance
    resetScreen();
    DAC_VALUE = 5;  //initialize DAC to some nonzero number
    
	while (1){
      PT_SCHEDULE(protothread_state(&pt_state));
	}
}