/*
 * GV_Operation_Mode.c
 *
 *  Created on: 2024. 2. 29.
 *      Author: SUPYO
 */

#include <Operation_Layer/Drive_Mode.h>
#include <Operation_Layer/MCU/Motor_Status.h>
#include "Operation_Layer/Lamp/HMI_LAMP.h"
#include "IG_Layer/Ignition.h"
#include "Global_Variable.h"
#include "Safety_Layer/EMERGENCY/Emergency.h"
#include "Logic/ADM_Integrated_Logic.h"
#include "Logic/Param_Match.h"

const float Auto_Brake_Tunning = 10;

unsigned int Auto_Brake_Flag = 0;
float temp_Vehicle_Speed = 0;

/*
 * ACU       : GV_VCU_Drive_Mode = 0
 * RC        : GV_VCU_Drive_Mode = 1
 * Emergency : GV_VCU_Drive_Mode = 2
 * Default   : GV_VCU_Drive_Mode = 3
 */
void Drive_Mode_Check(void)
{
	if(GV_RC_Emergency_Stop== 1 || Emergency_Switch == 1 || GV_ACU_Emergency_Stop == 1 || GV_RC_ALV_FLAG == 1 || GV_Act_Fault_Exist == 1)	// 비상정지
	{
		GV_VCU_Drive_Mode = 2;
	}
	else if(GV_ACU_Drive_mode == 1 && GV_VCU_Drive_Mode == 3 && GV_RC_Drive_Mode == 0) 							// ACU
	{
		GV_VCU_Drive_Mode = 0;

	}
	else if(GV_RC_Drive_Mode == 1 && GV_VCU_Drive_Mode == 3 && GV_ACU_Drive_mode == 0) 							// RC
	{
		GV_VCU_Drive_Mode = 1;
	}
	else if(GV_ACU_Drive_mode == 0 && GV_VCU_Drive_Mode == 0)													// 기본모드
	{
		GV_VCU_Drive_Mode = 3;
	}
	else if(GV_RC_Drive_Mode == 0 && GV_VCU_Drive_Mode == 1)													// 기본모드
	{
		GV_VCU_Drive_Mode = 3;
	}
	else if(GV_RC_Drive_Mode == 1 && GV_ACU_Drive_mode == 1 && GV_ACU_Emergency_Stop == 0)						// RC, ACU 둘이 충돌 시 RC모드로 전환
	{
		GV_VCU_Drive_Mode = 1;
	}

	if(GV_Charging_Status_val == 1 || GV_Charging_Status_val == 2)
	{
		GV_VCU_Drive_Mode = 3; //Author : JaeminSong, Date : 0830, Description: If Current_Mode is Charging, We need to change default Mode.
	}
}

//============================== Default Mode ==============================//
void Default_Mode_Func()
{
	HV_OFF_FUNC();				// 고전압 off

	if(Emergency_Switch == 0 && GV_RC_Emergency_Stop == 0 && GV_ACU_Emergency_Stop == 0)
	{
		GV_Out_Lamp_BRAKE_CMD 	= 0;
		GV_Out_Lamp_Hazard_CMD 	= 0;
		GV_RC_Lamp_Hazard_CMD	= 0;
		GV_RC_Lamp_BRAKE_CMD 	= 0;
	}

	// 기본 모드 시 DRL 제외 LAMP 모두 OFF
	GV_VCU_RC_Emergency_Stop 	= 0;
	GV_VCU_ACU_Emergency_Stop 	= 0;

	GV_Out_Lamp_HBEAM_CMD 	  	= 0;
	GV_Out_Lamp_Head_CMD 	  	= 0;
	GV_Out_Lamp_LTRN_CMD 	  	= 0;
	GV_Out_Lamp_Reverse_CMD   	= 0;
	GV_Out_Lamp_RTRN_CMD 	  	= 0;
	GV_Out_Lamp_Position_CMD	= 0;
	GV_Out_Lamp_DRL_CMD 	  	= 1;
	GV_Out_Wiper_CMD 		  	= 0;
}

//============================== ACU Mode ==============================//
void Autonomous_Mode_Func()
{
	HV_ON_FUNC();		// 고전압 체결

	if(GV_ACU_Emergency_Stop != 1)	GV_VCU_ACU_Emergency_Stop = 0;
}

//---------------------RC모드-------------------------//
void Mannual_Mode_Func()
{
	HV_ON_FUNC();	//	 고전압 체결

	if(GV_RC_Emergency_Stop != 1){
		GV_VCU_RC_Emergency_Stop = 0;
	}

	GV_VCU_Vx_Limit = GV_RC_VX_Limit;
}

//---------------------이머전시 모드-------------------------//
void Emergency_Stop_Mode_Func()
{
	GV_VCU_EPAM_Park_Request_to_EPC = 2;	// EPam release
	GV_VCU_GearSelStat = 2;					// gear N

	GV_Out_Lamp_BRAKE_CMD = 2;
	GV_Out_Lamp_Hazard_CMD = 2;

	GV_VCU_RC_Emergency_Stop = 1;
	GV_VCU_ACU_Emergency_Stop = 1;

	// 고전압 off
	HV_OFF_FUNC();

	//Author : JaeminSong , Date : 240830, Description : For Auto Brake in Emergency Mode
	if(Emergency_Switch == 0 && GV_RC_Emergency_Stop == 0 && GV_ACU_Emergency_Stop == 0 &&GV_RC_ALV_FLAG == 0 && GV_Act_Fault_Exist == 0)
	{
		GV_Out_Lamp_BRAKE_CMD = 0;
		GV_Out_Lamp_Hazard_CMD = 0;

		GV_VCU_RC_Emergency_Stop = 0;
		GV_VCU_ACU_Emergency_Stop =0;

		GV_VCU_Drive_Mode = 3;
	}
}

void Operation_IDB_RWA()
{
	/*for IDB Cmd*/
	GV_DriveTorqueCommand = GV_Motor_Torque_Cmd;
	GV_DriveTorqueCommandValid = 1;
	GV_PowertrainMode = 1;
	//GV_GearPosition = GV_VCU_GearSelStat;

	/*for RCU Cmd*/
	GV_RCU_PowertrainMode = 1;
	GV_RCU_GearPosition = GV_VCU_GearSelStat;

	/*for IDB Wakeup*/
	GV_IDB_032_WakeUpCommand = 1;
	/*for RCU Wakeup*/
	GV_RCU_033_WakeUpCommand = 1;

	/*for RWA1 Cmd*/
	GV_RWA_040_RackAngleCmdValid = 1;
	/*for RWA2 Cmd*/
	GV_RWA2_040_RackAngleCmdValid = 1;

	/*for RWA1 Wakeup*/
	GV_RWA_310_ModeCommand = 1;
	GV_RWA_310_WakeupCommand = 1;

	/*for RWA2 Wakeup*/
	GV_RWA2_310_ModeCommand = 0;
	GV_RWA2_310_WakeupCommand = 0;

	/*for RWS1 CMD*/
	GV_RWS1_040_RackAngleCmdValid = 1;

	/*for RWS1 Wakeup*/
	GV_RWS1_310_WakeupCommand = 1;
	GV_RWS1_310_ModeCommand = 1;

	/*for RWS2 CMD*/
	GV_RWS2_040_RackAngleCmdValid = 1;

	/*for RWS2 Wakeup*/
	GV_RWS2_310_WakeupCommand = 0;
	GV_RWS2_310_ModeCommand = 0;
}

void Vehicle_Environment()
{
	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;
}

void Drive_Mode_Func()
{
	Operation_IDB_RWA();
	Vehicle_Environment();
    Drive_Mode_Check();
	Input_Data_Set_Logic();
	ADM_Integrated_Logic_step();
	Output_Data_Set_Logic();

    if(GV_VCU_Drive_Mode == 0) //Autonomous Driving
    {
    	Autonomous_Mode_Func();
    }
    else if(GV_VCU_Drive_Mode == 1)  //RC Control
    {
    	Mannual_Mode_Func();
    }
    else if(GV_VCU_Drive_Mode == 2) // Emergency Stop
    {
    	Emergency_Stop_Mode_Func();
    }
    else if(GV_VCU_Drive_Mode == 3)
    {
    	Default_Mode_Func();
    }

    //---------------------------------
    //GV_RC_BrakeTorqueCommand_Master = GV_RC_BrakeTorqueCommand;
}