#include "Ignition.h"
#include "Global_Variable.h"
#include "BMS_SOC.h"
#include "Low_SOC_mode.h"
#include "NO_MCU_SIG.h"
#include "can.h"
#include "board.h"
#include "Motor_logic.h"


void Motor_init(void){


	if(GV_VCU_GearSelStat == 0 ){ //P

		GV_VCU_GearCmd= 7;
		GV_VCU_MtrDir = 0;

		GV_VCU_EPAM_Park_Request_to_EPC = 1;


	}else if(GV_VCU_GearSelStat == 1){ //R
		GV_VCU_GearCmd= 2;
		GV_VCU_MtrDir = 2;

		GV_VCU_EPAM_Park_Request_to_EPC = 2;

	}else if(GV_VCU_GearSelStat == 2){ //N

		GV_VCU_GearCmd= 3;
		GV_VCU_MtrDir = 0;

		GV_VCU_EPAM_Park_Request_to_EPC = 2;

	}else if(GV_VCU_GearSelStat == 3){ //D

		GV_VCU_GearCmd= 4;
		GV_VCU_MtrDir = 1;

		GV_VCU_EPAM_Park_Request_to_EPC = 2;
	}


	if(GV_VCU_BmsFlt == 0 && GV_VCU_BmsHvSt == 1 ){
		GV_VCU_PwrEnable = 1;
		GV_VCU_ControlMode = 1;
		GV_VCU_Ready 	= 1;
		GV_VCU_TorqueContol = 1;
	}

	GV_VCU_BmsFlt 	        = GV_ACU_BMS_FltSta;
	GV_VCU_BmsHvSt 	        = GV_BMS_HVOn_STA;
	GV_VCU_PosTrqLimit      = 80; //updated 0829
	GV_VCU_NegTrqLimit      = -50;
}

float Motor_Tunning = 2;
//void RC_Motor_Logic (void){
//
//
//	if(GV_VCU_Drive_Mode == 1){
//		if(GV_VCU_RC_long_Ctl_mode == 1){
//			GV_VCU_TrqCmd = GV_RC_Drive_ACC_Cmd * 10;
//		}else if(GV_VCU_RC_long_Ctl_mode == 0 ){
//			GV_VCU_TrqCmd = GV_RC_Vx_Command*Motor_Tunning;
//		}
//	}
//}


void ACU_Motor_Logic (void){

	if(GV_VCU_Drive_Mode == 0){
		if(GV_VCU_ACU_long_Ctl_mode == 1){
			GV_VCU_TrqCmd = GV_ACU_Drive_ACC_Cmd * 10;
		}else if(GV_VCU_ACU_long_Ctl_mode == 0 ){
			GV_VCU_TrqCmd = GV_ACU_Vx_Command / 5;
		}
	}
}