#include <Mega32.h>
#include <delay.h>
#include <math.h>
#include <stdio.h> 

#define gyro PINA.2
#define IR  PINB.0
#define Lprop PORTD.4
#define Rprop PORTD.5
#define numGrdgs 10   //number of gyroscope readings to be averaged      
#define defaultThrust 128
#define begin {
#define end }
 


unsigned char Ldrift, Rdrift, yawDetect, aamt;
unsigned int i, gyroAccum;     
unsigned int Ain, adcl, adch;
unsigned int timeBase500ms;
unsigned char obs, state, tt;
unsigned char testInc;

void initialize(void);
void readGyro (void);
//void calcYaw(void)
void nudgeL(void);
void nudgeR(void);
void detectObs(void);
void blimpSM(void);


interrupt [TIM2_COMP] void timer2_compare(void)  //occurs every 1ms
begin
         if (timeBase500ms>0) --timeBase500ms;

end
  


void main(void)
{
	initialize();
        while(1)
        {
        	//state machine stuff
          
        	if (timeBase500ms==0){
        	
	            blimpSM();
	        }
        }
}


void initialize(void)
{
        //IO Ports
	DDRB=0b00001000; //PORTB.3 for PWM output
	//B.1 fwd motorL ctrl, B.2 backward motorL ctrl, B.4 fwd motorR, B.5 backward motorR
	
	DDRD=0b00110000; //PORTD.4 and 5 for PWM output (OC1B and OC1A)
	// IR sensor input into PORTDB.0


	//init the A to D converter
   	//channel zero/ left adj /EXTERNAL Aref
  	//!!!CONNECT Aref jumper!!!!
   	ADMUX = 0b00000000; //reads A0
   	//enable ADC and set prescaler to 1/128*16MHz=125,000
   	//and clear interupt enable
   	//and start a conversion
   	ADCSR = 0b11000111; //get the first conversion done with coz longer 
   	//channel A0 for gyro
   	//channel A1 for accelerometer?
 
 tt=12;


	//Timer stuff
	TIMSK = 0b10000000;    //turn on timer 2 cmp match isr


	//Timer0 - used for PWM
	//fast PWM mode, full clock rate, toggle oc0 (pin B3)
   	//16 microsec per PWM cycle implies max freq for 16 samples of
	// 1/(16e-6*16) = 3900 Hz.
   	TCCR0 = 0b01101101;
	OCR0 = 240; //50% duty cycle


	//Timer1 - used for PWM
	TCCR1A = 0b11110001; //clear on COMP, fast PWM
	TCCR1B = 0b00001101; //fast PWM, no prescaler
	OCR1AL = defaultThrust; //50% duty cycle, controls left motor
	OCR1BL = defaultThrust; //50% duty cycle, controls right motor


	//Timer2 for timing
	OCR2 = 249;    //set the compare re to 250 time ticks
 	TCCR2 = 0b00001100; // prescaler to 64 and turn on clear on match...1ms clk

    //DDRC=0xff; //set to output, test using LEDs
    //PORTC=0x00;
    DDRC = 0xff; //set to input, buttons

   /*setFwdMotorL();
   setFwdMotorR();*/


	//crank up ISRs
	#asm
   		sei
	#endasm
}

void readGyro(void)        
{              
        gyroAccum = 0;  
        
         
        for(i=0;i<numGrdgs;i++)
        {
                if (ADCSR.6==0) ADCSR.6=1;
		            while(ADCSR.6==1) {};
                adcl = ADCL;
                adch = ADCH;
                Ain = adcl + (adch<<8);
                gyroAccum = gyroAccum+Ain;
                
        
        }  
         if ((gyroAccum>=3200) && (gyroAccum<=3220)) yawDetect=0;
         else if (gyroAccum<3200) yawDetect=1; //left yaw  
         else if (gyroAccum>3220) yawDetect=2; //right yaw  
              
}



void nudgeL(void)
{
        if (OCR1AL-1!=0) OCR1AL -=1;
        if (OCR1BL+1!=255) OCR1BL +=1;
}

void nudgeR(void)
{                   
        if (OCR1AL+1!=255) OCR1AL +=1;
        if (OCR1BL-1!=0) OCR1BL -=1;
}

void detectObs(void) //IR sensor to PORTB.0
{
        if (PINB.0==0) obs=1;
        else obs=0;
}        


void forward (void)
{
          OCR1AL=defaultThrust;
          OCR1BL=defaultThrust;
}                                       


      
void turnLeft(unsigned char amt)
{

        for(i=0;i<amt;i++) nudgeL();

}                       

void turnRight(unsigned char amt)
{

        for(i=0;i<amt;i++) nudgeR();

}

void stop(void){
    OCR1AL=0;
    OCR1BL=0;   
}

void blimpSM(void){
         
         timeBase500ms=500;  //reset the task timer  
         PORTC.7=PORTC.7^1;
         detectObs();                               
         
    switch(state){
        case 0: 
            
            if(--tt==0){
              state=1; 
            }            
            break;
        
        case 1:        //moving forward  
            //PORTC=0x02;
            if(obs==0){
                readGyro();
                if(yawDetect==1) turnRight(3);
                else if(yawDetect==2) turnLeft(3);
            }
            if(obs==1){
                state=2;
                tt=6;          //continue moving for 3 second before stopping
            }
            break;

        case 2:        //stop
            //PORTC=0x04;
                //500ms
               //delay for a while
            if(--tt==0){
                stop();    //props stop spinning
                state=3;
            }
            break;
            
        case 3:        //obstacle detected
            //PORTC=0x08;
            if(obs==1) {
                turnRight(5);
                //turnRight(10);
            }
            else if(obs==0) {
                  forward();
                  state=1;
            }             
            break;
   }   //end switch



}