//TODO should most/all of these .h files be in CPFcpp.hpp?

#include "CPFcpp.hpp"
#include "RecoveryState.cpp"
#include "DescendState.cpp"
#include "FindNeutralState.cpp"
#include "SBE41.hpp"

extern "C" void SysTick_Handler(void)
{
	HAL_IncTick();
	//HAL_SYSTICK_IRQHandler();
}

/* Private function prototypes -----------------------------------------------*/
static void SystemClock_Config(void);


int main(void)
{
	//SBE41 ctd1("CTD1");
	SBE41 ctd1 = SBE41::getInstance("CTD1");
	
	StateBase *stateArray[numStates];
	stateArray[recovery] = new Recovery("Recovery State", -1);
	stateArray[findNeutral] = new FindNeutral("Find Neutral State", 10 * 60 * 1000);
	stateArray[descend] = new Descend("Descend State", 2 * 3600 * 1000);
	
	bool runMission = true;
	bool doStateEntry = true;
	
	StateNames currentState = recovery;
	StateNames nextState;

	cout << "\t\tHAL_Init()" << endl;
	HAL_Init();
	
	//TODO For some reason, calling this makes the RTC run 3X too slow
	//SystemClock_Config();

	initPlatform();

	while (runMission)
	{
		if (doStateEntry)
		{
			stateArray[currentState]->doStateEntryActions();
			doStateEntry = false;
		}
		
		stateArray[currentState]->doStateActions();
		nextState = stateArray[currentState]->checkEvents();
		if (nextState != currentState)
		{
			stateArray[currentState]->doStateExit(false);
			currentState = nextState;
			doStateEntry = true;
			HAL_Delay(6000);
		}
		HAL_Delay(1500);
	}
}

void initPlatform()
{	
	cout << "\t\tinitPlatform" << endl;
	cout << "\t\tinitLEDs()" << endl;
	initLEDs();
}

/**
  * @brief  System Clock Configuration
  *         The system Clock is configured as follow : 
  *            System Clock source            = PLL (HSE)
  *            SYSCLK(Hz)                     = 100000000
  *            HCLK(Hz)                       = 100000000
  *            AHB Prescaler                  = 1
  *            APB1 Prescaler                 = 2
  *            APB2 Prescaler                 = 1
  *            HSE Frequency(Hz)              = 8000000
  *            PLL_M                          = 8
  *            PLL_N                          = 200
  *            PLL_P                          = 2
  *            PLL_Q                          = 7
  *            PLL_R                          = 2
  *            VDD(V)                         = 3.3
  *            Main regulator output voltage  = Scale1 mode
  *            Flash Latency(WS)              = 3
  * @param  None
  * @retval None
  */
static void SystemClock_Config(void)
{
	RCC_ClkInitTypeDef RCC_ClkInitStruct;
	RCC_OscInitTypeDef RCC_OscInitStruct;
	HAL_StatusTypeDef ret = HAL_OK;

	/* Enable Power Control clock */
	__HAL_RCC_PWR_CLK_ENABLE();

	/* The voltage scaling allows optimizing the power consumption when the device is 
	   clocked below the maximum system frequency, to update the voltage scaling value 
	   regarding system frequency refer to product datasheet.  */
	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

	/* Enable HSE Oscillator and activate PLL with HSE as source */
	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
	RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
	RCC_OscInitStruct.PLL.PLLM = 8;
	RCC_OscInitStruct.PLL.PLLN = 200;
	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
	RCC_OscInitStruct.PLL.PLLQ = 7;
	RCC_OscInitStruct.PLL.PLLR = 2;
	ret = HAL_RCC_OscConfig(&RCC_OscInitStruct);
  
	if (ret != HAL_OK)
	{
		while (1) {
			;
		} 
	}

	/* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 
	   clocks dividers */
	RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;  
	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;  
	ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3);
	if (ret != HAL_OK)
	{
		while (1) {
			;
		}  
	}
}

void initLEDs()
{
	cout << "\t\tinitLEDs" << endl;

	__GPIOB_CLK_ENABLE();
	GPIO_InitTypeDef greenLED;
	GPIO_InitTypeDef redLED;
	GPIO_InitTypeDef blueLED;

	greenLED.Pin = GPIO_PIN_0;
	greenLED.Mode = GPIO_MODE_OUTPUT_PP;
	greenLED.Speed = GPIO_SPEED_FREQ_HIGH;
	greenLED.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOB, &greenLED);

	blueLED.Pin = GPIO_PIN_7;
	blueLED.Mode = GPIO_MODE_OUTPUT_PP;
	blueLED.Speed = GPIO_SPEED_FREQ_HIGH;
	blueLED.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOB, &blueLED);

	redLED.Pin = GPIO_PIN_14;
	redLED.Mode = GPIO_MODE_OUTPUT_PP;
	redLED.Speed = GPIO_SPEED_FREQ_HIGH;
	redLED.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOB, &redLED);

	for (int i = 0; i < 4; i++)
	{
		HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0);
		HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);
		HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_14);
		HAL_Delay(200);
	}
}

static UART_HandleTypeDef hUSART1 = UART_HandleTypeDef();
void configUART1(bool useDMA)
{
	/**USART1 GPIO Configuration    
   PA10     ------> USART1_RX
   PA15     ------> USART1_TX 
   */
		
	__USART1_CLK_ENABLE();
	__GPIOA_CLK_ENABLE();
    
	GPIO_InitTypeDef GPIO_InitStructure;
 
	GPIO_InitStructure.Pin = GPIO_PIN_15;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
	GPIO_InitStructure.Alternate = GPIO_AF7_USART1;
	GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
	GPIO_InitStructure.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
    
	GPIO_InitStructure.Pin = GPIO_PIN_10;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_OD;
	HAL_GPIO_Init(GPIOA, &GPIO_InitStructure);
 
	hUSART1.Instance        = USART1;
	hUSART1.Init.BaudRate   = 9600;
	hUSART1.Init.WordLength = UART_WORDLENGTH_8B;
	hUSART1.Init.StopBits   = UART_STOPBITS_1;
	hUSART1.Init.Parity     = UART_PARITY_NONE;
	hUSART1.Init.HwFlowCtl  = UART_HWCONTROL_NONE;
	hUSART1.Init.Mode       = UART_MODE_TX_RX;
    
	if (HAL_UART_Init(&hUSART1) != HAL_OK)
	{
		cout << "Error in UART_Init" << endl;
		while (1) {}
	}
	
	NVIC_EnableIRQ(USART1_IRQn);
	__HAL_UART_ENABLE_IT(&hUSART1, UART_IT_IDLE);
			
	if (useDMA)
	{

	}
}