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

/* ---------- 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;
#define TICKS_IN_US	(F_CPU/8000000UL)

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

/* ----------------- iBus ------------------ */

typedef int16_t servo_val_t;
servo_val_t ibus_channel[N_IBUS_CHANNELS];
#define N_IBUS_CHANNELS_DIRECT 14
/*
 * channel value      ibus frame value
 *     -120 %               900
 *     -100 %              1000
 *        0 %              1500
 *      100 %              2000
 *      120 %              2100
 */
#define IBUS_SERVO_CENTER	1500

static void ibus_servo_frame(void)
{
	uint16_t csum = 0xFFFF;
	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;
	}

	for (i = 0; i < N_IBUS_CHANNELS_DIRECT; i++)
		ibus_channel[i] = (uint16_t)serial_frame[2 + 2*i]
			+ (((uint16_t)serial_frame[3 + 2*i] & 0x000F) << 8)
			- IBUS_SERVO_CENTER;
	// TODO channels 15-18
}

/* -------------- PWM output --------------- */

#define SERVO_PWM_CENTER	(1500 * TICKS_IN_US)

time_t pwm_channels[N_PWM_CHANNELS];
volatile uint8_t pwm_busy;
uint8_t pwm_data_ready;

static uint8_t pwm_channel;
static time_t pwm_frame_start;

// TODO: move this into PROGMEM?
static const uint8_t pwm_channel_bit[] = {
	_BV(PD2),
	_BV(PD3),
	_BV(PD4),
	_BV(PD5),
	_BV(PD6),
	_BV(PD7),
};

#define PWM_CH_MASK  (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
#define PWM_FRAME_LEN	(20000 * TICKS_IN_US)

static void pwm_init(void)
{
	uint8_t i;

	for (i = 0; i < N_PWM_CHANNELS; i++)
		pwm_channels[i] = 0;

	pwm_channel = N_PWM_CHANNELS + 1;
	pwm_data_ready = pwm_busy = 0;

	TIMSK1 &= ~_BV(OCIE1A);
	PORTD &= ~PWM_CH_MASK;
	DDRD |= PWM_CH_MASK;
}

static void pwm_set(uint8_t channel, servo_val_t value)
{
	time_t tm = SERVO_PWM_CENTER + (value * TICKS_IN_US);

	pwm_channels[channel] = tm;
}

static void pwm_send_pulse()
{
	uint8_t first_pulse = 0;

	if (pwm_channel == N_PWM_CHANNELS + 1) {
		if (!pwm_data_ready)
			return;
		pwm_channel = 0;
	}

	if (pwm_channel == 0) {
		first_pulse = 1;
		pwm_data_ready = 0;
	}

	// find a non-empty channel
	while (!pwm_channels[pwm_channel] && pwm_channel < N_PWM_CHANNELS)
		pwm_channel++;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		time_t now = TCNT1;
		if (pwm_channel < N_PWM_CHANNELS) {
			PORTD |= pwm_channel_bit[pwm_channel];
			OCR1A = now + pwm_channels[pwm_channel];
			if (first_pulse)
				pwm_frame_start = now;
		} else {
			OCR1A = pwm_frame_start + PWM_FRAME_LEN;
		}

		TIMSK1 |= _BV(OCIE1A);
		pwm_busy = 1;
	};
}

ISR(TIMER1_COMPA_vect) {
	PORTD &= ~PWM_CH_MASK;
	TIMSK1 &= ~_BV(OCIE1A);
	pwm_channel++;
	pwm_busy = 0;
}


/* -------- Custom mixing done here -------- */

static void do_mixes()
{
	int i;

	for (i = 0; i < N_PWM_CHANNELS; i++) {
		pwm_set(i, ibus_channel[i]);
	}
}

int main(void)
{
	led_init();
	timer_init();
	serial_init();
	pwm_init();

	serial_enable_rx();
	sei();

	while (1) {
		if (serial_frame_ready) {
			serial_frame_ready = 0;
			ibus_servo_frame();
			do_mixes();
			pwm_data_ready = 1;
		}
		if (!pwm_busy) {
			pwm_send_pulse();
		}
	}
}