//
//
// hello.bus.45.c
//
// 9600 baud serial bus hello-world
//
// Neil Gershenfeld
// 11/24/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/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.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 100 // bit delay for 9600 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 GPIO1_port PORTB
#define GPIO1_direction DDRB
#define GPIO1_pin (1 << PB2)

#define GPIO2_port PORTA
#define GPIO2_direction DDRA
#define GPIO2_pin (1 << PA7)


#define serial_port PORTA
#define serial_direction DDRA
#define serial_pins PINA
#define serial_pin_in (1 << PA6)
#define serial_pin_out (1 << PA5)

#define node_id 'x'

#define gate_delay() _delay_us(5) // delay between switching gates

#define MOSFET_port PORTA // MOSFET port
#define MOSFET_direction DDRA // MOSFET direction
#define GRP (1 << PA0) // right PMOSFET gate
#define GRN (1 << PA1) // right NMOSFET gate
#define GLN (1 << PA2) // left NMOSFET gate
#define GLP (1 << PA3) // left PMOSFET gate

void clearGates( void );
void mBreak(void);
void mForward(void);
void mBackward(void);

volatile uint16_t in1;
volatile uint16_t in2;

char buff[100];
volatile char dir;
int length = 20;
uint8_t speed = 255;

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_string(volatile unsigned char *port, unsigned char pin, char *str) {
    //
    // send string in str on port pin
    //
    uint8_t index = 0;
    while (1) {
        if (str[index]==0) break;
        put_char(&serial_port, serial_pin_out, str[index]);
        index++;
        }
    }

ISR(INT0_vect) {
    in1++;
    if (in1==length) {
        mBreak();
        in1=0;
        dir=0;
        cli();
        }
    }
    
ISR(TIM0_COMPA_vect){}
 

void echo(char echar) {
    output(serial_direction, serial_pin_out);
    put_char(&serial_port, serial_pin_out, echar);
    input(serial_direction, serial_pin_out);
    }

int main(void) {
    //
    // main
    //
    static char chr;
    //
    // 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);
    input(serial_direction, serial_pin_out);
    
    input(GPIO1_direction, GPIO1_pin);
    set(GPIO1_port, GPIO1_pin);
    input(GPIO2_direction, GPIO2_pin);
    set(GPIO2_port, GPIO2_pin);

    // initialize H-bridge pins on braking
    clear(MOSFET_port, (GRP|GLP|GRN|GLN));
    output(MOSFET_direction, GRP|GLP|GRN|GLN);
    set(MOSFET_port, (GRN|GLN));
    
    MCUCR |= (2<<ISC00);
    GIMSK |= (1<<INT0);
    sei();
    
    in1=0;
    mForward();
    _delay_ms(1000);
    
    
    output(serial_direction, serial_pin_out);
    put_string(&serial_pins, serial_pin_in, "started\r\n");
    input(serial_direction, serial_pin_out);
    //
    // main loop
    //
    while (1) {
      get_char(&serial_pins, serial_pin_in, &chr);
      if (chr == node_id) {
        echo(chr);
        if (in1!=0){
            sprintf(buff,"counts: %d\r\n", in1); 
            output(serial_direction, serial_pin_out);
            put_string(&serial_pins, serial_pin_in, buff);
            input(serial_direction, serial_pin_out);
            in1=0;
        }
        get_char(&serial_pins, serial_pin_in, &chr);
        echo(chr);
        if (chr=='a' || chr=='d'){
            dir = chr;
            echo(':');
            uint8_t idx = 0;
            while (1) {
                get_char(&serial_pins, serial_pin_in, &chr);
                if (chr<'0' || chr>'9') break;
                buff[idx] = chr;
                idx++;
                }
            buff[idx]=0;
            length = atoi(buff);
            sprintf(buff,"duration: %d\r\n", length); 
            output(serial_direction, serial_pin_out);
            put_string(&serial_pins, serial_pin_in, buff);
            input(serial_direction, serial_pin_out);
            idx=0;
            while (1) {
                get_char(&serial_pins, serial_pin_in, &chr);
                if (chr<'0' || chr>'9') break;
                buff[idx] = chr;
                idx++;
                }
            buff[idx]=0;
            speed = atoi(buff);
            sprintf(buff,"speed: %d\r\n", speed); 
            output(serial_direction, serial_pin_out);
            put_string(&serial_pins, serial_pin_in, buff);
            input(serial_direction, serial_pin_out);
        }

        if      (dir=='a') {
            sei();
            mBackward();
            }
        else if (dir=='d') {
            sei();
            mForward();
            }
        else {
            dir = 0;
            mBreak();
            cli();
            in1=0;
            output(serial_direction, serial_pin_out);
            put_string(&serial_pins, serial_pin_in, "stopped/r/n");
            input(serial_direction, serial_pin_out);
            }
        }
      }
   
   }

void clearGates( void ) {
    MOSFET_port &= ~(GRN|GRP|GLN|GLP);
    gate_delay();
}

void mBreak(void) {
    clearGates();
    set(MOSFET_port, (GRN|GLN));
}

void mForward(void) {
    clearGates();
    set(MOSFET_port, (GRP|GLN));
}
void mBackward(void) {
    clearGates();
    set(MOSFET_port, (GLP|GRN));
}