//                S M O K E I N G  P R O G R E S S 

/************************************************************************

A  B   X   Y
-------------
0  0   1   1
0  1   0   1
1  0   1   0
1  1   1   1

 B0 power out to power board
 D2 power off button
 
 B3 pwm1
 D7 pwm2
 
 D3 diag led
 D4 diag led
 D5 diag led
 D6 diag led
 
 C0 sensor 1
 C1 sensor 2
 C2 sensor 3
 C3 sensor 6
 C4 sensor 5
 C5 sensor 4
 
 3 sensors per side:
 
<-- outside  inside -->
 
 0   0   1   turn lightly (green)
 0   1   1   turn harder  (blue)
 1   1   1   turn harder  (yellow)
 1   1   0   turn hard    (orange)
 1   0   0   crank it     (red)
 
*************************************************************************/

#include <avr/io.h>

// misc

#define OUTPUT             1
#define INPUT              0
#define SETBIT(BIT, PORT)  PORT |= (1<<BIT)
#define CLRBIT(BIT, PORT)  PORT &= ~(1<<BIT)
#define ISSET(BIT, PORT)  (PORT & (1<<BIT)) != 0

// diag leds

#define LED_A_ON  PORTD |= (1<<5);
#define LED_A_OFF PORTD &= ~(1<<5);

#define LED_B_ON  PORTD |= (1<<6);
#define LED_B_OFF PORTD &= ~(1<<6);

#define LED_C_ON  PORTD |= (1<<3);
#define LED_C_OFF PORTD &= ~(1<<3);

#define LED_D_ON  PORTD |= (1<<4);
#define LED_D_OFF PORTD &= ~(1<<4);


// sensors

#define SENSOR1 (PINC & (1<<0)) != 0
#define SENSOR2 (PINC & (1<<1)) != 0
#define SENSOR3 (PINC & (1<<2)) != 0
#define SENSOR4 (PINC & (1<<5)) != 0
#define SENSOR5 (PINC & (1<<4)) != 0
#define SENSOR6 (PINC & (1<<3)) != 0

// pwm ranges
// max conf based on 3V/12V * 255 = 64

#define minconf 64
//#define maxconf 150 
#define maxconf 128

// turn speeds this is the speed of te slow track
/*
#define GREEN    maxconf*.70
#define BLUE     maxconf*.5
#define YELLOW   maxconf*.1
#define ORANGE   maxconf*.08
#define RED      0
*/

#define GREEN    confidence/2
#define BLUE     confidence/4
#define YELLOW   confidence/8
#define ORANGE   confidence/16
#define RED      0

// these are also used for tracking the blind spots

#define UNKNOWN 0
#define RIGHT   1
#define LEFT    2
#define CENTER  3

// Menu modes
#define OVERVIEW       0
#define LEFTDETAIL     1
#define RIGHTDETAIL    2


#define true  1
#define false 0

#define OR  ||
#define AND &&

//functions
//return  name    paramiters
void    poweroff  ();
void    poweron   ();
void    Delay     (int delay);
uint8_t Detect    (uint8_t side) ;
void    motorstop () ;
void    goforward () ;
void    goright   ( uint8_t speed ) ;
void    goleft    ( uint8_t speed ) ;
void    SetPWM    ( uint8_t channel, uint8_t value) ;
void    pwm_init  () ;
void    dobutton  () ;
void    beep      ( int freq, int time);
//void    downconf  ( int setback);
#define  downconf(A) if((confidence>minconf)&(confcount>200)){confidence-=A;confcount=0;}else confcount++

// global variables
uint8_t confidence;
uint8_t stop;   
int     temp;
int     blindspot;
int     displayMode;
int     loopcount;
int     confcount;

int main (void) {

    uint8_t sens1, sens2, sens3, sens4, sens5, sens6;
    uint8_t thres1, thres2, thres3, thres4, thres5, thres6;
          
    // set up directions 
    DDRA = (INPUT << PA0 | INPUT << PA1 |INPUT << PA2 |INPUT << PA3 |INPUT << PA4 |INPUT << PA5 |INPUT << PA6 |INPUT << PA7);
    DDRB = (OUTPUT << PB0 | INPUT << PB1 |INPUT << PB2 |OUTPUT << PB3 |OUTPUT << PB4 |INPUT << PB5 |INPUT << PB6 |INPUT << PB7);
    DDRC = (INPUT << PC0 | INPUT << PC1 |INPUT << PC2 |INPUT << PC3 |INPUT << PC4 |INPUT << PC5 |INPUT << PC6 |INPUT << PC7);
    DDRD = (INPUT << PD0 | INPUT << PD1 |INPUT << PD2 |OUTPUT << PD3 |OUTPUT << PD4 |OUTPUT << PD5 |OUTPUT << PD6 |OUTPUT << PD7);        
    
    // latch power on
    poweron();
    
    // preset confidence
    confidence = minconf;   
    blindspot  = UNKNOWN;    // this tells us what blindspot the line is in...
    
    displayMode = OVERVIEW;
    
    // activate pwm
    pwm_init();
     
    stop = true; 
    LED_B_ON
     
    // take life into own hands...
    goforward();              
       
    for(temp = 450; temp > 350; temp-=30) {
      beep(temp, 75);
    }    
    
     //------------| Main loop |---------------
       
     while(1) {         

         // check control panel
         if (!ISSET(2, PIND)) {  //if button is down pass on to find out if we go off or toggle speed
           dobutton();           
         }
	               
	 // check battery
	 // batt = adc(0);
	 
	 // check speed preset
	 // confidence += adc(1);
	 
         // check sensors
         sens1 =  !SENSOR1;
         sens2 =  !SENSOR2;
         sens3 =  !SENSOR3;
         sens4 =  !SENSOR4;
         sens5 =  !SENSOR5;
         sens6 =  !SENSOR6;
         
	 // set sensor status leds
         if (0) {
         } else if (displayMode == OVERVIEW ) {	 
            // General status leds
            if ( sens1 OR sens2 )  { LED_C_ON ; }else{ LED_C_OFF; }
            if ( sens5 OR sens6 )  { LED_D_ON ; }else{ LED_D_OFF; }
            if ( sens3 OR sens4 )  { LED_A_ON ; }else{ LED_A_OFF; }
         } else if (displayMode == RIGHTDETAIL ) {
            // Debug left side
            if ( sens1 )  { LED_C_ON ; }else{ LED_C_OFF; }
            if ( sens2 )  { LED_A_ON ; }else{ LED_A_OFF; }
            if ( sens3 )  { LED_D_ON ; }else{ LED_D_OFF; }
         } else if (displayMode == LEFTDETAIL ) {
            // Debug right side
            if ( sens4 )  { LED_C_ON ; }else{ LED_C_OFF; }
            if ( sens5 )  { LED_A_ON ; }else{ LED_A_OFF; }
            if ( sens6 )  { LED_D_ON ; }else{ LED_D_OFF; }
         }	 
	 
	 // do the hard work
         // [1] [2] [3] [4] [5] [6]
	 if (stop) { /* do nothing */ }
	 
	 //static 
	 // 1 on one side, anything on other side
	 // # o o ? ? ?
	 else if (sens1 AND (sens4 OR sens5 OR sens6)) { downconf(1);}
	 // o # o ? ? ?
	 else if (sens2 AND (sens4 OR sens5 OR sens6)) { downconf(1);}
	 // ? ? ? o o #
	 else if (sens6 AND (sens1 OR sens2 OR sens3)) {  downconf(1);}
	 // ? ? ? o # o
	 else if (sens5 AND (sens1 OR sens2 OR sens3)) { downconf(1); }
	 
	 //more than 3 in a row
	 // # # # # o o
	 else if (sens1 AND sens2 AND sens3 AND sens4) {  downconf(1);}
	 // o # # # # o
	 else if (sens2 AND sens3 AND sens4 AND sens5) {  downconf(1);}
	 // o o # # # #
     else if (sens3 AND sens4 AND sens5 AND sens6) { downconf(1); }

	 
	 // o o # # o o
	 else if (sens3 AND sens4) { goforward(); blindspot = CENTER; }       
	 
	 // # # # o o o
	 else if (sens1 AND sens2 AND sens3) { goleft(YELLOW);   blindspot = RIGHT; }
	 // o # # o o o
	 else if (sens2 AND sens3)           { goleft(BLUE);   blindspot = RIGHT; }
	 // # # o o o o 
	 else if (sens2 AND sens1)           { goleft(ORANGE); blindspot = RIGHT;}
	 // # o o o o o
     else if (sens1)                     { goleft(RED);     blindspot = RIGHT;  downconf(1); } 
	 // o # o o o o
     else if (sens2)                     { goleft(YELLOW);  blindspot = RIGHT; }	 
     // o o # o o o 
     else if (sens3)                     { goleft(GREEN);   blindspot = CENTER;} 
	 
	 // o o o # # #
	 else if (sens4 AND sens5 AND sens6 ) { goright(YELLOW); blindspot = LEFT;  }
	 // o o o # # o
	 else if (sens4 AND sens5)            { goright(BLUE); blindspot = LEFT; }
	 // o o o o # #
	 else if (sens5 AND sens6)            { goright(ORANGE);blindspot = LEFT; }
     // o o o o o #
     else if (sens6)                      { goright(RED);    blindspot = LEFT;  downconf(1); }
	 // o o o o # o
     else if (sens5)                      { goright(YELLOW); blindspot = LEFT; }
	 // o o o # o o 
     else if (sens4)                      { goright(GREEN); blindspot = CENTER;}        
     
     else if (blindspot = CENTER)         { goforward();  }
    
     loopcount++;
     if (loopcount > 1500) {
       loopcount = 0;
       if (confidence < maxconf) {
          confidence++;
       }
     }
                                                 
    } // end of while(on)
    
    poweroff();
  
} // end of main

//=============================| Functions |==========================


void dobutton() {
  // a short pulse means stop, a long one means off
  Delay(65535); // wait for a bit
  Delay(65535);
  Delay(65535);
  Delay(65535);
//  Delay(65535);
//  Delay(65535);
  if (ISSET(2, PIND)) {  // still down = long pulse
    if (stop) {
      beep(300, 100);
      stop = false;   
      motorstop();  
      LED_B_OFF
      confidence = minconf;
    } else {
      beep(500, 100);
      stop = true;
      motorstop();
      LED_B_ON
    }
    while(!ISSET(2, PIND)); // wait for button release 
    Delay(5000); //debounce       
  } else {       // short pulse, power off..    
    poweroff(); // will never return
  }
  
}

void poweron() {
  SETBIT( 0, PORTB );
}

void poweroff() {
  int temp;
  
  for(temp = 350; temp < 450; temp+=30) {
      beep(temp, 75);
    }   
  CLRBIT(0, PORTB);
  while(1);
}

void Delay(int delay) {
  int x;
  for (x = delay; x != 0; x--) {
    asm volatile ("nop"::); 
  }
}

void motorstop() {
  SetPWM(LEFT,  0);
  SetPWM(RIGHT, 0);
}

void goforward() {
  SetPWM(LEFT | RIGHT, confidence);
}

void goright ( uint8_t speed ) {
  SetPWM(LEFT, speed);
  SetPWM(RIGHT, confidence);
}

void goleft ( uint8_t speed ) {
  SetPWM(LEFT, confidence);
  SetPWM(RIGHT, speed);
}

void SetPWM( uint8_t channel, uint8_t value) {
  if (!stop) {
   if (channel & RIGHT) { OCR0 = value; }
   if (channel & LEFT)  { OCR2 = value; }
  } else {
    OCR0 = 0;
    OCR2 = 0;
  }
}

void pwm_init() {
  // clear pwm levels
  OCR0 = 0; 
  OCR2 = 0;
  
  // set up WGM, clock, and mode for timer 0
  TCCR0 = 0 << FOC0  | 
          1 << WGM00 | /* fast pwm */
          1 << COM01 | /* inverted */
          1 << COM00 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM01 | /* fast pwm */
          0 << CS02  | /* no prescale */
          0 << CS01  |
          1 << CS00  ;
  
  // set up WGM, clock, and mode for timer 2
  TCCR2 = 0 << FOC2  | 
          1 << WGM20 |
          1 << COM21 |
          1 << COM20 | /*   this bit 1 for interted, 0 for normal  */
          1 << WGM21 |
          0 << CS22  |
          0 << CS21  |
          1 << CS20  ;
  
 }

void beep( int freq, int time) {
  int cdown;
  
  for (cdown = time; cdown != 0; cdown -- ) {
    SETBIT( 4, PORTB );
    Delay(freq);
    CLRBIT( 4, PORTB );
    Delay(freq);
  }
  
}