#include <avr/io.h>
#include <util/delay.h>
#include <stdint.h>
#include <avr/iom168.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>

// macros for setting bits
#define sbi(var, mask)   ((var) |= (uint8_t)(1 << mask))
#define cbi(var, mask)   ((var) &= (uint8_t)~(1 << mask))
#define ABS(x)           ((x>0)?(x):(-x))
#define CAP(speed,max)   ((speed<max)?(speed):(max))

enum {
	forward = 0,
	stop,
	backward
};
enum {
	lmotor=0xF0,
	rmotor=0x0F
};


// constant pin locations
const uint8_t L_DIS = 4;
const uint8_t R_DIS = 5;

// setup EEPROM
uint8_t EEMEM EEmax_speed=0xAF;
uint8_t EEMEM EEmax_change=0x01;

// global variables for ramping speed
// default to stop, no speed
volatile uint16_t sp_l;
volatile uint16_t sp_r;
volatile uint8_t count;
volatile uint8_t ldest_speed    = 0x00;
volatile uint8_t ldest_dir              = 0x01;
volatile uint8_t rdest_speed    = 0x00;
volatile uint8_t rdest_dir              = 0x01;
volatile uint8_t max_speed;
volatile uint8_t max_change;

void serial_init(long baud)
{
        UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8;
        UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1);

        UCSR0B = (1<<RXEN0) | (1<<TXEN0);
        UCSR0C = (1<<USBS0) | (3 << UCSZ00);
}

uint8_t usart_get( void )
{
    while ( (UCSR0A & _BV(RXC0)) == 0 )
        ;
    return UDR0;
}


void usart_put( uint8_t b )
{
    while ( (UCSR0A & _BV(UDRE0)) == 0 )
        ;
    UDR0 = b;
}

// interrupt every 16ms
void timer_init()
{
        TCCR0A = 0b00000000;
        TCCR0B = 0b00000101;
        OCR0A = 250;
        TIMSK0 = 0b00000010;
}


void pwm_init()
{
        DDRB |= _BV(PB1);
        DDRB |= _BV(PB2);

        /*
         * TCCR1A:, datasheet pg. 131
         * [COM1A1][COM1A0][COM1B1][COM1B0][-][-][WGM11][WGM10]
         * Channel A:  Phase & Freq. correct
         *      OC1A set on match counting up
         *      OC1A cleared on match counting down
         *      OC1B cleared on match counting up
         *      OC1B set on match counting down
         *
         */
//      TCCR1A = 0b10110000;
        TCCR1A = 0b11110000;
        /*
         * TCCR1B:, datasheet pg. 133
         * [ICNC1][ICES1][-][WGM13][WGM12][CS12][CS11][CS10]
         * Set TOP to ICR1
         * CS1 = 0b001, ICR1 = 0x03FF: 7820Hz (10-bit resolution)
         * CS1 = 0b001, ICR1 = 0x0200: 15655Hz (9-bit resolution)
         */
        TCCR1B = 0b00010001;

        /* ICR1 defines TOP */
        ICR1 = 0x0200;

        OCR1A = ICR1>>1;
        OCR1B = ICR1>>1;
	sp_l = OCR1A;
	sp_r = OCR1B;
}

void io_init()
{
        DDRB = 0b11111111;
        DDRC = 0b11111111;
        DDRD = 0b11111110;
}

void eeprom_init()
{
        max_speed = eeprom_read_byte(&EEmax_speed);
        max_change = eeprom_read_byte(&EEmax_change);
}

void set_motors(uint8_t speed, uint8_t dir, uint8_t motor)
{

        switch(motor)   {
        case lmotor:
                ldest_speed = CAP(speed,max_speed);
                ldest_dir = dir;
		sp_l = (ICR1 >> 1) + ldest_speed * (ldest_dir - 1);
                if(dir == stop) {
                        ldest_speed = ICR1>>1;
			sp_l = ICR1 >> 1;
                        OCR1A = ICR1>>1;
                        sbi(PORTB,L_DIS);
                } else {
                        cbi(PORTB,L_DIS);
		}
                break;

        case rmotor:
                rdest_speed = CAP(speed,max_speed);
                rdest_dir = dir;
		sp_r = (ICR1 >> 1) + rdest_speed * (rdest_dir - 1);
                if(dir == stop) {
                        rdest_speed = ICR1>>1;
			sp_l = ICR1 >> 1;
                        OCR1B = ICR1>>1;
                        sbi(PORTB,R_DIS);
                } else {
                        cbi(PORTB,R_DIS);
		}
                break;
        default:
                break;

        }


}


int main(void)
{
#if 0
        enum {
		motor_state = 0,
		direction_state,
		speed_state,
		checksum_state
	};
#endif

        uint8_t state;
        uint8_t in;
        uint8_t temp;
        uint8_t motor;
        uint8_t speed;
        uint8_t dir;
	uint8_t chk;

        eeprom_init();
        serial_init(9600);
        io_init();
        pwm_init();
        timer_init();           // interrupt every 16 ms
        sei();                          // enable interrupts

	while (1) {
		motor = usart_get();
		speed = usart_get();
		dir = usart_get();
		chk = usart_get();
		temp = (motor ^ (3*speed) ^ (5*dir)) ^ in;
		usart_put(temp);

		if (!temp) {
			set_motors(speed, dir, motor);
		}
	}



#if 0
        while (1) {
                in = usart_get();
                switch (state) {
                case motor_state :
                        motor = in;
                        state = speed_state;
                        break;

                case speed_state :
                        speed = in;
                        state = direction_state;
                        break;

                case direction_state :
                        dir = in;
                        state = checksum_state;
                        break;

                case checksum_state :
                        temp = (motor ^ (3*speed) ^ (5*dir)) ^ in; // auchter's crazy checksum
                        usart_put(temp);
                        if (!temp)
                                set_motors(speed,dir,motor);
                        state = motor_state;
                        break;

                default:
                        state = motor_state;
                        break;
                }
        }
#endif 
}

// periodic interrupt, currently every 16ms
// ramps speed to desired speed
ISR(TIMER0_COMPA_vect)
{
#if 0
        uint8_t templ;
        uint8_t tempr;
        templ = (ICR1>>1) + ldest_speed * (ldest_dir - 1);
        tempr = (ICR1>>1) + rdest_speed * (rdest_dir - 1);
#endif
	if (ABS(sp_l - OCR1A) > max_change) {
		OCR1A += (sp_l > OCR1A) ? max_change : -max_change;
	} else {
		OCR1A = sp_l;
	}

	if (ABS(sp_r - OCR1B) > max_change) {
		OCR1B += (sp_r > OCR1B) ? max_change : -max_change;
	} else {
		OCR1B = sp_r;
	}

#if 0
        OCR1A += (templ > OCR1A ? max_change : -max_change);
        OCR1B += (tempr > OCR1B ? max_change : -max_change);
        OCR1A = ABS(OCR1A - templ) < max_change ? templ : OCR1A;
        OCR1B = ABS(OCR1B - tempr) < max_change ? tempr : OCR1B;
#endif
}