/*
 * File:   main.c
 * Author: Judy Stephen (jls633), John Draikiwicz (jwd94)
 * Main file for final project 
 * ECE 4760 Fall 2016
 * 
 * Target PIC:  PIC32MX250F128B
 */

////////////////////////////////////
// clock AND protoThreads configure!
// You MUST check this file!
#include "config.h"
// threading library
#include "pt_cornell_1_2.h"
#include <math.h>
////////////////////////////////////
// graphics libraries
#include "tft_master.h"
#include "tft_gfx.h"
// need for rand function
#include <stdlib.h>
#include <stdio.h>
////////////////////////////////////
#define BAUDRATE 9600
// string buffers 
char bufferADC9[60];
char bufferADC11[60];
char bufferADC5[60];
char timeBuffer[60];
// === thread structures ============================================
// thread control structs
// note that UART input and output are threads
static struct pt pt_screen, pt_DMA_output, pt_input, pt_flex, pt_blue;

// thread rate priorities
int t1_rate=3, t2_rate=3, t3_rate=3, t4_rate=0 ;
// turn threads 1 and 2 on/off and set thread timing
int cntl_blink = 1 ;

//ADC Flex Sensor Measurement Variables
static volatile int adc_9;
static volatile int adc_11;
static volatile int adc_5;
static unsigned int offset;
//command to send robot
volatile char command;
//threshold that signifies a flex sensor is bent
static int flexThreshold = 135;

static PT_THREAD (protothread_bluetooth(struct pt *pt))
{
    PT_BEGIN(pt);
      while(1) {
          //Appends '\r' to command being sent to signify end of data being
          //transmitted. Then write this to PT_send_buffer.
          sprintf(PT_send_buffer, "%c\r", command);
          //Spawn thread to transmit PT_send_buffer over UART to Bluetooth module
          PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output) );
          //Wait 500 msecs before transmitting again
          PT_YIELD_TIME_msec(500);
      } // END WHILE(1)
  PT_END(pt);
} // user thread

//======Flex Sensor Thread==============================
//Receives values of Flex Sensor from ADC then determines command to
//transmit to robot
static PT_THREAD (protothread_flex(struct pt *pt))
{
    PT_BEGIN(pt);
    while(1){
        offset=8*((~ReadActiveBufferADC10() & 0x01));
        //Read ADC Values for the selected pins
        adc_11=ReadADC10(2);
        adc_9=ReadADC10(1);
        adc_5=ReadADC10(0);
        AcquireADC10();

        //Command Set for controlling the robot - determine what action
        //the robot should take
        //all closed
        if ((adc_11<flexThreshold)&&(adc_9<flexThreshold)&&(adc_5<flexThreshold)){
            command = 'f';
        }
        //pinkie & ring closed, index extended
        else if ((adc_9<flexThreshold)&&(adc_5<flexThreshold)&&(adc_11>=flexThreshold)){
            command = 'r';
        }
        //pinkie & index extended, ring closed
        else if ((adc_11>=flexThreshold)&&(adc_9<flexThreshold)&&(adc_5>=flexThreshold)){
            command = 'b';
        }
        //ring & pinkie extended, index closed
        else if ((adc_5>=flexThreshold)&&(adc_9>=flexThreshold)&&(adc_11<flexThreshold)){
            command = 'l';
        }
        //all open
        else if ((adc_5>=flexThreshold)&&(adc_9>=flexThreshold)&&(adc_11>=flexThreshold)){
            command = 's';
        }
        //Wait 10 msecs before sampling again
        PT_YIELD_TIME_msec(10);
    }//Never Exit While Loop
    
    
PT_END(pt);    
} // flex sensor 

// === Main  ======================================================
void main(void) {
  SYSTEMConfigPerformance(PBCLK);

  //mPORTBSetPinsDigitalIn(BIT_2);

  ANSELA = 0; ANSELB = 0;

  // === setup system wide interrupts  ========
  INTEnableSystemMultiVectoredInt();
  
  // the ADC ///////////////////////////////////////
  // configure and enable the ADC
  CloseADC10(); // ensure the ADC is off before setting the configuration
  mPORTBSetPinsDigitalIn(BIT_4); //Pin 11
  // define setup parameters for OpenADC10
  // Turn module on | ouput in integer | trigger mode auto | enable autosample
  // ADC_CLK_AUTO -- Internal counter ends sampling and starts conversion (Auto convert)
  // ADC_AUTO_SAMPLING_ON -- Sampling begins immediately after last conversion completes; SAMP bit is automatically set
  // ADC_AUTO_SAMPLING_OFF -- Sampling begins with AcquireADC10();
  #define PARAM1  ADC_MODULE_ON | ADC_FORMAT_INTG16 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON //

  // define setup parameters for OpenADC10
  // ADC ref external  | disable offset test | disable scan mode | do 1 sample | use single buf | alternate mode off
  #define PARAM2  ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_3 | ADC_ALT_BUF_ON | ADC_ALT_INPUT_OFF

  // Define setup parameters for OpenADC10
  // use peripherial bus clock | set sample time | set ADC clock divider
  // ADC_CONV_CLK_Tcy2 means divide CLK_PB by 2 (max speed)
  // ADC_SAMPLE_TIME_5 seems to work with a source resistance < 1kohm
  #define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_12 //| ADC_CONV_CLK_Tcy2 //ADC_SAMPLE_TIME_15| ADC_CONV_CLK_Tcy2

  // define setup parameters for OpenADC10
  // set AN11 and  as analog inputs
  #define PARAM4 ENABLE_AN11_ANA | ENABLE_AN9_ANA | ENABLE_AN5_ANA

  // define setup parameters for OpenADC10
    // do not assign channels to scan
    #define PARAM5   SKIP_SCAN_AN0 | SKIP_SCAN_AN1 | SKIP_SCAN_AN2 | SKIP_SCAN_AN3 | SKIP_SCAN_AN4 | SKIP_SCAN_AN6 | SKIP_SCAN_AN7 | SKIP_SCAN_AN8 | SKIP_SCAN_AN10 | SKIP_SCAN_AN12 | SKIP_SCAN_AN13 | SKIP_SCAN_AN14 | SKIP_SCAN_AN15//~(SKIP_SCAN_AN5|SKIP_SCAN_AN9|SKIP_SCAN_AN11)
  
    // configure to sample AN11 connected to flex sensor
  SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF );//ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN11 | ADC_CH0_NEG_SAMPLEB_NVREF | ADC_CH0_POS_SAMPLEB_AN9); // configure to sample AN4 
  
  OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 ); // configure ADC using the parameters defined above

  EnableADC10(); // Enable the ADC


  // === config threads ==========
  // turns OFF UART support and debugger pin, unless defines are set
  // PT_setup() MUST REMAIN HERE AND NOT ELSEWHERE
  PT_setup();
  // init the threads
  PT_INIT(&pt_flex);
  PT_INIT(&pt_blue);

  // ===== init the display ==================
  tft_init_hw();
  tft_begin();
  tft_fillScreen(ILI9340_BLACK);
  //240x320 vertical display
  tft_setRotation(0); // Use tft_setRotation(1) for 320x240
            
  // seed random color
  srand(1);

  // round-robin scheduler for threads
  while (1){
      PT_SCHEDULE(protothread_flex(&pt_flex));
      PT_SCHEDULE(protothread_bluetooth(&pt_blue));
      }
  } // main

// === end  ======================================================