#include "System_Layer/Drive_Mode.h"
#include "System_Layer/CheckTime/CheckTime.h"


static bool IsVehicleStopped(void)
{
	return (GV_VCU_Vx_Fbk < 1.0f);
}

static bool HasAcuRequest(void)
{
	return (GV_ACU_Drive_mode == 1U);
}

static bool IsParkGear(void)
{
	return (GV_VCU_GearSelStat == 0U);
}

static bool IsRcParkRequest(void)
{
	return (GV_RC_GearSelStat == 0U);
}

static uint8_t s_prevRcManualOn = 0U;
static uint8_t s_prevRcAutoOn = 0U;
static RcModeReq_e s_lastRcModeReq = RC_REQ_DEFAULT;
static RcModeReq_e s_rcModeReqRaw = RC_REQ_NONE;

static DriveMode_e curMode = DRIVE_MODE_DEFAULT;
static DriveMode_e nextMode = DRIVE_MODE_DEFAULT;
static HandoverPhase_e nextPhase = HO_NONE;

static bool IsRcRequestCompleted(RcModeReq_e request, DriveMode_e mode)
{
	return ((request == RC_REQ_DEFAULT) && (mode == DRIVE_MODE_DEFAULT)) ||
		   ((request == RC_REQ_MANUAL) && (mode == DRIVE_MODE_RC)) ||
		   ((request == RC_REQ_AUTO) && (mode == DRIVE_MODE_ACU));
}


void CheckIgnition(void)
{
	CheckSystemFault();

	 switch (GV_IG_Process_Status)
	{
	 	// ----- 0 ----- //
		case KEY_STATE_OFF_COMPLETE:
			SET_SW_VER();
			Soc_Check_befor_IGN();	// Check SoC

			if (GV_VCU_Ignition == 1 && GV_BMS_Fault_Check == 0 && GV_HV_Apply_Check == 0)
			{
				GV_VCU_Ready = 1;
				GV_CE_ON_CMD = 1;
				GV_IG_Process_Status = KEY_STATE_ON_PROCESS;
			}
			break;

		// ----- 1 ----- //
		case KEY_STATE_ON_PROCESS:
			// Request init status from ECUs, Check if the ECUs are Ready
			GV_MCU_Init_Status = (GV_CE_ON_CMD == 1);
			GV_Steer_Init_Status = (GV_CE_ON_CMD == 1);
			GV_Brake_Init_Status = (GV_CE_ON_CMD == 1);
			GV_ACU_Init_Status = (GV_CE_ON_CMD == 1);
			GV_All_ECU_Ready_Flag = (GV_BCM_Init_Status == 1 && GV_MCU_Init_Status == 1 && GV_Steer_Init_Status == 1 && GV_Brake_Init_Status == 1 && GV_ACU_Init_Status == 1);

			if (GV_All_ECU_Ready_Flag == 1)	// When all ECUs are ready, switch to 'Key on complete' state
			{
				GV_EPAM_CMD = 0; // EPAM Release Cmd
				GV_IG_Process_Status = KEY_STATE_ON_COMPLETE;
			}
			else if (GV_VCU_Ignition == 0)	// If the key is turned off during starting, it switches to 'key off process' state
				GV_IG_Process_Status = KEY_STATE_OFF_PROCESS;
			break;

		// ----- 2 ----- //
		case KEY_STATE_ON_COMPLETE:
			ExecuteDriveMode();
			Motor_Status();
			Est_Vehicle_Speed();

			if (GV_VCU_Ignition == 0 && (GV_VCU_Drive_Mode == DRIVE_MODE_DEFAULT || GV_VCU_Drive_Mode == DRIVE_MODE_EMERGENCY))	// If the key is turned off, it switches to 'key off process' state
				GV_IG_Process_Status = KEY_STATE_OFF_PROCESS;
			break;

		// ----- 3 ----- //
		case KEY_STATE_OFF_PROCESS:
			CNT_GV_HV_ON_CMD = 0; // Reset counter
			GV_ALL_ECU_Off_Flag = 1;

			GV_IG_Process_Status = KEY_STATE_OFF_PROCESS_WAIT_HV;
			break;

		case KEY_STATE_OFF_PROCESS_WAIT_HV:	// Wait for high voltage cutoff
			HighVoltageOff();
			break;

		case KEY_STATE_OFF_PROCESS_WAIT_LDC:	// Wait for LDC voltage drop
			if (GV_LDC_INP_VOLT < 5)	// Check the LDC voltage, if the LDC voltage falls below 5V, it switches to 'key off complete' state
			{
				GV_CE_ON_CMD = 0;
				GV_IG_Process_Status = KEY_STATE_OFF_COMPLETE;
			}
			break;

		default:	// Exception
			GV_IG_Process_Status = KEY_STATE_OFF_COMPLETE;
			break;
	}
}


void ExecuteDriveMode()
{
	SetVehicleEnvironment();
	CheckDriveMode();
//    LongitudinalCtrlMode();

	Input_Data_Set_Logic();
	ADM_Integrated_Logic_step();
	Output_Data_Set_Logic();

	switch(GV_VCU_Drive_Mode)
	{
		case DRIVE_MODE_ACU:
			ExecuteACUMode();
			break;
		case DRIVE_MODE_RC:
			ExecuteRCMode();
			break;
		case DRIVE_MODE_EMERGENCY:
			ExecuteEmergencyMode();
			break;
		case DRIVE_MODE_DEFAULT:
			ExecuteDefaultMode();
			break;
	}

	GV_VCU_Drive_Mode = (uint8_t)nextMode;
	GV_VCU_HandoverPhase = nextPhase;
    //GV_RC_BrakeTorqueCommand_Master = GV_RC_BrakeTorqueCommand;
}


void CheckDriveMode(void)
{
	RcModeReq_e requestedRcMode = RC_REQ_NONE;
	bool autoWaitOn = false;

	curMode = (DriveMode_e)GV_VCU_Drive_Mode;
	nextMode = curMode;
	// nextPhase = HO_NONE;

	RcRequestCheck();

	if(isEmergency)
	{
		nextMode = DRIVE_MODE_EMERGENCY;

		nextPhase = HO_HOLD;	// Reset handover phase
		if(isEmergencyRc)			GV_VCU_EmergencyCause = EMG_REQ_RC;
		else if(isEmergencyAcu)		GV_VCU_EmergencyCause = EMG_REQ_ACU;
		else if(isEmergencySwitch)	GV_VCU_EmergencyCause = EMG_REQ_EXTERNAL_SW;
		else if(isEmergencyVcu)		GV_VCU_EmergencyCause = EMG_REQ_VCU;

		ACU_Arc_check();
	}
	else if(isCharging) //Author : JaeminSong, Date : 0830, Description: If Current_Mode is Charging, We need to change default Mode.
	{
		nextMode = DRIVE_MODE_DEFAULT;

		GV_Charging_Status_M_Torq = 0;
		GV_Motor_Torque_Cmd = GV_Charging_Status_M_Torq;
		GV_Charging_Status_Steer_Angle = 0;

		GV_VCU_EmergencyCause = EMG_NONE;
	}
	else
	{	
		requestedRcMode = s_rcModeReqRaw;

		if ((curMode == DRIVE_MODE_EMERGENCY) &&
			(GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL) &&
			(requestedRcMode != RC_REQ_MANUAL))
		{
			nextMode = DRIVE_MODE_EMERGENCY;
			nextPhase = HO_HOLD;
			GV_RC_ModeReq = s_rcModeReqRaw;
			Time_SetAutoOnWait(false);
			return;
		}

		if ((curMode == DRIVE_MODE_EMERGENCY) &&
			(GV_VCU_EmergencyCause == EMG_HO_CONFLICT) &&
			(requestedRcMode != RC_REQ_DEFAULT))
		{
			nextMode = DRIVE_MODE_EMERGENCY;
			nextPhase = HO_HOLD;
			GV_RC_ModeReq = s_rcModeReqRaw;
			Time_SetAutoOnWait(false);
			return;
		}

		if (!((curMode == DRIVE_MODE_EMERGENCY) &&
			  (((GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL) && (requestedRcMode != RC_REQ_MANUAL)) ||
			   ((GV_VCU_EmergencyCause == EMG_HO_CONFLICT) && (requestedRcMode != RC_REQ_DEFAULT)))))
			GV_VCU_EmergencyCause = EMG_NONE;

		switch (requestedRcMode)
        {
			case RC_REQ_NONE:
				nextMode = curMode;
				nextPhase = HO_NONE;
				break;

			case RC_REQ_DEFAULT:
				if ((curMode == DRIVE_MODE_DEFAULT) && (GV_VCU_HandoverPhase == HO_HOLD))
				{
					nextMode = DRIVE_MODE_DEFAULT;
					nextPhase = HO_NONE;
				}
				// Requirement: Default mode + not stopped + RC_REQ_DEFAULT -> Emergency
				else if (curMode == DRIVE_MODE_ACU && !HasAcuRequest() && !IsVehicleStopped())
				{
					nextMode = DRIVE_MODE_EMERGENCY;
					nextPhase = HO_HOLD;
					GV_VCU_EmergencyCause = EMG_HO_SPD_COND_FAIL;
				}
				else if(curMode == DRIVE_MODE_EMERGENCY)
					nextMode = DRIVE_MODE_EMERGENCY;
				// ACU mode -> Default handover
				else if (curMode == DRIVE_MODE_ACU)
				{
					if (!IsVehicleStopped())
					{
						nextPhase = HO_INPROG;
					}
					else
					{
						nextPhase = HO_PERMIT;
						if (!HasAcuRequest())
						{
							nextMode = DRIVE_MODE_DEFAULT;
							nextPhase = HO_NONE;
						}
					}
				}
				else
				{
					if (curMode != DRIVE_MODE_RC || IsVehicleStopped())
					{
						nextMode = DRIVE_MODE_DEFAULT;
						nextPhase = HO_NONE;
					}
				}
				break;

			case RC_REQ_MANUAL:
				if (curMode == DRIVE_MODE_ACU && !IsVehicleStopped())
				{
					nextPhase = HO_HOLD;
					nextMode = DRIVE_MODE_EMERGENCY;
					GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
				}
				else if (curMode == DRIVE_MODE_ACU && nextPhase != HO_HOLD)
				{
					nextPhase = HO_PERMIT;
					if (!HasAcuRequest())
					{
						nextMode = DRIVE_MODE_DEFAULT;
						nextPhase = HO_NONE;
					}
				}
				else if (curMode == DRIVE_MODE_DEFAULT)
				{
					nextMode = DRIVE_MODE_RC;
					nextPhase = HO_NONE;
				}
				else
				{
					nextMode = DRIVE_MODE_RC;
					nextPhase = HO_NONE;
				}
				break;

			case RC_REQ_AUTO:
				if (curMode == DRIVE_MODE_ACU && !HasAcuRequest())
				{
					nextPhase = HO_HOLD;
					nextMode = DRIVE_MODE_EMERGENCY;
					GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
				}
				else if ((curMode == DRIVE_MODE_DEFAULT) && (GV_VCU_HandoverPhase == HO_HOLD))
				{
					nextMode = DRIVE_MODE_DEFAULT;
					nextPhase = HO_HOLD;
				}
				else if (curMode == DRIVE_MODE_DEFAULT && nextPhase != HO_HOLD && IsVehicleStopped())
				{
					nextPhase = HO_PERMIT;
					if (HasAcuRequest())
					{
						nextMode = DRIVE_MODE_ACU;
						nextPhase = HO_NONE;
					}
					else
					{
						autoWaitOn = true;
						if (Time_GetAutoOnWaitSec() >= 30U)
						{
							nextPhase = HO_HOLD;
						}
					}
				}
				else
				{
					nextPhase = HO_NONE;
				}
				break;

			default:
				nextMode = curMode;
				nextPhase = HO_NONE;
				break;
        }
	}

	if (IsRcRequestCompleted(s_rcModeReqRaw, nextMode))
		GV_RC_ModeReq = RC_REQ_NONE;
	else
		GV_RC_ModeReq = s_rcModeReqRaw;

	Time_SetAutoOnWait(autoWaitOn);
}



void RcRequestCheck(void)
{
	if ((GV_RC_Drive_Mode == 1U) && (s_prevRcManualOn == 0U))	s_lastRcModeReq = RC_REQ_MANUAL;
	if ((GV_RC_ACU_Drive_Mode == 1U) && (s_prevRcAutoOn == 0U))	s_lastRcModeReq = RC_REQ_AUTO;

	GV_TradeOff_Emergency_Flag = 0;

	if ((GV_RC_ACU_Drive_Mode == 1U) && (GV_RC_Drive_Mode == 1U))
	{
		if (s_lastRcModeReq == RC_REQ_AUTO)	s_rcModeReqRaw = RC_REQ_AUTO;
		else 								s_rcModeReqRaw = RC_REQ_MANUAL;

		GV_TradeOff_Emergency_Flag = 1;
		GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
	}
	else if (GV_RC_ACU_Drive_Mode == 1U)
	{
		s_rcModeReqRaw = RC_REQ_AUTO;
		s_lastRcModeReq = RC_REQ_AUTO;

		GV_TradeOff_Emergency_Flag = 0;
	}
	else if (GV_RC_Drive_Mode == 1U)
	{
		s_rcModeReqRaw = RC_REQ_MANUAL;
		s_lastRcModeReq = RC_REQ_MANUAL;

		GV_TradeOff_Emergency_Flag = 0;
	}
	else
	{
		if (s_lastRcModeReq == RC_REQ_AUTO)
			s_rcModeReqRaw = RC_REQ_NONE;
		else
			s_rcModeReqRaw = RC_REQ_DEFAULT;

		s_lastRcModeReq = s_rcModeReqRaw;

		GV_TradeOff_Emergency_Flag = 0;
	}

	s_prevRcManualOn = (GV_RC_Drive_Mode == 1U) ? 1U : 0U;
	s_prevRcAutoOn = (GV_RC_ACU_Drive_Mode == 1U) ? 1U : 0U;
}



/*============================== Fault Check ==============================*/
void CheckSystemFault(void)
{
	GV_BMS_Fault_Check 	= !(GV_BMS_FltSta == 0 && GV_BMS_TEMP_FAULT == 0 && GV_BMS_Interlock == 1);	// BMS Fault Check
	GV_MCU_Fault_Check 	= !(GV_MCU_FltSta == 0);							// MCU Fault Check(0 = Connected)
	GV_LDC_Fault_Check 	= !(GV_LDC_FltSta == 0 && GV_LDC_TEMP_FAULT == 0);	// LDC Fault Check
	GV_OBC_Fault_Check 	= !(GV_OBC_Interlock == 1);							// OBC Fault Check
	GV_HV_Apply_Check 	= !(GV_BMS_HVOn_STA == 0);							// BMS High voltage Status Check
	GV_LDC_Apply_Check 	= !(GV_LDC_STA == 0);								// LDC Status Check
}


/*============================== Default Mode ==============================*/
void ExecuteDefaultMode()
{
	HighVoltageOff();
	DefaultLampSet();
	GV_VCU_RC_Emergency_Stop 	= 0;
	GV_VCU_ACU_Emergency_Stop 	= 0;
}


/*============================== ACU Mode ==============================*/
void ExecuteACUMode()
{
	HighVoltageOn();
	ACULampSet();

	if(GV_ACU_Emergency_Stop != 1)	GV_VCU_ACU_Emergency_Stop = 0;

	ACU_Arc_check();
}


/*============================== RC Mode ==============================*/
void ExecuteRCMode()
{
	HighVoltageOn();
	RCLampSet();

	if(GV_RC_Emergency_Stop != 1)	GV_VCU_RC_Emergency_Stop = 0;

	GV_VCU_Vx_Limit = GV_RC_VX_Limit;
}


/*============================== Emergency Mode ==============================*/
static bool s_emergencyAutoParkPending = true;
void ExecuteEmergencyMode()
{

	if (GV_VCU_Vx_Fbk >= 0.1f)			// 1) While vehicle is moving, force N.
		GV_VCU_GearSelStat = 2U; // N
	else if (s_emergencyAutoParkPending)// 2) First stop in emergency: apply P exactly once.
	{
		GV_VCU_GearSelStat = 0U; // P
		s_emergencyAutoParkPending = false;
	}
	else if ((GV_RC_GearSelStat == 0U) || (GV_RC_GearSelStat == 2U))	// 3) After first auto-park, allow RC to toggle only P/N.
		GV_VCU_GearSelStat = GV_RC_GearSelStat;
	else								// Default to P if RC command is invalid
		GV_VCU_GearSelStat = 0U;

	GV_VCU_RC_Emergency_Stop = 1;
	GV_VCU_ACU_Emergency_Stop = 1;

	// 고전??off
	HighVoltageOff();

	EmergencyLampSet();

	// Keep emergency latched for handover speed-fail until RC requests MANUAL.
	if (!isEmergency &&	GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL && s_rcModeReqRaw != RC_REQ_MANUAL)
		nextMode = DRIVE_MODE_EMERGENCY;
	else if (!isEmergency && GV_VCU_EmergencyCause == EMG_HO_CONFLICT && s_rcModeReqRaw != RC_REQ_DEFAULT)
		nextMode = DRIVE_MODE_EMERGENCY;

		// Exit emergency mode when no emergency source remains.
	else if(!isEmergency && !isEmergencyVcu &&
			GV_VCU_EmergencyCause != EMG_HO_CONFLICT &&
			s_rcModeReqRaw == RC_REQ_MANUAL &&
			IsRcParkRequest())
		nextMode = DRIVE_MODE_RC;
	else if(!isEmergency && !isEmergencyVcu &&
			s_rcModeReqRaw == RC_REQ_DEFAULT &&
			IsRcParkRequest())
		nextMode = DRIVE_MODE_DEFAULT;
	else if (!isEmergency && IsRcParkRequest())
	{
		GV_Lamp_BRAKE_CMD	= 0;
		GV_Lamp_Hazard_CMD	= 0;

		GV_VCU_RC_Emergency_Stop 	= 0;
		GV_VCU_ACU_Emergency_Stop	= 0;

		s_emergencyAutoParkPending = true;

		nextMode = DRIVE_MODE_DEFAULT;
		nextPhase = HO_NONE;
	}
	else
		nextMode = DRIVE_MODE_EMERGENCY;
}


/*============================== Longitudinal control mode change ==============================*/
void LongitudinalCtrlMode(void)
{
	// RC Mode
	if(GV_VCU_Drive_Mode == 1 && GV_RC_VCU_long_Ctl_mode == 1 && GV_RC_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_RC_Drive_ACC_Cmd == 0)
		GV_VCU_RC_long_Ctl_mode = 1;
	else if(GV_VCU_Drive_Mode == 1 && GV_RC_VCU_long_Ctl_mode == 0 && GV_RC_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_RC_Drive_ACC_Cmd == 0)
		GV_VCU_RC_long_Ctl_mode = 0;

	// ACU Mode
	if(GV_VCU_Drive_Mode == 0 && GV_ACU_long_Ctl_mode == 1 && GV_ACU_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_ACU_Drive_ACC_Cmd == 0)
		GV_VCU_ACU_long_Ctl_mode = 1;
	else if(GV_VCU_Drive_Mode == 0 && GV_ACU_long_Ctl_mode == 0 && GV_ACU_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_ACU_Drive_ACC_Cmd == 0)
		GV_VCU_ACU_long_Ctl_mode = 0;

	GV_VCU_AccControlFlag = GV_VCU_RC_long_Ctl_mode || GV_VCU_ACU_long_Ctl_mode;
}


/*============================== Set Vehicle Environment ==============================*/
void SetVehicleEnvironment()
{
	//----- Wake-up -----//
	GV_IDB_032_WakeUpCommand 	= 1;
	GV_RCU_033_WakeUpCommand 	= 1;

	GV_RWA_310_ModeCommand 		= 1;
	GV_RWA_310_WakeupCommand 	= 1;
	GV_RWA2_310_ModeCommand 	= 0;
	GV_RWA2_310_WakeupCommand 	= 0;

	GV_RWS1_310_WakeupCommand 	= 1;
	GV_RWS1_310_ModeCommand 	= 1;
	GV_RWS2_310_WakeupCommand 	= 0;
	GV_RWS2_310_ModeCommand 	= 0;


	//----- Command -----//
	GV_DriveTorqueCommand = GV_Motor_Torque_Cmd;
	GV_DriveTorqueCommandValid = 1;
	GV_PowertrainMode = 1;
	//GV_GearPosition = GV_VCU_GearSelStat;
	GV_RCU_PowertrainMode = 1;
	GV_RCU_GearPosition = GV_VCU_GearSelStat;

	GV_RWA_040_RackAngleCmdValid = 1;
	GV_RWA2_040_RackAngleCmdValid = 1;

	GV_RWS1_040_RackAngleCmdValid = 1;
	GV_RWS2_040_RackAngleCmdValid = 1;


	GV_Env_384_EnvironmentTemperature		= 0;
	GV_Env_384_EnvironmentTemperatureValid	= 0;
	GV_Env_384_TimestampDay					= 21;
	GV_Env_384_TimestampHour				= 0;
	GV_Env_384_TimestampMinute				= 0;
	GV_Env_384_TimestampMonth				= 6;
	GV_Env_384_TimestampSecond				= 0;
	GV_Env_384_TimestampYear				= 24;
}


/*============================== High voltage on ==============================*/
void HighVoltageOn(void)
{
	/*Check Fault Status, Contact HV Sys*/
	GV_HV_ON_CMD = 1;

	/*Check Fault Status, Contact LDC Sys*/
	if(GV_HV_Apply_Check == 1)
	{
		GV_LDC_ACT_CMD = 1;
		GV_LDC_TAGT_CMD = 120; // 20240821- 30->40?�로 변�? 추후 ?�율주행 ??180 ?�로 조정 // 20241010 - 150A �?조정// 20241024-> ?�이?�플??�� ?�계 120A �?조정
	}
}


/*============================== High voltage off ==============================*/
void HighVoltageOff(void)
{
	if (GV_LDC_Apply_Check == 0 && GV_HV_ON_CMD == 1)
		CNT_GV_HV_ON_CMD++;

	if (CNT_GV_HV_ON_CMD >= 500)	// Wait for 1000ms
	{
		CNT_GV_HV_ON_CMD = 0;
		GV_HV_ON_CMD = 0;
		GV_IG_Process_Status = KEY_STATE_OFF_PROCESS_WAIT_LDC;
	}

	GV_LDC_ACT_CMD = 0;
	GV_LDC_TAGT_CMD = 0;
}