#include "Ignition.h"
#include "Global_Variable.h"
#include "BMS_SOC.h"
#include "Low_SOC_mode.h"
#include "CE_ON_CMD.h"
#include "Angle_Speed.h"
#include "can.h"
#include "board.h"

// RWA 앵글 스피드 조절, 튜닝 변수
float RWA_Angle_Speed_limit = 4; //updated 0829, Author : JaeminSong

float Angle_Speed_Gain = 0.065;
// RWS 앵글 스피드 조절, 튜닝 변수
float RWS_Angle_Speed_limit = 2.7;
// 조향 앵글 각 270/s 로 제한

void Angle_Speed(void){
	GV_RWA_040_RackAngleCmd  = GV_MASTER_RWA_RackAngleCommand;
	GV_RWA2_040_RackAngleCmd = GV_MASTER_RWA_RackAngleCommand;
#if 0
	if(GV_VCU_Drive_Mode == 1 ){


		if(GV_MASTER_RWA_RackAngleCommand !=  Cur_RWA_RACK_ANGLE){

			if(GV_MASTER_RWA_RackAngleCommand < 0){ // 우회전 일 때


				if(Cur_RWA_RACK_ANGLE < GV_MASTER_RWA_RackAngleCommand){  // 우회전 가다가 좌회전 꺾을때
					Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE+(4 - 0.065 *GV_VCU_Vx_Fbk) ;

					GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
					GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
				}

				if(Cur_RWA_RACK_ANGLE > GV_MASTER_RWA_RackAngleCommand){ // 우회전 계속 할때
					Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE-(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk);

					GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
					GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
				}

			}


			if(GV_MASTER_RWA_RackAngleCommand > 0){ // 좌회전 일 때


				if(Cur_RWA_RACK_ANGLE > GV_MASTER_RWA_RackAngleCommand){// 좌회전 가다가 우회전 꺾을때
					Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE-(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk) ;

					GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
					GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
				}

				if(Cur_RWA_RACK_ANGLE < GV_MASTER_RWA_RackAngleCommand){// 좌회전 계속 할때
					Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE+(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk) ;

					GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
					GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
				}

			}


	}else {

		GV_RWA_040_RackAngleCmd = GV_MASTER_RWA_RackAngleCommand;
		GV_RWA2_040_RackAngleCmd= GV_MASTER_RWA_RackAngleCommand;

	}
	}


#endif


}

void RWS_Angle_Speed(void){


	if(GV_MASTER_RWS_RackAngleCommand !=  Cur_RWS_RACK_ANGLE){

		if(GV_MASTER_RWS_RackAngleCommand < 0){ // 우회전 일 때


			if(Cur_RWS_RACK_ANGLE < GV_MASTER_RWS_RackAngleCommand){  // 우회전 가다가 좌회전 꺾을때
				Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE+RWS_Angle_Speed_limit;

				GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
				GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;

			}

			if(Cur_RWS_RACK_ANGLE > GV_MASTER_RWS_RackAngleCommand){ // 우회전 계속 할때
				Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE-RWS_Angle_Speed_limit;

				GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
				GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
			}

		}


		if(GV_MASTER_RWS_RackAngleCommand > 0){ // 좌회전 일 때


			if(Cur_RWS_RACK_ANGLE > GV_MASTER_RWS_RackAngleCommand){// 좌회전 가다가 우회전 꺾을때
				Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE-RWS_Angle_Speed_limit;

				GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
				GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
			}

			if(Cur_RWS_RACK_ANGLE < GV_MASTER_RWS_RackAngleCommand){// 좌회전 계속 할때
				Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE+RWS_Angle_Speed_limit;

				GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
				GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
			}

		}

	}

}