//Zijun Wei
//Rover control

#include <avr/io.h>
#include <util/delay.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set

#define on_delay() _delay_us(3) // PWM on time
#define fast_off_delay() _delay_us(1) // PWM fast off time
#define medium_off_delay() _delay_us(3) // PWM medium off time
#define slow_off_delay() _delay_us(5) // PWM slow off time
#define PWM_count 20000 // number of PWM cycles
#define cycle_count 5 // number of speed cycles

#define bridge_port_1 PORTD // H-bridge port
#define bridge_direction_1 DDRD // H-bridge direction
#define IN1_1 (1 << PD5) // IN1_1, OC0B
#define IN1_2 (1 << PD6) // IN1_2, OC0A

#define bridge_port_2 PORTB // H-bridge port
#define bridge_direction_2 DDRB // H-bridge direction
#define IN2_1 (1 << PB1) // IN1_1, OC1A
#define IN2_2 (1 << PB2) // IN1_2, OC1B

void StateChange(uint8_t direction, uint8_t speed)
{
     switch (direction)
     {
      case 0: {
         //Forward, B inverting mode, set to be always off, and change A
         switch (speed)
         {
            case 0: 
            {
               // slow
               OCR0A = 144;
               OCR0B = 255;
               OCR1A = 144;  
               OCR1B = 255; 
               break;              
            }
            case 1: 
            {
               // med
               OCR0A = 174;
               OCR0B = 255;
               OCR1A = 174;  
               OCR1B = 255;  
               break;              
            }
            case 2: 
            {
               // fast
               OCR0A = 224;
               OCR0B = 255;
               OCR1A = 224;  
               OCR1B = 255;  
               break;              
            }
         }
         break; 

      }
      case 1: {
         //Reverse, A non-inverting mode, set to be always off, and change B
         switch (speed)
         {
            case 0: 
            {
               // slow
               OCR0A = 0;
               OCR0B = 110;
               OCR1A = 0;  
               OCR1B = 110;  
               break;              
            }
            case 1: 
            {
               // med
               OCR0A = 0;
               OCR0B = 80;
               OCR1A = 0;  
               OCR1B = 80;  
               break;              
            }
            case 2: 
            {
               // fast
               OCR0A = 0;
               OCR0B = 30;
               OCR1A = 0;  
               OCR1B = 30;  
               break;              
            }
         }
         break; 
      }
      case 2: {
         //Right, Left(PD, OC0 8-bit) faster than Right(PB, OC1, 16-bit); if rotate, med reverse directions
         switch (speed)
         {
            case 0: 
            {
               // Rotate
               OCR0A = 174;
               OCR0B = 255;//forward med
               OCR1A = 0;  
               OCR1B = 80; //backward med to match the opposite
               break;              
            }
            case 1: 
            {
               // ForwardTurn, larger A, faster
               OCR0A = 174;
               OCR0B = 255;
               OCR1A = 144;  
               OCR1B = 255;  
               break;              
            }
            case 2: 
            {
               // BackwardTurn, smaller B, faster
               OCR0A = 0;
               OCR0B = 80;
               OCR1A = 0;  
               OCR1B = 110;  
               break;              
            }
         }
         break; 

      }
      case 3: {
         //Left, Right(PB, OC1, 16-bit) faster than Left(PD, OC0, 8-bit); if rotate, med reverse directions
         switch (speed)
         {
            case 0: 
            {
               // Rotate
               OCR0A = 0;
               OCR0B = 80; //backward med to match the opposite
               OCR1A = 174;  
               OCR1B = 255;//forward med  
               break;              
            }
            case 1: 
            {
               // ForwardTurn, larger A, faster
               OCR0A = 144;
               OCR0B = 255;
               OCR1A = 174;  
               OCR1B = 255;  
               break;              
            }
            case 2: 
            {
               // BackwardTurn, smaller B, faster
               OCR0A = 0;
               OCR0B = 110;
               OCR1A = 0;  
               OCR1B = 80;  
               break;              
            }
         }
         break; 
      }
      case 4: {
         switch(speed)   //need to define a static state too
         {
            case 0:
            {
               OCR0A = 0;
               OCR0B = 255;
               OCR1A = 0;
               OCR1B = 255;   //all counter output always 0
               break;
            }
         }
         break;
      }
     }
   }

int main(void) {

   uint8_t direction = 0;
   //see StateChange: 0: forward; 1: backward; 2: Right; 3: Left

   uint8_t speed = 0;
   //see StateChange: 
   //0: slow for forward/backward; rotate for left/right
   //1: med for forward/backward; forward turn for left/right
   //2: fast for forward/backward; backward turn for left/right

   StateChange(direction, speed);

   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize H-bridge pins
   //
   clear(bridge_port_1, IN1_1);
   output(bridge_direction_1, IN1_1);
   clear(bridge_port_1, IN1_2);
   output(bridge_direction_1, IN1_2);  //H-bridge 1 control by 8-bit PWM generator

   clear(bridge_port_2, IN2_1);
   output(bridge_direction_2, IN2_1);
   clear(bridge_port_2, IN2_2);
   output(bridge_direction_2, IN2_2);  //H-bridge 2 control by 16-bit PWM generator

  
   output(PORTD, (1<<PD3)); //set up LED indicator of PWM
   // clear(PORTD, (1<<PD3));
   set(PORTD, (1<<PD3));
   //
   // main loop
   //
      CLKPR = (1 << CLKPCE);
      CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
      
      TCCR0A = (2<<COM0A0) | (3<<COM0B0) | (3<<WGM00); 
         //setting OR0A, setting both bits 6 and 7 in TCCR0A, activate OC0A in non-inverting fast PWM; 
         // setting OR0B, setting both bits 4 and 5 in TCCR0A, activate OC0B in inverting fast PWM; polarity has to be opposite for the two pins for any given moment, 
         // so that INT1 and INT2 are opposite, so that there is current going into the bridge

         // setting WGM00 and WGM01, and clearing WGM02, using fast PWM with TOP defined as 0xFF; when TOP == 0xFF, PWM frequency is defined by SysF/Prescaler/255
      TCCR0B = (0<<WGM02) | (0<CS02) | (0<<CS01) | (1<<CS00);  //no prescaler for counter clock rate

      TCCR1A = (2<<COM1A0) | (3<<COM1B0) | (1<<WGM10); 
         //setting OR0A, setting both bits 6 and 7 in TCCR0A, activate OC0A in non-inverting fast PWM; 
         // setting OR0B, setting both bits 4 and 5 in TCCR0A, activate OC0B in inverting fast PWM; polarity has to be opposite for the two pins for any given moment, 
         // so that INT1 and INT2 are opposite, so that there is current going into the bridge

         // setting WGM00 and WGM01, and clearing WGM02, using fast PWM with TOP defined as 0xFF; when TOP == 0xFF, PWM frequency is defined by SysF/Prescaler/255
      TCCR1B = (1<<WGM12) | (0<CS12) | (0<<CS11) | (1<<CS10);  //no prescaler for counter clock rate



   while (1) {
      _delay_us(1000000);
      StateChange(0, 1);
      _delay_us(1000000);
      StateChange(1, 1);
      _delay_us(1000000);
      StateChange(2, 0);
      _delay_us(1000000);
      StateChange(3, 0);
      _delay_us(1000000);
      StateChange(4, 0);

   }
}