#include "config.h"
#include "adc.h"
#include "dac.h"
#include "uart.h"
#include "timer.h"
#include <stdio.h>
#include <math.h>
#include "calibrate.h"
#include "util.h"
#include "led.h"
#include "mode.h"

static uint16_t dac = DAC_MID;

void sweep(void)
{
	int32_t d;
	int16_t a;

#define SWEEP ((DAC_HIGH - DAC_LOW) * (int32_t)2000 / 65535)

	/* sweep range */
	for (d = (int32_t)dac - SWEEP; d < (int32_t)dac + SWEEP; d++) {
		if (d < DAC_LOW) {
			uart1_put_dec(DAC_LOW);
			uart1_put_string(" 0\r\n");
			continue;
		}
		if (d > DAC_HIGH) {
			uart1_put_dec(DAC_HIGH);
			uart1_put_string(" 0\r\n");
			continue;
		}
		dac_write(d);
		msleep(1);
		a = adc_get();
		uart1_put_dec(d);
		uart1_put(' ');
		uart1_put_dec(a);
		uart1_crlf();
	}
}

void run_debug(void)
{
	int16_t adc;
	int32_t v;
	char buf[4];
	uart1_init(115200);

	led_pattern(0b10101010);

	uart1_put_string("Zoom NILM Debug\r\n");

	while (1) {
		dac = dac & DAC_HIGH; // mask off invalid bits
		dac_write(dac);
		uart1_put_hex16(dac);
		uart1_put(' ');
		uart1_put_dec(dac_get_actual_16bit(dac));
		uart1_put(' ');
		uart1_put(' ');
		adc = adc_get();
		uart1_put_hex16(adc);
		uart1_put(' ');
		uart1_put_dec(adc);
		uart1_crlf();
		switch (uart1_get()) {

		// small step
		case '[':
			dac--;
			break;
		case ']':
			dac++;
			break;

		// medium step
		case '-':
			dac -= 16;
			break;
		case '+':
		case '=':
			dac += 16;
			break;

		// big step
		case ',':
		case '<':
			dac -= 1024;
			break;
		case '.':
		case '>':
			dac += 1024;
			break;

		// set DAC to midpoint
		case '0':
			dac = DAC_MID;
			break;

		// set DAC to specified hex value
		case 'v':
		case 'V':
			buf[0] = uart1_get();
			buf[1] = uart1_get();
			buf[2] = uart1_get();
			buf[3] = uart1_get();
			v = hex_to_u16(buf);
			if (v < 0)
				uart1_put_string("bad value\r\n");
			else
				dac = v;
			break;

		// maintain ADC input at zero
		case 'z':
		case 'Z':
			uart1_put_string("zeroing input...\r\n");
			while (!uart1_can_get())
				dac = seek(dac, 1024);
			uart1_get();
			break;

		// test seeking
		case '1':
			uart1_put_string("seek 512\r\n");
			dac = seek(dac, 512);
			break;

		case '2':
			uart1_put_string("seek 1536\r\n");
			dac = seek(dac, 1536);
			break;

		// run calibration
		case 'c':
		case 'C':
			uart1_put_string("calibrating...\r\n");
			dac = do_calibrate();
			uart1_put_string("new g_scale ");
			uart1_put_float(g_scale);
			uart1_crlf();
			break;

		// sweep DAC
		case 's':
		case 'S':
			uart1_put_string("sweep around ");
			uart1_put_dec(dac);
			uart1_crlf();
			sweep();
			break;
		// dump raw ADC value
 	       	case 'r':
        	case 'R':
			while(!uart1_can_get()) {
				uart1_put_hex16(adc_get_raw());
				uart1_crlf();
			}
			uart1_get();
			break;
		}
	}
}