/* ************************************************************************** */
/*
 * Project: Bluetooth Model Train Control
 * Authors: Jack Plumb and Greg Flynn
 * Description: Code for the user interface controller for the Bluetooth
 * model train control
 */
/* ************************************************************************** */

/* ************************************************************************** */
/* ************************************************************************** */
/* Section: Included Files                                                    */
/* ************************************************************************** */
/* ************************************************************************** */
#include "config.h"
// threading library
#include "plib.h"
// config.h sets 40 MHz
#define	SYS_FREQ 40000000
#include "pt_cornell_1_2.h"
#include "xc.h"
#include <stdlib.h>
#include "config.h"

//Screen library
#include "tft_master.h"
#include "tft_gfx.h"

//adc for pots
#include "adc_intf.h"

// BLE library
#include "biscuit_central.h"
#include "ble_hci.h"

//define station numbers for BT addressing
#define STATION0 0x0000
#define STATION1 0x0001
#define STATION2 0x0002

//pin24
#define LEFT_BIT BIT_13
#define LEFT_AN 11
#define RIGHT_BIT BIT_15
#define RIGHT_AN 9

//pin2 RA0
#define BUTTON_BIT BIT_0

//pin12 RA4
#define SWITCH_BIT BIT_4

//pin 18 RB9
#define KILL_SWITCH_BIT BIT_9

// LED RGBpin
// (RPB3) (Pin 7)
#define BLUE BIT_3
// (RPB4) (Pin 11)
#define GREEN BIT_4
// (RA2) (Pin 9)
#define RED BIT_2

//RB7 (Pin 16)
#define RESET BIT_7

//define slave MAC addresses
uint8_t green_address[6] = {0x74, 0x03, 0x7B, 0x4C, 0x99, 0xB4};
uint8_t blue_address[6] = {0x10, 0x0E, 0x7B, 0x4C, 0x99, 0xB4};
uint8_t yellow_address[6] = {0x6A, 0x03, 0x7B, 0x4C, 0x99, 0xB4};
uint8_t orange_address[6] = {0x6E, 0x19, 0x7B, 0x4C, 0x99, 0xB4};
uint8_t pink_address[6] = {0x21, 0x0A, 0x7B, 0x4C, 0x99, 0xB4};

static struct pt pt_drawScreen, pt_sendBytes;

//get left pot value and return chosen station
char getLeft(){
    int pot = readADC(LEFT_AN);
    if(pot < 340) return 'S';
    else if (pot > 341 && pot < 680) return 'C';
    else return 'N';   
}

//get right pot value and return chosen station
char getRight(){
    int pot = readADC(RIGHT_AN);
    if(pot < 255) return 'S';
    else if (pot > 256 && pot < 511) return 'C';
    else if (pot > 512 && pot < 767) return 'N';
    else return 'P';   
}

//get switch position and return direction
char getDirection(){
    if(mPORTAReadBits(SWITCH_BIT)) return 'E';
    else return 'W';
}

char left, right, dir, prevDir, prevLeft, prevRight;
char leftA, prevLeftA, rightA, prevRightA;
char buffer[30];
char buffer1[30];
char buffer2[30];

char la_buff[10];
char ra_buff[10];

//used to draw screen
char getLeftArrow(){
    if(dir=='E') return ' ';
    else return '<';
}

//used to draw screen
char getRightArrow(){
    if(dir=='E') return '>';
    else return ' ';
}

//// LED RGBpin
//// (RPB4) (Pin 7)
//#define GREEN BIT_4
//// (RPA1) (Pin 3)
//#define BLUE BIT_0
//// (RA2) (Pin 9)
//#define RED BIT_2


//method to turn the multicolor led to different colors 
void ledon(char c){
    switch(c){
        case 'R' : //turn LED red
            mPORTBClearBits(BLUE | GREEN);
            mPORTASetBits(RED);
            break;
        case 'B' : //turn LED blue
            mPORTBClearBits(GREEN);
            mPORTAClearBits(RED);
            mPORTBSetBits(BLUE);
            break;
        case 'G' : //turn LED green
            mPORTBClearBits(BLUE);
            mPORTAClearBits(RED);
            mPORTBSetBits(GREEN);
            break;
        case 'C': //turn LED cyan
            mPORTBSetBits(BLUE);
            mPORTAClearBits(RED);
            mPORTBSetBits(GREEN);
            break;
    }   
}


static PT_THREAD (protothread_drawScreen(struct pt *pt))
{
    PT_BEGIN(pt);
    while(1) {
        
        left = getLeft(); //if left pot hasnt changed, don't redraw
        if(left != prevLeft){
            tft_setTextSize(4);
            tft_setCursor(30, 100);
            tft_setTextColor(ILI9341_BLACK);
            tft_writeString(buffer);
            tft_setCursor(30, 100);
            tft_setTextColor(ILI9341_WHITE);
            sprintf(buffer,"%c", left);
            tft_writeString(buffer);
            prevLeft = left;
        }
        
        
        right = getRight(); //if right pot hasnt changed, don't redraw
        if(right != prevRight){
            tft_setTextSize(4);
            tft_setCursor(260, 100);
            tft_setTextColor(ILI9341_BLACK);
            tft_writeString(buffer1);
            tft_setCursor(260, 100);
            tft_setTextColor(ILI9341_WHITE);
            sprintf(buffer1,"%c", right);
            tft_writeString(buffer1);
            prevRight = right;
        }
        
        dir = getDirection(); //if direction hasnt changed, don't redraw
        if(dir != prevDir){
            leftA = getLeftArrow();
            rightA = getRightArrow();
            tft_setTextSize(4);
            tft_setCursor(60, 100);
            tft_setTextColor(ILI9341_BLACK);
            tft_writeString(la_buff);
            tft_setCursor(60, 100);
            //print directional arrow
            sprintf(la_buff,"%c------%c", leftA, rightA);
            tft_setTextColor(ILI9341_WHITE);
            tft_writeString(la_buff);
            
            prevDir = dir;
            prevLeftA = leftA;
            rightA = rightA;
        }
        PT_YIELD_TIME_msec(100);
    }
    PT_END(pt);
}

uint8_t writeBuf[5];
static PT_THREAD (protothread_sendBytes(struct pt *pt))
{
    PT_BEGIN(pt);
    while(1) {
        if(mPORTAReadBits(BUTTON_BIT) ? 0 : 1){ 
            //user has pushed the send command button
            ledon('C'); //color indicated device is transmitting
            sprintf( writeBuf, "%c %c %c", left, right, dir ); //prepare packet
            
            //send to all slave devices
            biscuit_central_write_bytes(STATION0, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            biscuit_central_write_bytes(STATION1, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            biscuit_central_write_bytes(STATION2, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            
            ledon('G'); //color indicating transmission complete
            
            WriteTimer23(0); //reset interrupt timer
            ConfigIntTimer23(T23_INT_ON | T23_INT_PRIOR_2);
            PT_YIELD_TIME_msec(100);
        }
        
        if(mPORTBReadBits(KILL_SWITCH_BIT) ? 0 : 1){
            //user has pushed the clear all button     
            ledon('C'); //color indicated device is transmitting
            sprintf( writeBuf, "0 0 0"); //prepare clear all packet
            //send to all slave devices
            biscuit_central_write_bytes(STATION0, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            biscuit_central_write_bytes(STATION1, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            biscuit_central_write_bytes(STATION2, writeBuf, sizeof(writeBuf));
            PT_YIELD_TIME_msec(500);
            ledon('B'); //color indicating transmission complete
            
            WriteTimer23(0);
            ConfigIntTimer23(T23_INT_OFF); //turn interrupt off
            PT_YIELD_TIME_msec(100);
        }
        PT_YIELD_TIME_msec(50);
    }
    
    PT_END(pt);
}

//send clear signal after device has been running for set time
uint8_t intBuf[5];
void __ISR(_TIMER_23_VECTOR, ipl2) T2Int(void){
    LATAINV = 1;
    char zero = 0x30; //prepare clear all packet
    sprintf( intBuf, "%c %c %c", zero, zero, zero);
    biscuit_central_write_bytes(STATION0, intBuf, sizeof(intBuf));
    biscuit_central_write_bytes(STATION1, intBuf, sizeof(intBuf));
    biscuit_central_write_bytes(STATION2, intBuf, sizeof(intBuf));
    ledon('B');//color indicating device idle
    ConfigIntTimer23(T23_INT_OFF); //turn interrupt off
    mT1ClearIntFlag();
}

//delays durin init
//Written by Brandon and Nick
void delay( unsigned t)
{ //uses internal timer 4
    T4CON = 0x8000;
    while (t--)
    {
        TMR4 = 0;
        while( TMR4< 8000);
    }
}

//initialize the Bluetooth and connect to slaves
void initBT(){
    //reset device before connecting to new slaves
    mPORTBSetBits(RESET);
    delay(2500);
    mPORTBClearBits(RESET);
    delay(2500);   
    
    // set up BLE chip
    biscuit_central_init();
    delay(5000);
    
    //need to load one PIC with three addresses, 
    //and the other pic with the other two
    
//    biscuit_central_connect(blue_address);
//    delay(45000);
//    
//    biscuit_central_connect(green_address);
//    delay(45000);
//   
//    biscuit_central_connect(orange_address);
//    delay(45000);
    
    biscuit_central_connect(yellow_address);
    delay(45000);
    
    biscuit_central_connect(pink_address);
    delay(45000);
    
    ledon('B');
}

void init() {
    //initialize digital and analog IO
    mPORTASetPinsDigitalOut(RED);
        
    mPORTBSetPinsDigitalOut(GREEN | BLUE | RESET);
    
    mPORTBSetPinsAnalogIn(LEFT_BIT | RIGHT_BIT);
    
    mPORTASetPinsDigitalIn(BUTTON_BIT | SWITCH_BIT);
    
    mPORTBSetPinsDigitalIn(KILL_SWITCH_BIT);
    
    
    //enable pull down resistors for pots, buttons, and the switch
    CNPUB = (LEFT_BIT | RIGHT_BIT | KILL_SWITCH_BIT);
    CNPUA = (BUTTON_BIT | SWITCH_BIT);
    
    configureADC();
    
    //config interrupt timer
    OpenTimer23(T23_ON, 1199999999); //30sec
}

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

    ANSELA = 0; ANSELB = 0; CM1CON = 0; CM2CON = 0;

    // === config threads ==========
    // turns OFF UART support and debugger pin
    PT_setup();
    
    init();
    ledon('R');  //device initializing

    // === setup system wide interrupts  ========
    INTEnableSystemMultiVectoredInt();
    
    PT_INIT(&pt_drawScreen); 
    PT_INIT(&pt_sendBytes);
    
//    initialize screen
    tft_init_hw();
    tft_begin();
    tft_setRotation(1);
    tft_fillScreen(ILI9341_BLACK);
    tft_setCursor(0, 0);
    tft_setTextSize(1);
    tft_setTextColor(ILI9341_WHITE);
    
    tft_writeString("Loading...");
    
    //initialize the Bluetooth and connect to slaves
    initBT();
    
    tft_setCursor(0, 0);
    tft_setTextSize(1);
    tft_setTextColor(ILI9341_BLACK);
    
    tft_writeString("Loading...");
    
    // round-robin scheduler for threads
    while (1){
        PT_SCHEDULE(protothread_drawScreen(&pt_drawScreen));
        PT_SCHEDULE(protothread_sendBytes(&pt_sendBytes));
    }
  } // main


/* *****************************************************************************
 End of File
 */