/*
 * Param_Match.c
 *
 *  Created on: 2025. 4. 21.
 *      Author: MSI
 */

#include "ADM_Integrated_Logic.h"
#include "Global_Variable.h"

void Input_Data_Set_Logic(void)
{
	ADM_Integrated_Logic_U.GV_MCU_RPM					= GV_MCU_RPM;
	ADM_Integrated_Logic_U.GV_Drive_Mode				= GV_VCU_Drive_Mode;
	ADM_Integrated_Logic_U.GV_Vx_Fbk					= GV_VCU_Vx_Fbk;
	ADM_Integrated_Logic_U.GV_IMU_AX_Val				= GV_IMU_AX_Val;
	ADM_Integrated_Logic_U.GV_IMU_AY_Val				= GV_IMU_AY_Val;
	ADM_Integrated_Logic_U.GV_IMU_AZ_Val				= GV_IMU_AZ_Val;
	ADM_Integrated_Logic_U.GV_IMU_PitchRtVal			= GV_IMU_PitchRtVal;
	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag		= GV_ACU_ECU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag1		= GV_ACU_ECU_Fault_Flag1;
	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag2		= GV_ACU_ECU_Fault_Flag2;
	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag3		= GV_ACU_ECU_Fault_Flag3;
	ADM_Integrated_Logic_U.GV_ACU_Fault_Flag			= GV_RC_VCU_ACU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_MCU_Actuator_Fault_Flag	= GV_RC_VCU_MCU_Actuator_Fault_Flag;
	ADM_Integrated_Logic_U.GV_IDB_ECU_Fault_Flag   		= GV_VCU_RC_IDB_ECU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_RCU_ECU_Fault_Flag		= GV_VCU_RC_RCU_ECU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_RWA1_ECU_Fault_Flag		= GV_VCU_RC_RWA1_ECU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_RWA2_ECU_Fault_Flag		= GV_VCU_RC_RWA2_ECU_Fault_Flag;
	ADM_Integrated_Logic_U.GV_RWA_Actuator_Fault		= GV_RC_VCU_RWA_Actuator_Fault_Flag;
	ADM_Integrated_Logic_U.GV_BrakeTorqueCommand 		= GV_RC_BrakeTorqueCommand;
	ADM_Integrated_Logic_U.GV_MCU_EstTrq                = GV_MCU_EstTrq;

//	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag		= 0;
//	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag1		= 0;
//	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag2		= 0;
//	ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag3		= 0;
//	ADM_Integrated_Logic_U.GV_ACU_Fault_Flag			= 0;
//	ADM_Integrated_Logic_U.GV_MCU_Actuator_Fault_Flag	= 0;
//	ADM_Integrated_Logic_U.GV_IDB_ECU_Fault_Flag   		= 0;
//	ADM_Integrated_Logic_U.GV_RCU_ECU_Fault_Flag		= 0;
//	ADM_Integrated_Logic_U.GV_RWA1_ECU_Fault_Flag		= 0;
//	ADM_Integrated_Logic_U.GV_RWA2_ECU_Fault_Flag		= 0;
//	ADM_Integrated_Logic_U.GV_RWA_Actuator_Fault		= 0;

	if(GV_VCU_Drive_Mode == 0)								// ACU Mode
	{
		ADM_Integrated_Logic_U.GV_BrakeTorqueCommand 	= GV_ACU_BrakeTorqueCommand;
		ADM_Integrated_Logic_U.GV_Vx_Command			= GV_ACU_Vx_Command;
		ADM_Integrated_Logic_U.GV_Vx_Limit 				= 40;
		ADM_Integrated_Logic_U.GV_VCU_GearSelStat		= GV_ACU_VCU_GearPos;
		ADM_Integrated_Logic_U.GV_RWA_RackAngleCommand	= GV_ACU_RWA_cmd_deg;
		ADM_Integrated_Logic_U.GV_RWS_RackAngleCommand	= GV_ACU_RWS_cmd_deg;
	}
	else if(GV_VCU_Drive_Mode == 1)	// RC Mode
	{
		ADM_Integrated_Logic_U.GV_BrakeTorqueCommand 	= GV_RC_BrakeTorqueCommand;
		ADM_Integrated_Logic_U.GV_Vx_Command			= GV_RC_Vx_Command;
		ADM_Integrated_Logic_U.GV_Vx_Limit 				= GV_RC_VX_Limit;
		ADM_Integrated_Logic_U.GV_VCU_GearSelStat		= GV_RC_GearSelStat;
		ADM_Integrated_Logic_U.GV_RWA_RackAngleCommand	= GV_RC_RWA_RackAngleCommand;
		ADM_Integrated_Logic_U.GV_RWS_RackAngleCommand	= 0;
	}
	else if(GV_VCU_Drive_Mode == 2 || GV_VCU_Drive_Mode == 3)	// Emergency Mode
	{
		ADM_Integrated_Logic_U.GV_Vx_Command = 0;
	}
}


void Output_Data_Set_Logic(void)
{
	GV_Brake_Torque_Command		= ADM_Integrated_Logic_Y.GV_Brake_Command;
	GV_Motor_Torque_Cmd    		= ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd;
	GV_VCU_Hill_TrqCmd			= ADM_Integrated_Logic_Y.GV_Hill_Torque_Assist;
	GV_RWA_040_RackAngleCmd		= ADM_Integrated_Logic_Y.GV_Master_Rack_Angle_Cmd;
	GV_RWA2_040_RackAngleCmd	= ADM_Integrated_Logic_Y.GV_Master_Rack_Angle_Cmd;
	GV_RWS1_040_RackAngleCmd	= ADM_Integrated_Logic_Y.GV_RWS_RackAngleCmd1;
	GV_RWS2_040_RackAngleCmd	= ADM_Integrated_Logic_Y.GV_RWS_RackAngleCmd1;
	GV_VCU_GearSelStat			= ADM_Integrated_Logic_Y.GV_Gear_Postion_Out;
	GV_Act_Fault_Exist			= ADM_Integrated_Logic_Y.Act_Fault_Exist;	// EILS
//	GV_Act_Fault_Exist			= 0;										// Real
	GV_Target_MCU_Out 			= ADM_Integrated_Logic_Y.Target_MCU_Out;
	GV_Target_IDB_Out 			= ADM_Integrated_Logic_Y.Target_IDB_Out;
	GV_Target_RWA_Out 			= ADM_Integrated_Logic_Y.Target_RWA_Out;

	if (GV_RWA_040_RackAngleCmd > GV_RWA_Limit)			GV_RWA_040_RackAngleCmd = GV_RWA_Limit;
	else if (GV_RWA_040_RackAngleCmd < -GV_RWA_Limit)	GV_RWA_040_RackAngleCmd = -GV_RWA_Limit;

	if (GV_RWA2_040_RackAngleCmd > GV_RWA_Limit)		GV_RWA2_040_RackAngleCmd = GV_RWA_Limit;
	else if (GV_RWA2_040_RackAngleCmd < -GV_RWA_Limit)	GV_RWA2_040_RackAngleCmd = -GV_RWA_Limit;

	if (GV_RWS1_040_RackAngleCmd > 200)			GV_RWS1_040_RackAngleCmd = 200;
	else if (GV_RWS1_040_RackAngleCmd < -200)	GV_RWS1_040_RackAngleCmd = -200;

	if (GV_RWS2_040_RackAngleCmd > 200)			GV_RWS2_040_RackAngleCmd = 200;
	else if (GV_RWS2_040_RackAngleCmd < -200)	GV_RWS2_040_RackAngleCmd = -200;

	if(GV_VCU_Drive_Mode == 0)		// ACU mode
	{
		if(GV_VCU_GearSelStat == 1 || GV_VCU_GearSelStat == 3)	GV_Master_Brake_Torque_Command = GV_ACU_BrakeTorqueCommand + GV_Brake_Torque_Command;
		else													GV_Master_Brake_Torque_Command = GV_ACU_BrakeTorqueCommand;
	}
	else if(GV_VCU_Drive_Mode == 1)	// RC mode
	{
    	if(GV_VCU_GearSelStat == 1 || GV_VCU_GearSelStat == 3)	GV_Master_Brake_Torque_Command = GV_RC_BrakeTorqueCommand + GV_Brake_Torque_Command;
    	else											   		GV_Master_Brake_Torque_Command = GV_RC_BrakeTorqueCommand;
	}
	else if(GV_VCU_Drive_Mode == 2) // Emergency mode
	{
		GV_Master_Brake_Torque_Command = GV_RC_BrakeTorqueCommand + GV_Brake_Torque_Command;
	}
	else if(GV_VCU_Drive_Mode == 3) // Default mode
	{
		GV_Motor_Torque_Cmd = 0;
		GV_Master_Brake_Torque_Command = GV_RC_BrakeTorqueCommand;
//		GV_Master_Brake_Command = 0;	// 기본모드 시 브레이크 잡지 않음
//		GV_MASTER_RWA_RackAngleCommand = 0;	// 스티어링 중앙 정렬
//		GV_MASTER_RWS_RackAngleCommand = 0;
	}

	//-------------------- Brake saturation --------------------//
	if(GV_Master_Brake_Torque_Command >= 2500)	GV_Master_Brake_Torque_Command = 2500;

	//-------------------- Fault mode --------------------//
	GV_IDB_BrakeTorqueCommand = GV_Master_Brake_Torque_Command;
	GV_RCU_BrakeTorqueCommand = GV_Master_Brake_Torque_Command;

	if(GV_VCU_RC_IDB_ECU_Fault_Flag == 1)	GV_IDB_BrakeTorqueCommand = 0;
	if(GV_VCU_RC_RCU_ECU_Fault_Flag == 1)	GV_RCU_BrakeTorqueCommand = 0;

	if(GV_VCU_RC_RWA1_ECU_Fault_Flag == 1)	GV_RWA_040_RackAngleCmd = 0;
	if(GV_VCU_RC_RWA2_ECU_Fault_Flag == 1)	GV_RWA2_040_RackAngleCmd = 0;
	if(GV_RC_VCU_RWA_Actuator_Fault_Flag == 1)
	{
		GV_RWA_040_RackAngleCmd = 0;
		GV_RWA2_040_RackAngleCmd = 0;
	}
}
