#include <avr/io.h>
#include <avr/eeprom.h>
#include <util/delay.h>
#include <avr/sleep.h>
#include <avr/interrupt.h>

volatile uint16_t adcval;
unsigned char led_is_on = 0;
volatile unsigned char adccount = 0;

typedef struct {
	unsigned char pwmval, expected;
} led_level_t;

led_level_t led_modes[2] = {
	{ 0x50, 0x38 },
	{ 0x38, 0x04 },
};

unsigned char led_mode = 0;
unsigned char led_mode_changed = 0;

static void inline led_on()
{
        DDRB |= _BV( PB5 );
	PORTA |=  _BV( PA0 );
	led_is_on = 1;
}

static void inline led_off()
{
	led_is_on = 0;
        DDRB &= ~_BV( PB5 );
	PORTA &=~  _BV( PA0 );
//        ADCSRA &= ~(_BV(ADIE) | _BV(ADIF));
}

/* ------------ Logging/Debugging ----------- */

unsigned char debug_state EEMEM;

static void inline debug_setstate(unsigned char val)
{
	eeprom_write_byte(&debug_state, val);
}

#define LOG_BUFFER 64
uint16_t log_buffer[LOG_BUFFER] EEMEM;
unsigned char log_buffer_count;
uint16_t log_buffer2[LOG_BUFFER];
volatile unsigned char stop = 0;

static void inline init_log()
{
	debug_setstate(1);
	log_buffer_count = 0;
}

static void log_word(uint16_t word) {
	if (log_buffer_count >= LOG_BUFFER)
		return;
	
	// eeprom_write_word(&log_buffer[log_buffer_count], word);
	log_buffer2[log_buffer_count] = word;
	log_buffer_count++;

	if (log_buffer_count == LOG_BUFFER) {
		unsigned char i;
		for (i=0; i < LOG_BUFFER; i++) {
			eeprom_write_word(&log_buffer[i],
				log_buffer2[i]);
		}
		debug_setstate(0x42);
	}
}

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

volatile uint16_t jiffies = 0;

static void inline init_tmr()
{
	TCCR0A = _BV(WGM00);
	TCCR0B = _BV(CS02) | _BV(CS00); // 1 kHz
	OCR0A = 12; // 100 Hz
	TIMSK |= _BV(OCIE0A);
	DDRA |= _BV( PA0 );

	jiffies = 0;
}

static void inline tmr_handler()
{
	unsigned char c = jiffies & 0x0F;
	unsigned char c1 = jiffies & 0x7F;

	++jiffies;

#if 0
	if (c == 1)
		led_on();

	if (c == 9)
		led_off();
#endif

	if (c == 0x02 || c == 0x08)
		led_on();
		
	if (c == 0x05 || c == 0x0b)
		led_off();

#if 1
	if (c1 == 0x10) {
		led_mode = 0;
		led_mode_changed = 1;
		OCR1D = led_modes[led_mode].pwmval;
	} else if (c1 == 0x70) {
		led_mode = 1;
		led_mode_changed = 1;
		OCR1D = led_modes[led_mode].pwmval;
	}
#endif

	ADCSRA |= _BV(ADSC);
#if 0
	if (led_is_on && adcval != 0xFFEE) {
		adcval = 0xFFEE;

		ADCSRA |= _BV(ADIF) | _BV(ADIE) | _BV(ADSC);
	}
#endif
}

ISR(TIMER0_COMPA_vect)
{
	tmr_handler();
}

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

static void inline init_pwm()
{
	/* Async clock */
	PLLCSR = _BV(LSM) | _BV(PLLE);
	_delay_ms(1);
	while (PLLCSR & _BV(PLOCK) == 0)
		;
	PLLCSR |= _BV(PCKE);

	TCCR1C = _BV(COM1D0) | _BV(COM1D1) | _BV(PWM1D);
	TCCR1A = _BV(COM1A0) | _BV(COM1A1) | _BV(COM1B0) | _BV(COM1B1) | _BV(PWM1A) | _BV(PWM1B);
	// TCCR1B = 0x80| _BV(CS13) | _BV(CS11);
	TCCR1B = _BV(7)				// PWM1X: PWM inversion mode
		| _BV(CS10) 			// no clock prescaling
		;
	OCR1C = 0xFF;				// TOP value
	OCR1D = OCR1B = OCR1A = 0;
	// OCR1D = 0;
	DDRB |= _BV( PB5 );
	PORTB &= ~(_BV( PB5 ) | _BV( PB1 ) | _BV( PB3 ));

	// led_off();
	// TIMSK |= _BV(TOIE1);
}

#if 0
static void inline pwm_handler()
{
	// TIMSK &= ~_BV(TOIE1);
	// OCR1D = pwmval;
}

ISR(TIMER1_OVF_vect)
{
	pwm_handler();
}
#endif

/* ------------ A/D Converter ----------- */

static void inline init_adc()
{
	ADCSRA = _BV(ADEN)			// enable
		| _BV(ADPS1) | _BV(ADPS0)	// CLK/8 = 125 kHz
		// | _BV(ADPS2)			// CLK/16 = 62.5 kHz
		;
	ADMUX = _BV(REFS1)			// 1.1V internal reference
		| _BV(MUX4)|_BV(MUX3)	// port ADC5-6, gain 1
		;
	// ADCSRB = _BV(REFS2); 
	ADCSRB |= _BV(GSEL); 
	// Disable digital input on all bits used by ADC
	DIDR0 = _BV(ADC5D)|_BV(ADC6D) | _BV(AREFD);
	ADCSRA |= _BV(ADIE);
}

static void inline adc_handler()
{
	uint16_t new_pwm = led_modes[led_mode].pwmval;
	uint16_t old_pwm = new_pwm;
	uint16_t adc_exp = led_modes[led_mode].expected;

	adcval = ADCW;
	adccount++;

	// log_word(((adcval & 0x3FC) << 6) | pwmval);

	if (!led_is_on)
		return;

	// ADCSRA &= ~(_BV(ADIE) | _BV(ADIF));

	if (led_mode_changed) {
		led_mode_changed = 0;
		goto set_pwm;
	}

	log_word(((adcval & 0xFF) << 8) | old_pwm);

	if (2*adcval > 5*adc_exp) { // >2.5x expected, lower significantly
		new_pwm = 2*old_pwm/3;
	} else if (3*adcval > 4*adc_exp) { // >1.33x expected, lower a bit
		new_pwm = old_pwm - 1;
	} else if (4*adcval < 3*adc_exp) { // 0.75x expected, raise a bit
		new_pwm = old_pwm + 1;
	}

	if (new_pwm > 0x60) { // odpojeno?
		new_pwm = 0x60;
	}
	if (new_pwm < 2) { // zkrat?
		new_pwm = 2;
	}

set_pwm:
	if (new_pwm != old_pwm) {
		led_modes[led_mode].pwmval = new_pwm;
		OCR1D = new_pwm;
	}
	// ADCSRA |= _BV(ADSC);
}

ISR(ADC_vect)
{
	adc_handler();
}

int main(void)
{
	_delay_ms(1500);
	init_log();

	init_pwm();
	init_adc();
	init_tmr();

	led_on();
	debug_setstate(3);

	sei();
	while (1)
		; // sleep_mode();

#if 0
	while (1) {
		PORTA |=  _BV( PA0 );
		_delay_ms(200);
		PORTA &=~ _BV( PA0 );
		_delay_ms(200);
	}
#endif
}