/* ************************************************************************** */
/** Descriptive File Name

  @Company
    Lafayette

  @File Name
    sullivan_village.c

  @Summary
    The code that is loaded onto the PIC at the sullivan_village station

 */
/* ************************************************************************** */

/* ************************************************************************** */
/* ************************************************************************** */
/* 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 <math.h>

// Configure pins
// LED RGBpin
//(RPA1) (Pin 3)
#define GREEN BIT_1
// (RPA0) (Pin 2)
#define BLUE BIT_0
// (RPB0) (Pin 4)
#define RED BIT_0

// There are no switches for this stations

static struct pt pt_sendData, pt_input, pt_heartbeat, pt_read;

// What is the state of the LED
int on = 0;
static PT_THREAD (protothread_heartbeat(struct pt *pt)){
    PT_BEGIN(pt);
    while(1){
        
        // Flash an LED on RB5
        if(on){
            mPORTAClearBits(GREEN);
            on = 0;
            PT_YIELD_TIME_msec(900);
        }
        else{
            mPORTASetBits(GREEN);
            PT_YIELD_TIME_msec(100);
            on = 1;
        }   
        // NEVER exit while
    } // END WHILE(1)
    PT_END(pt);
}


// Bitwise xor for 6 bytes of data
uint8_t checksum(uint8_t data[6]){
    // data will be 7 bytes long
    uint8_t cs = 0;
    int i;
    for (i=0; i<6;i++){
        cs ^= data[i]; // bitwise xor of the data
    }
    return cs;
}

// Send packet over UART1 
void send_packet(uint8_t data[6]){
    while(!UARTTransmitterIsReady(UART1));
    UARTSendDataByte(UART1, 0xf1); // preamble
    int i;
    for (i=0; i<6; i++){
        while(!UARTTransmitterIsReady(UART1));
        UARTSendDataByte(UART1, data[i]);
    }
    while(!UARTTransmitterIsReady(UART1));
    UARTSendDataByte(UART1,checksum(data)); // send the cs
}

uint8_t buffer[6];
/* params
 *  dest: which slave PIC (0-2) to address
 *  dir: which rails to power positive vs negative (0,1,2,3)
 *  speed: PWM rate (0-F)
 * return: 0 no error
 */ 
int set_speed(uint8_t dest, uint8_t dir0, uint8_t dir1, uint8_t dir2,
               uint8_t speed0, uint8_t speed1, uint8_t speed2){
    
    // Preset bytes
    buffer[1] = 0x00; // The source is never used
    buffer[2] = 0x03; // Always 3 bytes
    
    if (dest > 0x02) return 1; // invalid destination
    buffer[0] = dest; // set destination
    
    // Set the speed and direction and clamp it
    if (dir0 > 0x03) return 2; // invalid direction
    if (dir1 > 0x03) return 2; // invalid direction
    if (dir2 > 0x03) return 2; // invalid direction
    if(speed0 > 0x0F) speed0 = 0xF; // Clamp the speed
    if(speed1 > 0x0F) speed1 = 0xF;
    if(speed2 > 0x0F) speed2 = 0xF;
    
    // store the speeds and the directions
    buffer[3] = (dir0 << 4) | speed0;
    buffer[4] = (dir1 << 4) | speed1;
    buffer[5] = (dir2 << 4) | speed2;
    
    // packet correctly formatted
    // send it
    send_packet(buffer);
    return 0; 
    
}


void set_tracks(char src, char dest, char dir){
    // Stop all tracks
    if(src == '0'){
        set_speed(0,0,0,0,0,0,0);
        set_speed(1,0,0,0,0,0,0);
        set_speed(2,0,0,0,0,0,0);
    }
    // North to North
    else if(src == 'N'){
        if(dir == 'E')
            set_speed(0,2,2,0,0xF,0xF,0);
        else if(dir == 'W')
            set_speed(0,1,1,0,0xF,0xF,0);
    }
    // Express to Express (Center)
    else if(src== 'C'){
        if(dir == 'E')
            set_speed(1,2,2,0,0xF,0xF,0);
        else if(dir == 'W')
            set_speed(1,1,1,0,0xF,0xF,0);
    }
    // South to South
    else if(src== 'S'){
        if(dir == 'E')
            set_speed(2,2,2,0,0xF,0xF,0);
        else if(dir == 'W')
            set_speed(2,1,1,0,0xF,0xF,0);
    }
    else{
        set_speed(2,1,1,1,0xF,0xF,0xF);
    }
        
}

// Setup the UART cable for working with the power station
int setup(){
    PPSOutput(1, RPB7, U1TX); //Assign U1RX to pin RPB7 -- Physical pin 16 on 28 PDIP
    UARTConfigure(UART1, UART_ENABLE_PINS_TX_RX_ONLY);
    UARTSetLineControl(UART1, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
    UARTSetDataRate(UART1, pb_clock, 9600);
    UARTEnable(UART1, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
}

char rbuffer[30];

uint8_t writeBuf[4];

char rsrc, rdest, rdir;

static PT_THREAD (protothread_read(struct pt *pt)) {
    PT_BEGIN(pt);
    
    //Clear the reset on the chip
    PT_YIELD_TIME_msec(50) ;
    mPORTAClearBits(BIT_4);
    
        while(1) {
            PT_YIELD_TIME_msec(50) ;
            
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input) );
            
            // scans UART terminal for specified format
            rsrc = '?';
            rdest = '?';
            rdir = '?';
            sscanf( PT_term_buffer, "%c %c %c", &rsrc, &rdest ,&rdir);
            set_tracks(rsrc, rdest, rdir);

            // NEVER exit while
        } // END WHILE(1)
    PT_END(pt);
} //  thread

int main(){

    // === config threads ==========
    // turns ON UART support and debugger pin
    PT_setup();
    setup();

    // === setup system wide interrupts  ========
    INTEnableSystemMultiVectoredInt();
    
    PT_INIT(&pt_heartbeat);
    PT_INIT(&pt_read);
    
    // Configure the LED 
    mPORTASetPinsDigitalOut(GREEN);
    mPORTBSetPinsDigitalOut(RED);
    mPORTASetPinsDigitalOut(BLUE);
    mPORTBClearBits(RED);
    mPORTAClearBits(BLUE);
            
            
    while(1){
        PT_SCHEDULE(protothread_heartbeat(&pt_heartbeat));
        PT_SCHEDULE(protothread_read(&pt_read));
    }
}