/**
 ******************************************************************************
 * @file    main.c
 * @author  Ac6
 * @version V1.0
 * @date    01-December-2013
 * @brief   Default main function.
 ******************************************************************************
 */

#include "stm32f4xx.h"
#include "stm32f4_discovery.h"
#include "main.h"
#include "math.h"
#include "odometry.h"
#include "string.h"

struct position pos;

int main(void) {
	BSP_LED_Init(LED3);
	BSP_LED_Init(LED4);
	BSP_LED_Init(LED5);
	BSP_LED_Init(LED6);

	if (HAL_Init() != HAL_OK) {
		Error_Handler();
	}

	SystemClock_Config();

	InitPosStruct(&pos);

	/*##-1- Configure the UART peripheral ##*/
	UartHandle.Instance = UART4;
	UartHandle.Init.BaudRate = 115200;
	UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
	UartHandle.Init.StopBits = UART_STOPBITS_1;
	UartHandle.Init.Parity = UART_PARITY_NONE;
	UartHandle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
	UartHandle.Init.Mode = UART_MODE_TX_RX;
	UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;

	if (HAL_UART_Init(&UartHandle) != HAL_OK) {
		Error_Handler();
	}

	/* Configure encoder 1 */
	TimHandle1.Instance = TIM2;
	TimHandle1.Init.Period = 0xFFFFFFFF;
	TimHandle1.Init.Prescaler = 0;
	TimHandle1.Init.ClockDivision = 0;
	TimHandle1.Init.CounterMode = TIM_COUNTERMODE_UP;

	EncConf1.EncoderMode = TIM_ENCODERMODE_TI12;

	EncConf1.IC1Filter = FILTER
	;
	EncConf1.IC1Polarity = TIM_ICPOLARITY_RISING;
	EncConf1.IC1Prescaler = TIM_ICPSC_DIV1;
	EncConf1.IC1Selection = TIM_ICSELECTION_DIRECTTI;

	EncConf1.IC2Filter = FILTER
	;
	EncConf1.IC2Polarity = TIM_ICPOLARITY_RISING;
	EncConf1.IC2Prescaler = TIM_ICPSC_DIV1;
	EncConf1.IC2Selection = TIM_ICSELECTION_DIRECTTI;

	if (HAL_TIM_Encoder_Init(&TimHandle1, &EncConf1) != HAL_OK) {
		Error_Handler();
	}

	/* Configure encoder 2 */
	TimHandle2.Instance = TIM5;
	TimHandle2.Init.Period = 0xFFFFFFFF;
	TimHandle2.Init.Prescaler = 0;
	TimHandle2.Init.ClockDivision = 0;
	TimHandle2.Init.CounterMode = TIM_COUNTERMODE_UP;

	EncConf2.EncoderMode = TIM_ENCODERMODE_TI12;

	EncConf2.IC1Filter = FILTER
	;
	EncConf2.IC1Polarity = TIM_ICPOLARITY_RISING;
	EncConf2.IC1Prescaler = TIM_ICPSC_DIV1;
	EncConf2.IC1Selection = TIM_ICSELECTION_DIRECTTI;

	EncConf2.IC2Filter = FILTER
	;
	EncConf2.IC2Polarity = TIM_ICPOLARITY_RISING;
	EncConf2.IC2Prescaler = TIM_ICPSC_DIV1;
	EncConf2.IC2Selection = TIM_ICSELECTION_DIRECTTI;

	if (HAL_TIM_Encoder_Init(&TimHandle2, &EncConf2) != HAL_OK) {
		Error_Handler();
	}

	HAL_RCC_GetSysClockFreq();
	uwPrescalerValue = (uint32_t) ((SystemCoreClock / 2) / 10000) - 1;
	/* Set TIM7 instance */
	TimHandle7.Instance = TIM7;
	TimHandle7.Init.Period = 200 - 1;
	TimHandle7.Init.Prescaler = uwPrescalerValue;
	TimHandle7.Init.ClockDivision = 0;
	TimHandle7.Init.CounterMode = TIM_COUNTERMODE_UP;
	if (HAL_TIM_Base_Init(&TimHandle7) != HAL_OK) {
		Error_Handler();
	}

	if (HAL_UART_Receive_IT(&UartHandle, (uint8_t *) aRxBuffer, 6) != HAL_OK) {
		Error_Handler();
	}
	BSP_LED_On(LED4);
	sprintf(aTxBuffer, "Stisknete start\n\r");
	if (HAL_UART_Transmit(&UartHandle, (uint8_t*) aTxBuffer, sizeof(aTxBuffer),
			19) != HAL_OK) {
		Error_Handler();
	}

	for (;;) {
		if (strstr(aState, "start") != NULL) { /*##Start the Encoder 2 ##*/
			if (HAL_TIM_Encoder_Start(&TimHandle2, TIM_CHANNEL_ALL) != HAL_OK) {
				Error_Handler();
			}
			/*##Stop the Encoder 1 ##*/
			if (HAL_TIM_Encoder_Start(&TimHandle1, TIM_CHANNEL_ALL) != HAL_OK) {
				Error_Handler();
			}

			/*##-2- Start the TIM Base generation in interrupt mode ##*/

			if (HAL_TIM_Base_Start_IT(&TimHandle7) != HAL_OK) {
				Error_Handler();
			}
			strcpy(aState,"");
		}
		if (strstr(aState, "stopt") != NULL) {
			/*##-2- Stop the TIM Base generation in interrupt mode ##*/
			if (HAL_TIM_Base_Stop_IT(&TimHandle7) != HAL_OK) {
				Error_Handler();
			}
			/*##Stop the Encoder 1 ##*/
			if (HAL_TIM_Encoder_Stop(&TimHandle2, TIM_CHANNEL_ALL) != HAL_OK) {
				Error_Handler();
			}
			/*##Stop the Encoder 2 ##*/
			if (HAL_TIM_Encoder_Stop(&TimHandle1, TIM_CHANNEL_ALL) != HAL_OK) {
				Error_Handler();
			}
			strcpy(aState,"");

		}
		if (strstr(aState, "reset") != NULL) {
			strcpy(aState,"");
			NVIC_SystemReset();
		}
	}
}
void Error_Handler(void) {
	BSP_LED_On(LED5);
	while (1) {
	}
}

void SystemClock_Config(void)

{

	RCC_OscInitTypeDef RCC_OscInitStruct;
	RCC_ClkInitTypeDef RCC_ClkInitStruct;
	RCC_PeriphCLKInitTypeDef PeriphClkInitStruct;

	/*Configure the main internal regulator output voltage */
	__HAL_RCC_PWR_CLK_ENABLE()
	;
	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
	/*Initializes the CPU, AHB and APB busses clocks */
	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE
			| RCC_OSCILLATORTYPE_LSI;
	RCC_OscInitStruct.HSEState = RCC_HSE_ON;
	RCC_OscInitStruct.LSIState = RCC_LSI_ON;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
	RCC_OscInitStruct.PLL.PLLM = 8;
	RCC_OscInitStruct.PLL.PLLN = 336;
	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
	RCC_OscInitStruct.PLL.PLLQ = 7;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
		Error_Handler();
	}

	/**Initializes the CPU, AHB and APB busses clocks
	 */
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
			| RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
		Error_Handler();
	}
	PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
	PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
	if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {
		Error_Handler();
	}
	/*Configure the Systick interrupt time*/
	HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);
	/*Configure the Systick */
	HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
	/* SysTick_IRQn interrupt configuration */
	HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
	strcpy(aState, aRxBuffer);
	if (HAL_UART_Receive_IT(UartHandle, (uint8_t *) aRxBuffer, 6) != HAL_OK) {
		Error_Handler();
	}
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) {

	BSP_LED_On(LED3);

}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
	BSP_LED_Toggle(LED6);
	counterValue1 = __HAL_TIM_GET_COUNTER(&TimHandle1);
	counterValue2 = __HAL_TIM_GET_COUNTER(&TimHandle2);
	pos.ticksRight = counterValue1;
	pos.ticksLeft = counterValue2;
	__HAL_TIM_SET_COUNTER(&TimHandle1, 0);
	__HAL_TIM_SET_COUNTER(&TimHandle2, 0);
	count++;
	ms = count * 20;
	if (ms == 1000) {
		ms = 0;
		count = 0;
		s = s + 1;
	}
	if (s == 60) {
		s = 0;
		min = min + 1;
	}
	if (min == 60) {
		min = 0;
		h = h + 1;
	}
	if (UartHandle.gState == HAL_UART_STATE_READY) {
		TikToPosition(&pos);
		sprintf(aTxBuffer, "X: %.4f Y: %.4f A: %.2f	%d:%d:%d:%d\n\r", pos.x,
				pos.y, pos.angelDeg, h, min, s, ms);
		if (HAL_UART_Transmit(&UartHandle, (uint8_t*) aTxBuffer,sizeof(aTxBuffer), 25) != HAL_OK) {
			Error_Handler();
		}
	}
}

