#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 "calibrate.h"
#include "util.h"
#include "mode.h"
#include "led.h"
#include "zoom.h"
#include "packet.h"

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

uint16_t dac_cmd;

#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 */

#define DEBUG_ISR_TIME

#define ADC_WINDOW_MIN 512
#define ADC_WINDOW_MIN_STEPTO 1280

#define ADC_WINDOW_MAX 1536
#define ADC_WINDOW_MAX_STEPTO 768

/* Run mode */
void TISR_HANDLER(5)
{
	static int count = 0;
	int16_t v;

#ifdef DEBUG_ISR_TIME
	LATAbits.LATA9 = 1;
#endif
	timer_clear_txif(5);
	
	/* Get most recent sample from 12-bit ADC. */
	v = adc_get();

	if (v < ADC_CLAMP_MIN || v >= ADC_CLAMP_MAX)
		possibly_clamped = 1;

	/* Send data to PC */
	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;
	}

#define WINDOW	
#ifdef WINDOW
	/* If ADC value is outside the window, step DAC */
	if (v < ADC_WINDOW_MIN)
		dac_cmd = adc_to_dac(dac_cmd, v, ADC_WINDOW_MIN_STEPTO, g_scale);
	else if (v > ADC_WINDOW_MAX)
		dac_cmd = adc_to_dac(dac_cmd, v, ADC_WINDOW_MAX_STEPTO, g_scale);
#else
	dac_cmd = adc_to_dac(dac_cmd, v, 1024, g_scale);
#endif

	/* Send it out */
	dac_write(dac_cmd);

#ifdef DEBUG_ISR_TIME
	LATAbits.LATA9 = 0;
#endif
}

void run_normal(void)
{
	int i;
	uart1_init(500000);

	led_pattern(0b00110011);

	/* Keep writing zero to the DAC for about 30 seconds after startup,
	   or until we receive a character on the UART */
	while(uart1_can_get())
		uart1_get();
	for (i = 0; i < 1500; i++) {
		dac_write(DAC_MID);
		if (uart1_can_get()) {
			uart1_get();
			break;
		}
		msleep(10);
	}

	led_on();

	/* Assume startup current is 0 */
	msleep(100);
	dac_cmd = do_calibrate();

	timer_setup_16bit(5, TIMER_RATE, 1);
	timer_set_priority(5, 6);

	while(1) {
		if (send_data) {
			/* There's data to send.  Disable the ISR briefly
			   while we grab it */
			uint16_t a, d, o;
			disable_int({
					if (possibly_clamped) {
						/* Mark a possible overflow in the output */
						o = 1;
						possibly_clamped = 0;
					} else o = 0;
					a = send_adc;
					d = send_dac;
					send_data = 0;
				});
			packet_send_adc_dac(a, d, o);
		}
		if (uart1_can_get()) {
			switch (uart1_get()) {
			case 'c':
				disable_int({
						send_data = 0;
						dac_cmd = do_calibrate();
					});
				packet_send_calibration(g_scale);
			default:
				break;
			}
		}
	}
}