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

  @Company
    Company Name

  @File Name
    Skillman.c

  @Summary
    The code that is loaded onto the PIC at the Skillman 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>

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

// Switches
// All RPB except 2 and 4
#define SW1 BIT_15
#define SW2 BIT_3
#define SW3 BIT_14
#define SW4 BIT_2
#define SW5 BIT_13
#define SW6 BIT_5
#define SW7 BIT_11
#define SW8 BIT_10


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)){
	// This heartbeat pulses at 1Hz 10% duty cycle
    PT_BEGIN(pt);
    while(1){
        
        // Flash an LED on RPA1
        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);
}


// Create a bitwise checksum of the 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 a packet over the UART channel 1
void send_packet(uint8_t data[6]){
    while(!UARTTransmitterIsReady(UART1));
    UARTSendDataByte(UART1, 0xf1); // preamble
    int i;
    for (i=0; i<6; i++){ // send the 6 bytes
        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; 
    
}

uint8_t dest, speed;

// Perform a blocking delay
// This is used to charge the capacitors
void delay( unsigned t)
{ //uses internal timer 2
    T2CON = 0x8000;     // enable tmr1, Tcy, 1:1
    while (t--)
    {
        TMR2 = 0;
       // while (TMR1<8000);
        while( TMR2< 8000);

    }
}

volatile int tS; // Is there a train on the South tracks
int tmp; // A int to write the IC time to, don't care about it
void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl2) IC4Int (void){
    set_speed(0,0,0,0,0,0,0); // Shutdown the tracks
    tmp = mIC4ReadCapture(); // This is required to clear the interrupt
    tS = 1; // Train detected
    mIC4ClearIntFlag(); // Clear the interrupt so there isn't a infinite loop
}


void set_tracks(char src, char dest, char dir){
    // Stop all tracks
    char s = 0xF;
    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,s,s,0);
        else if(dir == 'W')
            set_speed(0,1,1,0,s,s,0);
    }
    // Center to Center
    else if(src== 'C'){
        if(dir == 'E'){
            set_speed(1,2,2,2,s,s,s);
            set_speed(2,0,0,2,0,0,s);
        }
        else if(dir == 'W'){
            set_speed(1,1,1,1,s,s,s);
            set_speed(2,0,0,1,0,0,s);
        }
    }
    // South to South
    else if(src== 'S'){
        if(dest == 'P'){
            mPORTASetBits(SW2);
            delay(2000);
            if(dir == 'E'){
                set_speed(2,2,2,0,0x8,0x4,0);
                set_speed(0,0,0,2,0,0,0x2);
            }
            else if(dir == 'W'){
                set_speed(0,0,0,1,0,0,0xF);
                set_speed(2,1,1,0,0xF,0xF,0);
            }
            mPORTAClearBits(SW2);
            
        }
        else{ // dest == 'S'
            mPORTBSetBits(SW1);
            delay(2000);
            if(dir == 'E') {
                if(tS)
                    set_speed(0,0,0,0,0x0,0x0,0); // there is a train there
                else {
                    set_speed(2,2,2,2,s,s,s);
                    set_speed(0,0,0,2,0,0,s);
                }
            }
            else if(dir == 'W'){                
                set_speed(2,1,1,1,s,s,s);
                set_speed(0,0,0,1,0,0,s);
            }
            
            mPORTBClearBits(SW1);
        }
    }    
}

void setup_IC(){
    // Connect the IC#
    PPSInput(2, IC3, RPB1);
    PPSInput(3, IC1, RPB2);
    PPSInput(1, IC4, RPB3);
    // Set the pull down resistors
    mPORTBSetPinsDigitalIn(BIT_1 | BIT_2 | BIT_3);
    CNPDB = (BIT_1 | BIT_2 | BIT_3);
    
    
    OpenTimer2(T2_ON | T2_PS_1_256, 0xFFFF);
    
    OpenCapture1(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_FEDGE_RISE | IC_ON);
    ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_2 | IC_INT_SUB_PRIOR_2);
    OpenCapture3(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_FEDGE_RISE | IC_ON);
    ConfigIntCapture3(IC_INT_ON | IC_INT_PRIOR_2 | IC_INT_SUB_PRIOR_2);
    OpenCapture4(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_FEDGE_RISE | IC_ON);
    ConfigIntCapture4(IC_INT_ON | IC_INT_PRIOR_2 | IC_INT_SUB_PRIOR_2);
}


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));
    setup_IC();
}

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 BlueTooth 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);
            
            
    while(1){
        PT_SCHEDULE(protothread_heartbeat(&pt_heartbeat));
        PT_SCHEDULE(protothread_read(&pt_read));
    }
}
/* *****************************************************************************
 End of File
 */