#include "contiki.h"
#include "sys/clock.h"
#include "msp432-def.h"
//#include "rom_map.h"
#include "driverlib.h"

static unsigned long overflow_counter;

#define TICKS_PER_SEC 1024
#define INTERVAL (MAP_CS_getMCLK() / TICKS_PER_SEC)

void clock_init(void){
  MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ, GPIO_PIN0 | GPIO_PIN1, GPIO_PRIMARY_MODULE_FUNCTION); // PJ.0 and PJ.1: LF crystal in and out
  MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ, GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION); // PJ.2 and PJ.3: HF crystal in and out

  MAP_CS_setReferenceOscillatorFrequency(CS_REFO_32KHZ);
  MAP_CS_setExternalClockSourceFrequency(LFXTCLK_SPEED, HFXTCLK_SPEED);  // this is necessary for CS_startHFXT() to work properly

  MAP_CS_setDCOFrequency(DCOCLK_SPEED);    // set DCO frequency (this function uses CS_setDCOCenteredFrequency and CS_tuneDCOFrequency and is not available on pre-release devices)
  MAP_CS_startLFXT(CS_LFXT_DRIVE0);     // enable LF crystal in non-bypass mode without a timeout (lowest current drain with drive level = 0)

  // enable the HF crystal:
  MAP_CS_startHFXT(0);            // start the HF crystal

  // select clock sources
  MAP_CS_initClockSignal(CS_MCLK, MCLK_SRC, MCLK_DIV);          // MCLK   = HFXTCLK
  MAP_CS_initClockSignal(CS_HSMCLK, HSMCLK_SRC, HSMCLK_DIV);    // HSMCLK = DCOCLK
  MAP_CS_initClockSignal(CS_SMCLK, SMCLK_SRC, SMCLK_DIV);       // SMCLK  = DCOCLK
  MAP_CS_initClockSignal(CS_ACLK, ACLK_SRC, ACLK_DIV);          // ACLK   = LFXTCLK
  MAP_CS_initClockSignal(CS_BCLK, BCLK_SRC, BCLK_DIV);          // BCLK   = REFOCLk

  overflow_counter = 0;
  MAP_SysTick_enableModule();
  MAP_SysTick_setPeriod(INTERVAL);
  MAP_Interrupt_enableSleepOnIsrExit();
  MAP_SysTick_enableInterrupt();
  MAP_Interrupt_enableMaster();
}

clock_time_t clock_time(void){
	return (INTERVAL * overflow_counter + MAP_SysTick_getValue());
}

unsigned long clock_seconds(void){
	return ((INTERVAL * overflow_counter + MAP_SysTick_getValue()) / MAP_CS_getMCLK());
}

void clock_set_seconds(unsigned long sec){
	overflow_counter = ((float)TICKS_PER_SEC) * sec;
}

void clock_wait(clock_time_t t){
	clock_time_t start_time = MAP_SysTick_getValue();
	while(MAP_SysTick_getValue() < (start_time + t));
}

void  clock_delay_usec(uint16_t dt){
	clock_time_t number_ticks = (MAP_CS_getMCLK() * dt) / 1000000;
	clock_wait(number_ticks);
}

void SysTick_Handler(void){
	overflow_counter++;
}