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

__CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS);





static void UpdateUARTTimer(void);
static void ReceiveMachine(void);
static void UpdateMotors(void);
static void UpdateServos(void);
static void UpdateDirectionServo(void);
static void UpdateFlags(void);
static void UpdateTimeouts(void);



//These Variables are used to store the transmission Data
static unsigned char Byte_Data1 = 0x00;
static unsigned char Byte_Data2 = 0x40;
static unsigned char Transmit_FrameID = 1;


//These Variables deal with the state of the receiving state machine.
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;


//This Variable stores the counter that will be incremented every time the interrupt fires
	//it handles the timing for the servos.
static unsigned char ServoTimer = 0;


void interrupt ISR(void){
    TMR1IF = 0;  //reset flags

    //Make the interrupt fire next time at half the TMR1L rollover rate
    TMR1L = 128;
    TMR1H = 255;
    //increment our timer variable
    ServoTimer++;

    //Perform the servo update with the new time.
    UpdateServos();
}



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

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


    //Timer2 Init
    T2CKPS0 = 0; //set up prescale to 00 = 1X
    T2CKPS1 = 0;
    CCP1M0 = 0;//config mode bits
    CCP1M1 = 0;
    CCP1M2 = 1;
    CCP1M3 = 1;

    DC1B0 = 0; //690 only
    DC1B1 = 0;	//690 only
    CCPR1L = 127;
    TMR2ON = 1;

    //initialize outputs for Servos 0,1,2
    TRISC0 = 0;
    TRISC1 = 0;
    TRISC2 = 0;
    //initialize motor driver ports
    TRISC4 = 0;		//Motor Direction Port
    TRISC5 = 0;		//Motor PWM Port

    TRISC7 = 1;     //AN9 input for Voltage sensor
    TRISC3 = 1;     //inputs for manually setting servos
    TRISC6 = 1;     //inputs for manually setting servos


    //Initialize input for selecting motor driver 1 or 2
    TRISA0 = 1;
}




void main(void){
    InitUART();


	while(1){
        UpdateUART();
	UpdateMotors();
        UpdateFlags();
        UpdateDirectionServo();

	}
}
//#endif




static char Motor0Duty = 128;
static char Motor0Dir = 0;
static char ReceiveTimeout = 0;
static char ReceiveTimeoutLimit = 44;

static void UpdateMotors(){
	//Parse the received message from the controller for motor direction
	//different settings on pin RA0 tell us which motor we are controlling
	if (RA0 ==0){
		if ((ReadReceiveMessage(0) & 0b00000001)==0){
		RC4 = 0;
		}
		else{
		RC4 = 1;
		}
	}
	else{
		if ((ReadReceiveMessage(0) & 0b00000010)==0){
		RC4 = 0;
		}
		else{
		RC4 = 1;
		}
	}

}


//These variables store the positions for each servo in the various states
static char Servo0off = 19;//red
static char Servo0on = 36;//red
static char Servo1off = 36;//green
static char Servo1on = 19;//green

static char Servo0Duty = 19;//Servo0off;
static char Servo1Duty = 36;//Servo1off;
static char Servo2Duty = 27; //straight ahead


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

static void UpdateServos(){
//this function is called by the inturrupt ever time timer 1 is rolled over

    if (RA0 ==0){
        if (ServoTimeout < ServoTimeoutLimit){
	        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;
    	    }
        }
    }
    else{
    	if (ServoTimer < Servo2Duty){
    	    //make sure the bit is high
	        RC0 = 1;
        }
    	else {
	    //else make sure the bit is low
	    RC0 = 0;
        }
    }

}


static char LastServo0 = 0;
static char LastServo1 = 0;

//This function parses the received message and sets the servos for flag status
static void UpdateFlags(void){

	//This function is only this complicated so that updates only happen
	//once so we can reset the timeout when they happen. With good servo control, thats probably useless
    if (RA0 == 0){

        if ((ReadReceiveMessage(0) & 0b00000100)==0){ //real code
        //if ((ReadReceiveMessage(0) & 0b00000001)==0){   //test code responds to motor direction instead
            //this means servo zero should be 0
            if (LastServo0 == 1){
                //last time it was on so we should turn it off
                Servo0Duty = Servo0off;
                LastServo0 = 0;
                ServoTimeout = 0;
            }
        }
        else{
            //this means servo zero should be 1
            if (LastServo0 == 0){
                //last time it was off so we should turn it on
                Servo0Duty = Servo0on;
                LastServo0 =1;
                ServoTimeout = 0;
            }
        }
        if ((ReadReceiveMessage(0) & 0b00001000)==0){ //real code
        //if ((ReadReceiveMessage(0) & 0b00000010)==0){   //test code responds to motor direction instead
            //this means servo 1 should be 0
            if (LastServo1 == 1){
                //last time it was on so we should turn it off
                Servo1Duty = Servo1off;
                LastServo1 = 0;
                ServoTimeout = 0;
            }
        }
        else{
            //this means servo 1 should be 1
            if (LastServo1 == 0){
                //last time it was off so we should turn it on
                Servo1Duty = Servo1on;
                LastServo1 =1;
                ServoTimeout = 0;
            }
        }
    }
}


static signed int	Motor1power = 0;
static signed int	Motor2power = 0;
static signed int	Avgpower = 0;
static signed int	Differentialpower = 0;
static signed int	Direction  = 128;

//This code implements a servo that points in the direction of travel of the boat
//(intended to point a light for night use), or at least the intended user control,
//pretty cool, and worked, but not actually used for outside in sun at the competition

static void UpdateDirectionServo(void){
	    if (RA0 ==1){

		    //Handle Direction
		    Motor1power = ReadReceiveMessage(1);
		    Motor2power = ReadReceiveMessage(2);
		    if ((ReadReceiveMessage(0) & 0b00000001)==0){
			    Motor1power = (Motor1power * -1);
		    }
		    if ((ReadReceiveMessage(0) & 0b00000010)==0){
			    Motor2power = (Motor2power * -1);
		    }


		    Avgpower = (Motor1power + Motor2power)/2;
		    //now that we have made the power include direction -255 to +255
		    //straight forward or backward should give a straight ahead value of the difference being zero

		    //When we do the subtraction, we get -510 to +510 with forward being 0
		    Differentialpower = Motor1power - Motor2power;


		    if(Avgpower < 0){
			    Avgpower = (Avgpower * -1);
		    }

		    if (Avgpower > 32){
			    Direction = 128 - (23*Differentialpower / Avgpower);
		    }
		    else if((Differentialpower > 64) || (Differentialpower < -64)){
			    Direction = 128 - (10*Differentialpower);
		    }
		    else{
			//if (Differentialpower > 128){
                Direction = 128;
		    }

		    if (Direction >255){
			    Direction = 255;
	 	    }

		    if (Direction < 0){
			    Direction  = 0;
		    }

            Direction = ((Direction)/7) + 9;

	    Servo2Duty = Direction;

	    }
}




//run the things that have to run to receive messages
void UpdateUART(void){
    UpdateUARTTimer();
    ReceiveMachine();
}


//update the timer for timeout ability
static void UpdateUARTTimer(void){
   if (T0IF == 1){ //if timer 0 overflows
        T0IF = 0;
        ReceiveMessageTimer++;

        if (ReceiveMessageTimer >= 10){
            ReceiveMessageTimer = 0;
            ReceiveMessageTimerFlag = 1;
        }
    }
}



//Run the actual receive state machine, same as on the bee pic, just duplicated here
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 = 0;
                }
                break;
            case Receive_Data:
                ReceiveMessage[ReceiveMessageIndex] = ReceiveNewByte;
                //ReceiveMessageChecksum += ReceiveNewByte;
                ReceiveMessageIndex++;
                if (ReceiveMessageIndex >= 3){	//ReceiveMessageLength){
                    ReceiveState = Receive_Idle; //ReceiveState = Receive_Checksum;
					ReceiveMessageFlag = 1;				//added
                    //UpdateFlags();
                    UpdateDirectionServo();
                    ReceiveTimeout = 0;
                }
                ReceiveMessageTimer = 0;
                break;
        }
    }
    else if (ReceiveMessageTimerFlag == 1){
        ReceiveState = Receive_Idle;
        ReceiveMessageTimerFlag = 0;
    }
}



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

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