/*****************************/
// ECE 476 FINAL PROJECT
// The Autonomous Search Bot
// Diego Asturias (da65)
// John Sun (jzs3)
/*****************************/ 

/* the I2C bus is connected to PORTC */
/* the SDA signal is bit 1 */
/* the SCL signal is bit 0 */
#asm
    .equ __i2c_port=0x15
    .equ __sda_bit=1
    .equ __scl_bit=0
#endasm

//include files             
#include <mega32.h> 
#include <stdio.h>      
#include <delay.h>  //function declaration for delay_ms    
#include <i2c.h>    //now you can include the I2C Functions     

//timeout values for each task
flash unsigned char tUpdate = 50; 
flash unsigned char tPWM = 40; 

#define DEBUG 0

// *********************************************************
// FUNCTION DECLARATIONS 
// *********************************************************

//the  subroutines
void initialize(void); 	                //set up MCU 
void updateSystem(void);                //check system  
void gets_int(void);	                //set up receive interrupt   
void puts_int(void);                    //set up transmit interrupt                      
void rotateCar(int angle, int distance);//set up rotation - call first when moving
void moveCar(int distance);             //set up move
void updateServos(void);				//update the servo state machine 
void resetServors(void);
void autoControl(void);                 //set up autonomous mode  
unsigned char checkForMarble(void);     					//check camera for marble
unsigned char checkForBase(void);       					//check camera for base  
void initI2C(void);                     					//initilize the I2C line
void writeToCameraRegister(char registerNum, char data); 	//writes to the camera register                                  
void initCam(void);                     					//initilizes the camera variables  		   

//macro for absolute value   
inline int intAbs(int a)
{
    ((a<0)? (a=(-a)):(a=(a)));
    return a; 
}    


void error(void)
{
    while(1)
    {
        PORTD.7 = 1;
        delay_ms(100);
        PORTD.7 = 0;
        delay_ms(100);
    }
}

// *********************************************************
// VARIABLE DECLARATIONS 
// *********************************************************

//time variables
signed int timeUpdate, timePWM;	

//RXC ISR variables															
unsigned char r_index;                  	//current string index
signed char r_buffer[16];	            	//input string 
unsigned char r_ready;                  	//flag for receive done
signed char r_char;                     	//current character   

//TX empth ISR variables								
unsigned char t_index;                  	//current string index
unsigned char t_buffer[16];	            	//output string 
unsigned char t_ready;                  	//flag for transmit done
unsigned char t_char;                   	//current character  

//rotate & move car controls 
unsigned char servo1, servo2, servo3;   	//time to set PWM duty cycle
unsigned int rotateTime, moveTime, waitTime;//set up counters for each movement
int scaledRotate, scaledMove;           	//find how much to relate angle/distance to time
unsigned char moveForeward;
eeprom int romScaledRotate = 7;
eeprom int romScaledMove   = 35;   
      
//autonomous control variables
enum AutoState{Controlled = 0, Searching, Moving, Locating, Found} autoState;                
                                        //0 if not in autonomous mode
                                        //1 if turning and searching for marble/base
                                        //2 if moving
                                        //3 if located marble/base
                                        //4 if reached marble/base
                                        
unsigned char autoTurnCount;            //count to see how much car has rotated in search mode

enum AutoMode{foundMarble = -1, searchMarble, searchBase, foundBase} autoMode;
enum ServoState{Idle = 0, Waiting, Rotating, Foreward} servoState;

//camera variables
#define PCLK  (PINC.2)
#define VSYNC (PINC.3)
#define HREF  (PINC.4)