#include "cadence_sensor.h"
#include "adc.h"

//局部变量定义
const uint8_t ForwardDir_EncoderData[4] = {2,0,3,1};//判断正转用的编码顺序表,正转时信号为：2，0，1，3
const uint8_t BackwardDir_EncoderData[4] = {1,3,0,2};//判断反转用的编码顺序表，反转时信号为：3，1，0，2

//全局变量定义
MC_CadenceResult_Struct_t MC_CadenceResult = {0, MC_Cadence_Stop, TRUE, 0};
uint16_t MC_Cadence_Array[10] = {0};

/**************************局部函数定义*************************/
uint16_t torqueFilteredThroughCadence(uint16_t torque_temp, uint8_t modeFlag);

/**************************全局函数定义*************************/
uint8_t Cadence_ReadHallState(void)
{
  uint8_t CadenceValue;
  GPIO_PinState hall_a, hall_b;
	
	hall_a = HAL_GPIO_ReadPin(CADENCE_2_GPIO_Port, CADENCE_2_Pin);
	CadenceValue  = (uint8_t)hall_a;
	hall_b = HAL_GPIO_ReadPin(CADENCE_1_GPIO_Port, CADENCE_1_Pin);
	CadenceValue |= (uint8_t)hall_b << 1;
	return(CadenceValue & 0x03);
}

//踏频传感器IO初始化
void CadenceSensor_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct;
	
	__HAL_RCC_GPIOA_CLK_ENABLE();
	__HAL_RCC_GPIOB_CLK_ENABLE();
	
	GPIO_InitStruct.Pin = CADENCE_1_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(CADENCE_1_GPIO_Port, &GPIO_InitStruct);
	
	GPIO_InitStruct.Pin = CADENCE_2_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(CADENCE_2_GPIO_Port, &GPIO_InitStruct);
	
}

//踏频传感器检测处理
void CadenceSensor_Process(MC_CadenceResult_Struct_t* p_MC_CadenceResult, uint16_t StopDelayTime, uint8_t StarCount, TrueOrFalse_Flag_Struct_t UpSlopeFlag)
{
	static MC_CadenceSensorStatus_Struct_t MC_CadenceSensorStatus;
	static TrueOrFalse_Flag_Struct_t IsFirstEnterFalg = TRUE;
		
	//读取霍尔信号
	
	MC_CadenceSensorStatus.HallGropuStatus = Cadence_ReadHallState();
	if(IsFirstEnterFalg == TRUE)
	{
	  MC_CadenceSensorStatus.HallGropuStatus_Old = MC_CadenceSensorStatus.HallGropuStatus;
		IsFirstEnterFalg = FALSE;
	}
	//判断踏频正反转，踏频信号触发计数
	static uint16_t BackwordDelayCnt1 = 0;
	static uint16_t BackwordDelayCnt2 = 0;
	
	if(MC_CadenceSensorStatus.HallGropuStatus != MC_CadenceSensorStatus.HallGropuStatus_Old)
	{
	  if(MC_CadenceSensorStatus.HallGropuStatus_Old == ForwardDir_EncoderData[MC_CadenceSensorStatus.HallGropuStatus & 0x03])//上一次的编码，与正转的编码顺序一致，则为正转
		{
		  //正转
			p_MC_CadenceResult->Cadence_Dir = MC_Cadence_Forward;
			BackwordDelayCnt1 = 0;
			BackwordDelayCnt2 = 0;
			//踏频信号计数
		  p_MC_CadenceResult->TrigCount++;
		}
		else if(MC_CadenceSensorStatus.HallGropuStatus_Old == BackwardDir_EncoderData[MC_CadenceSensorStatus.HallGropuStatus & 0x03])//反转做延时判断
		{
		  //反转
			BackwordDelayCnt1++;
			if(BackwordDelayCnt1 >= 4)//检测到连续4次反向脉冲则判断为反转，约4/120*360=12度
			{
			  p_MC_CadenceResult->Cadence_Dir = MC_Cadence_Backward;
				BackwordDelayCnt1 = 0;
			}
		}
		else//蹋频波形异常情况下，延时判断为反转
		{
		  //反转
			BackwordDelayCnt2++;
			if(BackwordDelayCnt2 > 10)
			{
			  p_MC_CadenceResult->Cadence_Dir = MC_Cadence_Backward;
				BackwordDelayCnt2 = 0;
			}
		}
	}
	
	//踏频计算及启动和停止判断
	static uint32_t CadenceCalTimeCnt = 0;       //用于计算蹋频值
	static int32_t Cadence_ActiveFlt = 0;
	uint16_t CadenceTemp;
	
	static uint8_t CadenceStarFlagCnt = 0;       //用于判断启动
	static uint32_t CadenceStopJudgeTimeCnt = 0; //用于判断停止
	
	uint8_t CadenceStartThresholdValue = 2;  //踏频启动阈值，霍尔信号数，6度/个
	
	if((MC_CadenceSensorStatus.HallGropuStatus & 0x01) != (MC_CadenceSensorStatus.HallGropuStatus_Old & 0x01))
	{
	  //踏频计算及滤波处理
	  CadenceTemp	= 1000 / (HAL_GetTick() - CadenceCalTimeCnt);//转1圈有60个信号，根据两个信号之间的时间计算踏频值rpm
		CadenceCalTimeCnt = HAL_GetTick();
		
		Cadence_ActiveFlt += (((int32_t)CadenceTemp << 8) - Cadence_ActiveFlt) >> 4;
		p_MC_CadenceResult->Cadence_Data = (uint8_t)(Cadence_ActiveFlt >> 8);
		
		/*上坡时，启动阈值为1*/
		if(UpSlopeFlag == TRUE)
		{
			CadenceStartThresholdValue =  1;
		}
		else
		{
			CadenceStartThresholdValue =  StarCount;
		}
		
		//起步判断
		if(p_MC_CadenceResult->Cadence_Dir == MC_Cadence_Forward)
		{
		  CadenceStarFlagCnt++;
			if(CadenceStarFlagCnt >= CadenceStartThresholdValue)
			{
			  p_MC_CadenceResult->IsStopFlag = FALSE;
			}
		}
		else
		{
		  p_MC_CadenceResult->IsStopFlag = TRUE;
		}
		
		/*根据踏频的信号，对力矩进行滤波处理*/
		p_MC_CadenceResult->torqueByCadence = torqueFilteredThroughCadence(ADC_SensorData.TorqueSensor,1);
		
		//更新停机计时数值
		CadenceStopJudgeTimeCnt = HAL_GetTick();
	}
	
	//停机判断
	if(p_MC_CadenceResult->Cadence_Data < (1500 / StopDelayTime))
	{
	  StopDelayTime *= 3;
	}
	if((HAL_GetTick() - CadenceStopJudgeTimeCnt) > StopDelayTime)
	{
		p_MC_CadenceResult->IsStopFlag = TRUE;
		p_MC_CadenceResult->Cadence_Dir = MC_Cadence_Stop;
		p_MC_CadenceResult->Cadence_Data = 0;
		CadenceTemp = 0;
		Cadence_ActiveFlt = 0;
		CadenceStarFlagCnt =0;
		
		/*清零相关变量*/
		p_MC_CadenceResult->torqueByCadence = torqueFilteredThroughCadence(ADC_SensorData.TorqueSensor,0);
		
	}
	
	MC_CadenceSensorStatus.HallGropuStatus_Old = MC_CadenceSensorStatus.HallGropuStatus;
}

uint16_t torqueFilteredThroughCadence(uint16_t torque_temp, uint8_t modeFlag)
{
	#define T_FIFO_LENGTH 30                       //每个信号6度，30为180度 
	static uint16_t T_FIFO[T_FIFO_LENGTH]={0};
	static uint8_t T_Index=0;
	static uint32_t T_sum=0;
	static uint8_t filter_status_falg=0;
	uint16_t result_filtered=0;
	
	if( modeFlag != 0 )
	{
		/*根据踏频信号，采样最近180度内力矩平均值*/
		
		if(filter_status_falg != 0)
		{
			T_sum -= T_FIFO[T_Index];
			T_sum += torque_temp;
			T_FIFO[T_Index] = torque_temp;
		}
		
		T_Index++;
		
		if(T_Index >= T_FIFO_LENGTH)
		{
			T_Index = 0;	
			if(filter_status_falg == 0)
			{
				filter_status_falg = 1;
			}
			else if(filter_status_falg == 1)
			{
				filter_status_falg = 2;
			}
		}
		
		if(filter_status_falg == 2)
		{
			result_filtered = T_sum / T_FIFO_LENGTH;
		}
		else if(filter_status_falg == 1)
		{
			/*启动后的前180度-360度，根据实际数据个数求平均值*/
			if(T_Index == 0)
			{
				result_filtered = torque_temp;
			}
			else
			{
				result_filtered = T_sum / T_Index;
			}
		}
		else
		{
			/*启动后的前180度，使用实时值*/
			result_filtered = torque_temp;
		}
		
		/*力矩实时值低于滤波值的五分之一，使用实时值*/
		if(torque_temp < (result_filtered/5) )
		{
			result_filtered = torque_temp;
		}
		/*踏频信号采样*/
	}
	else
	{
		/*清零变量*/
		for(uint8_t i=0;i<T_FIFO_LENGTH;i++)
		{
				T_FIFO[i] = 0;
		}
		T_Index = 0;
		T_sum = 0;
		filter_status_falg=0;
	}		
	
	return result_filtered;
}