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

int send_data = 0;
uint16_t send_adc;
uint16_t send_dac;
int possible_overflow = 0;

#define DAC_MIN 0x0000
#define DAC_MAX 0xFFFF
float dac_current;

float dac_current_min;
float dac_current_max;

#define TIMER_RATE 8000   /* how often to read the ADC and update DAC */
#define PC_RATE 8000      /* how often to send data to the PC */

void TISR_HANDLER(5)
{
	static int count = 0;
	static uint16_t dac_cmd;
	uint16_t v;
	float i;

	/* toggle A9 every time an interrupt occurs */
	LATAbits.LATA9 = 1;
	timer_clear_txif(5);
	
	/* Get most recent sample from 12-bit ADC. */
	v = adc_get();

	if (v < 0 || v >= 2047)
		possible_overflow = 1;

	/* Send data to PC at 1 Hz */
	if (++count >= (TIMER_RATE / PC_RATE)) {
		count = 0;
		
		/* Send most recent sample and old DAC value */
		send_adc = v;
		send_dac = dac_cmd;
		send_data = 1;
	}

	/* Convert ADC value to current */
	i = adc12_to_current(v);

	/* Adjust DAC to null this current to 0 */
	dac_current -= i;

	if (dac_current < dac_current_min)
		dac_current = dac_current_min;
	if (dac_current > dac_current_max)
		dac_current = dac_current_max;

	/* Now send it out */
	dac_cmd = current_to_dac(dac_current);
	dac_write(dac_cmd);

	LATAbits.LATA9 = 0;
}

void send_to_pc(uint16_t adc, uint16_t dac)
{
	/* Sent data format:
  	   1Aaa aaaa  0aaa aaDd  0ddd dddd 0ddd dddd
	   Aaaaaaaaaaaa = 12-bit ADC value (2s compliment signed)
       Dddddddddddddddd = 16-bit DAC command (unsigned)
	*/	
	uart1_put(0x80 | ((adc & 0x0FE0) >> 5));
	uart1_put(((adc & 0x001F) << 2) | ((dac & 0xC000) >> 14));
	uart1_put((dac & 0x3F80) >> 7);
	uart1_put((dac & 0x007F));
}

void degauss(void)
{
	dac_write(65535); /* max */
	msleep(25);
	dac_write(0);     /* min */
	msleep(25);
	dac_write(32768); /* middle */
	msleep(25);
}

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

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

	while (1) {
		dac_write(dac);
		uart1_put_hex16(dac);
		uart1_put(' ');
		uart1_put_dec(dac);
		uart1_put(' ');
		uart1_put(' ');
		adc = adc_get();
		uart1_put_hex16(adc);
		uart1_put(' ');
		uart1_put_dec(adc);
		uart1_crlf();
		switch (uart1_get()) {
		case '[':
			dac--;
			break;
		case ']':
			dac++;
			break;
		case '-':
			dac -= 16;
			break;
		case '+':
		case '=':
			dac += 16;
			break;
		case ',':
		case '<':
			dac -= 1024;
			break;
		case '.':
		case '>':
			dac += 1024;
			break;
		case '0':
			dac = 32768;
			break;
		case 'd':
			uart1_put_string("degauss...");
			degauss();
			uart1_crlf();
			dac = 32768;
			break;
		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;
		}
	}
}

void run_normal(void)
{
	uart1_init(500000);

	/* Assume startup current is 0 */
	dac_current = 0.0;
	dac_write(current_to_dac(dac_current));
	msleep(100);
	degauss();

	/* Set min/max */
	dac_current_min = dac_to_current(DAC_MIN);
	dac_current_max = dac_to_current(DAC_MAX);

	timer_setup_16bit(5, TIMER_RATE, 1);

	while(1) {
		if (send_data) {
			/* There's data to send.  Disable the ISR briefly
			   while we grab it */
			uint16_t a, d;
			disable_int({
				if (possible_overflow) {
					/* Mark a possible overflow in the output
					   by setting ADC to an unlikely value */
					possible_overflow = 0;
					a = 0x0800;
				} else {
					a = send_adc;
				}
				d = send_dac;
				send_data = 0;
			});
			send_to_pc(a, d);
		}
	}
}

/* This mode is used when the programmer is connected,
   or PGD is tied to ground (jumper between ICD pins 3 and 4) */
#define MODE_1 run_debug

/* This mode is used when the programmer is not connected
   and PGD is left floating */
#define MODE_2 run_normal

int main(void)
{
	config_init();
	led_init();

	led_on();
	/* Detect jumper on B10/B11 */
	//TRISCbits.TRISC13 = 1;
	//CNPU1bits.CN1PUE = 1;

	adcext_init();
	dac_init();
	dac_write(32768);
	adc_init();

	led_off();
	msleep(100);
	led_on();
	
	degauss();

	/* If PGD is externally tied to ground, use MODE_1
	   (Short ICD pins 3 and 4) */
//	if (PORTCbits.RC13 == 0)
		MODE_1();
//	else
//		MODE_2();

	for (;;)
		continue;
}
