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

  @Company
    Lafayette

  @File Name
    metzgar.c

  @Summary
    The code that is loaded onto the PIC at the metzgar 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
// (RPB0) (Pin 4)
#define GREEN BIT_1
// (RPA1) (Pin 3)
#define BLUE BIT_0
// (RPA0) (Pin 2)
#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;
uint32_t tmp;

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 clear_switches(){
    mPORTAClearBits(SW2 | SW4);
    mPORTBClearBits(SW1 | SW3 | SW5 | SW6 | SW7 | SW8);
}

// ISRs
volatile uint32_t tN, tE, tS; // Train present
void __ISR(_INPUT_CAPTURE_3_VECTOR, ipl2) IC3Int (void){
    set_speed(0,0,0,0,0,0,0);
    tmp = mIC3ReadCapture();
    tN = 1;
    mIC3ClearIntFlag();
}

void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl2) IC1Int (void){
    set_speed(1,0,0,0,0,0,0);
    tmp = mIC1ReadCapture();
    tE = 1;
    mIC1ClearIntFlag();
}

void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl2) IC4Int (void){
    set_speed(2,0,0,0,0,0,0);
    tmp = mIC4ReadCapture();
    tS = 1;
    mIC4ClearIntFlag();
}

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


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

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
}


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

void setup_LED(){
    // Configure the LED 
    mPORTASetPinsDigitalOut(GREEN);
    mPORTASetPinsDigitalOut(RED);
    mPORTBSetPinsDigitalOut(BLUE);
    mPORTAClearBits(RED);
    mPORTBClearBits(BLUE);
}

void setup_switches(){
    mPORTASetPinsDigitalOut(SW2 | SW4);
    mPORTBSetPinsDigitalOut(SW1 | SW3 | SW5 | SW6 | SW7 | SW8);
    
}

void set_tracks(char src, char dest, char dir){
    int i; // used for blocking
    // 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);
        clear_switches();
    }
    // Straight
    
    // North to North
    if(dest == 'N'){
        mPORTBSetBits(SW8);
        if(dir == 'E'){
            if(tN) 
                set_speed(0,0,0,0,0x0,0x0,0); // there is a train there
            else set_speed(0,1,1,0,0x8,0x4,0);                      
        }
        else if(dir == 'W'){
            set_speed(0,2,2,0,0x8,0xF,0);
            tN = 0; // Train leaving  
        }
    }
    // Express to Express
    if(dest =='C'){
        if(dir == 'E'){
            if(tE) 
                set_speed(1,0,0,0,0x0,0x0,0); // there is a train there
            else set_speed(1,1,1,0,0x8,0x4,0);
        }
        else if(dir == 'W')
            set_speed(1,2,2,0,0xF,0xF,0);
            
    }
    // South to South
    if(dest == 'S'){
        mPORTBSetBits(SW5);
        if(dir == 'E')
            set_speed(2,2,2,0,0xF,0xF,0);
        else if(dir == 'W')
            if(tS) 
                set_speed(2,0,0,0,0x0,0x0,0); // there is a train there
            else set_speed(2,1,1,0,0x8,0x4,0);
    }
        
}

int setup(){
    ANSELA = 0; ANSELB = 0; CM1CON = 0; CM2CON = 0;
    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();
    setup_LED();
    setup_switches();
    clear_switches();
}

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);
            
//            char buf[3];
//            sprintf(buf, "%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_sendData);
    PT_INIT(&pt_heartbeat);
    PT_INIT(&pt_read);
    
            
            
    while(1){
//        PT_SCHEDULE(protothread_sendData(&pt_sendData));
        PT_SCHEDULE(protothread_heartbeat(&pt_heartbeat));
        PT_SCHEDULE(protothread_read(&pt_read));
    }
}
/* *****************************************************************************
 End of File
 */