#include <avr/io.h>

#include "lights.h"

static uint16_t target;
static uint16_t pwm_val;
static int16_t err_sum;
static unsigned char state;
unsigned char mode_changed;

#define SENSE_MOHM	3000	/* 1 Ohm */
/*
 * Voltage in uV at ADC reading == 1 is 1100/gain/1024
 * ADC module returns sum of 1 << PWMLED_ADC_SHIFT measurements
 * Voltage in uV measured is current in mA * sense resistance in mOhm
 */
#define MA_TO_ADC(ma) ((uint16_t) \
	((uint32_t)(ma) \
	* (SENSE_MOHM) \
	* (1 << (PWMLED_ADC_SHIFT)) \
	* 1024 \
	/ 1100000))

static uint16_t adc_max = MA_TO_ADC(30);

static uint16_t targets[N_PWMLED_MODES] = {
	MA_TO_ADC( 2),
	MA_TO_ADC(10),
	MA_TO_ADC(20),
};

#define ST_DISABLED 0
#define ST_OFF      1
#define ST_PROBING  2
#define ST_ON       3
// The above are constructed so that the following work:
#define ST_IS_ON(s)	((s) & 0x02)
#define ST_CAN_SET_MODE(s)	((s) & 0x01)

void init_pwmled()
{
	pwm_val = 0;
	err_sum = 0;
	target = targets[0];
	state = ST_OFF;
}

void pwmled_set_target(unsigned char mode)
{
	target = targets[mode];
	mode_changed = 1;
}

void pwmled_on_off(unsigned char mode)
{
	if (!ST_CAN_SET_MODE(state))
		return;

	if (mode) {
		state = ST_ON;
		mode_changed = 1;
		need_pwmled_adc = 1;
		pwm_set(pwm_val);
	} else {
		state = ST_OFF;
		need_pwmled_adc = 0;
		pwm_off();
	}
}

static inline void pwmled_err()
{
	state = ST_DISABLED;
	pwm_off();

	log_byte(0xF1);
	log_flush();

	set_error(ERR_PWMLED);
}


void pwmled_adc(uint16_t adcval)
{
	int32_t sum;
	unsigned char shift;

	if (!ST_IS_ON(state))
		return;

	// skip the first reading after mode change
	if (state == ST_ON && mode_changed) {
		mode_changed--;
		return;
	}

	if (adcval > adc_max) {
		pwmled_err();
		return;
	}

	shift = 5;

	sum = ((int32_t)pwm_val << shift)
		+ err_sum + target - adcval;

	if (sum < 0)
		sum = 0;

	pwm_val = sum >> shift;
	sum -= pwm_val << shift;
	err_sum = sum;

	if (pwm_val >= PWM_MAX
		|| (pwm_val > (2*PWM_MAX/3) && adcval < 0x08)) {
		pwmled_err();
		need_pwmled_adc = 0;
		return;
	}

	pwm_set(pwm_val);
}