//*******************************************************************************
// 
//******************************************************************************

#include "includes.h"
#include "fans.h"
#include "leds.h"
#include "ir_sensor.h"
#include "uart.h"
#include "power_monitor.h"

static void init();
static void fll_init();
static void process_command();

// True if the blimp is stopped.  All commands except Start are ignored.
static bool stopped;

void main(void)
{ 
	// Call initialize functions
	init();
	fll_init();
	fan_init();
	uart_init();
	led_init();
	ir_sensor_init();
	P5OUT &= ~BIT1;
	pmm_init();
	__bis_SR_register(GIE);
	
	// Enable sensors
	ir_sensor_start_sampling(0x1F);
	
	stopped = false;
	
	// Command processing loop
	while (1) {
		process_command();
		
		uint8_t status = ir_sensor_sample_status();
		
		// 0x1F = 00011111, meaning all five sensors have read values.
		if (status == 0x1F) {
			// Calculate space needed to send sensor values
			uint8_t i, space_needed = 1;
			for (i = 0; i < NUM_SENSORS; i++) {
				if (status & (1 << i)) {
					space_needed += 2;
				}
			}
			
			if (space_needed <= uart_send_space_left()) {
				// Write out the header
				uart_write((1 << 6) | status); 
				
				// Check for infrared results
				for (i = 0; i < NUM_SENSORS; i++) {
					if (status & (1 << i)) {
						uint16_t reading = ir_sensor_get_last_value(i);
						
						// Mark this sensor as unread, now that we've read it.
						ir_sensor_watch(i);
						
						// Write the values.
						uart_write(reading);
						uart_write(reading >> 8);
					}
				}
				
				uart_finish();
			}
		}
	}
}

/**
 * Processes a single command from the UART.
 */
void process_command() {
	if (!uart_has_data()) { return; }
	uint8_t command = uart_read();
	
	if (stopped) {
		if (command == 1) {
			stopped = false;
		}
	} else {
		if (command >> 6 == 0) {
			if (command == 0) {
				// Stop
				stopped = true;
				fan_stop_all();
				ir_sensor_start_sampling(0);
			} else if (command >> 5 == 1) {
				// Read infrared sensors
				ir_sensor_start_sampling(command & 0x1F);
			} else if (command >> 3 == 1) {
				/*
				// Set the LED based on the command.
				LEDS led = (command & 4) ? LED_RED : LED_GREEN;
				/*
				if (command & 1) {
					led_toggle(led);
				} else {
					led_set(led, (command & 2) ? LED_ON : LED_OFF);
				}
				*/
			}
		} else {
			// Fan command
			int8_t speed = (int8_t)(command << 2);
			FANS fan = (FANS)((command >> 6) - 1);
			set_fan(fan, speed);
		}
	}
}

static void init(){
	WDTCTL = WDTPW + WDTHOLD;          			// Stop WDT
	
	PMAPPWD = 0x02D52;                        // Enable Write-access to modify port mapping registers
		// Map UART to appropriate pins
		P4MAP4 = PM_UCA1TXD;
		P4MAP5 = PM_UCA1RXD;
	PMAPPWD = 0;                              // Disable Write-Access to modify port mapping registers	

	// Initialize the motor outputs
	P1DIR |= BIT0+BIT1+BIT2+BIT3+BIT4+BIT5;	// P1.0 to P1.5 output
	P1SEL |= BIT2+BIT3+BIT4+BIT5;             // P1.2 and P1.3 options select

	// Initialize the UART modules
	P4SEL |= BIT4 + BIT5;  
	
	// Initialize the LED light
	P5DIR |= BIT1;
	P5OUT |= BIT1;
}

static void fll_init(){
 	UCSCTL3 = SELREF_2;                     // Set DCO FLL reference = REFO
	UCSCTL4 |= SELA_2;               		// Set ACLK = REFO
	UCSCTL0 = 0x0000;                       // Set lowest possible DCOx, MODx	
	
	// Make sure XT1, XT2, & DCO stabilizes
	do
	{
	    UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + DCOFFG);
	                                            // Clear XT2,XT1,DCO fault flags
	    SFRIFG1 &= ~OFIFG;                      // Clear fault flags		
	} while(SFRIFG1&OFIFG);                   // Test oscillator fault flag
	
	__bis_SR_register(SCG0);                  // Disable the FLL control loop
	UCSCTL1 = DCORSEL_5;                      // Select DCO range 16MHz operation
	UCSCTL2 |= 255;                           // Set DCO Multiplier for 8MHz
		
	__bic_SR_register(SCG0);                  // Enable the FLL control loop
	
	// Worst-case settling time for the DCO when the DCO range bits have been
	// changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
	// UG for optimization.
	// 32 x 32 x 8 MHz / 32,768 Hz = 250000 = MCLK cycles for DCO to settle
	__delay_cycles(250000);
}