#include "ir_sensor.h"

#define NUM_SAMPLES 64
#define ACCUM_DIVISOR 1
#define SAMPLE_DELAY 8000
#define TIME_BETWEEN_SAMPLE_BLOCKS 200 // number of SAMPLE_DELAY time intervals to wait between samples

// Possible sensor states.
typedef enum {
	IR_NOT_STARTED,
	IR_SAMPLING,
	IR_FINISHED
} IR_SENSOR_STATE;

// Current sensor state.
static IR_SENSOR_STATE state;

// Current sample.
static uint8_t current_sample;

// Current sensor being sampled.
static uint8_t current_sensor;

// Accumulator for sensor value during sampling.
static long acc;

// Most recent values of sensors.
static uint16_t sensor_values[NUM_SENSORS];

// Bitfield of the sensors to sample.
static uint8_t sensors_to_sample;

// Bitfield of the sensors that have been sampled but not transmitted.
static uint8_t sensors_recently_read;

// Amount of time to wait until taking the next block of samples.
static uint16_t wait_time;

void stop_timer() {
	TA2CTL &= ~0x30;
}

void start_timer() {
	stop_timer();
	TA2CTL |= 0x10; // Start the timer.  However, it won't really do anything...
}

/**
 * IR sensor sample bitmask.
 */
uint8_t ir_sensor_sample_status() { return sensors_recently_read; }

/**
 * Initialize the IR sensor module.
 */
void ir_sensor_init() {
	// Initialize the ADC10 pins on the blimp
	P6SEL |= BIT0; // Only need SEL pins	
	
	// Now initialize IR decoder pins and enable pin
	P2DIR |= BIT0;
	P1DIR |= BIT6;
	PJDIR |= BIT0 + BIT1;

	// Initialize settings for ADC10
	ADC10CTL0 |= ADC10SHT_3 + ADC10ON;			// Turn on ADC10 and configure to 16
												// cycles for sampling period
	ADC10CTL1 |= ADC10SHP + ADC10CONSEQ_0 + ADC10SSEL_0 + ADC10DIV_0;		
												// Use SMCLK, s/w Trigger, and
												// use single-conversion mode
	ADC10CTL2 |= ADC10RES;						// 10 bit sample
	ADC10MCTL0 |= ADC10SREF_0 + ADC10INCH_0;	// A0 sample and AVCC/AVSS ref
	ADC10IE |= ADC10IE0;  

	// Set up the timer
	TA2CTL |= TASSEL_2 | MC_0 | TACLR;
	TA2CCTL0 = CCIE;
	TA2CCR0 = SAMPLE_DELAY;
	
	state = IR_NOT_STARTED;
	
	sensors_recently_read = 0;
	sensors_to_sample = 0;
	wait_time = 0;
	current_sensor = 0;
}

/**
 * Choose an infrared LED to turn on.
 */
void ir_sensor_enable(uint8_t sensor_index) {
	if (sensor_index & 1) { P1OUT |= BIT6; }
	else { P1OUT &= ~BIT6; }
	if (sensor_index & 2) { PJOUT |= BIT0; }
	else { PJOUT &= ~BIT0; }
	if (sensor_index & 4) { PJOUT |= BIT1; }
	else { PJOUT &= ~BIT1; }
}

/**
 * Start the regular IR sensor sampling + update loop.
 * 
 * desired_sensors is a bitfield indicating which sensors to sample.
 */
void ir_sensor_start_sampling(uint8_t desired_sensors) {
	if (sensors_to_sample == 0 && desired_sensors != 0) {
		wait_time = 0;
		start_timer();
	}
	sensors_to_sample = desired_sensors;
}

/**
 * Start a sample of a single sensor.
 */
void ir_sensor_start_sample(uint8_t sensor_index) {
	// Enable the light.
	ir_sensor_enable(sensor_index);
	P2OUT &= ~BIT0;
	current_sensor = sensor_index;
	
	// Initialize counters
	acc = 0;
	current_sample = 0;
	state = IR_SAMPLING;
	
	// Start the timer
	start_timer();
}

/**
 * Returns true if the sensor is sampling.
 */
bool ir_sensor_is_sampling() {
	return state != IR_NOT_STARTED && state != IR_FINISHED;
}

/**
 * Returns true if the sensor has a sample for the given sensor.
 */
bool ir_sensor_has_sample(uint8_t sensor_index) {
	return sensors_recently_read & (1 << sensor_index);
}

/**
 * After you call this method, ir_sensor_has_sample() will return
 * false until the next sample is read for the given sensor.
 */
void ir_sensor_watch(uint8_t sensor_index) {
	sensors_recently_read &= ~(1 << sensor_index);
}

/**
 * Returns the most recently read sensor value.
 * 
 * After you call this method, ir_sensor_has_sample() will return
 * false until the next sample is read.
 */
uint16_t ir_sensor_get_last_value(uint8_t sensor_index) {
	return sensor_values[sensor_index];
}

/**
 * Finds next sensor to sample, or makes the timer continue to wait
 * if there is no next sensor.
 */
void ir_sensor_start_next_sample() {
	while (current_sensor < NUM_SENSORS) {
		if ((sensors_to_sample & (1 << current_sensor)) != 0) {
			// Start sampling this next sensor!
			// TODO: dedup the current_sensor assignment.
			ir_sensor_start_sample(current_sensor);
			return;
		}
		current_sensor++;
	}
	
	// Otherwise, wait until next sampling interval
	current_sensor = 0;
	wait_time = TIME_BETWEEN_SAMPLE_BLOCKS;
	start_timer();
}

#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR (void)
{
	// Add sample
	if (P2OUT & BIT0) { acc -= ADC10MEM0; }
	else { acc += ADC10MEM0; }
	
	// Start next sample
	current_sample++;
	if (current_sample < NUM_SAMPLES) {
		// Toggle the LED
		P2OUT ^= BIT0;

		// Start the timer
		start_timer();
	// Or, end sampling.
	} else {
		// Turn off infrared LED to save power.
		P2OUT &= ~BIT0;
		
		// Store the sensor value
		sensor_values[current_sensor] = acc / ACCUM_DIVISOR;
		
		// Mark the sensor as read
		sensors_recently_read |= (1 << current_sensor);
		
		state = IR_FINISHED; // TODO: set once we're done with all samples
		current_sensor++; // TODO: do this in ir_sensor_next_sample()
		
		// Find the next sensor to sample
		ir_sensor_start_next_sample();
	}
}

/**
 * Timer interrupt to start the ADC sample and conversion.
 */
#pragma vector=TIMER2_A0_VECTOR
__interrupt void TIMER2_A0_ISR(void)
{
	if (wait_time > 0) {
		wait_time--;
	} else {
		if (state == IR_SAMPLING) {
			// Turn off the timer
			stop_timer();
			
			// Start sampling and conversion
			ADC10CTL0 |= ADC10ENC;
			ADC10CTL0 |= ADC10SC;
		} else {
			// Start sampling
			ir_sensor_start_next_sample();
		}
	}
}