#include <htc.h>
#include "pic16f690.h"
#include <stdio.h>
#include "bitdefs.h"
#include "AVCXbeePicUART.h"

//__CONFIG(_CP_OFF & _WDT_OFF & _PWRTE_ON & _HS_OSC)
__CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS);



static void UpdateUARTTimer(void);
static void ReceiveMachine(void);

static void UpdateServos(void);

static void UpdateTimeouts(void);
static void UpdateDisplayValues(void);




static unsigned char Byte_Data1 = 0x00;
static unsigned char Byte_Data2 = 0x40;
static unsigned char Transmit_FrameID = 1;






static ReceiveState_t ReceiveState = Receive_Idle;

static unsigned char ReceiveMessageFlag = 0;
static unsigned char ReceiveMessageTimer = 0;
static unsigned char ReceiveMessageTimerFlag = 0;

static unsigned char ReceiveMessage[20];
static unsigned char ReceiveMessageLength = 0;
static unsigned char ReceiveMessageIndex = 0;
static unsigned char ReceiveMessageChecksum;

static unsigned char ServoTimer = 0;


void interrupt ISR(void){
    TMR1IF = 0;

    //make the next timeout fire at half the timer low rollover freq
    TMR1L = 128;
    TMR1H = 255;
    ServoTimer++; //advance the software timer
    UpdateServos();//update the servos with the new timer
}



void InitUART(void){
// Initialization
	//Clear ANSEL registers
	ANSEL = 0;
	ANSELH = 0;

	//TRIS registers for EUSART
	TRISB5 = 1; //RX input
	TRISB6 = 1; //Sort of like slave select line
	TRISB7 = 0; //TX output


    //EUSART initialization
    //9600 baud rate:
    SPBRG = 129;
    BRGH = 1; //BRGH;
    //asynch mode:
    SYNC = 0; //SYNC;
    SPEN = 1; //SPEN;
    //enable tx and rx:
    TXEN = 1; //TXEN;
    CREN = 1; //CREN;

    //Timer0 Init
    T0CS = 0; //use Fosc/4
    PSA = 0; //prescale for T0
    PS0 = 1; //Set Prescale to 256.
    PS1 = 1;
    PS2 = 1;

    //Timer1 Init
    T1CKPS0 = 1; //Compbined thes two prescale bits will give us
    T1CKPS1 = 0; //		a prescale of 2 (00 = 1, 01 = 2, 10 = 4, 11 = 8)
    TMR1CS = 0; //clock select, 0 for internal  fosc/4
    TMR1ON = 1; //turn the timer on
    //this timer will run at 24000000/4/8 = 750000Hz
    //this means it will roll over at 2929 and roll over big at 11.44 Hz ... measured at 9.5 Hz
    //11.4 hz will probably be ok, at least according to http://home.earthlink.net/~tdickens/68hc11/servo/servo.html
    //might want to change the prescale to divide by two though to give 44 hz, and 12khz
        //did it, and got 38Hz

    //inturrupts
    TMR1IE = 1;
    PEIE = 1;
    GIE = 1;


    //initialize outputs for Servos 0,1,2,3,4,5
    TRISC0 = 0;
    TRISC1 = 0;
    TRISC2 = 0;
    TRISC3 = 0;
    TRISC4 = 0;
    TRISC5 = 0;


    //initialize a received message to set the servos in the right start position
    ReceiveMessage[0] = 0xB9;
    ReceiveMessage[1] = 0b00000000;
    ReceiveMessage[2] = 0b00000000;
    ReceiveMessage[3] = 0x88;//Team color Byte none

}


void main(void){
    InitUART();
	//debug code
	TRISA0 = 1;

	while(1){

        UpdateUART();
        UpdateDisplayValues();
        RA0 ^= 1;//debut pin

	}
}







//these are all the values for the different possible display servo positions
static char ServoRed = 45;
static char ServoRedPending = 35;
static char ServoOff = 27;
static char ServoGreenPending =18;
static char ServoGreen = 9;

static char Servo0Duty = 27;
static char Servo1Duty = 27;
static char Servo2Duty = 27;
static char Servo3Duty = 27;
static char Servo4Duty = 27;
static char Servo5Duty = 27;


static unsigned char    ServoTimeout = 0;
static unsigned char    ServoTimeoutLimit = 254;

//this function is called by the timer interrupt to update the servo signals

static void UpdateServos(){
    //Timer counted by interrupt above on rollover of timer1
    //just go through and check the time on every servo.  Repetitive, but no need to worry about space here�

    //if the value is less than duty 0...
    if (ServoTimer < Servo0Duty){
        //make sure the bit is high
        RC0 = 1;
    }
    else {
        //else make sure the bit is low
        RC0 = 0;
    }

    if (ServoTimer < Servo1Duty){
        //make sure the bit is high
        RC1 = 1;
    }
    else {
        //else make sure the bit is low
        RC1 = 0;
    }

    if (ServoTimer < Servo2Duty){
        //make sure the bit is high
        RC2 = 1;
    }
	else {
        //else make sure the bit is low
        RC2 = 0;
    }

    if (ServoTimer < Servo3Duty){
        //make sure the bit is high
        RC3 = 1;
    }
    else {
        //else make sure the bit is low
        RC3 = 0;
    }
    if (ServoTimer < Servo4Duty){
        //make sure the bit is high
        RC4 = 1;
    }
    else {
        //else make sure the bit is low
        RC4 = 0;
    }

    if (ServoTimer < Servo5Duty){
        //make sure the bit is high
        RC5 = 1;
    }
    else {
        //else make sure the bit is low
        RC5 = 0;
    }
}


//This code simply parses the display message by brute force..

static void UpdateDisplayValues(void){
    //bit 0, index 1 for on off
    if ((ReceiveMessage[1] & 0b00000001)==0){
        Servo0Duty = ServoOff;
    }
    else{
        //bit 0 index 2 for color
        if ((ReceiveMessage[2] & 0b00000001)==0){
            Servo0Duty = ServoRed;
        }
        else{
            Servo0Duty = ServoGreen;
        }
    }

    //Keep on doing the same thing for other bit numbers
    if ((ReceiveMessage[1] & 0b00000010)==0){
        Servo1Duty = ServoOff;
    }
    else{
        if ((ReceiveMessage[2] & 0b00000010)==0){
            Servo1Duty = ServoRed;
        }
        else{
            Servo1Duty = ServoGreen;
        }
    }

    //and the same thing again and again.
    if ((ReceiveMessage[1] & 0b00000100)==0){
        Servo2Duty = ServoOff;
    }
    else{
        if ((ReceiveMessage[2] & 0b00000100)==0){
            Servo2Duty = ServoRed;
        }
        else{
            Servo2Duty = ServoGreen;
        }
    }
    //I guess this is why you shouldn't do repetitive code...
    if ((ReceiveMessage[1] & 0b00001000)==0){
        Servo3Duty = ServoOff;
    }
    else{
        if ((ReceiveMessage[2] & 0b00001000)==0){
            Servo3Duty = ServoRed;
        }
        else{
            Servo3Duty = ServoGreen;
        }
    }
    //because you have to add a comment for each copy and pasted block!
    if ((ReceiveMessage[1] & 0b00010000)==0){
        Servo4Duty = ServoOff;
    }
    else{
        if ((ReceiveMessage[2] & 0b00010000)==0){
            Servo4Duty = ServoRed;
        }
        else{
            Servo4Duty = ServoGreen;
        }
    }

    //finally the team select indicator is going to have some different code..
    //not any more exciting though, just do a switch on the class assigned team setting values.
    switch(ReceiveMessage[3]){
        case 0x01:
            Servo5Duty = ServoGreen;
            break;
        case 0x02:
            Servo5Duty = ServoGreenPending;
            break;
        case 0x88:
            Servo5Duty = ServoOff;
            break;
        case 0xFD:
            Servo5Duty = ServoRedPending;
            break;
        case 0xFE:
            Servo5Duty = ServoRed;
            break;
    }

}





//perform the functions we need to perform for art, in this case its only recieve
void UpdateUART(void){
    UpdateUARTTimer();
    ReceiveMachine();
}

static void UpdateUARTTimer(void){
   if (T0IF == 1){ //if timer 0 overflows
        T0IF = 0;
        ReceiveMessageTimer++;
        if (ReceiveMessageTimer >= 10){
            ReceiveMessageTimer = 0;
            ReceiveMessageTimerFlag = 1;
        }
    }
}



//the receive state machine is an exact copy of the code in the motor controller pic, just receives and stores.
static void ReceiveMachine(void){
    if (RCIF == 1){
        unsigned char ReceiveNewByte = RCREG;
        switch (ReceiveState){
            case Receive_Idle:
                if (ReceiveNewByte == Start_Motor){
                    ReceiveMessageFlag = 0;
                    ReceiveState = Receive_Data;
                    ReceiveMessageTimer = 0;
		    ReceiveMessageIndex = 1;
                }
                break;
            case Receive_Data:
                ReceiveMessage[ReceiveMessageIndex] = ReceiveNewByte;
                //ReceiveMessageChecksum += ReceiveNewByte;
                ReceiveMessageIndex++;
                if (ReceiveMessageIndex >= 4){
                    ReceiveState = Receive_Idle;
		    ReceiveMessageFlag = 1;
                }
                ReceiveMessageTimer = 0;
                break;
        }
    }
    else if (ReceiveMessageTimerFlag == 1){
        ReceiveState = Receive_Idle;
        ReceiveMessageTimerFlag = 0;
    }
}




//helper functions
unsigned char CheckReceiveMessageFlag(void){
    unsigned char ReturnVal = ReceiveMessageFlag;
    ReceiveMessageFlag = 0;
    return ReturnVal;
}

unsigned char ReadReceiveMessage(unsigned char Index){
    return ReceiveMessage[Index];
}