#include "Mezz_read_PWM.h"
#include "stm32f4xx_hal_can.h"
#include "stm32f4xx_hal_uart.h"
#include "string.h"
#include "stdlib.h"

volatile uint32_t recSteer = 0;
volatile uint32_t recThrottle = 0;
volatile uint32_t rising_edge_ch3 = 0;
uint32_t validity = 8;
CAN_RxHeaderTypeDef   RxHeader;

volatile const float WheelCuricumference 	= 0.267;
volatile const float FrontDiffRatio 	 	= 2.6;
volatile uint32_t timer3Value_actual 	 	= 0;
volatile uint32_t timer3Value_previous 	 	= 0;
volatile float distanceOdo 				 	= 0;
volatile float velocityOdo 				 	= 0;
volatile const uint8_t counterTriggerLevel 	= 4;
volatile uint8_t counterImpulses 			= 0;

uint8_t init_PWMs = 1;

extern TIM_HandleTypeDef htim1;
extern CAN_HandleTypeDef hcan1;
extern TIM_HandleTypeDef htim3;




uint8_t Mezz_read_PWM(uint32_t * steer, uint32_t * trottle, float * velocityFromDiff)
{
	*steer = recSteer;
	*trottle = recThrottle;
	*velocityFromDiff = velocityOdo;

#ifdef MACRON
	if(init_PWMs == 1)
	{
		HAL_TIM_Base_Start_IT(&htim1);
		HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_2);
		HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_3);

		HAL_TIM_Base_Start_IT(&htim3);
		HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_3);
		init_PWMs = 0;
	}
#endif

return validity;
}
#ifdef MACRON


//----------------------PWM-----------------------
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	// If PWM Input capture overflows we say data are invalid.
	// (Timer is user reseted on 1. channel rising edge)
	if(htim->Instance == TIM1)
	{
		validity = 0; // Lost receiver signal)
		HAL_GPIO_TogglePin(GPIOD,LED_RED_Pin);
	}

	// When velocity ARR register overflow then it is set to 0
	if(htim->Instance == TIM3)
	{
		//HAL_GPIO_WritePin(GPIOD,LED_RED_Pin, GPIO_PIN_SET);
		velocityOdo = 0.0;
	}
}

//-----------------PWM--------------------
// Reading section of receiver and velocity sensor
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance == TIM1)
	{
		HAL_GPIO_WritePin(GPIOD,LED_RED_Pin, GPIO_PIN_RESET);
		if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2)
		{
			if((GPIOE->IDR & RX_IC_CH2_Pin) != (uint32_t)GPIO_PIN_RESET)
			{
				TIM1->CNT = 0;
				validity = 1;
			}
			else
			{
				recSteer = TIM1->CCR2;// - rising_edge_ch2;
			}
		}
		if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
		{
			if((GPIOE->IDR & RX_IC_CH3_Pin) != (uint32_t)GPIO_PIN_RESET)
			{
				rising_edge_ch3 = TIM1->CNT;
			}
			else
			{
				recThrottle = TIM1->CCR3 - rising_edge_ch3;
			}
		}
	}

	if(htim->Instance == TIM3)
	{
		if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
		{
			HAL_GPIO_TogglePin(GPIOD,LED_GREEN_Pin);
			if(counterImpulses >= counterTriggerLevel)
			{
				timer3Value_actual = __HAL_TIM_GET_COUNTER(&htim3);
				__HAL_TIM_SET_COUNTER(&htim3,0);
				distanceOdo = (WheelCuricumference * counterImpulses) / (16 * FrontDiffRatio);
				// 1 timer tick is 100us, ARR is 40000. ElapsetimeCallback every 4s -> m/s
				velocityOdo = (distanceOdo * 10000) / (timer3Value_actual);
				counterImpulses = 0;
			}
			counterImpulses++;
		}
	}

}
#endif