#include <avr/io.h>

#include "lights.h"

typedef struct {
	uint16_t target, pwm;
	int16_t err_sum;
	unsigned char mode, state, probe_steps;
	uint16_t mode_pwm[N_PWMLED_MODES];
} pwmled_t;

pwmled_t pwmleds[N_PWMLEDS];

#define PWMLED2_TESTING_WITH_350MA_LED

#define SENSE_MOHM	33	/* 0.033 Ohm */
#define MA_MOHM_GAIN_TO_ADC(ma, mohm, gain) (\
	((unsigned long)(ma))*(mohm) /* voltage at sensing resistor in uV */ \
	/(1100000UL/gain/1024UL)     /* voltage of ADC reading == 1 */ \
)

static uint16_t adc_max[N_PWMLEDS] = {
	MA_MOHM_GAIN_TO_ADC( 400, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC(  30, SENSE_MOHM, 20),
#ifdef PWMLED2_TESTING_WITH_350MA_LED
	MA_MOHM_GAIN_TO_ADC( 400, SENSE_MOHM,  1)
#else
	MA_MOHM_GAIN_TO_ADC(2500, SENSE_MOHM,  1)
#endif
};

static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = {
	/* pwmled0 */
	MA_MOHM_GAIN_TO_ADC(  20, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC(  50, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM, 20),
	/* pwmled1 */
	16, 32, 64, 112,
#if 0
	MA_MOHM_GAIN_TO_ADC(   5, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC(  12, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC(  16, SENSE_MOHM, 20),
	MA_MOHM_GAIN_TO_ADC(  20, SENSE_MOHM, 20),
#endif
	/* pwmled2 */
	24, 32, 40, 48
#if 0
#ifdef PWMLED2_TESTING_WITH_350MA_LED
	MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC( 140, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC( 250, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM,  1),
#else
	MA_MOHM_GAIN_TO_ADC( 150, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC( 700, SENSE_MOHM,  1),
	MA_MOHM_GAIN_TO_ADC(2400, SENSE_MOHM,  1),
#endif
#endif
};

#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 pwmled_init()
{
	unsigned char i, j;

	for (i = 0; i < N_PWMLEDS; i++) {
		pwmled_t *led = pwmleds + i;
		led->err_sum = 0;
		led->target = adc_vals[i*N_PWMLED_MODES];
		led->pwm = 0;
		led->mode = 1;
		led->state = ST_PROBING;
		led->probe_steps = 0;

		for (j = 0; j < N_PWMLED_MODES; j++)
			led->mode_pwm[j] = 0;
	}
}

void pwmled_set_mode(unsigned char n, unsigned char mode)
{
	pwmled_t *led = pwmleds + n;

	if (!ST_CAN_SET_MODE(led->state))
		return;

	if (led->mode) // save the previous state
		led->mode_pwm[led->mode - 1] = led->pwm;

	if (n == 1)
		log_byte(mode);

	led->mode = mode;

	if (mode > 0 && mode <= N_PWMLED_MODES) {
		led->target = adc_vals[n*N_PWMLED_MODES + mode - 1];
		led->state = ST_ON;
		led->pwm = led->mode_pwm[mode - 1];
		led->err_sum = 0;
		pwm_set(n, led->pwm);
	} else {
		led->state = ST_OFF;
		pwm_off(n);
	}
}

void pwmled_adc(unsigned char n, uint16_t adcval)
{
	pwmled_t *led = pwmleds + n;
	int16_t sum, pwm_div;
	uint16_t old_pwm;

	if (!ST_IS_ON(led->state))
		return;

	// FIXME: test for maximum adcval value (adc_max[n])

	sum = led->err_sum += (int16_t)(led->target) - (int16_t)adcval;

	// FIXME: try to work faster during probing
	pwm_div = 64; //led->state == ST_PROBING ? (1 << 5) : (1 << 8);
	old_pwm = led->pwm;

	if (sum >= pwm_div) {
		uint16_t diff = sum/pwm_div;
		led->pwm += diff;
		led->err_sum -= diff * pwm_div;

	} else if (sum <= -pwm_div) {
		uint16_t diff = (-sum)/pwm_div;

		if (led->pwm >= diff)
			led->pwm -= (int16_t)diff;
		else
			led->pwm = 0;
		led->err_sum += diff * pwm_div;
	}

	if (led->state == ST_PROBING) {
		if (led->pwm == old_pwm) {
			if (led->probe_steps < 10)
				led->probe_steps++;
		} else {
			led->probe_steps = 0;
		}

		if (led->probe_steps >= 10 || old_pwm > led->pwm) {

			led->mode_pwm[led->mode - 1] = led->pwm;

			if (led->mode < N_PWMLED_MODES) {
				led->probe_steps = 0;
				led->err_sum = 0;

				led->mode++;
				led->target = adc_vals[n*N_PWMLED_MODES+led->mode-1];
			} else {
				unsigned char i;

				led->state = ST_OFF;

				log_byte(0xF0);
				log_byte(n);
				log_word(jiffies);

				for (i = 0; i < N_PWMLED_MODES; i++)
					log_word(led->mode_pwm[i]);

				log_flush();
			}
		}
	}

	if (led->pwm == old_pwm)
		return;

	if (led->pwm > (PWM_MAX << PWM_STEP_SHIFT)/2) {
		pwm_off(n);
		led->state = ST_DISABLED;
		log_byte(0xF1);
		log_byte(n);
		log_word(jiffies);
		log_flush();
	}

	pwm_set(n, led->pwm);
}