/*
* File:        Final_Project.c
* Author:      Erissa Irani, Rohit Jha, Amanda Pustis
*
* Target PIC:  PIC32MX250F128B
*/

////////////////////////////////////
// clock AND protoThreads configure!
// You MUST check this file!
#include "config.h"
// threading library
#include "pt_cornell_1_1.h"

////////////////////////////////////
// graphics libraries
#include "tft_master.h"
#include "tft_gfx.h"
// need for rand function
#include <stdlib.h>
#include "bodyoutline.h"
#include "port_expander.h"
#include <math.h>
////////////////////////////////////

typedef signed int fix16;
#define multfix16(a,b) ((fix16)(((( signed long long)(a))*(( signed long long)(b)))>>16)) //multiply two fixed 16:16
#define float2fix16(a) ((fix16)((a)*65536.0)) // 2^16
#define fix2float16(a) ((float)(a)/65536.0)
#define fix2int16(a)    ((int)((a)>>16))
#define int2fix16(a)    ((fix16)((a)<<16))
#define divfix16(a,b) ((fix16)((((signed long long)(a)<<16)/(b))))
#define sqrtfix16(a) (float2fix16(sqrt(fix2float16(a))))
#define absfix16(a) abs(a)
// string buffer
char buffer[60];

// === thread structures ============================================
// thread control structs
// note that UART input and output are threads
static struct pt pt_timer, pt_body ;

// system 1 second interval tick
int sys_time_seconds ;
static int i, test, test_prev, test_prev2, button_pushes, prev_sp,sp,
       button_debounce, PushState, button_pushed;
static short x,y,inc, temp;
static int adc_0, adc_1, base_value, final_value;
static int prev_adc_0, prev_adc_1, prev_basevalue1, prev_basevalue2;
// === Timer Thread =================================================
// update a 1 second tick counter
static PT_THREAD (protothread_timer(struct pt *pt))
{
   PT_BEGIN(pt);
    tft_setCursor(0, 0);
    tft_setTextColor(ILI9340_WHITE);  tft_setTextSize(1);
    tft_writeString("Time in seconds since boot\n");
     while(1) {
       // yield time 1 second
       PT_YIELD_TIME_msec(1000) ;
       sys_time_seconds++ ;
       
       // draw sys_time
       tft_fillRoundRect(0,10, 100, 14, 1, ILI9340_BLACK);// x,y,w,h,radius,color
       tft_setCursor(0, 10);
       tft_setTextColor(ILI9340_YELLOW); tft_setTextSize(2);
       sprintf(buffer,"%d", sys_time_seconds);
       tft_writeString(buffer);
       // NEVER exit while
     } // END WHILE(1)
 PT_END(pt);
} // timer thread

// === Color Thread =================================================
// draw 3 color patches for R,G,B from a random number
static int color ;
static int i;

void home_screen(){ //Formatting of home screen
 tft_fillScreen(ILI9340_RED);
 tft_setCursor(70, 75);
 tft_setTextColor(ILI9340_WHITE); tft_setTextSize(4);
 sprintf(buffer,"%s","Welcome");
 tft_writeString(buffer);
 tft_setTextSize(3);tft_setCursor(20,115);
 sprintf(buffer, "%s", "I've got yo back!");
 tft_writeString(buffer);
 tft_setTextSize(2);tft_setCursor(60,150);
 sprintf(buffer, "%s", "Press the Button");
 tft_writeString(buffer);
 tft_setCursor(55,170);
 sprintf(buffer, "%s", "Twice to Calibrate");
 tft_writeString(buffer);
} // color thread

void calibration(){ //calibration mode display and measurements
   SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN0);
   AcquireADC10();
   tft_fillScreen(ILI9340_BLUE);
   tft_setTextColor(ILI9340_WHITE); tft_setTextSize(2);
   tft_setCursor(50, 50);
   sprintf(buffer,"%s","Sit Up Until System");
   tft_writeString(buffer);
   tft_setCursor(70, 70);
   sprintf(buffer, "%s","is Calibrated");
   tft_writeString(buffer);
   if (base_value!=0 && prev_basevalue1!=0 && prev_basevalue2!=0){
      while (temp<500){
          if (temp%100==0){
              tft_fillRoundRect(12.5+inc*60,150,60,20,1,ILI9340_WHITE);
              inc++;
          }
        temp++;
        tft_setCursor(50, 95);
        tft_fillRoundRect(50,95, 100, 25, 1, ILI9340_BLUE);// x,y,w,h,radius,color
        tft_setCursor(50, 100);
        tft_setTextColor(ILI9340_BLUE); tft_setTextSize(2);
        sprintf(buffer,"%d", temp);
        tft_writeString(buffer);
       }
       while (base_value>prev_basevalue1+2 || base_value<prev_basevalue1-2 ||
               prev_basevalue1>prev_basevalue2+2 ||
               prev_basevalue1<prev_basevalue2-2){
               prev_basevalue2=prev_basevalue1;
               prev_basevalue1=base_value;
               base_value=ReadADC10(0);
           }
       tft_setTextColor(ILI9340_WHITE);
       tft_setCursor(20, 100);
       sprintf(buffer, "%s","Calibrated! Press button");
       tft_writeString(buffer);
       tft_setCursor(60, 120);
       sprintf(buffer, "%s","to start using!");
       tft_writeString(buffer);
       final_value=base_value;
   }else{
       prev_basevalue2=prev_basevalue1;
       prev_basevalue1=base_value;
       base_value=ReadADC10(0);
       calibration();
   }
}

void body_screen(){ //body outline screen display    tft_fillScreen(ILI9340_BLACK);
   for (i=0; i<12619; i=i+2){
       x=BodyOutline[i]; //use header file to fill in outline
       y=BodyOutline[i+1];
       tft_drawPixel(x,y,ILI9340_WHITE);  
       }
     tft_fillRoundRect(150,130,10,70,1, ILI9340_GREEN);//spine
       
}

void stat_screen(){ //counts number of “back breakers”->display
   tft_fillScreen(ILI9340_BLACK);
   tft_setTextColor(ILI9340_RED);
   tft_setCursor(10,100);
   tft_setTextSize(2);
   tft_fillRoundRect(10,95,320,100,1,ILI9340_BLACK);
   sprintf(buffer, "# of Back Breakers: %d", sp);
   tft_writeString(buffer);
}

void debounce_button(button_debounce){
   switch(PushState){
   case 0: //NoPush
       if (button_debounce) PushState=1; //1=MaybePush
       else PushState=0; //stay at NoPush
       break;
   case 1: //MaybePush
        if (button_debounce) { //if button pressed
           PushState=2;   //go to state 2=pushed
           switch(button_pushes){
           case(1):
               calibration(); //if one button push, then calibration mode
           break;
           case(2): //2 pushes, body_screen
               body_screen();
           break;
           case(3): //3 pushes, stat_screen
               stat_screen();
           break;          
       }
           if (button_pushes==3){//allows system to cycle between body screen and stat screen
               button_pushes=2;
           } else{
               button_pushes++;
           }
        } else PushState=0; //go to NoPush state
        break;
   case 2:  
        if (button_debounce) PushState=2; //if press is still happening, stay at pushed state
        else PushState=3;    //if no push signal, go to state 3=MaybeNoPush
        break;

   case 3:
        if (button_debounce) PushState=2; //if press is still there, go back to pushed state
        else{
            PushState=0;    //go to no push- press is done
            button_pushed=0;
        }
   }
}
//=====Sensor/Vibration Motor Thread===========================================
static PT_THREAD (protothread_bodyout(struct pt *pt))
{
   PT_BEGIN(pt);
   PT_YIELD_TIME_msec(30);
   test=mPORTBReadBits(BIT_3); //read signal from external button to see if test mode has been entered
   button_debounce=!(test);
   debounce_button(button_debounce);
       
   /****** BACK/SPINE **************/
   SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN0);
   AcquireADC10();
   PT_YIELD_TIME_msec(1);
   adc_0=ReadADC10(0);
   
   //for testing
//    if (adc_0!=prev_adc_0){
//        tft_fillRoundRect(10,10, 300, 30, 1, ILI9340_BLACK);// x,y,w,h,radius,color
//        tft_setCursor(10, 10);
//        tft_setTextColor(ILI9340_WHITE); tft_setTextSize(2);
//        sprintf(buffer, "Bottom spine (AN0): %d", adc_0);
//        tft_writeString(buffer);
//        prev_adc_0=adc_0;
//    }
   //end of testing code
   
   if (adc_0<=(final_value-final_value*.07)){ //long sensor checks if value is below a threshold of calibration value
   mPORTBSetBits(BIT_5); //sets vibration motor high
       if (prev_adc_0>(final_value-final_value*.07)){ //make sure that threshold goes from no bad posture to bad posture
           sp++; //increase number of “back breakers”
           if (button_pushes==2){ //show increase in back breaker number
               tft_fillRoundRect(10,95,320,100,1,ILI9340_BLACK);
               tft_setTextColor(ILI9340_RED);
               tft_setCursor(10,100); tft_setTextSize(2);
               sprintf(buffer, "# of Back Breakers: %d", sp);
               tft_writeString(buffer);
           }
           if(button_pushes==3){ //turn green rectangle into red
              tft_fillRoundRect(150,130,10,70,1, ILI9340_RED);
           }
       }
   }
   else{
       mPORTBClearBits(BIT_5); //set vibration motor low (turn off)
       if(button_pushes==3){ //reset green rectangle
           tft_fillRoundRect(150,130,10,70,1, ILI9340_GREEN);
       }
   }
   prev_adc_0=adc_0;
//end of spine
   
 PT_END(pt);
} // body outline thread

// === Main  ==================================================================
void main(void) {
 ANSELA = 0; ANSELB = 0;

 // === configure threads ========================
 // turns OFF UART support and debugger pin, unless defines are set
 PT_setup();

 // === setup system wide interrupts  ============
 INTEnableSystemMultiVectoredInt();

 // init the threads
 PT_INIT(&pt_timer);
 PT_INIT(&pt_body);

 // === setup system wide interrupts  =============
 INTEnableSystemMultiVectoredInt();
 
 //=== configure internal ADC =====================
 CloseADC10(); // ensure the ADC is off before setting the configuration
 #define PARAM1 ADC_FORMAT_INTG16 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_OFF //
 #define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_OFF | ADC_SAMPLES_PER_INT_1 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
 #define PARAM3 ADC_CONV_CLK_PB | ADC_SAMPLE_TIME_5 | ADC_CONV_CLK_Tcy2
 #define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN4_ANA | ENABLE_AN9_ANA | ENABLE_AN11_ANA  
 #define PARAM5 SKIP_SCAN_ALL
 // configure ADC using the parameters defined above
 OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 );      
 EnableADC10();
 
 //===initialize the display ======================
 tft_init_hw();
 tft_begin();
 tft_setRotation(1); // Use tft_setRotation(1) for 320x240
 home_screen();
 //tft_fillScreen(ILI9340_BLACK);
 mPORTBSetPinsAnalogIn(BIT_14); //SCK for TFT  
 EnablePullUpB(BIT_14);        
 
 //=== Configure Pins & Enable Internal Pull Ups ==
 //Configure Analog Pins for input from sensors
 mPORTASetPinsAnalogIn(BIT_0);
 mPORTASetPinsAnalogIn(BIT_1);
 mPORTBSetPinsAnalogIn(BIT_15);
 mPORTBSetPinsAnalogIn(BIT_13);
 mPORTBSetPinsAnalogIn(BIT_2);

 //=== Enable Pull Up Resistors for Analog Pins ===
 EnablePullUpA(BIT_0);
 EnablePullUpA(BIT_1);
 EnablePullUpB(BIT_15);
 EnablePullUpB(BIT_13);
 EnablePullUpB(BIT_2);

 //=== Set Driver Pins ============================
 mPORTBSetPinsDigitalOut(BIT_5); //driver pin 1
 mPORTBSetPinsDigitalOut(BIT_10);// driver pin 2
 mPORTBSetPinsDigitalOut(BIT_9);// driver pin 3
 mPORTBSetPinsDigitalOut(BIT_8);// driver pin 4
 mPORTBSetPinsDigitalOut(BIT_7);// driver pin 5
 
 //=== Clear Driver Pins ===========================
 mPORTBClearBits(BIT_5);
 mPORTBClearBits(BIT_10);
 mPORTBClearBits(BIT_9);
 mPORTBClearBits(BIT_8);
 mPORTBClearBits(BIT_7);
   
 //===push button ===================================
 mPORTBSetPinsDigitalIn(BIT_3);
 EnablePullUpB(BIT_3);

//=== Schedule Threads ===============================
 while (1){
     PT_SCHEDULE(protothread_bodyout(&pt_body));
 }
} // main
// === end  ===================================================================