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

int send_data = 0;
uint16_t send_adc, send_dac;

/* DAC limits */
#define DAC_MIN 0x0000
#define DAC_MAX 0xffff

/* A "step" in the DAC output at the resolution we want. */
#define DAC_STEP 0x0004

/* 1 bit at the ADC is this much change at the DAC */
#define ADC_DAC_SCALING (1.0/21.234)

uint16_t dac_cmd = ((uint32_t)DAC_MAX + DAC_MIN) / 2;

#define TIMER_RATE 8000L
void TISR_HANDLER(5)
{
	static int count = 0;
	uint16_t v;
	int32_t adjustment;
	int32_t newdac;

	timer_clear_txif(5);
	
	/* Get most recent sample from 12-bit ADC. */
	v = adc_get();

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

	/* Update DAC.  Add whatever is necessary to cause the ADC
	   to change by 150% towards the center of n0x0800
       (intentional overshoot to increase slew rate capability) */
	adjustment = ((int32_t) v - 0x0800) * 1.50 * ADC_DAC_SCALING;
	newdac = dac_cmd + adjustment;
	if (1 || adjustment) {
		if (newdac < DAC_MIN)
			dac_cmd = DAC_MIN;
		else if (newdac > DAC_MAX)
			dac_cmd = DAC_MAX;
		else
			dac_cmd = newdac;
		dac_write(dac_cmd);
	}
}

void TISR_HANDLER(6)
{
	uint16_t v;
	timer_clear_txif(6);
	v = adc_get();
	send_adc = v;
	send_data = 1;
}

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

void run_debug(void)
{
	uint16_t dac = 32768;
	uart1_init(115200);	

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

	while (1) {
		dac_write(dac);
		uart1_put_dec(dac);
	    uart1_put(' ');
		uart1_put_hex16(adc_get());
		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 'g':
			/* change baudrate */
			/* read ADC at 8KHz */	
			/* send DAC and ADC values to PC */
			uart1_init(500000);
			timer_setup_16bit(6, 8000, 1);
			send_dac = dac;
			for (;;) {
				if (send_data) {
					send_to_pc();
					send_data = 0;
				}
			}
			break;
		}
	}
}

void run_normal(void)
{
	uart1_init(500000);
//	uart1_init(115200);
//	uart1_put_string("Zoom NILM Normal\r\n");
	timer_setup_16bit(5, TIMER_RATE, 1);
//	uart1_put_string("Timer OK\r\n");

	while(1) {
		if (send_data) {
			send_to_pc();
			send_data = 0;
		}
	}
}

int main(void)
{
	config_init();

	/* Pull PGD high internally. */
	TRISCbits.TRISC13 = 1;
	CNPU1bits.CN1PUE = 1;

	adcext_init();
	dac_init();
	adc_init();

	/* If PGD is externally tied to ground, run in debug mode.
	   (Short ICD pins 3 and 4) */
	if (PORTCbits.RC13 == 0)
		run_debug();
	else
		run_normal();

	for (;;)
		continue;
}
