#include <avr/io.h>
#include <avr/interrupt.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 > 0x0FF
#error PWM_TOP too high
#endif

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

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

void init_pwm()
{
	int i;

	step = 0;

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

	enable_pll();

	TCCR1 = _BV(CTC1) | _BV(CS10);	// no clock prescaling
	GTCCR = _BV(COM1A1) | _BV(COM1B1) | _BV(PWM1A) | _BV(PWM1B);

	OCR1C = PWM_TOP;
	OCR1A = OCR1B = 0;		// initial stride is 0

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

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

void pwm_off(unsigned char n)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		pwm[n] = 0;

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

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

	switch (n) {
	case 0:
		OCR1A = stride;
		break;
	case 1:
		OCR1B = stride;
		break;
	}
}

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

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		pwm[n] = stride;

		pwm_update_hw(n);

		switch(n) {
		case 0: DDRB |= _BV(PB1); break;
		case 1: DDRB |= _BV(PB4); 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);
}