#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/power.h>
#include <util/delay.h>
#include <util/atomic.h>

#include "lights.h"

#define PWM_STEP_SHIFT 2 /* sub-LSB precision */
#define PWM_TOP (((PWM_MAX) + (4 << (PWM_STEP_SHIFT))) >> (PWM_STEP_SHIFT))
#if PWM_TOP > 0x3FF
#error PWM_TOP too high
#endif

volatile unsigned char channels_running;

static uint16_t pwm[N_PWMLEDS];
static volatile unsigned char step;
static unsigned char pll_enabled;

static void enable_pll()
{
	/* Async clock */
	PLLCSR = _BV(PLLE);

	/* Synchronize to the phase lock */
	_delay_us(100);
	while ((PLLCSR & _BV(PLOCK)) == 0)
		;
	PLLCSR |= _BV(PCKE);

	pll_enabled = 1;
}

void init_pwm()
{
	int i;

	step = 0;
	channels_running = 0;
	pll_enabled = 0;

	for (i = 0; i < N_PWMLEDS; i++)
		pwm[i] = 0;

	// PWM channel D is inverted, ...
	TCCR1C = _BV(COM1D1) | _BV(COM1D0) | _BV(PWM1D);
	// PWM channels A and B are not
	TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(PWM1A) | _BV(PWM1B);
	TCCR1D = 0;
	TCCR1B = _BV(CS10); 			// no clock prescaling

	TC1H = PWM_TOP >> 8;
	OCR1C = PWM_TOP & 0xFF;				// TOP value

	TC1H = PWM_TOP >> 8;		// PWM3 is inverted
	OCR1D = PWM_TOP & 0xFF;

	TC1H = 0x00;
	OCR1B = OCR1A = 0;		// initial stride is 0

	DDRB  &= ~(_BV( PB1 ) | _BV( PB3 ) | _BV( PB5 )); // tristate it
	PORTB &= ~(_BV( PB1 ) | _BV( PB3 ) | _BV( PB5 )); // set to zero
}

void susp_pwm()
{
	unsigned char i;

	for (i = 0; i < N_PWMLEDS; i++)
		pwm[i] = 0;

	DDRB &= ~(_BV( PB1 ) | _BV( PB3 ) | _BV( PB5 ));
	TCCR1D = TCCR1C = TCCR1B = TCCR1A = 0;
	TIMSK = 0;
	TIFR = 0;

	PLLCSR &= ~(_BV(PLLE) | _BV(PCKE));
}

void pwm_off(unsigned char n)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		pwm[n] = 0;
		channels_running &= ~(1 << n);

		switch (n) {
		case 0: DDRB &= ~_BV(PB1); break;
		case 1: DDRB &= ~_BV(PB3); break;
		case 2: DDRB &= ~_BV(PB5); break;
		}
	}
}

static void pwm_update_hw(unsigned char n)
{
	unsigned char hi, lo;
	uint16_t stride = (pwm[n] + step) >> PWM_STEP_SHIFT;

	if (n == 2)
		stride = PWM_TOP - stride;

	hi = stride >> 8;
	lo = stride & 0xFF;

	switch (n) {
	case 0:
		TC1H = hi;
		OCR1A = lo;
		break;
	case 1:
		TC1H = hi;
		OCR1B = lo;
		break;
	case 2:
		TC1H = hi;
		OCR1D = lo;
		break;
	}
}

void pwm_set(unsigned char n, uint16_t stride)
{
	if (stride > PWM_MAX)
		stride = PWM_MAX;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		pwm[n] = stride;
		channels_running |= (1 << n);

		if (!pll_enabled) {
			power_timer1_enable();
			enable_pll();
		}

		pwm_update_hw(n);

		switch(n) {
		case 0: DDRB |= _BV(PB1); break;
		case 1: DDRB |= _BV(PB3); break;
		case 2: DDRB |= _BV(PB5); break;
		}
	}
}

void pwm_timer()
{
	unsigned char i;

	if (++step >= (1 << PWM_STEP_SHIFT))
		step = 0;

	for (i = 0; i < N_PWMLEDS; i++)
		if (pwm[i])
			pwm_update_hw(i);
}

void pwm_disable_if_not_needed()
{
	if (channels_running)
		return;

	pll_enabled = 0;
	DDRB &= ~(_BV(PB1) | _BV(PB3) | _BV(PB5));
	PLLCSR &= ~(_BV(PLLE) | _BV(PCKE));

	power_timer1_disable();
}