//
//
// bus.snap.c
//
// 115200 baud bus command processor
//
// set lfuse to 0x7E for 20 MHz xtal
//
// 2011-11-11 forked from neil's bus.io.command.c
// 2011-12-14 peek@mit.edu added stepper motor commands and functionality
//            packets are in the form node,nr of chrs to follow,<chrs>,time (t),
//            nr of time to follow, G (go)
// 2012-04-18 peek@mit.edu
//

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

#define output(directions,pin) (directions |= pin) // set port direction for output
#define input(directions,pin) (directions &= (~pin)) // set port direction for input 
#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 bus_port PORTA
#define bus_direction DDRA
#define bus_pins PINA
#define bus_pin_in (1 << PA7)
#define bus_pin_out (1 << PA6)

#define led_port PORTA
#define led_direction DDRA
#define led_pin (1 << PA0)

#define step_pin (1 << PA5) //io1, bottom right
#define step_port PORTA
#define step_direction DDRA 
#define dir_pin (1 << PA3) //io2, second from bottom right
#define dir_port PORTA
#define dir_direction DDRA

void get_bus_char(char *rxbyte) {
   //
   // read character into rxbyte from bus
   //    doesn't assumes line driver (doesn't invert bits)
   //
   *rxbyte = 0;
   while (0 == pin_test(bus_pins,bus_pin_in))
      //
      // wait for start bit (bus pin in is high)
      //
      ;
   //
   // delay to middle of first data bit
   //
   half_bit_delay();
   bit_delay();
   //
   // unrolled loop to read data bits
   //
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 0);
   else
      *rxbyte |= (1 << 0);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 1);
   else
      *rxbyte |= (1 << 1);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 2);
   else
      *rxbyte |= (1 << 2);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 3);
   else
      *rxbyte |= (1 << 3);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 4);
   else
      *rxbyte |= (1 << 4);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 5);
   else
      *rxbyte |= (1 << 5);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 6);
   else
      *rxbyte |= (1 << 6);
   bit_delay();
   if pin_test(bus_pins,bus_pin_in)
      *rxbyte |= (0 << 7);
   else
      *rxbyte |= (1 << 7);
   //
   // wait for stop bit
   //
   bit_delay();
   half_bit_delay();
   }

void put_bus_char(char txchar) {
   //
   // send character in txchar to bus
   //    doesn't assumes line driver (doesn't invert bits)
   //
   // start bit
   //
   set(bus_port,bus_pin_out);
   bit_delay();
   //
   // unrolled loop to write data bits
   //
   if bit_test(txchar,0)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,1)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,2)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,3)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,4)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,5)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,6)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   if bit_test(txchar,7)
      clear(bus_port,bus_pin_out);
   else
      set(bus_port,bus_pin_out);
   bit_delay();
   //
   // stop bit
   //
   clear(bus_port,bus_pin_out);
   bit_delay();
   //
   // char delay
   //
   bit_delay();
   }

int main(void) {
   //
   // main
   //
   static char chr,args,dir,i,step_lo,step_hi,time_lo,time_hi;
   const uint16_t step_on = 1; // 1ms pulse to step
   uint16_t steps, step_off, time, k, j;
   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize bus/indicator led
   //
   clear(bus_port, bus_pin_out);
   input(bus_direction, bus_pin_out);
   clear(led_port, led_pin);
   output(led_direction, led_pin);
   //
   // initialize direction pin
   //
   clear(dir_port, dir_pin);
   output(dir_direction, dir_pin);
   //
   // initialize step  pin
   //
   clear(step_port, step_pin);
   output(step_direction, step_pin);
   //
   //
   // main loop
   //
   //
   // main loop
   //
   time = 0;
   steps = 1;
   dir = 0;
   while (1) {
      //
      // get command from bus
      //
      get_bus_char(&chr);
      //
      // is it a go command?
      //
      if (chr == 'G') {
         //
         // yes, execute command if dir set
         //
         if (dir == 1) {
            set(led_port, led_pin);
            set(bus_port, bus_pin_out);
            output(bus_direction, bus_pin_out);
            set(dir_port, dir_pin); //clockwise
            //make sure there is enough time to move the motors
            if (time > steps) {
                step_off = time - (step_on*steps);
                step_off = step_off / steps;
                for(k=0;k<steps;k++){
                    set(step_port, step_pin);
                    _delay_ms(step_on);
                    clear(step_port, step_pin);
                    for (j=0;j<step_off;j++) _delay_ms(1);
                    }
                }
            }
         else if (dir == -1) {
            set(led_port, led_pin);
            set(bus_port, bus_pin_out);
            output(bus_direction, bus_pin_out);
            clear(dir_port, dir_pin); //counterclockwise
            //make sure there is enough time to move
            if (time > steps) { 
                step_off = time - (step_on*steps);
                step_off = step_off / steps;
                for(k=0;k<steps;k++){
                    set(step_port, step_pin);
                    _delay_ms(step_on);
                    clear(step_port, step_pin);
                    for (j=0;j<step_off;j++) _delay_ms(1);
                    }
                }
            }
         //
         // release bus to acknowledge completion and clear dir
         //
         input(bus_direction, bus_pin_out);
         clear(bus_port, bus_pin_out);
         clear(led_port, led_pin);
         dir = 0;
         }
      //
      // no, is it an X?
      //
      else if (chr == 'Y') { //capitals are positive~
         //
         // yes, read arguments
         //
         dir = 1; //clockwise
         get_bus_char(&args); //assuming 2 will follow
         get_bus_char(&step_hi);
         get_bus_char(&step_lo);
         steps = (((unsigned char) step_hi) << 8) + ((unsigned char) step_lo);
      }
      //
      // no, is it an x?
      //
      else if (chr == 'y') { //capitals are positive~
         //
         // yes, read arguments
         //
         dir = -1; //counterclockwise
         get_bus_char(&args); //assuming 2 will follow
         get_bus_char(&step_hi);
         get_bus_char(&step_lo);
         steps = (((unsigned char) step_hi) << 8) + ((unsigned char) step_lo);
      }


      //
      // no, is it a T?
      //
      else if (chr == 'T') {
         //
         // yes, read arguments
         //
         get_bus_char(&args);
         get_bus_char(&time_hi);
         get_bus_char(&time_lo);
         //set(led_port, led_pin);
         time = (((unsigned char) time_hi) << 8) + ((unsigned char) time_lo);
         //for (j=0;j<time;j++) _delay_ms(1);
         //clear(led_port, led_pin);
         }
      //
      // no, skip rest of command
      //
      else {
         get_bus_char(&args);
         for (i = 0; i < args; ++i) {
            get_bus_char(&chr);
            }
         }

      }
   }