/*
 * File:   main.c
 * Author: Judy Stephen (jls633), John Draikiwicz (jwd94)
 * Main file for car used in final project
 * This controls the motors and recieves commands via 
 * Bluetooth from the glove. Includes ultrasonic for obstacle avoidance
 * but the ultrasonic code is still buggy. Could comment out lines109-112 
 * and program will still work without ultrasonic sensor
 * Last Updated: December 8, 2016
 * 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>
////////////////////////////////////
#include "motor.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 buffer
char bufferADC9[60];
char timeBuffer[60];
// === thread structures ============================================
// thread control structs
// note that UART input and output are threads
static struct pt pt_DMA_output, pt_input, pt_cmd, pt_ultrasonic;
// system 1 second interval tick
int sys_time_seconds ;

// 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 ;

static volatile int button;
static unsigned int offset;
static int count;
static volatile BYTE data;
static char rxchar[60];
static int RX_data;
static float V;

// ==== Motor variables =======================================================
int timer2_period = 2000 ; //The actual period of the wave
int pwm_on_time = 500 ; 

// ==== Ultrasonic variables ==================================================
int timer3_period = 0xffff; //The actual period of the wave
volatile unsigned int startEcho;
volatile unsigned int endEcho;
float intervalEcho;
volatile unsigned int beginUltrasonic;
float distanceToObject;
float prevDistance;
// == Timer 2 ISR =====================================================
// just toggles a pin for timing strobe
void __ISR(_TIMER_2_VECTOR, ipl2) Timer2Handler(void)
{
    // generate a trigger strobe for timing other events
    mPORTBSetBits(BIT_0);
    // clear the timer interrupt flag
    mT2ClearIntFlag();
    mPORTBClearBits(BIT_0);
}

// == Input Capture 1 ISR ============================================
void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl3) C1Handler(void){
    //if (mIC4ReadCapture() == 1){
    //    endEcho = ReadTimer3();
    //}
}


// === Command Thread ======================================================
// The serial interface
static char cmd[16]; 
static int value;

static PT_THREAD (protothread_cmd(struct pt *pt))
{
    PT_BEGIN(pt);
        while(1) {
          
            
           //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            // string is returned in "PT_term_buffer"
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input) );
            // returns when the thead dies
            // in this case, when <enter> is pushed
            // now parse the string
            sscanf(PT_term_buffer, "%s", cmd);

            // Ultrasonic is still buggy 
            // the program will work without this first if statement for ultrasonic
            if (distanceToObject > 20 && distanceToObject < 25 ){
                // stop the robot due to ultrasonic detecting nearby object
                stop_wheels();
            }
            if (PT_term_buffer[1]=='s') {
                stop_wheels();
            }
            else if (PT_term_buffer[1]=='f'){
                straight_fwd_wheels();
            } 
            else if (PT_term_buffer[1]=='b') {
                straight_bwd_wheels();
            }
            else if (PT_term_buffer[1]=='l'){
                move_left();
            }
            else if (PT_term_buffer[1]=='r') {
                move_right();
            }
            // never exit while
        } // END WHILE(1)
  PT_END(pt);
} // thread 3


static PT_THREAD (protothread_ultrasonic(struct pt *pt)){
    PT_BEGIN(pt);
    prevDistance = 0;
    int iter = 0;
    while(1){
        beginUltrasonic = PT_GET_TIME();
        mPORTASetBits(BIT_3);       // send the trigger signal
        PT_YIELD_TIME_msec(1);      // for 1 msec (need at least 10 microseconds)
        mPORTAClearBits(BIT_3);     // emd trigger
        
        WriteTimer3(0x0000);        // zero the Timer3
        startEcho = ReadTimer3();
        endEcho = mIC4ReadCapture();
        
        intervalEcho = endEcho - startEcho; // interval in number of clock ticks
                                            // clock running at 40MHz (25ns clock period)
        
        /* Speed of sound is 340meters/sec, then divide this by 2 for 1/2 distance
         * so speed of sound for 1/2 the distance is 170 meters per seconds
         * 170 meters per seconds * CLK_PERIOD * 100 = 4.25e-4 centimeters
         * distanceToObject = INTERVAL_CLOCK_TICKS * CLK_PERIOD * 170meters/sec */
        // convert interval to distance and print distance 
        distanceToObject = intervalEcho * 0.000425;     // distance in cm
        
        // wait 60ms including the above code (want to use 60ms measurement cycle)
        PT_YIELD_TIME_msec(60 - (PT_GET_TIME() - beginUltrasonic)) ; 
    }
    PT_END(pt);  
} // ultrasonic thread

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

  ANSELA = 0; ANSELB = 0; 
  
  // ==== Motor config =========================================================
  OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, timer2_period);
  ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
  mT2ClearIntFlag(); // and clear the interrupt flag
  
  // left wheel
  OpenOC1(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , pwm_on_time, pwm_on_time); 
  PPSOutput(1, RPA0, OC1);
  OpenOC2(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, pwm_on_time, pwm_on_time);
  PPSOutput(2, RPB5, OC2); // OC2 is PPS group 2, map to RPB5 (pin 14)
  
  // right wheel
  OpenOC3(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , pwm_on_time, pwm_on_time); 
  PPSOutput(4, RPB9, OC3); // OC3 is PPS group 4, map to RPB9 (pin 18)
  OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , pwm_on_time, pwm_on_time); 
  PPSOutput(3, RPA2, OC4); // OC3 is PPS group 4, map to RPB9 (pin 18)
  
  stop_wheels();
  
  // ==== Ultrasonic sensor ====================================================
  
  OpenTimer3(T3_ON | T3_PS_1_1, timer3_period); // Setup Timer 3
  mT3ClearIntFlag();                            // clear interrupt flag Timer 3
  
  // Enable Input Capture Module for falling edge
  // - Capture falling edge
  // - Enable capture interrupts
  // - use Timer 3 source 
  OpenCapture4( IC_EVERY_FALL_EDGE | IC_INT_4CAPTURE | IC_TIMER3_SRC | IC_ON);
  PPSInput(1, IC4, RPB4);                     // IC1 connected to pin 11 (group 1) for ECHO
  mIC4ClearIntFlag();                         // clear interrupt flag Input Capture 1
  
  mPORTASetPinsDigitalOut(BIT_3); // pin 10 set as output TRIGGER for ultrasonic

  // ==== Protothreads =========================================================
  // init the threads
  PT_INIT(&pt_cmd);
  PT_INIT(&pt_ultrasonic);
  
  // === config threads ==========
  // turns OFF UART support and debugger pin, unless defines are set
  PT_setup();
  
  // === setup system wide interrupts  ========
  INTEnableSystemMultiVectoredInt();

  // round-robin scheduler for threads
  while (1){
    PT_SCHEDULE(protothread_cmd(&pt_cmd));
    PT_SCHEDULE(protothread_ultrasonic(&pt_ultrasonic));
    }
  } // main

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