#include "enviolo_can.h"
#include "can.h"
#include "speed_sensor.h"
#include "motor_control.h"
#include "log_save.h"
#include "can_process.h"
#include "eeprom_24c02.h"
#include "math_tools.h"

/********全局变量定义********/
EnvioloGearInfo1_Struct_t EnvioloGearInfo1 = {0, 0, 0, 0, 0, 0, 0, 0};
EnvioloGearInfo2_Struct_t EnvioloGearInfo2 = {0, 0, 0, 0, 0, 0, 0, 0};
EnvioloConfigParams_Struct_t EnvioloConfigParams = {43, 20, 30, 90, 0, 119, 5, 0};        //默认前牙盘43T，后牙盘20T，最低踏频30rpm，最高踏频90rpm，车轮周长219cm，自动关机时间5min
EnvioloControlParams_Struct_t EnvioloControlParams = {ENVIOLO_INACTIVE, 0, 0, 0, 0, 0};   //默认未激活，目标变速比0，目标踏频0
GearBox_CanData_Struct_t GearBox_CanData = {FALSE, 0, 0, {0, }};                          //CAN报文数据
ComOK_Check_Struct_t GearBox_OnlineFlag = {FALSE, 0};                                     //CAN在线检测
GearBox_OBC_SetMode_Struct_t GearBox_OBC_SetMode = {GEAR_MODE_NULL, 0, 0, 0, 0, 0, 0, 0}; //默认无效值，目标0
GearBox_OBC_SetParams_Struct_t GearBox_OBC_SetParams = {9, 30, 90, 0, 0, 0, 0, 0};        //默认9档，最低踏频30rpm，最高踏频90rpm
GearBox_Status_Struct_t GearBox_Status = GEAR_STATUS_OFFLINE;                             //变速器状态默认为初始化
GearBox_CalFlag_Struct_t GearBox_CalFlag = GEAR_CAL_SUCCESS;                              //变速器默认校准完成
uint16_t GearBox_RatioTable[24] = {500, 662, 824, 986, 1148, 1310, 1472, 1634, 1800, };   //变速器变速比与档位对照表，默认9速

/********局部函数定义********/
//发送Enviolo变速器配置参数
void GearBox_Send_EnvioloConfigParams(EnvioloConfigParams_Struct_t* p_EnvioloConfigParams)
{
    uint8_t Data[8] = {0,};
	memcpy(Data, (uint8_t*)&p_EnvioloConfigParams->FrontChain, 8);
	CAN_SendData(ENVIOLO_CONFIG_ID, Data, 8);
}

//发送Enviolo变速器工作参数
void GearBox_Send_EnvioloControlParams(EnvioloControlParams_Struct_t* p_EnvioloControlParams)
{
  uint8_t Data[8] = {0,};
	//避免大小端和内存对齐问题，赋值处理
	Data[0] = p_EnvioloControlParams->GearMode;
	Data[1] = p_EnvioloControlParams->RatioMode_Data >> 8;
	Data[2] = p_EnvioloControlParams->RatioMode_Data & 0xFF;
	Data[3] = p_EnvioloControlParams->CadenceMode_Data >> 8;
	Data[4] = p_EnvioloControlParams->CadenceMode_Data & 0xFF;
	Data[5] = 0;
	Data[6] = 0;
	Data[7] = 0;
	CAN_SendData(ENVIOLO_CONTROL_ID, Data, 8);
}

//根据变速器的变速比计算档位
uint8_t GearBox_CalGearSt(uint16_t Ratio, uint8_t Num)
{
	uint8_t Result = 0;
	if(Ratio <= 500)
		Result = 1;
	else if(Ratio >= 1800)
		Result = Num;
	else
    Result = (Ratio - 500) / (1300 / Num) + 1;
  return Result;
}

/*************************全局函数定义*******************************/

//变速器CAN报文解析
void GearBox_CanProcess(GearBox_CanData_Struct_t* CanData)
{
	if(CanData->RefreshFlag == TRUE)
	{
	  //更新通信检测标志
		GearBox_OnlineFlag.IsOK_Flag = TRUE;
		GearBox_OnlineFlag.OK_TrigTime = HAL_GetTick();
		//数据解析
		if(CanData->ID == ID_ENVIOLO_TO_MC_1)
		{
      EnvioloGearInfo1.Pedal_Rpm = CanData->Data[0];
			EnvioloGearInfo1.Actual_Ratio = CanData->Data[1];
			EnvioloGearInfo1.Bike_Speed = CanData->Data[2];
			EnvioloGearInfo1.Battery_Vol = CanData->Data[3];
			EnvioloGearInfo1.Wheel_Rpm = (uint16_t)(CanData->Data[4] << 8) + CanData->Data[5];
			//更新轮速，仅配置为通过通信获取车轮周期时更新
			if(MC_ConfigParam1.SpeedSignal == MC_SPEED_COMMUNICATION)
			{
				if(EnvioloGearInfo1.Wheel_Rpm == 0)
					MC_SpeedSensorData.DiffTime_ms = 0xFFFF;
				else
				{
				  MC_SpeedSensorData.DiffTime_ms = 60000 / EnvioloGearInfo1.Wheel_Rpm; //周期 = 60 / 转速(rpm) * 1000 (ms)
				  MC_SpeedSensorData.IsTrigFlag = TRUE;	
				}				
			}
			EnvioloGearInfo1.StatusFlag.Flag = CanData->Data[6];
			EnvioloGearInfo1.ErrorFlag.Flag = CanData->Data[7];
			//变速器校准成功标志1
			if(EnvioloGearInfo1.StatusFlag.StatusFlagBit.CaliSuccessFlag == 1)
			{
				GearBox_CalFlag = GEAR_CAL_SUCCESS;
			}
			//变速器校准成功标志0
			else
			{
				GearBox_CalFlag = GEAR_CAL_NEED;
			}
		}
		else if(CanData->ID == ID_ENVIOLO_TO_MC_2)
		{
      memcpy((uint8_t*)&EnvioloGearInfo2.PCB_Version_Major, (uint8_t*)CanData->Data, 8);
		}
		CanData->RefreshFlag = FALSE;
	}
}

//发送仪表变速器参数
void GearBox_Send_OBC_Params(GearBox_CalFlag_Struct_t CalFlag, GearBox_OBC_SetParams_Struct_t* p_GearBox_OBC_SetParams, uint8_t* p_EnvioloSoftVer)
{
  uint8_t Data[16] = {0,};
	Data[0] = CalFlag;
	memcpy(Data + 1, (uint8_t*)&p_GearBox_OBC_SetParams->GearNum, 3);
	Data[4] = *p_EnvioloSoftVer;
	SendData(ID_MC_TO_PBU, MODE_REPORT, 0x5510, Data);
}

//初始化变速器工作参数
void GearBox_Init(void)
{
  //根据仪表配置的变速器档位数量初始化变速比与档位对照表    
  do
	{
    uint16_t K;
		if(GearBox_OBC_SetParams.GearNum < 5)
			GearBox_OBC_SetParams.GearNum = 5;
		else if(GearBox_OBC_SetParams.GearNum > 24)
			GearBox_OBC_SetParams.GearNum = 24;
		K = (18000 - 5000) / (GearBox_OBC_SetParams.GearNum - 1);
		ArrayFillZero(GearBox_RatioTable, sizeof(GearBox_RatioTable) >> 1);
		for(uint8_t i = 0; i < (GearBox_OBC_SetParams.GearNum - 1); i++)
		{
			GearBox_RatioTable[i] = 500 + (i * K) / 10;
		}
		GearBox_RatioTable[GearBox_OBC_SetParams.GearNum - 1] = 1800;
	}while(0);

	//初始化变速器参数
	do
	{
		EnvioloConfigParams.FrontChain = MC_ConfigParam1.TeethNum_F * 24 / 10;  //前牙盘，小牙盘升速比2.4
		EnvioloConfigParams.RearTeeth = MC_ConfigParam1.TeethNum_B;             //后牙盘
		EnvioloConfigParams.WheelSize = (MC_ConfigParam1.WheelSize + MC_ConfigParam1.WheelSizeAdj) - 100;  //周长，偏移100cm
		EnvioloConfigParams.CadenceMin = GearBox_OBC_SetParams.CadenceMin;  //最低踏频
		EnvioloConfigParams.CadenceMax = GearBox_OBC_SetParams.CadenceMax;  //最高踏频
	}while(0);
}

//变速器处理主函数
void GearBox_Loop(void)
{
  //开机3s后变速器仍离线直接退出
	if((HAL_GetTick() > 3000) && (GearBox_Status == GEAR_STATUS_OFFLINE))
	{
		MC_RunInfo.GearBoxInfo = GEAR_STATUS_OFFLINE;
		return;
	}
	
	//解析变速器CAN数据
	GearBox_CanProcess(&GearBox_CanData);

	//变速器在线检测
	if(GearBox_OnlineFlag.IsOK_Flag == TRUE)
	{
    if((HAL_GetTick() - GearBox_OnlineFlag.OK_TrigTime) > 3000) //变速器超时3s认为离线
		{
			GearBox_OnlineFlag.IsOK_Flag = FALSE;
		}
	}

	//变速器状态更新
	switch(GearBox_Status)
	{
		case GEAR_STATUS_OFFLINE: //初始状态为离线模式，检测到变速器通讯正常，跳转为初始化模式
		{
			MC_RunInfo.GearBoxInfo = GearBox_Status;
			//发送指令，激活变速器
			do
			{
				static uint32_t SendPeriodCnt = 0;
				if((HAL_GetTick() - SendPeriodCnt) > 200)
				{
					EnvioloControlParams.GearMode = ENVIOLO_ACTIVE;
					GearBox_Send_EnvioloControlParams(&EnvioloControlParams);
					SendPeriodCnt = HAL_GetTick();
				}
			}while(0);

			//变速器通讯正常，跳转初始化模式
			if(GearBox_OnlineFlag.IsOK_Flag == TRUE)
			{
				GearBox_Status = GEAR_STATUS_INITIAL;
			}
			break;
		}
		case GEAR_STATUS_INITIAL:  //初始化模式，发送变速器状态给仪表
		{
      MC_RunInfo.GearBoxInfo = (GearBox_Status & 0xE0) + 0x00;
			//初始化完成，主动发送OBC变速器参数，发送5次。
			do
			{
				static uint32_t SendPeriodCnt = 0;
				static uint8_t SendCount = 0;
				if(SendCount >= 5)
					continue;
				if((HAL_GetTick() - SendPeriodCnt) > 200)
				{
					GearBox_Send_OBC_Params(GearBox_CalFlag, &GearBox_OBC_SetParams, &EnvioloGearInfo2.Soft_Version);
					SendPeriodCnt = HAL_GetTick();
					SendCount++;
				}
			}while(0);
			
			//等待仪表设置变速器工作模式，跳转到对应的模式
			if(GearBox_OBC_SetMode.Mode != GEAR_MODE_NULL)
			{
				if(GearBox_OBC_SetMode.Mode == GEAR_MODE_MANUAL) //仪表设置变速器进入手动模式
				{
					GearBox_Status = GEAR_STATUS_MANUAL;
				}
				else if((GearBox_OBC_SetMode.Mode == GEAR_MODE_AUTO) || (GearBox_OBC_SetMode.Mode == GEAR_MODE_CAL)) //仪表设置变速器进入自动模式或校准模式
				{
					GearBox_Status = GEAR_STATUS_AUTO;
				}
			}			
			break;
		}
		case GEAR_STATUS_MANUAL: case GEAR_STATUS_AUTO:  //手动、自动、校准，三种模式共用一个状态
		{
			//进入校准模式
			if(GearBox_OBC_SetMode.Mode == GEAR_MODE_CAL)
			{
				EnvioloControlParams.GearMode = ENVIOLO_CAL_MODE;
				EnvioloControlParams.RatioMode_Data = 0;
				EnvioloControlParams.CadenceMode_Data = 0;
			}
			//进入手动模式
			else if(GearBox_OBC_SetMode.Mode == GEAR_MODE_MANUAL)
			{
				GearBox_Status = GEAR_STATUS_MANUAL;
				EnvioloControlParams.GearMode = ENVIOLO_MANUAL_MODE;
				if(GearBox_OBC_SetMode.Value < 1)
					EnvioloControlParams.RatioMode_Data = GearBox_RatioTable[0];
				else if(GearBox_OBC_SetMode.Value > GearBox_OBC_SetParams.GearNum)
					EnvioloControlParams.RatioMode_Data = GearBox_RatioTable[GearBox_OBC_SetParams.GearNum - 1];
				else
					EnvioloControlParams.RatioMode_Data = GearBox_RatioTable[GearBox_OBC_SetMode.Value - 1];
				EnvioloControlParams.CadenceMode_Data = 0;
			}
			//进入自动模式
			else if(GearBox_OBC_SetMode.Mode == GEAR_MODE_AUTO)
			{
				GearBox_Status = GEAR_STATUS_AUTO;
				EnvioloControlParams.GearMode = ENVIOLO_AUTO_MODE;
				if(GearBox_OBC_SetMode.Value < GearBox_OBC_SetParams.CadenceMin)
					EnvioloControlParams.CadenceMode_Data = GearBox_OBC_SetParams.CadenceMin;
				else if(GearBox_OBC_SetMode.Value > GearBox_OBC_SetParams.CadenceMax)
					EnvioloControlParams.CadenceMode_Data = GearBox_OBC_SetParams.CadenceMax;
				else
				  EnvioloControlParams.CadenceMode_Data = GearBox_OBC_SetMode.Value;
				EnvioloControlParams.RatioMode_Data = 0;
			}
			//更新变速器运行信息，高5位表示变速器状态，低5位表示变速器当前档位
			if(GearBox_OnlineFlag.IsOK_Flag == FALSE) //离线模式
			{
				MC_RunInfo.GearBoxInfo = GEAR_STATUS_OFFLINE;
			}
			else //在线时发送变速器信息
			{
        if(GearBox_Status == GEAR_STATUS_MANUAL) //手动模式低五位发送挡位
				{
					MC_RunInfo.GearBoxInfo = (GearBox_Status & 0xE0) + (GearBox_CalGearSt(EnvioloGearInfo1.Actual_Ratio * 100, GearBox_OBC_SetParams.GearNum) & 0x1F);
				}
				else if(GearBox_Status == GEAR_STATUS_AUTO) //自动模式低五位发送变速比，单位0.1
				{
					MC_RunInfo.GearBoxInfo = (GearBox_Status & 0xE0) + (EnvioloGearInfo1.Actual_Ratio & 0x1F);
				}
			}			
			//定时发送变速器控制指令
			do
			{
				static uint32_t SendPeriodCnt = 0;
				static uint8_t SendPluseCnt = 0;
				static uint8_t SendCalCnt = 0;

				if((HAL_GetTick() - SendPeriodCnt) > 1000) 
				{
					if(GearBox_OBC_SetMode.Mode == GEAR_MODE_CAL) //校准指令只发送5次
					{
						if(SendCalCnt < 5)
						{
							GearBox_Send_EnvioloControlParams(&EnvioloControlParams); //发送控制指令
							SendCalCnt++;
						}
						SendPluseCnt = 0;
					}
					else //手动和自动指令定时发送
					{
						if(SendPluseCnt < 1)
						{
							GearBox_Send_EnvioloConfigParams(&EnvioloConfigParams); //发送配置参数
							SendPluseCnt++;
						}
						else if(SendPluseCnt < 5)
						{
							GearBox_Send_EnvioloControlParams(&EnvioloControlParams); //发送控制指令
							SendPluseCnt++;
							if(SendPluseCnt >= 5)
							{
								SendPluseCnt = 0;
							}
						}
						SendCalCnt = 0;
					}
					SendPeriodCnt = HAL_GetTick();
				}				 
			}while(0);
			//校准模式定时发送踩踏提示
			do
			{
			  static uint32_t SendPeriodCnt = 0;
				static uint8_t SendCount = 0xFF;
				GearBox_CalFlag_Struct_t CalFlgTemp = GEAR_CAL_PEDAL;				
				
				if((HAL_GetTick() - SendPeriodCnt) > 1000)
				{
				  //校准中发送踩踏提示
					if(GearBox_CalFlag == GEAR_CAL_NEED)
					{
					  CalFlgTemp = GEAR_CAL_PEDAL;
						GearBox_Send_OBC_Params(CalFlgTemp, &GearBox_OBC_SetParams, &EnvioloGearInfo2.Soft_Version);
						SendCount = 0;
					}
					//校准成功发送校准完成，发送5次
					else if(GearBox_CalFlag == GEAR_CAL_SUCCESS)
					{
					  CalFlgTemp = GEAR_CAL_SUCCESS;
						if(SendCount < 5)
						{
							GearBox_Send_OBC_Params(CalFlgTemp, &GearBox_OBC_SetParams, &EnvioloGearInfo2.Soft_Version);
							SendCount++;
						}
					}
					SendPeriodCnt = HAL_GetTick();
				}
			}while(0);
			break;
		}		
		default:
			break;
	}

}