#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;

/* 128 KHz, after each ADC sample */
void fast_callback(void)
{
	/* Run at 64 KHz */
	static int count = 0;
	if (count++ < 1) return;
	count = 0;

	/* Get most recent sample. */
	uint16_t v = adc_dmabuf[adc_offset];

#if 1
	/* Update DAC.  Add whatever is necessary to cause the ADC
	   to change by 150% towards the center of 0x0800
       (intentional overshoot to increase slew rate capability) */
	{
		int32_t adjustment = ((int32_t) v - 0x0800) * 1.50 * ADC_DAC_SCALING;
		int32_t newdac = dac_cmd + adjustment;
		//if (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);
		//}
	}
#endif
#if 0
	/* Update DAC if necessary */
	if (v < ADC_BIGMIN && dac_cmd < (DAC_MAX - DAC_BIGSTEP))
		dac_cmd += DAC_BIGSTEP;
	else if (v < ADC_MIN && dac_cmd < (DAC_MAX - DAC_STEP))
		dac_cmd += DAC_STEP;
	else if (v > ADC_BIGMAX && dac_cmd > (DAC_MIN + DAC_BIGSTEP))
		dac_cmd -= DAC_BIGSTEP;
	else if (v > ADC_MAX && dac_cmd > (DAC_MIN + DAC_STEP))
		dac_cmd -= DAC_STEP;
	else
		return;
	dac_write(dac_cmd);
#endif
}

/* 8 KHz, after DMA buffer filled */
void slow_callback(void)
{
	/* Get most recent sample. */
	uint16_t v = adc_dmabuf[adc_offset];

	/* Send most recent sample to PC */
	send_adc = v;
	send_dac = dac_cmd;
	send_data = 1;
}

void debug_callback(void)
{
	send_adc = adc_dmabuf[adc_offset];
}

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));
}

int main(void)
{
	int debug = 0;
	int i;
	
	config_init();
	adcext_init();
	dac_init();
	adc_init();

	if (debug) {
		uart1_init(115200);	
		adc_dma_callback = debug_callback;

		uint16_t dac = 32768;
		while (1) {
			dac_write(dac);
			uart1_put_dec(dac);
	        uart1_put(' ');
			uart1_put_dec(send_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;
			}
		}
	} else {
		uart1_init(500000);

		adc_adc_callback = fast_callback;
		adc_dma_callback = slow_callback;

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