//
//
// hello.ftdi.44.echo.c
//
// 115200 baud FTDI character echo, with flash string
//
// set lfuse to 0x7E for 20 MHz xtal
//
// Neil Gershenfeld
// 12/8/10
//
// (c) Massachusetts Institute of Technology 2010
// Permission granted for experimental and personal use;
// license for commercial sale available from MIT.
//

#include <avr/io.h>
#include <util/delay.h>
#include <avr/pgmspace.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 bit_delay_time 8.5 // bit delay for 115200 with overhead
#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(10) // 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 50000 // number of PWM cycles
#define cycle_count 5 // number of speed cycles

#define bridge_port PORTA // H-bridge port
#define bridge_direction DDRA // H-bridge direction
#define IN1 (1 << PA2) // IN1
#define IN2 (1 << PA3) // IN2
#define IN3 (1 << PA4) // IN1
#define IN4 (1 << PA5) // IN2

#define serial_port PORTA
#define serial_direction DDRA
#define serial_pins PINA
#define serial_pin_in (1 << PA0)
#define serial_pin_out (1 << PA1)

#define max_buffer 25

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 put_flash_string(volatile unsigned char *port, unsigned char pin, PGM_P str) {
   //
   // print a null-terminated string from flash
   //
   static char chr;
   static int index;
   index = 0;
   do {
      chr = pgm_read_byte(&(str[index]));
      put_char(port, pin, chr);
      ++index;
      } while (chr != 0);
   }

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

int main(void) {
   //
   // main
   //
   static char chr;
   static char buffer[max_buffer] = {0};
   static int index;
   static uint16_t count;
   static uint8_t cycle;
   static uint8_t sense0;
   static uint8_t sense1;
   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize output pins
   //
   set(serial_port, serial_pin_out);
   output(serial_direction, serial_pin_out);
   //
   // initialize H-bridge pins
   //
   clear(bridge_port, IN1);
   output(bridge_direction, IN1);
   clear(bridge_port, IN2);
   output(bridge_direction, IN2);
   clear(bridge_port, IN3);
   output(bridge_direction, IN3);
   clear(bridge_port, IN4);
   output(bridge_direction, IN4);
   //
   // main loop
   //
   index = 0;
   while (1) {
     /* // */
     /* // init A/D */
     /* // */
     /* ADCSRB =  (1 << ADLAR); // left adjust */
     /* ADMUX = (0 << MUX3) | (1 << MUX2) | (1 << MUX1) | (0 << MUX0); // ADC6 = PA6 */
     /* ADCSRA = (1 << ADEN) // enable */
     /*   | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128 */
     /*  // */
     /*  // initiate conversion */
     /*  // */
     /*  ADCSRA |= (1 << ADSC); */
     /*  // */
     /*  // wait for completion */
     /*  // */
     /*  while (ADCSRA & (1 << ADSC)) */
     /*     ; */
     /*  // */
     /*  // record result */
     /*  // */
     /*  sense0 = ADCH; */
     /* // */
     /* // init A/D */
     /* // */
     /* ADCSRB =  (1 << ADLAR); // left adjust */
     /* ADMUX = (0 << MUX3) | (1 << MUX2) | (1 << MUX1) | (1 << MUX0); // ADC7 = PA7 */
     /* ADCSRA = (1 << ADEN) // enable */
     /*   | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128 */
     /*  // */
     /*  // initiate conversion */
     /*  // */
     /*  ADCSRA |= (1 << ADSC); */
     /*  // */
     /*  // wait for completion */
     /*  // */
     /*  while (ADCSRA & (1 << ADSC)) */
     /*     ; */
     /*  // */
     /*  // record result */
     /*  // */
     /*  sense1 = ADCH; */
      
      get_char(&serial_pins, serial_pin_in, &chr);
      int temp = chr-'0';
      static char message[] PROGMEM = "Sense0, Sense1 = ";
      put_flash_string(&serial_port, serial_pin_out, (PGM_P) message);
      //buffer[index++] = chr;
      //if (index == (max_buffer-1))
      //   index = 0;
      //buffer =snprintf(buffer, max_buffer, "%d", sense0);//
      //buffer = (char[25])(((uint16_t)'0')+sense0);
      int i;
      for (i = 0; i<max_buffer; i++) {
      	buffer[i] = (char)(((uint8_t)'0')+sense0);}
      put_ram_string(&serial_port, serial_pin_out, buffer);
      put_char(&serial_port, serial_pin_out, ',');
      //buffer =snprintf(buffer, max_buffer, "%d", sense1);//
      //buffer = (char[25])(((uint16_t)'0')+sense1);
      put_ram_string(&serial_port, serial_pin_out, buffer);
      if (temp == 0) {
      	//stop motor 1
      	clear(bridge_port, IN1);
      	clear(bridge_port, IN2);}
      if (temp == 1) {
      	//motor 1 forward
      	clear(bridge_port, IN2);
      	set(bridge_port, IN1);}
      if (temp == 2) {
      	//motor 1 backwards
      	clear(bridge_port, IN1);
      	set(bridge_port, IN2);}
      if (temp == 3) {
      	//stop motor 2
      	clear(bridge_port, IN3);
      	clear(bridge_port, IN4);}
      if (temp == 4) {
      	//motor 2 forward
      	clear(bridge_port, IN3);
      	set(bridge_port, IN4);}
      if (temp == 5) {
      	//motor 2 backwards
      	clear(bridge_port, IN4);
      	set(bridge_port, IN3);}
      //put_char(&serial_port_out, serial_pin_out, '!');
      put_char(&serial_port, serial_pin_out, 10); // new line
      get_char(&serial_pins, serial_pin_in, &chr);
      static char message[] PROGMEM = "hello.ftdi.44.echo.c: you typed \"";
      put_flash_string(&serial_port, serial_pin_out, (PGM_P) message);
      buffer[index++] = chr;
      if (index == (max_buffer-1))
         index = 0;
      put_ram_string(&serial_port, serial_pin_out, buffer);
      put_char(&serial_port, serial_pin_out, '\"');
      put_char(&serial_port, serial_pin_out, 10); // new line
      }
   }