#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))


// 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 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;
}

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)
{
	enum {forward=0,stop,backward};
	enum {lmotor=0xF0,rmotor=0x0F};
	switch(motor)	{
	case lmotor:
		ldest_speed = CAP(speed,max_speed);
		ldest_dir = dir;
		if(dir == stop) {
			ldest_speed = 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;
		if(dir == stop) {
			rdest_speed = ICR1>>1;
			OCR1B = ICR1>>1;
			sbi(PORTB,R_DIS);
		}
		else
			cbi(PORTB,R_DIS);
		break;
	default:
		break;

	}


}


int main(void)
{
	enum {motor_state=0,direction_state,speed_state,checksum_state};
	enum {forward=0,stop,backward};
	enum {lmotor=0xF0,rmotor=0x0F};

	uint8_t state = motor_state;
	uint8_t in;
	uint8_t temp;
	uint8_t motor=0;
	uint8_t speed=0;
	uint8_t dir=0;

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

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

// periodic interrupt, currently every 16ms
// ramps speed to desired speed
ISR(TIMER0_COMPA_vect)
{
	uint8_t templ;
	uint8_t tempr;
	templ = (ICR1>>1) + ldest_speed * (ldest_dir - 1);
	tempr = (ICR1>>1) + rdest_speed * (rdest_dir - 1);
	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;
}