#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;
		}	
	}
}

void SpeedCal_ByCommunication(MC_SpeedSensorData_Struct_t* p_MC_SpeedSensorData, uint16_t* AvgResult, uint8_t WheelSize)
{
  //车速计算
	static uint32_t PeriodTimeCnt = 0;
	static uint16_t FiltTemp[10];
	
	if((HAL_GetTick() - PeriodTimeCnt) >= 50)
	{
		//超时清零
		static uint32_t ClearDelayTimeCnt = 0;
		
		if(p_MC_SpeedSensorData->IsTrigFlag == TRUE)
		{
			ClearDelayTimeCnt = HAL_GetTick();
			p_MC_SpeedSensorData->IsTrigFlag = FALSE;
		}
		if((HAL_GetTick() - ClearDelayTimeCnt) > 4000) // 超时信号触发标志未更新，认为停车
		{
			p_MC_SpeedSensorData->IsStopFlag = TRUE;
			p_MC_SpeedSensorData->DiffTime_ms = 0xFFFF;
		}
	  
		//停止判断
		if((p_MC_SpeedSensorData->DiffTime_ms & 0xFFFF) == 0xFFFF)
		{
		  p_MC_SpeedSensorData->IsStopFlag = TRUE;
		}
		else
		{
			p_MC_SpeedSensorData->IsStopFlag = FALSE;
		}
		
		//车速计算
		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
			
			//滑动均值滤波
			*AvgResult = MovingAverageFilter(p_MC_SpeedSensorData->Speed_Data, FiltTemp, sizeof(FiltTemp) >> 1);
		}
		else
		{
		  p_MC_SpeedSensorData->Speed_Data = 0;
			*AvgResult = 0;
			ArrayFillZero(FiltTemp, sizeof(FiltTemp) >> 1);
			return;
		}
		
		PeriodTimeCnt = HAL_GetTick();
	}
}