#include <plib.h>
#include <math.h>
#include "tft_gfx.h"
#include "tft_master.h"
#include <stdio.h> 
#include <stdlib.h> 
#include "pt_cornell_TFT.h"
#include "config.h"
#include "i2c_helper.h"

#define	SYS_FREQ 40000000
#define _SUPPRESS_PLIB_WARNING 1
#define PWM_PERIOD 10000 // Number of timer ticks per PWM period
#define PWM_MIN 1200 // Minimum PWM value
#define IMU_READ_PERIOD 50 // in ms

#define P_SCALE 2;
#define I_SCALE 0.05;
#define D_SCALE 0.1;

#define COMP_FILTER_G_COEF 0.98
#define COMP_FILTER_A_COEF 0.02

static struct pt pt_motor, pt_pid, pt_adc;

char buffer[60]; // string buffer
static volatile float kP, kI, kD; // PID coefficients
static volatile float errorInteg; // Integral accumulator for PID
static volatile float tilt, prev_tilt, ref_tilt; // tilt variables in degrees
static volatile int adc_p, adc_i, adc_d;


//Input Capture Event Interrupt
void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl2) IC4Handler(void)
{
    int junk = mIC4ReadCapture();
    ref_tilt = tilt;
    mIC4ClearIntFlag();
}

// Set the speed of the motors A and B with PWM duty cycles a and b < PWM_PERIOD
// a and b can be negative, meaning that the motor should spin in reverse
static void setSpeed(int a,int b)
{
    if (a >= 0){
        SetDCOC1PWM(abs(a));
        SetDCOC2PWM(0);
    }
    if (a < 0){
        SetDCOC1PWM(0);
        SetDCOC2PWM(abs(a));
    }
    if (b >= 0){
        SetDCOC3PWM(abs(b));
        SetDCOC5PWM(0);
    }
    if (b < 0 ){
        SetDCOC3PWM(0);
        SetDCOC5PWM(abs(b));
    }
}

// Test thread to test motor functionality
static PT_THREAD (protothread_motor(struct pt *pt))
{
  PT_BEGIN(pt);
    while(1) {
        setSpeed(8000,8000);
        PT_YIELD_TIME_msec(1000);
        setSpeed(5000,5000);
        PT_YIELD_TIME_msec(1000);
        setSpeed(-5000,-5000);
        PT_YIELD_TIME_msec(1000);
        setSpeed(-8000,-8000);
        PT_YIELD_TIME_msec(1000);
    }
  PT_END(pt);
}

// Thread for reading potentiometer values for adjusting PID coefficients
static PT_THREAD (protothread_adc(struct pt *pt))
{
  PT_BEGIN(pt);
    while(1) {
		adc_p = ReadADC10(0);  		// read the result of channel 0
		adc_i = ReadADC10(1);  		// read the result of channel 1
		adc_d = ReadADC10(2);  		// read the result of channel 9

        // Write to LCD for debugging
        tft_fillRoundRect(10, 90, 120, 30, 1, ILI9340_BLACK);
        tft_fillRoundRect(10, 120, 120, 30, 1, ILI9340_BLACK);
        tft_fillRoundRect(10, 150, 120, 30, 1, ILI9340_BLACK);

        tft_setTextColor(ILI9340_YELLOW); tft_setTextSize(2);
        
        tft_setCursor(10, 90);
        sprintf(buffer,"%d", adc_p);
        tft_writeString(buffer);

        tft_setCursor(10, 120);
        sprintf(buffer,"%d", adc_i);
        tft_writeString(buffer);

        tft_setCursor(10, 150);
        sprintf(buffer,"%d", adc_d);
        tft_writeString(buffer);
        
        PT_YIELD_TIME_msec(100);

    }
  PT_END(pt);
}


// PID controller thread
static PT_THREAD (protothread_pid(struct pt *pt))
{
  PT_BEGIN(pt);
    while(1) {
        // Read data from IMU in format
        // {xAccel, yAccel, zAccel, xGyro, yGyro, zGyro}
        float values[6];
        readImuValues(values);
        
        // Parse the IMU data
        float yAccel = values[1];
        float zAccel = values[2];
        float xGyro  = values[3];

        // CFilter and fuse the data using a complementary filter
        prev_tilt = tilt;
        float accTilt = atan2f(yAccel, zAccel)*180/M_PI; // Tilt angle from accelerometer (in degrees)
        tilt = COMP_FILTER_G_COEF*(tilt + xGyro*IMU_READ_PERIOD*0.001) + COMP_FILTER_A_COEF*accTilt;
        
        // Calculate PID values
        float error = tilt - ref_tilt;
        float errorDiff = (tilt - prev_tilt)*1000/IMU_READ_PERIOD;
        
        // Reset the integral if the robot passes tthe vertical
        if (error > 1.0) {
            errorInteg += error;
        }
        else
        {
            errorInteg = 0;
        }
        
        // Compute the PID control output
        float P = error*adc_p*P_SCALE;
        float I = errorInteg*adc_i*I_SCALE;
        float D = errorDiff*adc_d*D_SCALE;
        
//        int pidOut = (int) (P + I + D); //591, 1023, 483
//        int pidOut = (int) (P + I + D)*abs(P + I + D)/10; //192, 120, 14
//        int pidOut = (int) (P + I + D)*abs(P + I + D)/100; //180, 207, 0
//        int pidOut = (int) (P + I + D)*abs(P + I + D)/1000; //640, 448, 0
        int pidOut = (int) ((P + I + D) + (P + I + D)*abs(P + I + D)/1000); // 640, 560, 0
        
        if (pidOut > PWM_PERIOD)
        {
            pidOut = PWM_PERIOD;
        }
        else if (pidOut < -PWM_PERIOD)
        {
            pidOut = -PWM_PERIOD;
        }
//        else if (abs(pidOut) < PWM_MIN)
//        {
//            if (pidOut >= 0)
//            {
//                pidOut = PWM_MIN;
//            }
//            else
//            {
//                pidOut = -PWM_MIN;
//            }
//        }
        
        // Set the motor speeds according to the PID controller
        setSpeed(pidOut, pidOut);
        
        // Write to LCD for debugging
        tft_fillRoundRect(10, 30, 120, 30, 1, ILI9340_BLACK);
        tft_fillRoundRect(10, 60, 120, 30, 1, ILI9340_BLACK);

        tft_setTextColor(ILI9340_YELLOW); tft_setTextSize(2);

        tft_setCursor(10, 30);
        sprintf(buffer,"%f", ((int)(error*10))/10.0);
        tft_writeString(buffer);

        tft_setCursor(10, 60);
//        sprintf(buffer,"%d", pidOut);
        sprintf(buffer,"%f", ((int)(xGyro*10))/10.0);
//        sprintf(buffer,"%f", X_GYRO_OFF);
        tft_writeString(buffer);

        PT_YIELD_TIME_msec(IMU_READ_PERIOD); // 20 measurements per second
    }
  PT_END(pt);
}

void main(void) {
    PT_setup();
    
    INTEnableSystemMultiVectoredInt();
    
    // set up timer2 to generate the wave period
    OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, PWM_PERIOD);
    
    //Setting up Output Compares and allocating it to timer 2
    OpenOC1(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0,0);
    OpenOC2(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0,0);
    OpenOC3(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0,0);
    OpenOC5(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0,0);
    
    //Allocating pins to each output compare
    PPSOutput(1, RPB3, OC1);    //OC1 is PPS Group 1, maps to RPA0 (pin 2)
    PPSOutput(2, RPB5, OC2);    //OC2 is PPS Group 2, maps to RPA1 (pin 3)
    PPSOutput(3, RPA2, OC5);    //OC5 is PPS Group 3, maps to RPA2 (pin 9)
    PPSOutput(4, RPA3, OC3);    //OC3 is PPS Group 4, maps to RPA3 (pin 10)

 	//Enable channel
    //BRG computed value for the baud rate generator. The value is
    // calculated as follows: BRG = (Fpb / 2 / baudrate) - 2.
	OpenI2C1( I2C_ON, 48 );
    
    CloseADC10();
    #define PARAM1  ADC_FORMAT_INTG16 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON //OpenADC10 setup
	#define PARAM2  ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_3 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
    #define PARAM3  ADC_CONV_CLK_PB | ADC_SAMPLE_TIME_15 //ADC_SAMPLE_TIME_15| ADC_CONV_CLK_Tcy2
	#define PARAM4	ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN9_ANA // OpenADC10 setup parameters. Set pins 2,3, and 26 as analog
	#define PARAM5	SKIP_SCAN_AN2 | SKIP_SCAN_AN3 | SKIP_SCAN_AN4 | SKIP_SCAN_AN5 \
        | SKIP_SCAN_AN6 | SKIP_SCAN_AN7 | SKIP_SCAN_AN8 | SKIP_SCAN_AN10 | SKIP_SCAN_AN11 \
        | SKIP_SCAN_AN12 | SKIP_SCAN_AN13 | SKIP_SCAN_AN14 | SKIP_SCAN_AN15    // OPENADC10 setup parameters; No channels are assigned to be scanned 

	SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF); // use ground as the negative reference
	OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 ); // configure ADC using the parameters defined above
	EnableADC10(); // Enable the ADC
    
    while ( ! mAD1GetIntFlag() ) {}
   
    OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_256 , 0xffff);
//    ConfigIntTimer3(T3_INT_OFF | T3_INT_PRIOR_2);
//    mT3ClearIntFlag();
    
    // === set up input capture ================================
    OpenCapture4(  IC_EVERY_EDGE | IC_INT_4CAPTURE | IC_ON );
    // turn on the interrupt so that every capture can be recorded
    ConfigIntCapture4(IC_INT_ON | IC_INT_PRIOR_2 | IC_INT_SUB_PRIOR_3 );
    INTClearFlag(INT_IC4);
    mIC4ClearIntFlag();
    EnableIntIC4;
    
    // connect PIN 16 to IC4 capture unit
    PPSInput(1, IC4, RPB7);
    mPORTBSetPinsDigitalIn(BIT_7 );    //Set port as input
    
    tft_init_hw();
    tft_begin();
    tft_fillScreen(ILI9340_BLACK);
    tft_setRotation(1);
    
    PT_INIT(&pt_motor);
    PT_INIT(&pt_pid);
    PT_INIT(&pt_adc);
    
    // Take the MPU 6050 out of sleep mode
    char data[] = {0};
    i2c_write(0x6b, data, 1);
    
    // Set the gyro sensitivity to 131 lsb/(degrees/second))
    i2c_write(0x1b, data, 1);
    
    // Calibrate the gyroscopes (robot must be stable on flat surface)
    calibrateGyros();
    
    while(1){
//        PT_SCHEDULE(protothread_motor(&pt_motor));
        PT_SCHEDULE(protothread_pid(&pt_pid));
        PT_SCHEDULE(protothread_adc(&pt_adc));
        //while(1);
    }
}