/* I-Bus servo mixer for FlySky iA6B receiver */

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

#define N_SERVO_CHANNELS	18
#define SERVO_FRAME_SIZE	32

/* ----------------- USART ----------------- */

// I-Bus uses 115200n8
#define UART_BAUD       115200
#define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)

#define BUFLEN SERVO_FRAME_SIZE
static volatile uint8_t buffer[BUFLEN];
static volatile uint8_t buf_offset;

static void handle_rx_packet(void);

static void serial_init(void)
{
	UBRR0 = UBRR_VAL;

	UCSR0A = 0;
	UCSR0B = _BV(RXEN0) | _BV(RXCIE0);
        UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
}

static void serial_enable_rx(void)
{
	UCSR0B &= ~(_BV(TXEN0) | _BV(TXCIE0));
	UCSR0B |= _BV(RXEN0) | _BV(RXCIE0);
}

static void led_init(void)
{
	DDRB |= _BV(PB5);
	PORTB &= ~_BV(PB5);
}

static void inline led1_off(void)
{
	PORTB &= ~_BV(PB5);
}

static void inline led1_on(void)
{
	PORTB |= _BV(PB5);
}

#define serial_rx_vect USART_RX_vect

#define serial_data UDR0

static void recv_restart(void)
{
	// led2_on();

	buf_offset = 0;
	serial_enable_rx();
}

// USART receive interrupt
ISR(serial_rx_vect)
{
	uint8_t val = serial_data;

	// a shorthand - for now, we accept 4-byte packets only
	if (buf_offset == 0 && val != SERVO_FRAME_SIZE)
		return;

	buffer[buf_offset++] = val;

	if (buf_offset == buffer[0]) {
		handle_rx_packet();
		buf_offset = 0;
	}
}

/* ----------------- Timer ----------------- */
typedef uint16_t time_t;

#define SERVO_MASK	(_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))

static const prog_uint8_t servo_bits[] = {
	_BV(PD2),
	_BV(PD3),
	_BV(PD4),
	_BV(PD5),
	_BV(PD6),
	_BV(PD7),
};

static void timer_init(void)
{
	TCCR1A = 0; // no PWM or WGM output
	TCCR1B = _BV(CS11); // clk/8

	DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5)
	PORTD &= ~_BV(PD2)
}

static time_t inline get_time(void)
{
	time_t rv;

	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		rv = TCNT1;
	};

	return rv;
}

// run this inside interrupt or in atomic context
static void interrupt_after(uint16_t delay)
{
	uint16_t now = TCNT1;
	now += delay;
	OCR1A = now;
	TIMSK1 |= _BV(OCIE1A);
}

ISR(TIMER1_COMPA_vect) {
	static uint16_t led = 2000;

	led++;
	if (led & 1) {
		led1_off();
		PORTD &= ~_BV(PD2);
		interrupt_after(65000);
	} else {
		led1_on();
		PORTD |= _BV(PD2);
		interrupt_after(led);
	}
	if (led >= 4000)
		led = 2000;
}

/* ----------------- iBus ------------------ */
static void ibus_init(void)
{
	recv_restart();
}

static void handle_rx_packet(void)
{
	uint16_t csum = 0xFFFF, servo;
	uint8_t i, cmd, dev;

	for (i = 0; i < buf_offset-2; i++)
		csum -= (uint16_t)buffer[i];

	if ((buffer[buf_offset-2] != (csum & 0xFF))
		|| (buffer[buf_offset-1] != (csum >> 8))) { // invalid csum
		buf_offset = 0; // start over
		return;
	}

	servo = buffer[12];
	servo |= ((uint16_t)buffer[13] & 0x000F) << 8;

	/*
	 * -120 % == 0x384 ==  900
	 * -100 % == 0x3e8 == 1000
	 *    0 % == 0x5dc == 1500
	 */
	if (servo < 0x0834)
		led1_on();
	else
		led1_off();
}

int main(void)
{
	led_init();

	// serial_init();
	timer_init();
#if 0
	ibus_init();

#endif
	interrupt_after(1000);

	sei();

	while (1) {
	}

}