/* 
 * File:  main.c
 * Project: Bike.X
 * Author: Claire Chen and Mark Zhao
 */

//=========================Header/Definitions==============================
#define	SYS_FREQ 40000000 // change the frequency of the clock
#include "nrf24l01.h"
// These two defines are for the tft
#define spi_channel	1
#define spi_divider 10
#define PACKET_SIZE 4
#define OUT_OF_RANGE 500
#define TIMEOUT_TIME 100
//Config file
#include "config.h"
// threading library
#include "pt_cornell_1_2.h"

#include "servo.h"
#include "ultrasonic.h"


////////////////////////////////////
// graphics libraries
#include "tft_gfx.h"
#include "tft_master.h"
#include "graphics.h"
////////////////////////////////////
#include <stdio.h>
#include <math.h>
//===============================Global Variables===========================
static struct pt pt_sonar, pt_radio, pt_blink, pt_tft;
char buffer[120]; // a buffer for writing to the tft
char tx_packet[PACKET_SIZE] = {0};
char rx_packet[PACKET_SIZE] = {0};
int counter = 0;

//TFT Variables
volatile unsigned int tft_debug;

//Angle/Math Variables
unsigned int distance_cm = 0; //This is the received distance from the ultrasonic
unsigned int curr_angle = 0; //This is the current angle the servo is pointed
int increment = INCREMENT_DEG;


//Ultrasonics
volatile short capture1_rise = 0, capture1_fall = 0, capture1_delta = 99;
int period;

volatile unsigned int echo_level = 0;

//===============================Initialization Functions===================
/*
 * Initialize protothreads and enable interrupts
 */
void init_pthreads(void){
  // === config threads ==========
  // turns OFF UART support and debugger pin, unless defines are set
  PT_setup();
  PT_INIT(&pt_sonar);
  PT_INIT(&pt_blink);
  PT_INIT(&pt_tft);
  PT_INIT(&pt_radio);
}

/*
 * Initialize TFT
 */
void init_tft(void) {
  tft_init_hw();
  tft_begin();
  tft_fillScreen(ILI9340_BLACK);
  //240x320 vertical display
  tft_setRotation(1); // Use tft_setRotation(1) for 320x240
}

// set up the radio
void init_radio() {
    nrf_setup(); // initializing function

    nrf_set_arc(0x0A); // 10 retransmits
    
    nrf_set_ard(0x00); // 250 us between retransmission

    nrf_set_rf_ch(0x01); // freq = 2.401 GHz

    nrf_en_aa(0); // enable autoack on pipe 0

    nrf_set_pw(4, 0); //set the payload width to be 1 byte

    // set up addresses for autoack mode
    // The tx address and the pipe 0 rx address must be the same for autoack mode
    nrf_set_address_width(5);
    nrf_set_rx_addr(0, 0xAABBCCDDFF, 5);
    nrf_set_tx_addr(0xAABBCCDDFF);
}
////=========================== Threads ======================================
static PT_THREAD (protothread_radio(struct pt *pt))
{
  PT_BEGIN(pt);
  static volatile char counter = 0; // synchronous counter

  // while the packet isn't acknowledged/received continously resend
  while (!nrf_send_payload(tx_packet, 4)) {
      // blink a circle on screen while waiting
      tft_fillCircle(20, 20, 10, ILI9340_BLACK);
      nrf_delay_ms(300);
      tft_fillCircle(20, 20, 10, ILI9340_RED);
      nrf_delay_ms(300);
      PT_YIELD_TIME_msec(100);
  }
  // set radio to receive mode to start
  nrf_state_rx_mode();

  while (1) {
    //Turn the servo
    turn_servo(curr_angle);

    //Begin by sending out a ping and get the distance back
    send_pulse();
    distance_cm = (capture1_delta * 1.6) / 58;
    PT_YIELD_TIME_msec(60);
    send_pulse();
    //if(abs((capture1_delta*1.6/58) - distance_cm) >= 100 ){
      //distance_cm = OUT_OF_RANGE;
    //}
    //else{
      //distance_cm = (distance_cm + ((capture1_delta*1.6)/58))/2;
    //}
    distance_cm = (capture1_delta * 1.6) / 58;
    //distance_cm = 200;
    if(curr_angle >= 0 && curr_angle <= 180){
      //Now format the packet accordingly
      if(distance_cm >= 0 && distance_cm <= MAX_DIST){ //There is an object
        tx_packet[0] = 'd';
        tx_packet[1] = (unsigned char) curr_angle;
        tx_packet[2] = (unsigned char)((distance_cm >> 8) & 0xff);
        tx_packet[3] = (unsigned char)((distance_cm) & 0xff);
      }
      else{
        //Nothing in range
        tx_packet[0] = 'n';
        tx_packet[1] = 0;
        tx_packet[2] = (500 >> 8) & 0xff;
        tx_packet[3] = 500 & 0xff;
      }
    }

    // wait until a payload is received before doing anything
    if (nrf_payload_available()) { // when a payload has been received
      //draw_sweep(distance_cm, curr_angle);
      // get new counter value from transmitter
      nrf_get_payload(rx_packet, 4);
      tft_debug = ((unsigned int)rx_packet[2] << 8) | rx_packet[3];
      //counter++;
      // send incremented payload
      //Add the increment
      curr_angle = curr_angle + increment;

      //Okay, check the current angle. Make sure we're in bounds
      if(curr_angle <= 0){
        increment = INCREMENT_DEG;
        curr_angle = 0;
      }
      else if(curr_angle >= 180){
        increment = INCREMENT_DEG * -1;
        curr_angle = 180;
      }
      nrf_send_payload(tx_packet, 4); // send counter back to other radio
      nrf_state_rx_mode(); // put the radio in rx mode after the transmission
    }
    else{
      nrf_send_payload(tx_packet, 4); // send counter back to other radio
      nrf_state_rx_mode(); // put the radio in rx mode after the transmission
    }
    PT_YIELD_TIME_msec(300);
    draw_sweep(distance_cm, curr_angle);
    PT_YIELD_TIME_msec(300);
    
  }
  
  PT_END(pt);
}


// Blink circle on TFT once per second.
static PT_THREAD (protothread_blink(struct pt *pt)){
    PT_BEGIN(pt);
    tft_setCursor(0,0); //Draw small yellow circle at top left hand corner
    while(1){
        PT_YIELD_TIME_msec(500);
        tft_fillCircle(10,230,10,ILI9340_YELLOW);
        PT_YIELD_TIME_msec(500);
        tft_fillCircle(10,230,10,ILI9340_BLACK);
    }
    PT_END(pt);
}


/**
 * This is our main graphics thread
 */
static PT_THREAD (protothread_tft(struct pt *pt)){
  PT_BEGIN(pt);
  tft_setTextColor(ILI9340_WHITE);
  tft_setTextSize(2);
  //Initialize by drawing a "sonar circle" on the TFT
  tft_drawCircle(MIDDLE_X, 0, 161, ILI9340_GREEN);

  while(1){ 
    sprintf(buffer, "angle = %d", curr_angle);
    tft_fillRect(25, 200, 300, 300, ILI9340_BLACK);  //Clear the previous val.
    tft_setCursor(30, 220);
    tft_writeString(buffer);
    sprintf(buffer, "dist. = %d", distance_cm);
    //tft_fillRect(40, 180, 300, 150, ILI9340_BLACK);  //Clear the previous val.
    tft_setCursor(180, 220);
    tft_writeString(buffer);
    PT_YIELD_TIME_msec(250);
  }
  
  PT_END(pt);
}
// == Capture 1 ISR ====================================================
// check every capture for consistency
void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl3) C1Handler(void)
{
    // read level of input capture pin
    echo_level = mPORTAReadBits(BIT_2) >> 2;
    
    if (echo_level == 0){
      capture1_fall = mIC1ReadCapture();
      capture1_delta = capture1_fall - capture1_rise;
    }
    else{
      capture1_rise = mIC1ReadCapture();
    }
    
    mIC1ClearIntFlag();
}

void main(void) {
    INTEnableSystemMultiVectoredInt();
    init_pthreads();
    init_ultrasonic();
    init_radio(); // setup the radio for this program
    tft_init_hw(); // setup for tft
    tft_begin(); // setup for tft
    init_servo();
    tft_fillScreen(ILI9340_BLACK);
    //320x240 horizontal display
    tft_setRotation(1); // Use tft_setRotation(1) for 320x240
    turn_servo(90);
    
    
  while(1){
    PT_SCHEDULE(protothread_radio(&pt_radio));
    PT_SCHEDULE(protothread_blink(&pt_blink));
    PT_SCHEDULE(protothread_tft(&pt_tft));
  }
} // main
// === end  ======================================================