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

/* 500Hz -> 2ms */
#define MAXPULSFREQ         500

/* timer1 value for a 2ms puls */
#define TIMER_MAXPULS       F_CPU/MAXPULSFREQ

/* min/max values */
#define MINPULS 13500
#define MAXPULS 30000

/* baud rate for serial communication */
#define BAUD 9600UL

/* hardware settings for baud rate */
#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)
#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1)))
#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD)
 
/* compile time check if baud rate is ok */
#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
  #error systematic error of baud rate is > 1%
#endif


static uint16_t servo_pulslength[20];
static uint8_t activated = 0;

/* init IO pins */
static inline void init_io() {
	/* PB1 = left:red, PB2 = left:green, PB3 = left:blue */
	DDRB  = (1 << PB1) | (1 << PB2) | (1 << PB3);

	/* PD3 = right:red, PD5 = right:green, PD6 = right:blue */
	DDRD  = (1 << PD3) | (1 << PD5) | (1 << PD6);

	/* These pins are broken, so enable them */
	DDRC = (1 << PC4) | (1 << PC5) | (1 << PC6);
	PORTC = (1 << PC4) | (1 << PC5) | (1 << PC6);
}

/* INTERRUPT: timer for software pwm to control servos */
ISR(TIMER1_OVF_vect) {
	static uint8_t servo_indexhalf = 0;

	switch(servo_indexhalf) {
		case 0 : PORTB |=  (1 << PB1); break;
		case 1 : PORTB &= ~(1 << PB1); break;
		case 2 : PORTB |=  (1 << PB2); break;
		case 3 : PORTB &= ~(1 << PB2); break;
		case 4 : PORTB |=  (1 << PB3); break;
		case 5 : PORTB &= ~(1 << PB3); break;
		case 6 : PORTD |=  (1 << PD3); break;
		case 7 : PORTD &= ~(1 << PD3); break;
		case 8 : PORTD |=  (1 << PD5); break;
		case 9 : PORTD &= ~(1 << PD5); break;
		case 10: PORTD |=  (1 << PD6); break;
		case 11: PORTD &= ~(1 << PD6); break;
	}

	/* set time for the next interrupt */
	TCNT1 = servo_pulslength[servo_indexhalf];

	servo_indexhalf++;

	if(servo_indexhalf == 20)
		servo_indexhalf = 0;
}

/* setup serial communication */
static inline void init_uart() {
	UBRR0   = UBRR_VAL;
	UCSR0B |= (1 << TXEN0);
	UCSR0B |= (1 << RXEN0);

	/* Frame Format: Asynchron 8N1 */
	UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
}

/* set servo speed */
static inline void servo_set(uint8_t index, uint8_t value) {
	if(!activated)
		value = 0;

	if (index < 6) {
		uint16_t tmp = MINPULS+(MAXPULS-MINPULS)/256*value;
		/* calculate hightime */
		servo_pulslength[index<<1]=(-1)*tmp;
		/* sum of low and hightime for one servo is 2ms */
		servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-tmp);
	}
}

/* init servos, reset speed */
static inline void init_servo() {
	uint8_t  i;

	/* disable all motors */
	for(i=0;i<6;i++)
		servo_set(i, 0);

	/* init timer */
	TCNT1   = 0 - 16000;
	TCCR1A  = 0;
	TCCR1B  = (1 << CS10);
	TIMSK1 |= (1 << TOIE1);
}

/* get a single byte from serial device */
static inline uint8_t uart_getc() {
	while (!(UCSR0A & (1<<RXC0)));
	return UDR0;
}

/* put a single byte to serial device */
static inline void uart_putc(uint8_t c) {
    while (!(UCSR0A & (1<<UDRE0)));
    UDR0 = c;
}

int main() {
	/* initialize hardware */
	init_io();
	init_servo();
	init_uart();
	sei();

	uint8_t c;

	while(1) {
		c = uart_getc();

		/* set speed for single engine */
		if(c >= 0x01 && c <= 0x06) {
			uint8_t speed = uart_getc();
			servo_set(c-1, speed);
		/* set speed for all engines */
		} else if(c == 0xFF) {
			uint8_t speed = uart_getc();
			for(int i=0;i<6;i++)
				servo_set(i, speed);
		/* activate engines */
		} else if(c == 0xCA) {
			if(uart_getc() != 0xFE) continue;
			if(uart_getc() != 0xBA) continue;
			if(uart_getc() != 0xBE) continue;
			activated=1;
		/* disable engines */
		} else if(c == 0xDE) {
			if(uart_getc() != 0xAD) continue;
			if(uart_getc() != 0xBA) continue;
			if(uart_getc() != 0xBE) continue;
			activated=0;
			for(int i=0;i<6;i++)
				servo_set(i, 0);
		/* ignore unknown commands */
		} else if(c != 0x00) {
			continue;
		}

		/* acknowledge command */
		uart_putc('.');
	}

	return 0;
}