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

#include "lights.h"

#define ZERO_ADC    2

//#define NUM_ADCS	ZERO_ADC
#define NUM_ADCS	2

volatile static unsigned char current_adc;
volatile unsigned char adc_enabled;
volatile unsigned char need_battery_adc, need_pwmled_adc;
static uint16_t adc_sum, read_zero, drop_count, read_count, n_reads_log;
volatile uint16_t jiffies;

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, ADC3 (PB3), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX1) | _BV(MUX0);
		break;
	case 1: // battery voltage: 1.1V, ADC1 (PB2), single-ended
		ADMUX = _BV(REFS1) | _BV(MUX0);
		break;
	case ZERO_ADC: // zero: 1.1V, GND, single-ended
		ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX2) | _BV(MUX0);
		break;
	}
}

void start_next_adc()
{
	if (need_battery_adc) {
		need_battery_adc = 0;
		current_adc = 1;
		read_zero = 1;
		drop_count = 1;
		read_count = 1;
		n_reads_log = 0;
	} else if (need_pwmled_adc) {
		current_adc = 0;
		read_zero = 0;
		drop_count = 1;
		read_count = 1 << PWMLED_ADC_SHIFT;
		n_reads_log = PWMLED_ADC_SHIFT;
	} else {
		ADCSRA &= ~_BV(ADEN);
		power_adc_disable();
		adc_enabled = 0;
		return;
	}

	if (!adc_enabled) {
		power_adc_enable();
		ADCSRA |= _BV(ADEN);
		adc_enabled = 1;
	}

	adc_sum = 0;

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

	ADCSRA |= _BV(ADSC);
}

#if 0
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
	}
}
#endif

/*
 * 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()
{
	need_battery_adc = 0;
	need_pwmled_adc = 0;
	current_adc = 0;
	adc_enabled = 1;

	power_adc_enable();
	ACSR |= _BV(ACD);	// but disable the analog comparator

	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(ADC3D) | _BV(ADC2D);
	
	// 1.1V, GND
	ADMUX = _BV(REFS1) | _BV(MUX3) | _BV(MUX2) | _BV(MUX0);

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

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

	start_next_adc();
}

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

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

	if (read_zero) {
		setup_mux(current_adc);
		read_zero = 0;
		ADCSRA |= _BV(ADSC); // drop this one, start the next
		return;
	}

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

	if (read_count) {
		ADCSRA |= _BV(ADSC); // immediately start the next conversion
		adc_sum += adcval;
		read_count--;
		return;
	}

	/*
	 * Now we have performed read_count measurements and have them
	 * in adc_sum.
	 */
	switch (current_adc) {
	case 0:
		// pwmled_adc(current_adc, adc_sum);
		pwmled_adc(adc_sum);
		break;
	case 1:
		battery_adc(adc_sum);
		break;
	}

	start_next_adc();
}