//
// hello.servo.44.c
//
// servo motor hello-world
//
// set lfuse to 0x5E for 20 MHz xtal
//
// Neil Gershenfeld
// 4/8/12
//
// (c) Massachusetts Institute of Technology 2012
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose. Copyright is
// retained and must be preserved. The work is provided
// as is; no warranty is provided, and users accept all 
// liability.
//

#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 position_delay() _delay_ms(500)

#define PWM_port PORTA
#define PWM_pin (1 << PA6)
#define PWM_direction DDRA

#define input(directions,pin) (directions &= (~pin)) // set port direction for input
#define serial_pin_in (1 << PA4)
#define serial_port PORTA
#define serial_direction DDRB
#define serial_pins PINA
#define input_port PORTB
#define input_direction DDRB
#define input_pin (1 << PB2)
#define input_pins PINB
#define bit_delay_time 102 // 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 node_id 0

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();
}

int main(void) {
    //
    // main
    //
    // set clock divider to /1
    //
    CLKPR = (1 << CLKPCE);
    CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
    //
    // set up timer 1
    //
    TCCR1A = (1 << COM1A1) | (0 << COM1A0); // clear OC1A on compare match
    TCCR1B = (0 << CS12) | (1 << CS11) | (0 << CS10) | (1 << WGM13); // prescaler /8, phase and frequency correct PWM, ICR1 TOP
    ICR1 = 25000; // 20 ms frequency
    //
    // set PWM pin to output
    //
    clear(PWM_port, PWM_pin);
    set(PWM_port, PWM_pin);
    output(PWM_direction, PWM_pin);

    //initialize pins
    set(input_port, input_pin); // turn on pull-up
    input(input_direction, input_pin);

    uint8_t chr, cm;

    //
    // main loop
    //
    while (1) {
        //
        // wait for button down
        //
        while (0 != pin_test(input_pins,input_pin))
        ;
        OCR1A = 650;
        position_delay();

        //
        // wait for button up
        //
        while (0 == pin_test(input_pins,input_pin))
        ;
        int i;
        int j;

        get_char(&serial_pins, serial_pin_in, &chr);
        get_char(&serial_pins, serial_pin_in, &cm);

        if (chr == node_id) {
            for(i = 1; i < 20; i = i+1){
                OCR1A = 600 + 100 * i;
                for(j=0; j < cm; j = j + 1){
                    _delay_ms(100);
                }
            }
        }
    }
}