#include "config.h"
#include "calibrate.h"
#include "util.h"
#include "adc.h"
#include "dac.h"
#include "led.h"
#include "timer.h"
#include "uart.h"

//#define DEBUG_CALIBRATION

float g_scale;  /* delta(DAC) / delta(ADC) */

/* Initialize.  Assume some relatively-safe scaling if no calibration is run */
void calibrate_init(void)
{
	g_scale = (DAC_RANGE / 65536.0);  /* 1 count at 16-bit DAC
                                           * means 1 counts at ADC */
}

/* Given the current DAC and ADC values d1 and a1,
   compute a new DAC value d2 to give the desired ADC value a2 */
uint16_t adc_to_dac(uint16_t d1, int16_t a1, int16_t a2, float scale)
{
	int32_t delta;
	int32_t d2;

	delta = (int32_t)((a2 - a1) * scale + 0.5);
	d2 = d1 + delta;
	return clamp(DAC_MIN, d2, DAC_MAX);
}

/* Calculate a new scale factor given two DAC and ADC points */
float calculate_scale(uint16_t d1, float a1, uint16_t d2, float a2)
{
	float scale;
	float a = a2 - a1;

	/* Correct for known errors */
	d1 = dac_get_actual_float(d1);
	d2 = dac_get_actual_float(d2);

	if (a < 0.1 && a > -0.1)
		scale = 1.0;
	else {
		scale = ((float)d2 - d1) / a;
		if (scale < 0.01)
			scale = 0.01;
		if (scale > 20)
			scale = 20;
	}
	return scale;
}

/* Seek with the DAC to reach a specific ADC value.  Uses g_scale as
   an initial guess for scaling factor, but adjusts it dynamically. */
uint16_t seek(uint16_t starting_dac, int16_t desired_adc)
{
	uint16_t old_dac, dac;
	int16_t old_adc, adc;
	float scale = g_scale;
	int steps = 0;

	dac = starting_dac;

	/* goto current location */
	dac_write(dac);
	msleep(1);
	adc = adc_get();

	while (1)
	{
		/* give up if we're not making progress */
		if (steps++ > SEEK_MAX_STEPS) {
			// 2 flashes, delay, repeat
			led_pattern(0b00101000);
			break;
		}

		old_dac = dac;
		old_adc = adc;

		/* jump to the desired value */
		dac = adc_to_dac(old_dac, old_adc, desired_adc, scale);

		/* write it out */
		dac_write(dac);
		msleep(1);
		adc = adc_get();

#ifdef DEBUG_CALIBRATION
		uart1_put_hex16(dac);
		uart1_put(' ');
		uart1_put_hex16(adc);
		uart1_put(' ');
		uart1_put_hex16(desired_adc);
		uart1_put(' ');
		uart1_put_hex32(*(uint32_t *)&scale);
		uart1_crlf();
#endif

		/* if we're close, accept it */
		if (abs(adc - desired_adc) <= SEEK_FUZZ_ADC) {
			led_pattern(0b11111110);
			break;
		}

		/* otherwise, if we were within ADC clamp limits, and
		   the DAC changed a non-trivial amount, readjust
		   scale factor */
		if (adc > ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MIN &&
		    adc < ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MAX &&
		    abs((int32_t)dac - old_dac) >= SEEK_FUZZ_DAC) {
			scale = calculate_scale(old_dac, old_adc, dac, adc);
		}

		/* if we totally overshot the window, cut the scale in half */
		if ((adc < ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MAX) ||
		    (adc > ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MIN))
		{
			scale *= 0.5;
		}
	}

	return dac;
}

/* Perform calibration */
uint16_t do_calibrate(void)
{
	uint16_t daczero, x1, x2;
	float y1, y2;

	/* Zero ADC */
	daczero = seek((DAC_MIN + DAC_MAX) / 2, CALIBRATE_ADC_ZERO);

	/* Go down take an accurate sample */
	x1 = seek(daczero, CALIBRATE_ADC_LOW);
	y1 = oversample(x1);

	/* Go up and take an accurate sample */
	x2 = seek(daczero, CALIBRATE_ADC_HIGH);
	y2 = oversample(x2);

	/* Calculate scale */
	g_scale = calculate_scale(x1, y1, x2, y2);

#ifdef DEBUG_CALIBRATION
	uart1_put_string("calibrate x1=");
	uart1_put_dec(x1);
	uart1_put_string(" y1=");
	uart1_put_float(y1);
	uart1_put_string(" x2=");
	uart1_put_dec(x2);
	uart1_put_string(" y2=");
	uart1_put_float(y2);
	uart1_put_string(" scale=");
	uart1_put_float(g_scale);
	uart1_crlf();
#endif

	/* Return to zero position */
	dac_write(daczero);

	return daczero;
}

/* Oversample to get a nice ADC value */
float oversample(uint16_t dac)
{
	int i;
	int32_t sum = 0;

	for (i = 0; i < OVERSAMPLE_COUNT; i++) {
		dac_write(dac);
		msleep(1);
		sum += adc_get();
	}

	return (float)sum / (float)OVERSAMPLE_COUNT;
}