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

#include "lights.h"

#define AMBIENT_ADC N_PWMLEDS
#define BATTERY_ADC (N_PWMLEDS + 1)
#define ADC1_GAIN20 (N_PWMLEDS + 2)
#define BUTTON_ADC  (N_PWMLEDS + 3)
#define ZERO_ADC    (N_PWMLEDS + 4)

#define NUM_ADCS	ZERO_ADC

struct {
	unsigned char read_zero_log : 2;
	unsigned char read_drop_log : 2;
	unsigned char read_keep_log : 4;
} adc_params[NUM_ADCS] = {
	{ 0, 1, PWMLED_ADC_SHIFT },	// pwmled 1
	{ 0, 1, PWMLED_ADC_SHIFT },	// pwmled 2
	{ 0, 1, PWMLED_ADC_SHIFT },	// pwmled 3
	{ 0, 1, AMBIENT_ADC_SHIFT },	// ambient
	{ 0, 1, 0 },			// battery
	{ 0, 1, 0 },			// gain20
	{ 0, 1, 0 },			// buttons
};

volatile static unsigned char current_adc, current_slow_adc;
static uint16_t adc_sum, zero_count, drop_count, read_count, n_reads_log;
#define ADC1_GAIN20_OFFSET_SHIFT	6
static uint16_t adc1_gain20_offset;


static void setup_mux(unsigned char n)
{
	/* ADC numbering: PWM LEDs first, then others, zero at the end */
	switch (n) {
	case 0: // pwmled 1: 1.1V, ADC0,1 (PA0,1), gain 20
		ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX1) | _BV(MUX0);
		break;
	case 1: // pwmled 2: 1.1V, ADC2,1 (PA2,1), gain 20
		ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
		break;
	case 2: // pwmled 3: 1.1V, ADC4 (PA5), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX2);
		break;
	case AMBIENT_ADC: // ambient light: 1.1V, ADC5 (PA6), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX2) | _BV(MUX0);
		break;
	case BATTERY_ADC: // batt voltage: 1.1V, ADC6 (PA7), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX2) | _BV(MUX1);
		break;
	case ADC1_GAIN20: // gain stage offset: 1.1V, ADC1,1, gain 20
		ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX2) | _BV(MUX0);
		break;
	case BUTTON_ADC: // buttons: 1.1V, ADC3, single-ended
		PORTA |= _BV(PA3); // +5V to the voltage splitter
		ADMUX = _BV(REFS1) | _BV(MUX1) | _BV(MUX0);
		break;
	case ZERO_ADC: // zero: 1.1V, ADC1 (PA1), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX0);
		break;
	}
}

static void start_next_adc()
{
	if (current_adc == 0) {
		if (current_slow_adc > N_PWMLEDS) {
			// read one of the non-PWMLED ADCs
			current_adc = --current_slow_adc;
		} else {
			// no more non-PWMLEDs to do, start with PWMLEDs
			current_adc = N_PWMLEDS-1;
		}
	} else if (current_adc >= N_PWMLEDS) {
		// one of the non-PWMLED ADCs just finished, skip to PWMLEDs.
		current_adc = N_PWMLEDS-1;
	} else {
		// next PWMLED
		current_adc--;
	}

#if 0
	log_byte(0x90 + current_adc); // debug ADC switching
#endif

	adc_sum = 0;
	// we use the last iteration of zero_count to set up the MUX
	// to its final destination, hence the "1 +" below:
	if (adc_params[current_adc].read_zero_log)
		zero_count = 1 + (1 << (adc_params[current_adc].read_zero_log-1));
	else
		zero_count = 1;

	if (adc_params[current_adc].read_drop_log)
		drop_count = 1 << (adc_params[current_adc].read_drop_log - 1);
	else
		drop_count = 0;

	read_count = 1 << adc_params[current_adc].read_keep_log;
	n_reads_log = adc_params[current_adc].read_keep_log;

	// set up mux, start one-shot conversion
	if (zero_count > 1)
		setup_mux(ZERO_ADC);
	else
		setup_mux(current_adc);

	ADCSRA |= _BV(ADSC);
}

void timer_start_slow_adcs()
{
	if (current_slow_adc > N_PWMLEDS) { // Don't start if in progress
		log_byte(0x80 + current_slow_adc);
	} else {
		current_slow_adc = NUM_ADCS;
		// TODO: kick the watchdog here
	}
}

/*
 * Single synchronous ADC conversion.
 * Has to be called with IRQs disabled (or with the ADC IRQ disabled).
 */
static uint16_t read_adc_sync()
{
	uint16_t rv;

	ADCSRA |= _BV(ADSC); // start the conversion

	// wait for the conversion to finish
	while((ADCSRA & _BV(ADIF)) == 0)
		;

	rv = ADCW;
	ADCSRA |= _BV(ADIF); // clear the IRQ flag

	return rv;
}

void init_adc()
{
	unsigned char i;
	current_slow_adc = NUM_ADCS;
	current_adc = 0;

	ADCSRA = _BV(ADEN)			// enable
		| _BV(ADPS1) | _BV(ADPS0)	// CLK/8 = 125 kHz
		// | _BV(ADPS2)			// CLK/16 = 62.5 kHz
		;
	// ADCSRB |= _BV(GSEL); // gain 8 or 32

	// Disable digital input on all bits used by ADC
	DIDR0 = _BV(ADC0D) | _BV(ADC1D) | _BV(ADC2D) | _BV(ADC3D)
		| _BV(ADC4D) | _BV(ADC5D) | _BV(ADC6D);

	// 1.1V, ADC1,1, gain 20
	ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX2) | _BV(MUX0);

	/* Do first conversion and drop the result */
	read_adc_sync();

	adc1_gain20_offset = 0;

	for (i = 0; i < (1 << ADC1_GAIN20_OFFSET_SHIFT); i++) {
		adc1_gain20_offset += read_adc_sync()
			- (adc1_gain20_offset >> ADC1_GAIN20_OFFSET_SHIFT);
	}

	ADCSRA |= _BV(ADIE); // enable IRQ

	start_next_adc();
}

void susp_adc()
{
	ADCSRA = 0;
	DIDR0 = 0;
}

static void adc1_gain20_adc(uint16_t adcsum)
{
	// running average
	adc1_gain20_offset += adcsum
			- (adc1_gain20_offset >> ADC1_GAIN20_OFFSET_SHIFT);
}

ISR(ADC_vect) { // IRQ handler
	uint16_t adcval = ADCW;

	if (zero_count) {
		if (zero_count > 1) {
			ADCSRA |= _BV(ADSC);
			zero_count--;
			return;
		} else {
			setup_mux(current_adc);
			zero_count = 0;
			/* fall through */
		}
	}

	if (drop_count) {
		ADCSRA |= _BV(ADSC); // drop this one, start the next
		drop_count--;
		return;
	}

	if (read_count) {
		ADCSRA |= _BV(ADSC);
		adc_sum += adcval;
		read_count--;
		return;
	}

	/*
	 * Now we have performed read_count measurements and have them
	 * in adc_sum.
	 */

	// For inputs with gain, subtract the measured gain stage offset
	if (current_adc < 2) {
		uint16_t offset = adc1_gain20_offset
			>> (ADC1_GAIN20_OFFSET_SHIFT - n_reads_log);

		if (adc_sum > offset)
			adc_sum -= offset;
		else
			adc_sum = 0;
	}

	switch (current_adc) {
	case 0:
	case 1:
	case 2:
		pwmled_adc(current_adc, adc_sum);
		break;
	case AMBIENT_ADC:
		ambient_adc(adc_sum);
		break;
	case BATTERY_ADC:
		battery_adc(adc_sum);
		break;
	case BUTTON_ADC:
		button_adc(adc_sum);
		break;
	case ADC1_GAIN20:
		adc1_gain20_adc(adcval);
		break;
	}

	start_next_adc();
}