/* 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 IBUS_SERVO_FRAME_SIZE	32
#define IBUS_SERVO_FRAME_ID	0x40	// first byte after the length

/* ---------- LEDs for debugging ---------- */

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

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

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

/* ----------------- Timer ----------------- */

typedef uint16_t time_t;

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


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

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

volatile uint8_t serial_frame[IBUS_SERVO_FRAME_SIZE];
volatile uint8_t serial_frame_ready;

static volatile uint8_t serial_frame_pos;

void serial_init(void)
{
	UBRR0 = UBRR_VAL;

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

	serial_frame_ready = serial_frame_pos = 0;
}

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

#define serial_rx_vect USART_RX_vect

#define serial_data UDR0

/*
 * USART receive interrupt
 *
 * In order to save the latency, we do only the necessary things here,
 * and postpone parsing the frame to tne non-irq time in the main
 * loop. Also, we expect the main loop to finish before the second
 * byte of the next frame is read, so we do not do atomic reads on top
 * of serial_frame[].
 */
ISR(serial_rx_vect)
{
	uint8_t val = serial_data;

	// a shorthand - for now, we accept fixed-size packets only
	if (serial_frame_pos == 0 && val != IBUS_SERVO_FRAME_SIZE)
		goto restart;

	if (serial_frame_pos == 1
		&& serial_frame[0] == IBUS_SERVO_FRAME_SIZE
		&& val != IBUS_SERVO_FRAME_ID)
		goto restart;

	serial_frame[serial_frame_pos++] = val;

	if (serial_frame_pos == serial_frame[0])
		serial_frame_ready = 1;
restart:
	serial_frame_pos = 0;
}

/* ----------------- Servos ----------------- */

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

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

static void xxx_init(void)
{
	DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
	PORTD &= ~_BV(PD2);
}

// 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_servo_frame(void)
{
	uint16_t csum = 0xFFFF, servo;
	uint8_t i;

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

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

	servo = serial_frame[12];
	servo |= ((uint16_t)serial_frame[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();
	timer_init();
	serial_init();
#if 0
	ibus_init();

#endif
	serial_enable_rx();
	sei();

	while (1) {
		if (serial_frame_ready) {
			serial_frame_ready = 0;
			ibus_servo_frame();
		}
	}
}