#include <msp430.h>
#include <msp430xgeneric.h>
#include <stdio.h>
#include <string.h>

#include "mw_main.h"

#include "mw_uart.h"
#include "mw_lcd.h"
#include "mw_bt.h"
#include "mw_adc.h"
#include "mw_bt.h"
#include "mw_acc.h"
#include "bt_hci.h"
#include "bt_l2cap.h"

#include "oswald_main.h"
#include "oswald_hal.h"

#include "bluetooth_init_cc256x.h"

uint16_t _event_src = 0;


static void set16mhz(void)
{
	UCSCTL0 = 0x00;			// Set lowest possible DCOx, MODx
	UCSCTL1 = DCORSEL_5;		// Select suitable range
	UCSCTL2 = 488 + FLLD_1;		// Set DCO Multiplier
	UCSCTL4 = SELA__XT1CLK | SELS__DCOCLKDIV  |  SELM__DCOCLKDIV ;

	// Worst-case settling time for the DCO when the DCO range bits have been 
	// changed is n x 32 x 32 x f_FLL_reference. See UCS chapter in 5xx UG 
	// for optimization.
	// 32 x 32 x / f_FLL_reference (32,768 Hz) = .03125 = t_DCO_settle
	// t_DCO_settle / (1 / 18 MHz) = 562500 = counts_DCO_settle
  
	// __delay_cycles(562500);  
	int i;
	for (i=0;i<10;i++){
		__delay_cycles(56250);  
	}
}

static void setup_clocks(void)
{
	unsigned long i;

	/* use external oscillator */
	P7SEL |= BIT0 + BIT1;
	if ((UCSCTL6 & XT1DRIVE_3) != XT1DRIVE_3) {
		UCSCTL6_L |= XT1DRIVE1_L + XT1DRIVE0_L;
	}
	i = 50000;
	while ((SFRIFG1 & OFIFG) && i--) {
		UCSCTL7 &= ~(DCOFFG + XT1LFOFFG + XT1HFOFFG + XT2OFFG);
		SFRIFG1 &= ~OFIFG;
	}
	UCSCTL6 = (UCSCTL6 & ~(XT1DRIVE_3)) |(XT1DRIVE_0);

	set16mhz();

	UCSCTL8 |= SMCLKREQEN;

	/* enable oscillator fault NMI IRQ */
//	SFRIE1 = OFIE;
}

#if 0
#pragma vector=PWR_PORT_VECTOR
__interrupt void PWR_ISR (void)
{
	/* clear all possible sources */
	PWR_PORT_IFG &= ~(BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2 | BAT_CHARGE_PWR_BIT);
	_event_src |= POWER_SRC_EVENT;
	LPM3_EXIT();
	nop();
}
#endif

static void mw_init_vibrate_pwm(void)
{
#ifdef MW_DIGITAL_V2
	// Start with P7.3 as an output
	P7OUT &= ~BIT3;   // Low when a digital output
	P7SEL &= ~BIT3;   // P7 option select = false
	P7DIR |=  BIT3;   // P7 outputs

	TA1CTL = 0;

	// No expansion divide
	TA1EX0 = 0;

	// do a PWM with 64 total steps.  This gives a count up of 32 and
	// a count down of 32
	TA1CCR0 =  31;

	// Compare channel 2 is used as output
	TA1CCTL2 = OUTMOD_6;         // PWM output mode: 6 - toggle/set
	TA1CCR2 = 10;                // 10 is a 2/3 duty cycle
#endif
}


static void setup_pins(void)
{
	CONFIG_SRAM_PINS();
	CONFIGURE_BUTTON_PINS();
#ifdef MW_DEVBOARD_V2
	CONFIG_LED_PINS();		// debug LEDs on devboard
#endif
	DISABLE_LCD_LED();		// frontlight
	CONFIG_DEBUG_PINS();
	CONFIG_ACCELEROMETER_PINS();
	DISABLE_ACCELEROMETER_POWER(); // starts from config 5 and later

	HARDWARE_CFG_SENSE_INIT();

	APPLE_CONFIG();
	APPLE_POWER_DISABLE();

	CONFIG_BT_PINS();
	BT_CLK_REQ_CONFIG_AS_OUTPUT_LOW();
	BT_IO1_CONFIG_AS_OUTPUT_LOW();
	BT_IO2_CONFIG_AS_OUTPUT_LOW();
	mw_disable_bt();

	LIGHT_SENSE_INIT();
	LIGHT_SENSOR_SHUTDOWN();

	BATTERY_SENSE_INIT();
	BATTERY_SENSE_DISABLE();
	BAT_CHARGE_DIR &= ~(BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2 | BAT_CHARGE_PWR_BIT);
	BAT_CHARGE_OUT |= BAT_CHARGE_PWR_BIT | BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2;						// pull-up
	BAT_CHARGE_REN |= BAT_CHARGE_PWR_BIT;	// enable resistors
	// BAT_CHARGE_IE |= BAT_CHARGE_PWR_BIT;
	BAT_CHARGE_OUT |= BAT_CHARGE_ENABLE_PIN;	// !CE, negative logic
	BAT_CHARGE_DIR |= BAT_CHARGE_ENABLE_PIN;

	/* disable reset function, enable NMI, do not enable NMI IRQ */
	/* to avoid accidential reset on charger clip connect */
#ifndef MW_DEVBOARD_V2 // but only on real watch
	SFRRPCR &= ~SYSRSTRE;
	SFRRPCR |= SYSNMI;
#endif
}

#pragma vector=WDT_VECTOR
__interrupt void WDT_ISR (void)
{
	/* eventually we will do something here, not for now */
	_event_src |= WATCHDOG_EVENT;
	nop();
}

static void setup_wdt(void)
{
#if 1
	WDTCTL = WDTPW + WDTHOLD;	// disable watchdog
#else
	WDTCTL = WDT_ADLY_1000;		// 1 second timeout
	SFRIE1 |= WDTIE;		// Enable WDT interrupt
#endif
}

#if 1
#pragma vector=UNMI_VECTOR
__interrupt void NMI_ISR (void)
{
#if defined MW_DEVBOARD_V2
	LED7_TOGGLE();
	debug_uart_tx_char('n');
#endif
	while ((SFRIFG1 & OFIFG)) {
		UCSCTL7 &= ~(DCOFFG + XT1LFOFFG + XT1HFOFFG + XT2OFFG);
		SFRIFG1 &= ~OFIFG;
	}
}
#endif

#pragma vector=RTC_VECTOR
__interrupt void RTC_ISR (void)
{
	switch (RTCIV) {
		case RTCIV_NONE:
			break;
		case RTCIV_RTCRDYIFG:
		case RTCIV_RTCTEVIFG:
		case RTCIV_RTCAIFG:
		case RTCIV_RT0PSIFG:
			break;
		case RTCIV_RT1PSIFG:
			RTCPS1CTL &= ~RT1PSIFG;
			_event_src |= RTC_1HZ_EVENT;
			LPM3_EXIT;
			break;
		default:
			break;
	};
}

void setup_rtc(void)
{
	// stop it
	RTCCTL01 = RTCHOLD;

	// calibration
	RTCCTL2 = 0x00 & 0x3f;
	RTCCTL2 |= RTCCALS;

	// Set the counter for RTC mode
	RTCCTL01 |= RTCMODE;

	// set 128 Hz rate for prescale 0 interrupt
	RTCPS0CTL |= RT0IP_7;

	// enable 1 pulse per second interrupt using prescale 1
	RTCPS1CTL |= RT1IP_6 | RT1PSIE;

	// 1 Hz calibration output
	RTCCTL23 |= RTCCALF_3;

	// setting the peripheral selection bit makes the other I/O control a don't care
	// P2.4 = 1 Hz RTC calibration output
	// Direction needs to be set as output
	RTC_1HZ_PORT_SEL |= RTC_1HZ_BIT;  
	RTC_1HZ_PORT_DIR |= RTC_1HZ_BIT;  

	RTCYEAR = (unsigned int) 2013;
	RTCMON = (unsigned int) 1;
	RTCDAY = (unsigned int) 1;
	RTCDOW = (unsigned int) 2;
	RTCHOUR = (unsigned int) 01;
	RTCMIN = (unsigned int) 0;
	RTCSEC = (unsigned int) 0;

	// Enable the RTC
	RTCCTL01 &= ~RTCHOLD;
	nop();
}

#if defined MW_DEVBOARD_V2
static void dbg_out_rtc(void)
{
	char clk_str[16];
	snprintf(clk_str, 16, "%02d:%02d.%02d %d\n", RTCHOUR, RTCMIN, RTCSEC, RTCDOW);
	debug_uart_tx(clk_str);
	// bt_l2cap_send_channel(0x40, clk_str, strlen(clk_str));
}
#endif

static void handle_button_event(void)
{
	unsigned char _button_state = 0;
#if 0
	char clk_str[16];
	snprintf(clk_str, 16, "0x%02x\n", _button_state);
	debug_uart_tx(clk_str);
#endif
	while (_button_state != (BUTTON_PORT_IN & ALL_BUTTONS)) {
		__delay_cycles(562500);
		_button_state = (BUTTON_PORT_IN & ALL_BUTTONS);
		__delay_cycles(562500);
	}
	// BUTTON_PORT_IE |= INT_EDGE_SEL_BUTTONS;

	if (_button_state & SW_A) {
		debug_uart_tx("switch A\n");
		oswald_handle_button_press(BUTTON_A);
	}
	if (_button_state & SW_B) {
		debug_uart_tx("switch B\n");
		oswald_handle_button_press(BUTTON_B);
	}
	if (_button_state & SW_C) {
		debug_uart_tx("switch C\n");
		oswald_handle_button_press(BUTTON_C);
	}
	if (_button_state & SW_D) {
		debug_uart_tx("switch D\n");
		oswald_handle_button_press(BUTTON_D);
	}
	if (_button_state & SW_E) {
		debug_uart_tx("switch E\n");
		oswald_handle_button_press(BUTTON_E);
	}
	if (_button_state & SW_F) {
		debug_uart_tx("switch F\n");
		oswald_handle_button_press(BUTTON_F);
	}
}

void check_pwr_state(void)
{
	if (BAT_CHARGE_IN & BAT_CHARGE_PWR_BIT) {
		BAT_CHARGE_OUT |= BAT_CHARGE_ENABLE_PIN;
		BAT_CHARGE_REN &= ~(BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2); // disable pull-up
	} else {
		BAT_CHARGE_OUT &= ~BAT_CHARGE_ENABLE_PIN;
		BAT_CHARGE_REN |= BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2; // enable pull-up
	}
}

static void handle_bt_uart_rx_event(void)
{
	const unsigned char *rx;
	unsigned char len, *rp, p;

	rx = mw_bt_get_rx_buf(&rp, &len);

	p = 0;
	while (p < len) {
		p += bt_feed_packet_data(rx[(*rp+p)%BT_RX_MAX_SIZE]);
	}
	// all consumed
	*rp = (*rp + len) % BT_RX_MAX_SIZE;
}

#if defined MW_DEVBOARD_V2
static void handle_uart_rx_event(void)
{
	char c;
#ifndef CC256x_TRANSP
	char tstr[255];

	if (debug_uart_rx_char(&c)) {
		debug_uart_tx_char(c);
		if (c == 'a') {
			debug_uart_tx("\nenabling ACC\n");
			mw_acc_enable();
		} else if (c == 'A') {
			debug_uart_tx("\ndisabling ACC\n");
			mw_acc_disable();
		} else if (c == 'r') {
			int16_t x,y,z;
			debug_uart_tx("\nread ACC: ");
			mw_acc_read(&x, &y, &z);
			snprintf(tstr, 64, "x:%d y:%d z:%d\n", x,y,z);
			debug_uart_tx(tstr);
		} else if (c =='R') {
			int16_t al;
			al = mw_get_amblight_adc_val();
			snprintf(tstr, 64, "light: %d\n", al);
			debug_uart_tx(tstr);
		} else if (c == 'b') {
			debug_uart_tx("\nenabling BT\n");
			mw_enable_bt();
		} else if (c == 'B') {
			debug_uart_tx("\ndisabling BT\n");
			mw_disable_bt();
		} else if (c == 'c') {
			debug_uart_tx("\nCharger status: ");
			snprintf(tstr, 16, "0x%04x 0x%04x ", BAT_CHARGE_IN, (BAT_CHARGE_IN & BAT_CHARGE_PWR_BIT));
			debug_uart_tx(tstr);
			if (BAT_CHARGE_IN & BAT_CHARGE_PWR_BIT)
				debug_uart_tx("no ext pwr, ");
			else
				debug_uart_tx("ext pwr connected, ");
			switch (BAT_CHARGE_IN & (BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2)) {
				case BAT_CHARGE_STAT1:
					debug_uart_tx("charge done\n");
					break;
				case BAT_CHARGE_STAT2:
					debug_uart_tx("fast charge\n");
					break;
				case (BAT_CHARGE_STAT1 | BAT_CHARGE_STAT2):
					debug_uart_tx("suspend, sleep or fault\n");
					break;
				default:
					debug_uart_tx("precharge\n");
					break;
			}
			if (BAT_CHARGE_IN & BAT_CHARGE_ENABLE_PIN)
				debug_uart_tx(" !charge\n");
			else
				debug_uart_tx(" charge\n");
		} else if (c == 'd') {
			debug_uart_tx("charging disabled\n");
			BAT_CHARGE_OUT |= BAT_CHARGE_ENABLE_PIN;
		} else if (c == 'e') {
			debug_uart_tx("charging enabled\n");
			BAT_CHARGE_OUT &= ~BAT_CHARGE_ENABLE_PIN;
		} else if (c == 'l') {
			debug_uart_tx("backlight LED on\n");
			hal_lcd_set_backlight(TRUE);
		} else if (c == 'L') {
			debug_uart_tx("backlight LED off\n");
			hal_lcd_set_backlight(FALSE);
		} else if (c == 'u') {
			mw_lcd_update_screen();
		} else if (c == '+') {
			nop();
		} else if (c == '-') {
			nop();
		} else if (c == 't') {
			int i;
			debug_uart_tx("cc256x ini content:\n");
			for (i=0; i<16; i++) {
				snprintf(tstr, 16, "0x%04x 0x%02x\n", i, cc256x_init_script[i]);
				debug_uart_tx(tstr);
			}
		} else if (c == 'H') {
			uint8_t dclass[3];
			dclass[0] = BT_MW_DEVICE_CLASS & 0xff;
			dclass[1] = (BT_MW_DEVICE_CLASS & 0xff00) >> 8;
			dclass[2] = (BT_MW_DEVICE_CLASS & 0xff0000) >> 16;

			debug_uart_tx("HCI reset\n");
			bt_hci_cmd(HCI_HC_BB_OGF, HCI_RESET_OCF, 0, NULL);
			bt_hci_cmd(HCI_HC_BB_OGF, HCI_W_COD_OCF, 3, dclass);
		} else if (c == 'S') {
			debug_uart_tx("Scan enable\n");
			tstr[0] = HCI_BB_SCAN_INQUIRY | HCI_BB_SCAN_PAGE;
			bt_hci_cmd(HCI_HC_BB_OGF, HCI_W_SCAN_EN_OCF, 1, (uint8_t *)tstr);
		} else if (c == 'h') {
			RTCHOUR++;
			if (RTCHOUR > 23)
				RTCHOUR = 0;
		} else if (c == 'm') {
			RTCMIN++;
			if (RTCMIN > 59)
				RTCMIN = 0;
		} else if (c == 'N') {
			debug_uart_tx("Set name\n");
			tstr[0] = 'O';
			tstr[1] = 's';
			tstr[2] = 'w';
			tstr[3] = 'a';
			tstr[4] = 'l';
			tstr[5] = 'd';
			tstr[6] = 0x00;
			bt_hci_cmd(HCI_HC_BB_OGF, HCI_W_LOCAL_NAME_OCF, 0x07, (uint8_t *)tstr);
		} else if (c == 'R') {
			bt_hci_cmd(HCI_INFO_PARAM_OGF, HCI_R_BD_ADDR_OCF, 0, NULL);
		}
	}
#endif
}
#endif

void start_timer(int cycles)
{
	TA0EX0 = TAIDEX_0;
	TA0CTL = TASSEL_1 | TACLR | MC__STOP;              // SMCLK, clear TAR
	TA0CCTL0 = CCIE;                         // CCR0 interrupt enabled
	TA0CCR0 = cycles;
	TA0CTL |= MC_1;                         // Start Timer_A in continuous mode
}

void stop_timer(void)
{
	TA0CCTL0 &= ~CCIE;                         // CCR0 interrupt enabled
	TA0CTL = MC__STOP;                         // Start Timer_A in continuous mode
}
 
// Timer A0 interrupt service routine
#pragma vector=TIMER0_A0_VECTOR
__interrupt void TIMER0_A0_ISR (void)
{
	TA0CTL &= ~(TAIFG);
#if defined xMW_DEVBOARD_V2
	LED6_TOGGLE();
#endif
	_event_src |= TIMER_500MS_EVENT | TIMER_100MS_EVENT;
	LPM3_EXIT;
}

uint8_t handle_event(void)
{
#if defined MW_DEVBOARD_V2
	char tstr[64];
#endif
	if (_event_src == 0)
		return 1;

	while (_event_src != 0) {
		if (_event_src & WATCHDOG_EVENT) {
			_event_src &= ~WATCHDOG_EVENT;
			debug_uart_tx_char('w');
		} else if (_event_src & RTC_1HZ_EVENT) {
			_event_src &= ~RTC_1HZ_EVENT;
			check_pwr_state();
			oswald_one_second_tick();
#if defined MW_DEVBOARD_V2
			dbg_out_rtc();
			LED7_TOGGLE();
#endif
		} else if (_event_src & BT_UART_RCV_EVENT) {
			_event_src &= ~BT_UART_RCV_EVENT;
			handle_bt_uart_rx_event();
		} else if (_event_src & DBG_UART_RCV_EVENT) {
			_event_src &= ~DBG_UART_RCV_EVENT;
#if defined MW_DEVBOARD_V2
			handle_uart_rx_event();
#endif
		} else if (_event_src & BUTTON_EVENT) {
			_event_src &= ~BUTTON_EVENT;
			handle_button_event();
		} else if (_event_src & TIMER_500MS_EVENT) {
			_event_src &= ~TIMER_500MS_EVENT;
			oswald_halfsecond_tick();
		} else if (_event_src & TIMER_100MS_EVENT) {
			_event_src &= ~TIMER_100MS_EVENT;
			oswald_centisecond_tick();
		} else if (_event_src & ACCEL_EVENT) {
			_event_src &= ~ACCEL_EVENT;
			mw_acc_handle_irq();
		} else {
#if defined MW_DEVBOARD_V2
			snprintf(tstr, 64, "unhandled event in 0x%04x\n", _event_src);
			debug_uart_tx(tstr);
#endif
		}
	}
	return 0;
}

#pragma vector=BUTTON_PORT_VECTOR
__interrupt void BUTTON_ISR (void)
{
	BUTTON_PORT_IFG &= ~ALL_BUTTONS;
	// BUTTON_PORT_IE  &= ~INT_EDGE_SEL_BUTTONS;
	_event_src |= BUTTON_EVENT;
	//_button_state = (BUTTON_PORT_IN & ALL_BUTTONS);
	LPM3_EXIT;
}

#pragma vector=PORT1_VECTOR
__interrupt void PORT1_GPIO_ISR (void)
{
	if (P1IFG & BT_IO_CTS) {
		P1IFG &= ~BT_IO_CTS;

		debug_uart_tx("BT CTS irq\n");
		bt_hci_ehcill_wake();

		LPM3_EXIT;
	};
	if (P1IFG & ACCELEROMETER_INT_PIN) {
		P1IFG &= ~ACCELEROMETER_INT_PIN;
		// debug_uart_tx("ACC irq\n");
		_event_src |= ACCEL_EVENT;
		LPM3_EXIT;
	}
}


#if 0
#pragma vector=NOVECTOR
__interrupt void UNEXP_ISR (void)
{
	debug_uart_tx("unexpected IRQ occured\n");
}
#endif


int main(void)
{
	int idle = 0;

	setup_wdt();
	setup_pins();
	setup_clocks();
	setup_rtc();

	/* enable interrupts, we will need them! */
	__eint();

#if defined MW_DEVBOARD_V2
	init_debug_uart();
	debug_uart_tx("\nOswald on MetaWatch\n");
#endif

	mw_lcd_init();
	mw_lcd_clear();
	mw_lcd_update_screen();

	mw_init_adc();
	mw_init_vibrate_pwm();

#if 0
	RTCYEAR = (unsigned int) 2013;
	RTCMON = (unsigned int) 1;
	RTCDAY = (unsigned int) 1;
	RTCDOW = (unsigned int) 2;
	RTCHOUR = (unsigned int) 01;
	RTCMIN = (unsigned int) 0;
	RTCSEC = (unsigned int) 0;
#endif

	oswald_set_time(RTCHOUR, RTCMIN, RTCSEC, TRUE);
	oswald_set_date(RTCDAY, RTCMON, RTCYEAR, TRUE);
	oswald_init();

	while (1) {
		/* handle pending events */
		if (handle_event())
			idle++;
		else
			idle = 0;

		/* enter LPM3 sleep mode waiting for interrupt */
		if (idle > 100) {
			idle = 0;
			// debug_uart_tx("z");
			LPM3;
		}
	};

return 0;
}