#include "e_ctrl.h"
#include "foc/foc_config.h"

static e_Ctrl g_eCtrl;

void eCtrl_init(u16 ebrk_time, u16 accl_time){
	g_eCtrl.ebrk_time = ebrk_time;
	g_eCtrl.accl_time = accl_time;
	if (g_eCtrl.accl_time == 0) {
		g_eCtrl.accl_time = DEFAULT_D_TIME;
	}
	if (g_eCtrl.ebrk_time == 0) {
		g_eCtrl.ebrk_time = DEFAULT_D_TIME;
	}
	ramp_ctrl_init(&g_eCtrl.torque_ramp);
	ramp_ctrl_init(&g_eCtrl.speed_ramp);

	ramp_set_points(&g_eCtrl.torque_ramp, THROTTLE_MIN_IDQ, THROTTLE_MIN_IDQ);
	ramp_set_step_value(&g_eCtrl.torque_ramp, TORQUE_STEP);

	ramp_set_points(&g_eCtrl.speed_ramp, THROTTLE_MIN_RPM, THROTTLE_MIN_RPM);
	ramp_set_step_value(&g_eCtrl.speed_ramp, SPEED_STEP);
}


void eCtrl_set_TrqCurrent(float c) {
	ramp_t *ramp = &g_eCtrl.torque_ramp;
	float c_now = ramp_get_interpolation(ramp);
	float step_val = TORQUE_STEP;
	int   sign = 1;

	if (c < c_now) {
		sign = -1;
	}
	u32 step_count = 1; 
	u32 step_ms = eCTRL_STEP_TS;	
	if (sign > 0) { //增加扭矩
		step_count = (c - c_now)/TORQUE_STEP + 1;
		step_ms = g_eCtrl.accl_time / step_count;	
		if (step_ms < eCTRL_STEP_TS) {
			step_ms = eCTRL_STEP_TS;
			step_val = (c - c_now)/(g_eCtrl.accl_time / step_ms);
		}
	}else if (sign < 0) {
		step_count = (c_now - c)/TORQUE_STEP + 1;
		step_ms = g_eCtrl.ebrk_time / step_count;
		if (step_ms < eCTRL_STEP_TS) {
			step_ms = eCTRL_STEP_TS;
			step_val = (c_now - c)/(g_eCtrl.ebrk_time / step_ms);
		}		
	}
	ramp_set_step_value(ramp, sign * step_val);
	ramp_set_step_time(ramp, step_ms);
	ramp_set_target(ramp, c);
	
	ramp_exc(ramp);
}


void eCtrl_set_TargetSpeed(float s) {
	ramp_t *ramp = &g_eCtrl.speed_ramp;
	float s_now = ramp_get_interpolation(ramp);
	float step_val = SPEED_STEP;
	int   sign = 1;

	if (s < s_now) {
		sign = -1;
	}
	u32 step_count = 1; 
	u32 step_ms = eCTRL_STEP_TS;	
	if (sign > 0) { //加速
		step_count = (s - s_now)/SPEED_STEP + 1;
		step_ms = g_eCtrl.accl_time / step_count;	
		if (step_ms < eCTRL_STEP_TS) {
			step_ms = eCTRL_STEP_TS;
			step_val = (s - s_now)/(g_eCtrl.accl_time / step_ms);
		}
	}else if (sign < 0) {
		step_count = (s_now - s)/SPEED_STEP + 1;
		step_ms = g_eCtrl.ebrk_time / step_count;
		if (step_ms < eCTRL_STEP_TS) {
			step_ms = eCTRL_STEP_TS;
			step_val = (s_now - s)/(g_eCtrl.ebrk_time / step_ms);
		}		
	}
	ramp_set_step_value(ramp, sign * step_val);
	ramp_set_step_time(ramp, step_ms);
	ramp_set_target(ramp, s);
	
	ramp_exc(ramp);
}



float eCtrl_get_RefSpd(void) {
	return ramp_get_interpolation(&g_eCtrl.speed_ramp);
}

float eCtrl_get_RefTorque(void) {
	return ramp_get_interpolation(&g_eCtrl.torque_ramp);
}


float eCtrl_get_FinalSpd(void) {
	return ramp_get_target(&g_eCtrl.speed_ramp);
}

float eCtrl_get_FinalTorque(void) {
	return ramp_get_target(&g_eCtrl.torque_ramp);
}

void eCtrl_brake_signal(bool hw_brake) {
	if (hw_brake != g_eCtrl.hw_brake) {
		g_eCtrl.hw_brake = hw_brake;
		if (hw_brake) {
			g_eCtrl.brake_ts = shark_get_mseconds();
		}
	}
	
	if (g_eCtrl.hw_brake) {
		float ebrk_torque = 0.0f;
		float ebrk_speed = 0.0f;
		if (shark_get_mseconds() - g_eCtrl.brake_ts >= eCTRL_Brake_TIME) {
			if (g_eCtrl.accl_time != DEFAULT_D_TIME) {
				ebrk_torque = eCTRL_NEG_TORQUE;
			}
		}
		eCtrl_set_TrqCurrent(ebrk_torque);
		eCtrl_set_TargetSpeed(ebrk_speed);
	}
}