//Zijun Wei
//Rover control

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

#define output(directions,pin) (directions |= pin) // set port direction for outputs
#define input(directions,pin) (directions &= (~pin)) // set port direction for inputs
#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


//for communication
#define bit_delay_time 8.6 // bit delay for 115200 with overhead, 8.5
#define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay
#define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay
#define char_delay() _delay_ms(10) // char delay

// #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 serial_port PORTD
#define serial_direction DDRD
#define serial_pins PIND
#define serial_pin_in (1 << PD0)
#define serial_pin_out (1 << PD1)

#define max_buffer 50

#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

//listening and talking

void get_char(volatile unsigned char *pins, unsigned char pin, char *rxbyte) {
   //
   // read character into rxbyte on pins pin
   //    assumes line driver (inverts bits)
   //
   *rxbyte = 0;
   while (pin_test(*pins,pin))
      //
      // wait for start bit
      //
      ;
   //
   // delay to middle of first data bit
   //
   half_bit_delay();
   bit_delay();
   //
   // unrolled loop to read data bits
   //
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 0);
   else
      *rxbyte |= (0 << 0);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 1);
   else
      *rxbyte |= (0 << 1);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 2);
   else
      *rxbyte |= (0 << 2);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 3);
   else
      *rxbyte |= (0 << 3);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 4);
   else
      *rxbyte |= (0 << 4);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 5);
   else
      *rxbyte |= (0 << 5);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 6);
   else
      *rxbyte |= (0 << 6);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 7);
   else
      *rxbyte |= (0 << 7);
   //
   // wait for stop bit
   //
   bit_delay();
   half_bit_delay();
   }

void put_char(volatile unsigned char *port, unsigned char pin, char txchar) {
   //
   // send character in txchar on port pin
   //    assumes line driver (inverts bits)
   //
   // start bit
   //
   clear(*port,pin);
   bit_delay();
   //
   // unrolled loop to write data bits
   //
   if bit_test(txchar,0)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,1)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,2)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,3)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,4)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,5)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,6)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,7)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   //
   // stop bit
   //
   set(*port,pin);
   bit_delay();
   //
   // char delay
   //
   bit_delay();
   }

void get_string(volatile unsigned char *pins, unsigned char pin, char *str)
{
   static int index;
   index = 0;
   // static char buffer[max_buffer];
   static char chr;
   while (index<max_buffer-1)
      {
         get_char(pins, pin, &chr);
         if (chr != 13)   //the acsii for enter/carriage return is 13 in decimal
            {
               str[index++] = chr;   //index increase after the chr in stored into str
            }
         else
         {
            index = 0;
            break;
         }
      }
}

void put_string(volatile unsigned char *port, unsigned char pin, char *str) {
   //
   // print a null-terminated string
   //
   static int index;
   index = 0;
   do {
      put_char(port, pin, str[index]);
      ++index;
      } while (str[index] != 0);
   }




//motion

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) {

   static char chr;


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

   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

  
   set(serial_port, serial_pin_out);
   input(serial_direction, serial_pin_out);

   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) {
      // StateChange(4, 0); //reconfirm we start at static state

      get_char(&serial_pins, serial_pin_in, &chr);
      char_delay();
      if (chr == 70)
      {
      	set(PORTD, (1<<PD3));  
      	_delay_ms(2000);
      	clear(PORTD, (1<<PD3));
         // StateChange(0, 1);
         // _delay_us(10000000);
         // StateChange(4, 0);
         // _delay_us(10000000);
      }
      // _delay_us(1000000);
      // StateChange(0, 1);
      // _delay_us(1000000);
      // StateChange(1, 1);
      // _delay_us(1000000);
      // StateChange(2, 0);
      // _delay_us(1000000);
      // StateChange(3, 0);

   }
}