#include "final.h"              

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

//**********************************************************
//Timer 0 Overflow
interrupt [TIM0_COMP] void timer0_compare()
{   
    //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()
{
	r_char=UDR;    //get a char
	UDR=r_char;    //then print it
	//build the input string
	if (r_char != '\r')
	  if (r_char != 0x08) //don't record backspaces 
	    r_buffer[r_index++] = r_char; //add string to the buffer, and shift point over
	  //handle backspace            
	  else 
	    r_index--;
	else
	{
		putchar('\n');  			//use putchar to avoid overwrite
		r_buffer[r_index]='\0';    	//insert null character
		r_ready=TRUE;				//signal cmd processor
		UCSRB.7=0;   				//stop rec ISR
		r_index = 0;
	}
}

//**********************************************************
//UART xmit-empty ISR (transmit Data Register Empty, can put a new char in the transmit buffer)
interrupt [USART_DRE] void uart_send()
{
	t_char = t_buffer[++t_index];
	if (t_char == 0) 	
	{
		UCSRB.5=0; //kill isr 
		t_ready=TRUE; //transmit done 
	}
	else
	    UDR = t_char ;     //send the char 
}

//**********************************************************
//trasnmit a string from SRAM using interrupt driven serial communication
void transmit_str(unsigned char *str)
{   
    while(!t_ready);
    strncpy(t_buffer,str,buffer_length-1);
    enable_transmit_interrupt();
}

//**********************************************************
//trasnmit a string from flash using interrupt driven serial communication
void transmit_strf(unsigned char flash *str)
{   
    while(!t_ready);
    strncpyf(t_buffer,str,buffer_length-1);
    enable_transmit_interrupt();
}

// *********************************************************
// CAR CONTROL 
// *********************************************************  
  
//**********************************************************          
//updateSystem - checks for changes in inputs
void updateSystem()    
{   
    signed int distance, angle;
    unsigned char user_mode; 
    
    timeUpdate = tUpdate;  		           //reset the task timer
    
    //if new input from PDA...    
    if (r_ready)    
    {   
        #ifdef HyperTerm
            parseCommandASCII(r_buffer, &user_mode, &angle, &distance, ',');
            //angle = angle << 1;
            printf("Command: mode = %i, angle = %i, dist = %i\r\n", user_mode, angle, distance); 
        #else
            user_mode = r_buffer[0];              //store new user input
            angle = ((int)r_buffer[1])<<1;
            distance = r_buffer[2];
        #endif
        enable_receive_interrupt();     //reset receive interrupt
        resetServors();                 //reset the servoState
        
        switch (user_mode)
        {   
            //we go into controlled mode
            case UsrControl:
                #ifdef HyperTerm
	                transmit_strf("User Control\r\n");
                #endif
                autoState = Controlled; //make sure autonomous mode is off
                rotateMoveCar(angle, distance);   //move car
                break;
            
            //we set the car to search for sound!
            case UsrListen:
                #ifdef HyperTerm
	                transmit_strf("UserListen\r\n");
                #endif
                autoState = Stabilize;          //turn on autonomous state machine
                autoTurnCount = 0;      
                break;
            
            //we store to rom the new calibrations, and set the,
            case UsrEEPROMUpdate:
                #ifdef HyperTerm
	                transmit_strf("User EEProm Update\r\n");
                #endif
                romScaledRotate = angle;
                scaledRotate = angle;
                romScaledMove = distance;
                scaledMove = distance;
                break;
            
            //if we don't have one of these cases ERROR!!!
            default:
                #ifdef HyperTerm
                    transmit_strf("updateSystem()\r\n");
                    delay_ms(50);
                #endif
                error();
        }     
    }  
    
    //if in autonomous mode, run state machine
    if (autoState != Controlled) 
        autoControl();     
}  

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

//**********************************************************
//run function to rotate car   
void rotateMoveCar(signed int angle, signed int distance)
{   
	//if soemthing it telling the car to rotate 
	//while it's not idle that's BAAAADDDD
	if(servoState != Idle)
	{
	    #ifdef HyperTerm
	        transmit_strf("rotateMoveCar()\r\n");
	    #endif
		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(signed int distance)
{                   
    //calculate time needed to move
    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()
{   
	PORTB = 0;    		//moo
	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(0 == moveForeward)
	        	{   
	        		servo1 = 0;
	        		servo2 = 0;
        			servo3 = 0;
	        	}
	        	else if(1 == moveForeward)
	        	{   
	        		servo1 = 0;
	        	 	servo2 = 6;
        			servo3 = 2;
	        	}
	        	else if(2 == moveForeward)
	        	{     
	        		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 < 270)
	            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<300))
	            moveTime--;     
	            
	        else
	        {
	            servo1 = 0;
	            servo2 = 0;
	            servo3 = 0;
	            servoState = Idle;
	        }
	        break;
	        
	    default:
            #ifdef HyperTerm
                transmit_strf("updateServos()\r\n");
            #endif
	        error();
	}
}

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

// *********************************************************
// AUTONOMOUS MODE 
// *********************************************************
   
//********************************************************** 

void autoControl()
{         
    unsigned int index = 0;
    unsigned char tLeft, tRight, minDiff=1200, reading;
    signed int distance, angle = 0; 
    
    if(servoState != Idle)
        return;
    
    switch (autoState) 
    {
    	case Stabilize:
    	    #ifdef HyperTerm
    	        transmit_strf("Stabilize\r\n");
    	    #endif                     
    	    delay_ms(2000);
    	    //wait for silence on all 3 mics
    	    reading = PINA & 0x38;
    		if (0 == reading)
    			autoState = Listen;
    		micIndex1 = array_size;
    		micIndex2 = array_size;
    		micIndex3 = array_size;
    		break;                 
    		
    	case Listen:
    	    #ifdef HyperTerm
    	        printf("Listen %i\r\n",sampleNum);
    	    #endif
    		//alert the user that the robot has entered the listening phase
    		PORTD.7 = TRUE;                  
    		//wait for 1st impulse
		    do {
		         ADCbuffer[0] = PINA & 0x38;
	    	} while (!ADCbuffer[0]);	
    	        	
		    //check to make sure only one mic has heard it at this point
			    do {	// may take a while (900 readings) ****
      			    ADCbuffer[index] = PINA;
      	    		index++;     
      		    	delay_us(60);                
   			    } while (index < array_size);   
               autoState = Locate;
                            
    		//find the first index when each mic heard the pulse
            for (index = 0; index < array_size; index++)
		    {
			    if ((ADCbuffer[index] & 0x08) && (index < micIndex1))
				    micIndex1 = index;                          
			    if ((ADCbuffer[index] & 0x10) && (index < micIndex2))
				    micIndex2 = index;
			    if ((ADCbuffer[index] & 0x20) && (index < micIndex3))
				    micIndex3 = index;
		    }
            
		    #ifdef HyperTerm
                printf("mic1 = %i, mic2 = %i, mic3 = %i\r\n",micIndex1, micIndex2, micIndex3);
		    #endif
		        		    
                autoState = Locate;
            
             PORTD.7 = FALSE;                                               
       	    //alert the user that the robot has exited the listening phase
    	    break;
    	case Locate:
    	
    	    #ifdef HyperTerm
    	        transmit_strf("Locate\r\n");
    	        delay_ms(20); 
		    #endif 
			
			if((micIndex1 < micIndex2) & (micIndex1 < micIndex3))
			    angle = 170;
			    
			if((micIndex2 < micIndex1) & (micIndex2 < micIndex3))
			    angle = 60;
			
			if((micIndex3 < micIndex1) & (micIndex3 < micIndex2))
			    angle = 265;

			distance = 20;
			autoState = Move;  
			//also read the distance/angle records to attempt to determine if the source was found and set the state
			//if(___)
			//	autoState = Done;
            recordMove(angle, distance);	//record the move, for the next cycle and for determining when the system is done  
          
          sampleNum = 0;
          sampleAvgMic1 = 0;
          sampleAvgMic2 = 0;
          sampleAvgMic3 = 0;      
          
          // End of Angie's ***Stickers***
    		break;
    	case Move:
    	    #ifdef HyperTerm
    	        transmit_strf("Move\r\n");
    	    #endif
    		rotateMoveCar(angle_record[buffer_length-1], distance_record[buffer_length-1]);
    		autoState = Stabilize;
    		break;
    	case Done:
    	    done();
    	    break;
    		    
        //Unaccounted for case  
        default:
            #ifdef HyperTerm
                transmit_strf("autoControl()\r\n");
            #endif
            error();
    }
}

void recordMove(signed int ang, signed int dist) {
    unsigned int index;
    #ifdef HyperTerm
        transmit_strf("Move: Angle = ");
        delay_ms(ms_delay);
        sprintf(t_buffer, "%i", ang);
        enable_transmit_interrupt();
        transmit_strf(", Distance = ");
        delay_ms(ms_delay);
        sprintf(t_buffer, "%i", dist);
        enable_transmit_interrupt();
        transmit_strf("\r\n");
    #endif    
    for (index = 0; index < buffer_length - 1; index++) {
        distance_record[index] = distance_record[index+1];
        angle_record[index] = angle_record[index+1];
    }
    distance_record[index] = dist;
    angle_record[index] = ang;
}

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

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

//********************************************************** 
//set up transmit interrupts   
void enable_transmit_interrupt() 
{
    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()
{     
    //set up ports 
    DDRB 		 = 0xff;            	//PORTB for car wheels 
    PORTB 		 = 0;                          
    DDRA 		 = 0x00;            	//PINA for speaker data
    PORTA 		 = 0;
    DDRC 		 = 0b00000011;      	//PORTC for camera input
    DDRD.7 	 	 = 1;             		//PORTD for LED 
    PORTD.7 	 = 1;
        
    //serial setup for debugging using printf, etc.     
    UCSRB 		 = 0x18;                 //enable receive/transmit
    UBRRL 		 = 103;                  //set baud rate to 9600
    t_ready		 = TRUE;
    t_index		 = 0;
    r_ready		 = TRUE;
    r_index		 = 0;
    
    //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   = Stabilize;     
    //autoState = Controlled;             //not in autonomous mode  
    
    sampleNum   = 0;
    sampleAvgMic1   = 0;
    sampleAvgMic2   = 0;
    sampleAvgMic3   = 0;
          
    //DEBUG     
    #ifdef HyperTerm
        autoState = Controlled;
        printf("Starting Control Mode <mode>,<angle>,<distance>,\r\n");
    #endif
    #if DEBUG
        autoTurnCount = 0;
        autoState = Stabilize; 
    #endif
    	
    
    //enable the interrupt service routine
    #asm("sei");
    
    //enable receive interrupts
    enable_receive_interrupt(); 
}  

//********************************************************** 
//set up transmit interrupts 
void parseCommandASCII(unsigned char command_buffer[], unsigned char* usr_mode,
          signed int* usr_angle, signed int* usr_dist, unsigned char deliminator)
{
    unsigned char index, arg_index = 1, cmd_index[3];
    cmd_index[0] = 0;
    for (index = 0; command_buffer[index] != '\0' && index < buffer_length && arg_index < 3; index++)
    {
        if (command_buffer[index] == deliminator)
        {
            command_buffer[index] = '\0';
            cmd_index[arg_index] = index + 1;
            arg_index++;
        }
        
    }
    *usr_mode = (unsigned char) atoi(&command_buffer[cmd_index[0]]);
    *usr_angle = atoi(&command_buffer[cmd_index[1]]);
    *usr_dist = atoi(&command_buffer[cmd_index[2]]);
}

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

//Entry point and task scheduler loop
void main()
{                
    initialize();
    //main task scheduler loop -- never exits!    
    //Suuuppperrr Looooooop!
    while(TRUE)
    {    
        
        //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();
        	

    }   
}