#include "final.h"

// *********************************************************
// INTERRUPTS 
// *********************************************************    

//**********************************************************
//Timer 0 Overflow
interrupt [TIM0_COMP] void timer0_overflow(void)
{   
    //Decrement the times if they are not already zero
    if (timeUpdate > 0)	
        timeUpdate--;
        
    if (timePWM > 0)
        timePWM--;
}  

//**********************************************************
//UART character-ready ISR
interrupt [USART_RXC] void uart_rec(void)
{
    r_char=UDR;                     //get a char
	if(r_char == 127)               //if 
	    r_index = 0;   
	    
	else if(r_char == 126)          //if end of transmission
    {
		r_ready=1;				   	//signal cmd processor
		UCSRB.7=0;   				//stop rec ISR 
	}
	else             
	    r_buffer[r_index++]=r_char; //build the input string 
	
	
}

/**********************************************************/
//UART xmit-empty ISR
interrupt [USART_DRE] void uart_send(void)
{
	t_char = t_buffer[++t_index];   //check buffer
	if (t_char == 0)                //if 0, end transmission 	
	{
		UCSRB.5=0;                  //kill isr 
		t_ready=1;                  //transmit done 
	}
	else	
	    UDR = t_char ;              //otherwise, send the char 
}

// *********************************************************
// CAR CONTROL 
// *********************************************************  
  
//**********************************************************          
//updateSystem - checks for changes in inputs
void updateSystem(void)    
{   
    int distance, angle;
    unsigned char mode; 
    
    timeUpdate = tUpdate;  		           //reset the task timer
    
    //if new input from PDA...    
    if (r_ready)    
    {   
        mode = r_buffer[0];              //store new user input
        angle = ((int)r_buffer[1])<<1;

        distance = r_buffer[2];
        gets_int();                         //reset receive interrupt
        resetServors();                     //reset the servoState
        
        switch (mode)
        {   
            //we go into controlled mode
            case 0:
                autoState = Controlled;         //make sure autonomous mode is off
                rotateCar(angle, distance);   //move car
                break;
            
            //we set the car to search for marble
            case 1:
                autoState = Searching;          //turn on autonomous state machine
                autoTurnCount = 0;      
                autoMode = searchMarble;        //search for ball 
                break;
            
            //we set the car to search for base
            case 2:
                autoState = Searching;          //turn on autonomous state machine
                autoTurnCount = 0;      
                autoMode = searchBase;          //search for base  
                break;
            
            //we store to rom the new calibrations, and set the,
            case 3:
                romScaledRotate = angle;
                scaledRotate = angle;
                romScaledMove = distance;
                scaledMove = distance;
                break;
            
            //if we don't have one of these cases ERROR!!!
            default:
                error();
        }     
    }  
    
    //if in autonomous mode, run state machine
    if (autoState != Controlled) 
        autoControl();  
}  
 


// *********************************************************
// CAR MOVEMENT 
// *********************************************************

//**********************************************************
//run function to rotate car   
void rotateCar(int angle, int distance)
{   
	//if soemthing it telling the car to rotate 
	//while it's not idle that's BAAAADDDD
	if(servoState != Idle)
		error();

    //calculate time needed to turn
    rotateTime = (scaledRotate*intAbs(angle))>>3;     
    
    //check which direction to turn
    if (angle < 0)
    {
        servo1 = 6;    //turn all wheels to right
        servo2 = 6;
        servo3 = 6;
    }
    else if (angle > 0)
    {
        servo1 = 2;    //turn all wheels to left
        servo2 = 2;
        servo3 = 2;
    } 
    else
    {
    	servo1 = 0;    //turn all wheels off
        servo2 = 0;
        servo3 = 0;
        rotateTime = 0; 
    }
    moveCar(distance);
    servoState = Rotating;
}

//**********************************************************
//run function to move car   
void moveCar(int distance)
{   
    //calculate time needed to turn
    moveTime = (scaledMove*intAbs(distance))>>3;
    
    //move front two wheels forward
    if (distance > 0)
    	moveForeward = 1;
    
    //move front two wheels backward
    else if (distance < 0)
    	moveForeward = 2;
    
    else 
    {
    	moveForeward = 0;
    	moveTime = 0;
    }  

}

//**********************************************************
//update the servo state machine
void updateServos(void)
{   
	PORTB = 0;    		//TODO: check with John why this is here
	timePWM = tPWM;		//reset the PWM counter
	
	//run the proper state machine code    
	switch (servoState)
	{
		//nothing to see here.... 
	    case Idle:
	        break;
	    
	    //we just wait here a while till we go into the foreward state
	    case Waiting:
	        if (waitTime > 0)
	            waitTime--;
	            
	        else
	        {    
	        	if(moveForeward == 0)
	        	{   
	        		servo1 = 0;
	        		servo2 = 0;
        			servo3 = 0;
	        	}
	        	else if(moveForeward == 1)
	        	{   
	        		servo1 = 0;
	        	 	servo2 = 6;
        			servo3 = 2;
	        	}
	        	else if(moveForeward == 2)
	        	{     
	        		servo1 = 0;
	        		servo2 = 2;
        			servo3 = 6;
	        	}
	            servoState = Foreward;
	        }   
	            
	        break;
	    
	    //we keep on decreasing rotateTime till it's 0, 
	    //then turn off servos and go into waiting mode   
	    case Rotating:
	        if (rotateTime > 0 && rotateTime < 160)
	            rotateTime--; 
	        
	        else
	        {
	            servo1 = 0;
	            servo2 = 0;
	            servo3 = 0;
	            waitTime = 10;
	            servoState = Waiting;
	        }
	         
	        break;
	   
	   	//we keep on decreasing moveTime till it's 0,
	   	//then we turn off servos and go into idle mode 
	    case Foreward:  
	        if (moveTime > 0)
	            moveTime--;     
	            
	        else
	        {
	            servo1 = 0;
	            servo2 = 0;
	            servo3 = 0;
	            servoState = Idle;
	        }
	        break;
	        
	    default: 
	        error();
	}
}

void resetServors(void)
{
    servoState = Idle;
    servo1 = 0;
    servo2 = 0;
    servo3 = 0;
    PORTB = 0;
}			

// *********************************************************
// AUTONOMOUS MODE 
// *********************************************************
   
//********************************************************** 
//state machine for autonomous mode 
void autoControl(void)
{         
    unsigned char foundPos;
    int distance, angle;   
    
    if(servoState != Idle)
        return;
    
    switch (autoState) 
    {   
        case Searching:                 //turn to look for marble/base
            //if turned 315 degrees, change state to 2
            if (autoTurnCount == 9)
                autoState = Moving;
            //else check camera
            else
            {
                if (autoMode == searchBase)           //if looking for base
                    foundPos = checkForBase();
                else if(autoMode == searchMarble)                   //if looking for marble
                    foundPos = checkForMarble(); 
                else
                    error();
                    
                //if found, change to found state
                if (foundPos != 0)
                    autoState = Locating;
                //if marble is not found, turn left 30 degrees
                else 
                {    
                    autoTurnCount++;    //increase counter
                    rotateCar(30, 0);        //run motion
                }
            }
            break;
        
        case Moving:                         //move forward one unit step
            //change back to state 1
            autoState = Searching;
            autoTurnCount = 0;
            //move foward 1 foot
            rotateCar(0, 30);
            break;
        
        case Locating:                             //found marble/base, turn towards it and move forward
            //check to see where marble/base is
            if (autoMode == searchBase)
                foundPos = checkForBase();
            else if(autoMode == searchMarble)
                foundPos = checkForMarble();  
            else
                error();
                
            //check to see if it disappears -> we have reached target        
            if (autoMode == searchMarble && foundPos == 0 || autoMode == searchBase && foundPos == 100)
                autoState = Found;
               
            else if(autoMode == searchBase && foundPos == 0)
            {
               autoState = Searching;
               autoTurnCount = 0;   
            }  
                
            //otherwise, move towards it
            else
            {   
                 
                //turn car by scaled amount
                if (foundPos < 40 || foundPos > 48)
                    angle = ((int)foundPos-44)>>2;
                //if in front of camera, do not turn
                else
                    angle = 0;
                    
                //move forward by scaled amount
                if (foundPos > 44)
                    distance = 20 -((foundPos-44)>>2);
                else
                    distance = 20 - ((44-foundPos)>>2);    
                //run servos
                rotateCar(angle, distance);  
            }
            break;
        
        case Found:                             //capture marble if mode = 0, release marble if mode = 1
            switch(autoMode)
            {   
                case searchMarble:   			 //if mode = 0 (move forward)      
                    autoMode = foundMarble;      //set transient state
                    //move 20 cm
                    rotateCar(0, 5);
                    break;
                
                case foundMarble:  //if mode = 0 (found marble)              
                    //find base
                    autoState = Searching;
                    autoMode = searchBase;
                    autoTurnCount = 0;
                    //turn 180 degrees
                    delay_ms(1000);
                    rotateCar(90, 0);
                    break;
                    
                //if found base (mode = 1)
                case searchBase:        
                    //set transient mode
                    autoMode = foundBase;
                    //back up 45 cm
                    delay_ms(500);
                    rotateCar(-7, -45);
                    break;
                    
                case foundBase:
                    //check for marbles
                    autoMode =  searchMarble;
                    autoState = Searching;
                    autoTurnCount = 0; 
                    //turn car around
                    rotateCar(90, 0);
                    break;
                    
                default:
                    error();
            }
            break;
                   
        default:
            error();
    }
}

// use camera to find marble  
unsigned char checkForMarble(void)
{
    unsigned char xPos, pixelCount; 
    unsigned char maxCountperLine;
    unsigned char horiz,vert;
    unsigned char red,green,blue; 
    unsigned char pixels[88];
	unsigned char RGrawData[88], GBrawData[88];     
	
    maxCountperLine = 0;
    xPos = 0;
    pixelCount = 0;
    TIMSK 	   = 0;                         //turn off interupts, so we aren't bothered by the silly timer 
    initCam();
    
    while(!VSYNC);                          //this stalls the microcontroller till the begning of the next frame
    while(VSYNC); 
    
    while(!HREF);                           
    while(HREF);   
    
    for(vert = 0; vert < 144; vert++)
    {   
        while(!HREF);
        pixelCount = 0;
        for(horiz = 0; horiz < 88; horiz++)
        {   
            while(!PCLK);                   //we wait for it to go back up, then we can read in next pixel
            GBrawData[horiz] = PINA;
            while(PCLK);                    //we wait for a PLCK clock tick to go down, before moving onto next pixel 
            while(!PCLK);                   //we wait for it to go back up, then we can read in next pixel
            RGrawData[horiz] = PINA;
            while(PCLK);
        }
        for(horiz = 0; horiz < 88; horiz++)
        {   
            blue  = (GBrawData[horiz]&0x0f);
            green = (RGrawData[horiz]>>4)&0x0f;
            red   = RGrawData[horiz]&0x0f;
            if(red > green<<2 && red > blue<<2)
            {   
                pixels[pixelCount] = horiz;
                pixelCount++;
            } 
        } 
        if(pixelCount > maxCountperLine)
        {
            maxCountperLine = pixelCount;
            xPos = pixels[pixelCount>>1];
        }
    }
    TIMSK = 2;
    
    if(xPos+1 > 88 || xPos+1 < 1)
        error();
         
    if(maxCountperLine < 3)
        return 0;
        
    else
        return xPos+1;
}

unsigned char checkForBase(void)
{
    unsigned char pixelCount; 
    unsigned char maxCountperLine;
    unsigned char horiz,vert;
    unsigned char red,green,blue; 
    unsigned char pixels[88];
	unsigned char RGrawData[88], GBrawData[88]; 
	unsigned char xPos;
    
    delay_ms(150);
	
    maxCountperLine = 0;
    pixelCount = 0;
    TIMSK = 0;                              //turn off interupts, so we aren't bothered by the silly timer 
    initCam();
    while(!VSYNC);                          //this stalls the microcontroller till the begning of the next frame
    while(VSYNC); 
    
    while(!HREF);                           
    while(HREF);   
    
    for(vert = 0; vert < 144; vert++)
    {   
        while(!HREF);
        pixelCount = 0;
        for(horiz = 0; horiz < 88; horiz++)
        {   
            while(!PCLK);                   //we wait for it to go back up, then we can read in next pixel
            GBrawData[horiz] = PINA;
            while(PCLK);                    //we wait for a PLCK clock tick to go down, before moving onto next pixel 
            while(!PCLK);                   //we wait for it to go back up, then we can read in next pixel
            RGrawData[horiz] = PINA;
            while(PCLK);
        }
        for(horiz = 0; horiz < 88; horiz++)
        {   
            blue  = (GBrawData[horiz]&0x0f);
            green = (RGrawData[horiz]>>4)&0x0f;
            red   = RGrawData[horiz]&0x0f;
            if(green > red<<1 && green > blue<<1)
            {   
                pixels[pixelCount] = horiz;
                pixelCount++;
            } 
        } 
        if(pixelCount > maxCountperLine)
        {
            maxCountperLine = pixelCount;
            xPos = pixels[pixelCount>>1];
        }
    }  
    TIMSK = 2; 
    
    if(xPos+1 > 88 || xPos+1 < 1)
        error();
    
    if(maxCountperLine > 54)
        return 100;
    
    if(maxCountperLine < 10)
        return 0;
        
    else
        return xPos+1;
}     

//********************************************************** 
//init I2C
void initI2C(void)
{
    TWSR = 0;                               //not necesary, but the spec says to keep it at 0, or bad things happen
	TWBR = (16000000 / 100000 - 16) / 2;    //this is 72...this sets up the I2C master clock at 100000bps
	                                        //NOTE: for more info see pg.173 of the mega32 manual
	                                        
	//NOTE: I SHOULD intilize the i2c line according to code vision, but it breaks if I do   
	//i2c_init();                             //initialize the I2C bus
}   

//here we'll write to all the registers to change the properties of the camera
void initCam(void)
{   
    delay_ms(5);  
    writeToCameraRegister(0x12,0x80);   //we initiate a soft reset, to make sure that all the regsiters are at default values
    delay_ms(10); 
    writeToCameraRegister(0x14,0x20);   /* reduce frame size */
    delay_ms(5); 
    writeToCameraRegister(0x12,0x08);   /* set RGB mode, with no AWB */
	delay_ms(5); 
	writeToCameraRegister(0x28,0x05);   /* set color sequencer */
    delay_ms(5); 
    writeToCameraRegister(0x13,0x01);   /* un-tri-state the Y/UV lines */
    delay_ms(5); 
    writeToCameraRegister(0x11,0x10);   //reduce the fps     

                 
    //TODO: see how long of delays we really need, or we solve the problem in the writeToCameraRegister
    //if we even need delays between each time we write to the camera register
}

//this writes to the camera regsiter
void writeToCameraRegister(char registerNum, char data)
{   
	
    i2c_start();            //when writing to I2C we need to send the start condition     
	i2c_write(0xc0);        //write the bus the slave address of the device we want to acess 
	i2c_write(registerNum); //we write to the bus the address we want to access 
	i2c_write(data);        //we write the data to previous line's memory address       
 /*
	while(i2c_start()!= 1) delay_ms(10);            //when writing to I2C we need to send the start condition     
	while(i2c_write(0xc0)!= 1) delay_ms(10);        //write the bus the slave address of the device we want to acess 
	while(i2c_write(registerNum)!= 1) delay_ms(10); //we write to the bus the address we want to access 
	while(i2c_write(data)!= 1) delay_ms(10);        //we write the data to previous line's memory address   
 */	
	 
	//while(!(i2c_start() && i2c_write(0xc0) && i2c_write(registerNum) && i2c_write(data)));
	i2c_stop();                        //we end the transmission with the stop condition   
	
	//TODO: if the slave doesn't adknowledge a signal the current scheme resends the current instruction
	//however this throws it into an infinate loop. To avoid this we need delays before each time we write 
	//to each camera register. I need to look into seeing if i have to resend the start command and resend 
	//all the information, or if the current scheme is fine
}

// *********************************************************
// RECEIVE/TRANSMIT FUNCTIONS 
// *********************************************************
   
//********************************************************** 

//set up receive interrupts 
void gets_int(void) 
{
    r_ready=0;    //get ready to receive
    r_index=0;    //reset buffer counter
    UCSRB.7=1;    //turn on ISR
}

//********************************************************** 
//set up transmit interrupts   
void puts_int(void) 
{
    t_ready=0;
    t_index=0;
    if (t_buffer[0]>0) 
    {
  	    putchar(t_buffer[0]);
  	    UCSRB.5=1;
    }
}



// *********************************************************
// INITIALIZATION 
// *********************************************************
//Set it all up
void initialize(void)
{     
    //set up ports 
    DDRB 		 = 0xff;            	//PORTB for car wheels 
    PORTB 		 = 0;                          
    DDRA 		 = 0x00;            	//PINA for camera data
    PORTA 		 = 0;
    DDRC 		 = 0b00000011;      	//PORTC for camera input
    DDRD.7 	 	 = 1;             		//PORTD for LED 
    PORTD.7 	 = 0;
        
    //serial setup for debugging using printf, etc.     
    UCSRB 		 = 0x18;                 //enable receive/transmit
    UBRRL 		 = 103;                  //set baud rate to 9600
    
    //set up timer 0     
    OCR0 		 = 124;	   				//0.5 mSec
    TIMSK 	 	 = 2;	   				//turn on timer 0 cmp-match ISR 
    TCCR0 		 = 0b00001011;			//prescalar to 64  and Clr-on-match        

    //init the task timers
    timeUpdate   = tUpdate; 
    timePWM 	 = tPWM;    
    
    //set car control variables
    servo1 		 = 0;             		//turn PWM off
    servo2 		 = 0;
    servo3 		 = 0;
    servoState   = Idle; 				//the servos are Idle
    
    //set car movement variables
    rotateTime   = 0;         			//turn off car control timers
    moveTime 	 = 0;
    waitTime 	 = 0;
    scaledMove   = romScaledMove;   	//CALIBRATED SCALED VARIABLES
    scaledRotate = romScaledRotate;   	//CALIBRATED SCALED VARIABLES
    
    //set autonomous variables
    autoState = Controlled;             //not in autonomous mode     
    
    //initilize the camera sub-system
    initI2C();
    //initCam(); 
    
    //DEBUG
#if DEBUG
    autoTurnCount = 0;      
    autoMode = searchMarble;  
    autoState = Searching; 
#endif
    
    //crank up the ISRs
    #asm
  	    sei
    #endasm  
    
    //enable receive interrupts
    gets_int(); 
    
}  

// *********************************************************
// MAIN PROGRAM 
// *********************************************************     

//Entry point and task scheduler loop
void main(void)
{
    initialize();
    //main task scheduler loop -- never exits!
    while(1)
    {    
        //check system
        if (timeUpdate == 0)	
            updateSystem();
                  
        //check wheels 
        //turn high when...
        if (timePWM == servo1-1)      //check back wheel
            PORTB.0 = 1;   
            
        if (timePWM == servo2-1)      //check left wheel
            PORTB.1 = 1;
            
        if (timePWM == servo3-1)      //check right wheel
            PORTB.2 = 1;
        
        //turn low when...   
        if (timePWM == 0) 
        	updateServos();
        	
    }   
}