#include <stdbool.h>
#include "speed_sensor.h"
#include "math_tools.h"
#include "enviolo_can.h"


//局部变量定义

//全局变量定义
MC_SpeedSensorData_Struct_t MC_SpeedSensorData = {0, TRUE, FALSE, 0, 0xFFFFFFFF, 0};
uint16_t MC_Speed_Array[10] = {0};

/**************************局部函数定义*************************/


/**************************全局函数定义*************************/
//速度传感器IO初始化
void SpeedSensor_GPIO_Init(void)
{
   GPIO_InitTypeDef GPIO_InitStruct;

	__HAL_RCC_GPIOD_CLK_ENABLE();
	
  GPIO_InitStruct.Pin = SPEED_SENSOR_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(SPEED_SENSOR_GPIO_Port, &GPIO_InitStruct);
	
	HAL_NVIC_SetPriority(SPEED_SENSOR_EXTI_IRQn, 2, 0);
  HAL_NVIC_EnableIRQ(SPEED_SENSOR_EXTI_IRQn);
	
}

//车速计算和停止判断处理
void SpeedSensor_Process(MC_SpeedSensorData_Struct_t* p_MC_SpeedSensorData, uint16_t* AvgResult, uint8_t WheelSize, uint8_t Poles)
{
	//车速计算
	static uint32_t PeriodTimeCnt = 0;
	static uint16_t FiltTemp[10];
	
	if((HAL_GetTick() - PeriodTimeCnt) >= 50)
	{
		//停止判断
		static uint32_t StopDelayTimeCnt = 0;
		
		if(p_MC_SpeedSensorData->IsTrigFlag == TRUE)
		{
			StopDelayTimeCnt = HAL_GetTick();
			p_MC_SpeedSensorData->IsTrigFlag = FALSE;
		}
		if((HAL_GetTick() - StopDelayTimeCnt) > (4000 / Poles)) // 超时信号触发标志未更新，认为停车
		{
			p_MC_SpeedSensorData->IsStopFlag = TRUE;
		}
		
		//车速计算，计算周期50ms
		if(p_MC_SpeedSensorData->IsStopFlag == FALSE)
		{
			if(p_MC_SpeedSensorData->DiffTime_ms == 0)
			{
				p_MC_SpeedSensorData->DiffTime_ms = 0xFFFFFFFF;
			}
			p_MC_SpeedSensorData->Speed_Data = (uint32_t)WheelSize * 360 / Poles  / p_MC_SpeedSensorData->DiffTime_ms;//单位0.1km/h
			
			//滑动均值滤波
			*AvgResult = MovingAverageFilter(p_MC_SpeedSensorData->Speed_Data, FiltTemp, sizeof(FiltTemp) >> 1);
			
			//在确定停止前进行衰减
			if((HAL_GetTick() - p_MC_SpeedSensorData->TrigSysTime) > (p_MC_SpeedSensorData->DiffTime_ms * 2))
			{
		  	p_MC_SpeedSensorData->DiffTime_ms = (p_MC_SpeedSensorData->DiffTime_ms * 1038) >> 10;
			}
			
		}
		else
		{
			p_MC_SpeedSensorData->Speed_Data = 0;
			*AvgResult = 0;
			ArrayFillZero(FiltTemp, sizeof(FiltTemp) >> 1);
			p_MC_SpeedSensorData->DiffTime_ms = 0xFFFFFFFF;
			return;
		}
		PeriodTimeCnt = HAL_GetTick();
	}
}

void SpeedCal_ByCadence(uint16_t CadenceData, uint16_t* SpeedData, uint8_t T_Front, uint8_t T_Tail, uint16_t WheelSize, uint16_t* BikeSpeed)
{
  static uint32_t PeriodTimeCnt = 0;
	static uint8_t FltCount = 0;
	static uint32_t FltSum = 0;
	
	if((HAL_GetTick() - PeriodTimeCnt) >= 50)
	{
	  PeriodTimeCnt = HAL_GetTick();
		*SpeedData = ((CadenceData * T_Front / T_Tail * WheelSize * 144) / 10000);//车速 = 踏频 * 2.4 * 前飞齿数 / 后飞齿数 * 60 / 1000 km/h，结果单位0.1km/h
		FltSum += *SpeedData;
		FltCount++;
		if(FltCount >= 8)
		{
		  FltCount = 0;
			*BikeSpeed = FltSum >> 3;
			FltSum = 0;
		}	
	}
}

//SpeedCal_ByCommunication(&MC_SpeedSensorData, &MC_RunInfo.BikeSpeed, MC_ConfigParam1.WheelSize + MC_ConfigParam1.WheelSizeAdj);
void SpeedCal_ByCommunication(MC_SpeedSensorData_Struct_t* p_MC_SpeedSensorData, uint16_t* AvgResult, uint8_t WheelSize)
{

	//粗暴简单将自动变速器的值传递过来,因为自动变速器已做滤波
	if(p_MC_SpeedSensorData->IsStopFlag == FALSE)
	{
		*AvgResult = CanEnvioloData.u16Get_Gear_Vehicle_Speed;
		p_MC_SpeedSensorData->Speed_Data = CanEnvioloData.u16Get_Gear_Vehicle_Speed;
	}
	else
	{
		p_MC_SpeedSensorData->Speed_Data = 0;
		*AvgResult = 0;
	}

	#if 0
	  //车速计算
//	static uint32_t PeriodTimeCnt = 0;
//	static uint16_t u16FiltTemp[10];
//	static bool BArrayFullFalg = false;
//	static uint8_t i = 0;	
	if((HAL_GetTick() - PeriodTimeCnt) >= 200)
	{
		if(p_MC_SpeedSensorData->IsStopFlag == FALSE)
		{
			if(BArrayFullFalg == false)
			{
				u16FiltTemp[i++] = CanEnvioloData.u16Get_Gear_Vehicle_Speed;
				*AvgResult = MovingAverageFilter(CanEnvioloData.u16Get_Gear_Vehicle_Speed, u16FiltTemp, i);
				if(i >= 10)
				{
					BArrayFullFalg = true;
					i = 0;
				}
			}
			else
			{
				*AvgResult = MovingAverageFilter(CanEnvioloData.u16Get_Gear_Vehicle_Speed, u16FiltTemp, sizeof(u16FiltTemp) >> 1);
				//p_MC_SpeedSensorData->Speed_Data = *AvgResult;
			}
			p_MC_SpeedSensorData->Speed_Data = *AvgResult;
		}
		
		//车速计算
//		if(p_MC_SpeedSensorData->IsStopFlag == FALSE)
//		{
////		  if(p_MC_SpeedSensorData->DiffTime_ms == 0)
////			{
////				p_MC_SpeedSensorData->DiffTime_ms = 0xFFFFFFFF;
////			}
//			//p_MC_SpeedSensorData->Speed_Data = (uint32_t)WheelSize * 360 / p_MC_SpeedSensorData->DiffTime_ms;//单位0.1km/h
//			p_MC_SpeedSensorData->Speed_Data = CanEnvioloData.u16Get_Gear_Vehicle_Speed;
//			//滑动均值滤波
//			*AvgResult = MovingAverageFilter(p_MC_SpeedSensorData->Speed_Data, u16FiltTemp, sizeof(u16FiltTemp) >> 1);
//			//*AvgResult = CanEnvioloData.u16Get_Gear_Vehicle_Speed;
//		}
		else
		{
		  p_MC_SpeedSensorData->Speed_Data = 0;
			*AvgResult = 0;
			ArrayFillZero(u16FiltTemp, sizeof(u16FiltTemp) >> 1);
			return;
		}
		
		PeriodTimeCnt = HAL_GetTick();
	}
	#endif
}