#include "controller.h"
#include "foc/mc_config.h"
#include "foc/foc_config.h"
#include "foc/core/foc_observer.h"
#include "foc/motor/motor_param.h"
#include "foc/motor/motor.h"
#include "foc/samples.h"
#include "foc/core/f_calc.h"
#include "foc/motor/current.h"
#include "foc/mc_error.h"

static void mot_contrl_pid(mot_contrl_t *ctrl);
static void mot_contrl_ulimit(mot_contrl_t *ctrl);
static void mot_contrl_rtlimit(mot_contrl_t *ctrl);
static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl);

void mot_contrl_init(mot_contrl_t *ctrl) {
	memset(ctrl, 0, sizeof(mot_contrl_t));
	ctrl->foc.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
	ctrl->foc.half_period = FOC_PWM_Half_Period;
	ctrl->force_angle = INVALID_ANGLE;
	ctrl->adv_angle = INVALID_ANGLE;
	ctrl->hwlim.dc_curr = CONFIG_HW_MAX_DC_CURRENT;
	ctrl->hwlim.mot_vel = CONFIG_HW_MAX_MOTOR_RPM;
	ctrl->hwlim.phase_curr = CONFIG_HW_MAX_PHASE_CURR;
	ctrl->hwlim.phase_vol = CONFIG_HW_MAX_PHASE_VOL;
	ctrl->hwlim.dc_vol      = CONFIG_HW_MAX_DC_VOLTAGE;
	ctrl->hwlim.torque  = mc_conf()->m.max_torque; //电机的最大扭矩
	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电池能支持的最大弱磁电流
	ctrl->protlim.dc_curr = HW_LIMIT_NONE;
	ctrl->protlim.torque = HW_LIMIT_NONE;
	ctrl->torque_acc_time = 500; //will be set after start
	ctrl->torque_dec_time = 500; //will be set after start
	ctrl->ebrk_ramp_time = 500;  //will be set after start
	foc_init(&ctrl->foc);
}
bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
	if (ctrl->b_start == start) {
		return true;
	}
	if (start) {
		line_ramp_init(&ctrl->torque_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->vel_lim, CONFIG_LIMIT_RAMP_TIME);
		line_ramp_init(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
		line_ramp_init(&ctrl->target_vd, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
		line_ramp_init(&ctrl->target_vq, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
		line_ramp_init(&ctrl->target_vel, CONFIG_CRUISE_RAMP_TIME);
		line_ramp_init(&ctrl->target_current, CONFIG_CURRENT_RAMP_TIME);
		line_ramp_init(&ctrl->input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
		mot_contrl_pid(ctrl);
		mot_contrl_ulimit(ctrl);
		mot_contrl_rtlimit(ctrl);
		etcs_init(&ctrl->etcs);
	}
	ctrl->b_ebrk_running = false;
	ctrl->b_AutoHold = false;
	ctrl->b_cruiseEna = false;
	ctrl->b_mtpa_calibrate = false;
	ctrl->b_hw_braker = false;

	ctrl->mode_req = CTRL_MODE_OPEN;
	ctrl->mode_running = CTRL_MODE_OPEN;

	ctrl->force_angle = INVALID_ANGLE;
	ctrl->adv_angle = INVALID_ANGLE;
	ctrl->angle_last = INVALID_ANGLE;

	ctrl->dc_curr_filted = 0;
	ctrl->dc_curr_calc = 0;
	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
	ctrl->autohold_torque = 0;
	ctrl->out_current_vec = 0;
	ctrl->target_idq.d = 0;
	ctrl->target_idq.q = 0;
	ctrl->target_torque = 0;
	ctrl->target_torque_raw = 0;

	foc_init(&ctrl->foc);
	foc_observer_init();
	ctrl->b_start = start;
	return true;
}

bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode) {
	if (mode > CTRL_MODE_EBRAKE) {
		mot_contrl_set_error(ctrl, FOC_Param_Err);
		return false;
	}
	ctrl->mode_req = mode;
	return true;
}

u8 mot_contrl_mode(mot_contrl_t *ctrl) {	
	u8 preMode = ctrl->mode_running;

	if (!ctrl->b_start) {
		ctrl->mode_running = CTRL_MODE_OPEN;
	}else if (ctrl->mode_req == CTRL_MODE_OPEN) {
		ctrl->mode_running = CTRL_MODE_OPEN;
	}else if (ctrl->mode_req == CTRL_MODE_SPD || ctrl->b_cruiseEna){
		ctrl->mode_running = CTRL_MODE_SPD;
	}else if (ctrl->mode_req == CTRL_MODE_CURRENT) {
		ctrl->mode_running = CTRL_MODE_CURRENT;
	}else if (ctrl->mode_req == CTRL_MODE_EBRAKE) {
		ctrl->mode_running = CTRL_MODE_EBRAKE;
	}else {
		if (!ctrl->b_cruiseEna) {
			ctrl->mode_running = CTRL_MODE_TRQ;
		}
	}
	if (preMode != ctrl->mode_running) {
		if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_TRQ)) {
			line_ramp_set_acctime(&ctrl->input_torque, ctrl->torque_acc_time);
			line_ramp_set_dectime(&ctrl->input_torque, ctrl->torque_dec_time);
			line_ramp_update(&ctrl->input_torque);
			if (preMode == CTRL_MODE_SPD) {
				ctrl->target_torque_raw = ctrl->target_torque;
				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
			}else if (preMode == CTRL_MODE_CURRENT) {
				ctrl->target_torque_raw = line_ramp_get_interp(&ctrl->target_current);
				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
			}
		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
			PI_Controller_Reset(&ctrl->pi_vel, ctrl->target_torque);
		}else if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_EBRAKE)) {
			line_ramp_reset(&ctrl->input_torque, ctrl->target_torque);
			line_ramp_set_time(&ctrl->input_torque, ctrl->ebrk_ramp_time);
			line_ramp_set_target(&ctrl->input_torque, motor_get_ebreak_toruqe(ctrl->foc.in.mot_velocity));
		}else if ((preMode == CTRL_MODE_EBRAKE) && (ctrl->mode_running == CTRL_MODE_SPD)) {
			PI_Controller_Reset(&ctrl->pi_vel, F_get_air());
		}
	}
	if (ctrl->mode_running == CTRL_MODE_OPEN) {
		line_ramp_step(&ctrl->target_vd);
		line_ramp_step(&ctrl->target_vq);
	}
	return ctrl->mode_running;
}

static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
	static float a_max = 0, b_max = 0, c_max = 0;
	static u32 _unbalance_cnt = 0;
	static u32 _unbalance_time = 0;
	foc_t *foc = &ctrl->foc;
	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US / 2.0f;
	if (lowpass > 1.0f) {
		lowpass = 1.0f;
	}
	LowPass_Filter(ctrl->phase_curr_filted[0], foc->in.curr_abc[0], lowpass);
	LowPass_Filter(ctrl->phase_curr_filted[1], foc->in.curr_abc[1], lowpass);
	ctrl->phase_curr_filted[2] = -(ctrl->phase_curr_filted[0] + ctrl->phase_curr_filted[1]);
	if ((ctrl->angle_last == INVALID_ANGLE) || (foc->mot_vel_radusPers < 100) || ctrl->out_current_vec < 50) {
		ctrl->angle_last = foc->in.mot_angle;
		a_max = b_max = c_max = 0;
		_unbalance_cnt = 0;
		_unbalance_time = get_tick_ms();
		_cycle_cnt = 0;
		_last_mod_cnt = 0;
		return;
	}
	float delta_angle = foc->in.mot_angle - ctrl->angle_last;
	if (delta_angle > 200 || delta_angle < -200) { //one cycle
		_cycle_cnt ++;
	}
	ctrl->angle_last = foc->in.mot_angle;
	u32 mod_cnt = _cycle_cnt % CONFIG_PHASE_UNBALANCE_PEAK_CNT;
	bool trigger = false;
	if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
		trigger = true;
	}
	_last_mod_cnt = mod_cnt;

	a_max = MAX(a_max, ctrl->phase_curr_filted[0] * (2.2f));
	b_max = MAX(b_max, ctrl->phase_curr_filted[1] * (2.2f));
	c_max = MAX(c_max, ctrl->phase_curr_filted[2] * (2.2f));
	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
		float i_min = 1000.0f, i_max = 0;
		if (a_max > i_max) {
			i_max = a_max;
		}
		if (a_max < i_min) {
			i_min = a_max;
		}
		if (b_max > i_max) {
			i_max = b_max;
		}
		if (b_max < i_min) {
			i_min = b_max;
		}
		if (c_max > i_max) {
			i_max = c_max;
		}
		if (c_max < i_min) {
			i_min = c_max;
		}
		float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
		if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
			if ((_unbalance_cnt++ >= 500) || (get_delta_ms(_unbalance_time) >= 1000*10)) {
				if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
					mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
				}
			}
		}else {
			_unbalance_cnt = 0;
			_unbalance_time = get_tick_ms();
		}
		a_max = b_max = c_max = 0;
	}
}


bool mot_contrl_update(mot_contrl_t *ctrl) {
	foc_t *foc = &ctrl->foc;

	phase_current_get(foc->in.curr_abc);
	clark(foc->in.curr_abc[0], foc->in.curr_abc[1], foc->in.curr_abc[2], &foc->in.curr_ab);

	foc_observer_update(foc->out.vol_albeta.a * TWO_BY_THREE, foc->out.vol_albeta.b * TWO_BY_THREE, foc->in.curr_ab.a, foc->in.curr_ab.b);
	float enc_angle = motor_encoder_get_angle();
	float enc_vel = motor_encoder_get_speed();
	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
		/* detect encoder angle error, do something here */
		if (!foc_observer_sensorless_stable()) {
			foc->in.mot_velocity = 0;
			return false;
		}
		enc_angle = foc_observer_sensorless_angle();
		enc_vel = foc_observer_sensorless_speed();
	}
	if (!ctrl->b_mtpa_calibrate && (ctrl->force_angle != INVALID_ANGLE)) {
		foc->in.mot_angle = ctrl->force_angle;
	}else {
		foc->in.mot_angle = enc_angle;
	}
#ifdef CONFIG_DQ_STEP_RESPONSE
	foc->in.mot_angle = 0;
#endif
	foc->in.mot_velocity = enc_vel;
	foc->in.dc_vol = get_vbus_float();
	foc->in.b_openloop = ctrl->mode_running == CTRL_MODE_OPEN;
	phase_curr_unbal_check(ctrl);
	if (foc->in.b_openloop) {
		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->target_vd);
		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->target_vq);
	}

	foc_update(foc);

	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
	lowpass = fclamp(lowpass, 0.001f, 1.0f);
	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
	LowPass_Filter(ctrl->out_idq_filterd.q, foc->out.curr_dq.q ,lowpass);
	return true;
}

static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
	ctrl->pi_power.max = maxTrq;
	float errRef = line_ramp_get_interp(&ctrl->dc_curr_lim) - ctrl->dc_curr_filted;
	return PI_Controller_Run(&ctrl->pi_power, errRef);
}

static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
	ctrl->pi_vel_lim.max = maxTrq;
	ctrl->pi_vel_lim.min = 0;

	float err = line_ramp_get_interp(&ctrl->vel_lim) - ctrl->foc.in.mot_velocity;
	return PI_Controller_RunVel(&ctrl->pi_vel_lim, err);
}

/* current vector or torque to dq axis current */
static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
		float target_current = line_ramp_get_interp(&ctrl->target_current);
		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
			float s, c;
			arm_sin_cos(ctrl->adv_angle + 90.0f, &s, &c);
			ctrl->target_idq.d = target_current * c;
			
			if (ctrl->target_idq.d > ctrl->hwlim.fw_id) {
				ctrl->target_idq.d = ctrl->hwlim.fw_id;
			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
				ctrl->target_idq.d = -ctrl->hwlim.fw_id;
			}
			ctrl->target_idq.q = sqrtf(SQ(target_current) - SQ(ctrl->target_idq.d));
			if (s < 0) {
				ctrl->target_idq.q = -ctrl->target_idq.q;
			}
		}else {
			ctrl->target_idq.d = 0;
			ctrl->target_idq.q = target_current;
		}
	}else if (ctrl->mode_running != CTRL_MODE_OPEN) {
		motor_mpta_fw_lookup(ctrl->foc.in.mot_velocity, ctrl->target_torque, &ctrl->target_idq);
	}
	u32 mask = cpu_enter_critical();
	foc_set_target_idq(&ctrl->foc, &ctrl->target_idq);
	cpu_exit_critical(mask);
}

static void crosszero_step_towards(float *value, float target) {
	static float no_cro_step = CONFIG_CrossZero_NorStep;
	float v_now = *value;
	bool cross_zero = false;
	float nor_step = mc_conf()->cz.normal_step;
	float min_step = mc_conf()->cz.min_step;
	float min_ramp_torque = mc_conf()->cz.low;
	float high_ramp_torque = mc_conf()->cz.high;
	if (target > 0) {
		if (v_now < -min_ramp_torque) {
			step_towards(value, -min_ramp_torque + 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}else if (target == 0) {
		if (v_now > high_ramp_torque) {
			step_towards(value, high_ramp_torque - 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}else {
		if (v_now > high_ramp_torque) {
			step_towards(value, high_ramp_torque - 0.001f, nor_step);
			cross_zero = true;
		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
			step_towards(value, target, min_step);
			cross_zero = true;
		}
	}
	if (!cross_zero) {
		step_towards(&no_cro_step, nor_step, 0.1f);
		step_towards(value, target, no_cro_step);
	}else {
		no_cro_step = 0.5f;
	}
}

/*called in media task */
void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
	foc_t *foc = &ctrl->foc;
	float etcs_out = etcs_process(&ctrl->etcs);
	if (ctrl->b_AutoHold) {
		float hold_torque = min(ctrl->protlim.torque, mc_conf()->c.max_autohold_torque);
		ctrl->pi_lock.max = hold_torque;
		ctrl->pi_lock.min = -hold_torque;
		float vel_count = motor_encoder_get_vel_count();
		float errRef = 0 - vel_count;
		ctrl->target_torque = PI_Controller_Run(&ctrl->pi_lock ,errRef);
		mot_contrl_dq_assign(ctrl);
		return;
	}
	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
		line_ramp_step(&ctrl->target_current);
	}else if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		float maxTrq = line_ramp_step(&ctrl->input_torque);
		if (line_ramp_get_target(&ctrl->input_torque) < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			maxTrq = 0;
		}
		crosszero_step_towards(&ctrl->target_torque, maxTrq);
	}else if (ctrl->mode_running == CTRL_MODE_TRQ) {
		float refTorque = line_ramp_step(&ctrl->input_torque);
		refTorque = min(refTorque, line_ramp_get_interp(&ctrl->torque_lim)) * etcs_out;
		float maxTrq = mot_contrl_vel_limiter(ctrl, refTorque);
		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
	}else if (ctrl->mode_running == CTRL_MODE_SPD){
		float refSpeed;
		float maxSpeed;
		if (ctrl->b_cruiseEna) {
			refSpeed = line_ramp_step(&ctrl->cruise_vel);
			maxSpeed = line_ramp_get_target(&ctrl->cruise_vel);
		}else {
			refSpeed = line_ramp_step(&ctrl->target_vel);
			maxSpeed = line_ramp_get_target(&ctrl->target_vel);
		}
		float max_input = line_ramp_get_interp(&ctrl->torque_lim) * etcs_out;
		if (maxSpeed >= 0) {
			ctrl->pi_vel.max = max_input;
#ifdef CONFIG_SERVO_MOTOR
			ctrl->pi_vel.min = -max_input;
#else
			ctrl->pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
#endif
		}else if (maxSpeed < 0) {
			ctrl->pi_vel.min = -max_input;
#ifdef CONFIG_SERVO_MOTOR
			ctrl->pi_vel.max = max_input;
#else
			ctrl->pi_vel.max = CONFIG_MAX_NEG_TORQUE;
#endif
		}

		if ((maxSpeed == 0) && (ctrl->foc.in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
			ctrl->pi_vel.max = 0;
			ctrl->pi_vel.min = 0; //防止倒转
		}
		float errRef = refSpeed - ctrl->foc.in.mot_velocity;
		float maxTrq = PI_Controller_RunVel(&ctrl->pi_vel, errRef);
		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
	}

	mot_contrl_dq_assign(ctrl);
}

static void mot_contrl_pid(mot_contrl_t *ctrl) {
	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
	PI_Controller_Reset(&ctrl->pi_power, 0);
	ctrl->pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
	ctrl->pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
	ctrl->pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
	ctrl->pi_power.DT = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_lock, 0);
	ctrl->pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
	ctrl->pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
	ctrl->pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
	ctrl->pi_lock.DT = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_vel_lim, 0);
	ctrl->pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
	ctrl->pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
	ctrl->pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
	ctrl->pi_vel_lim.DT = slow_ctrl_ts;

	PI_Controller_Reset(&ctrl->pi_vel, 0);
	ctrl->pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
	ctrl->pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
	ctrl->pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
	ctrl->pi_vel.DT = slow_ctrl_ts;
}

static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
	ctrl->userlim.dc_curr = min(mc_conf()->c.max_idc, ctrl->hwlim.dc_curr);
	ctrl->userlim.mot_vel = min(mc_conf()->c.max_rpm, ctrl->hwlim.mot_vel);
	ctrl->userlim.torque = mc_conf()->c.max_torque;//MAX_TORQUE;
	ctrl->userlim.phase_curr = min(mc_conf()->c.max_phase_curr, ctrl->hwlim.phase_curr);
	ctrl->userlim.dc_vol_min = mc_conf()->c.max_dc_vol;
	ctrl->userlim.dc_vol_max = mc_conf()->c.min_dc_vol;
	ctrl->userlim.ebrk_dc_curr = 0xFF;
	ctrl->userlim.ebrk_torque = mc_get_ebrk_torque();
}

static void mot_contrl_rtlimit(mot_contrl_t *ctrl) {
	line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
	line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
	line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
}

void mot_contrl_slow_task(mot_contrl_t *ctrl) {
	line_ramp_step(&ctrl->torque_lim);
	line_ramp_step(&ctrl->dc_curr_lim);
	line_ramp_step(&ctrl->vel_lim);
	mot_contrl_dq_calc(ctrl);
}


u8 mot_contrl_protect(mot_contrl_t *ctrl) {
	u8 changed = FOC_LIM_NO_CHANGE;
	float dc_lim = (float)vbus_under_vol_limit();
	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
	
	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
			changed = FOC_LIM_CHANGE_H;
		}else {
			changed = FOC_LIM_CHANGE_L;
		}
		ctrl->protlim.dc_curr = dc_lim;
		ctrl->protlim.torque = torque_lim;
	}
	return changed;
}

float mot_contrl_get_speed(mot_contrl_t *ctrl) {
	float speed = ctrl->foc.in.mot_velocity;
	if (!ctrl->b_start || foc_observer_is_encoder()) {
		speed = motor_encoder_get_speed();
	}else {
		if (foc_observer_sensorless_stable()) {
			speed = foc_observer_sensorless_speed();
		}else {
			speed = 0;
		}
	}
	return speed;
}

void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
#ifdef CONFIG_SPEED_LADRC
	ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
	ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
#else
	PI_Controller_Change_Kpi(&ctrl->pi_vel, wcv, b0);
#endif
}

void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
#ifdef CONFIG_SPEED_LADRC
	ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
	ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
#else
	PI_Controller_Change_Kpi(&ctrl->pi_vel_lim, wcv, b0);
#endif
}

void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit) {
	if (ibusLimit > ctrl->hwlim.dc_curr) {
		ibusLimit = ctrl->hwlim.dc_curr;
	}
	if (ctrl->protlim.dc_curr != HW_LIMIT_NONE) {
		ibusLimit = min(ibusLimit, ctrl->protlim.dc_curr);
	}
	ctrl->userlim.dc_curr = ibusLimit;
	if (ABS(ctrl->dc_curr_filted) <= ibusLimit){
		line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
	}else {
		line_ramp_set_target(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
	}
}

void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel) {
	if (vel > ctrl->hwlim.mot_vel) {
		vel = ctrl->hwlim.mot_vel;
	}
	ctrl->userlim.mot_vel = vel;
	if (ABS(ctrl->foc.in.mot_velocity) <= vel) {
		line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
	}else {
		line_ramp_set_target(&ctrl->vel_lim, ctrl->userlim.mot_vel);
	}
}

void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time) {
	line_ramp_set_time(&ctrl->vel_lim, (float)time);
	line_ramp_update(&ctrl->vel_lim);
}


void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque) {
	if (torque > ctrl->hwlim.torque) {
		torque = ctrl->hwlim.torque;
	}
	if (ctrl->protlim.torque != HW_LIMIT_NONE) {
		torque = min(torque, ctrl->protlim.torque);
	}
	ctrl->userlim.torque = torque;
	if (ABS(ctrl->target_torque) <= torque){
		line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
	}else {
		line_ramp_set_target(&ctrl->torque_lim, ctrl->userlim.torque);
	}
}

void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time) {
	line_ramp_set_time(&ctrl->torque_lim, (float)time);
	line_ramp_update(&ctrl->torque_lim);
}

float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl) {
	if (!foc_observer_is_encoder()) {
		return 0; //无感运行关闭能量回收
	}
	return ctrl->userlim.ebrk_torque;
}

void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time) {
	ctrl->ebrk_ramp_time = time;
	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		line_ramp_set_time(&ctrl->input_torque, time);
		line_ramp_update(&ctrl->input_torque);
	}
}

void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq) {
	line_ramp_set_target(&ctrl->target_vd, vd);
	line_ramp_set_target(&ctrl->target_vq, vq);
}

void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq) {
	line_ramp_reset(&ctrl->target_vd, vd);
	line_ramp_reset(&ctrl->target_vq, vq);
}

bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
	if (enable != ctrl->b_cruiseEna) {
		float motSpd = mot_contrl_get_speed(ctrl);
		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
			mot_contrl_set_error(ctrl, FOC_NowAllowed_With_Speed);
			return false;
		}
		line_ramp_reset(&ctrl->cruise_vel, motSpd);
		ctrl->b_cruiseEna = enable;
	}
	return true;
}


bool mot_contrl_resume_cruise(mot_contrl_t *ctrl) {
	ctrl->b_cruiseEna = true;
	line_ramp_set_time(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
	return true;
}

bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm) {
	if (ctrl->b_cruiseEna) {
		if (rpm < CONFIG_MIN_CRUISE_RPM) {
			rpm = CONFIG_MIN_CRUISE_RPM;
		}
		float vel = min(ABS(rpm),ctrl->userlim.mot_vel)*SIGN(rpm);
		line_ramp_set_target(&ctrl->cruise_vel, vel);
		return true;
	}
	mot_contrl_set_error(ctrl, FOC_NotCruiseMode);
	return false;
}

bool mot_contrl_set_current(mot_contrl_t *ctrl, float is) {
	is = fclamp(is, -ctrl->userlim.phase_curr, ctrl->userlim.phase_curr);
	line_ramp_set_target(&ctrl->target_current, is);
	return true;
}

void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec) {
	ctrl->torque_acc_time = acc;
	ctrl->torque_dec_time = dec;
	if (ctrl->mode_running == CTRL_MODE_TRQ) {
		line_ramp_set_acctime(&ctrl->input_torque, acc);
		line_ramp_set_dectime(&ctrl->input_torque, dec);
		line_ramp_update(&ctrl->input_torque);
	}
}

void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc) {
	ctrl->torque_acc_time = acc;
	if (ctrl->mode_running == CTRL_MODE_TRQ) {
		line_ramp_set_acctime(&ctrl->input_torque, acc);
		line_ramp_update(&ctrl->input_torque);
	}
}

bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
		return false;
	}
	float torque_min = 0;
	float torque_max = ctrl->userlim.torque;
	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
		torque_min = -ctrl->userlim.torque;
		torque_max = 0;
	}
	torque = fclamp(torque, torque_min, torque_max);
	line_ramp_set_target(&ctrl->input_torque, torque);
	return true;
}

void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable) {
	if (enable) {
		ctrl->b_mtpa_calibrate = true;
		ctrl->adv_angle = 0;
	}else {
		ctrl->adv_angle = INVALID_ANGLE;
		ctrl->b_mtpa_calibrate = false;
	}
}

void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock) {
	if (ctrl->b_AutoHold != lock) {
		motor_encoder_lock_pos(lock);
		PI_Controller_Reset(&ctrl->pi_lock, 0);
		if (!lock) {
			float hold_torque = ctrl->target_torque * 1.1f;
			if (ctrl->mode_running == CTRL_MODE_TRQ) {
				PI_Controller_Reset(&ctrl->pi_vel_lim, hold_torque);
			}else if (ctrl->mode_running == CTRL_MODE_SPD) {
				PI_Controller_Reset(&ctrl->pi_vel, hold_torque);
			}
			line_ramp_reset(&ctrl->input_torque, hold_torque);
			ctrl->autohold_torque = hold_torque;
		}else {
			ctrl->autohold_torque = 0;
		}
		ctrl->b_AutoHold = lock;
	}
}

static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl) {
	return (ctrl->b_hw_braker && mc_hwbrk_can_shutpower());
}

bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start) {
	bool enable = ctrl->b_ebrk_running;
	if (mot_contrl_get_ebrk_torque(ctrl) == 0) {
		enable = false;
	}else if (start && ctrl->foc.in.mot_velocity >= CONFIG_MIN_RPM_FOR_EBRAKE){
		enable = true;
	}else if (!start && !is_hw_brake_shutting_power(ctrl)) {
		enable = false;
	}
	if (enable != ctrl->b_ebrk_running) {
		ctrl->b_ebrk_running = enable;
		if (enable) {
			ctrl->mode_req = CTRL_MODE_EBRAKE;
		}else {
			ctrl->mode_req = CTRL_MODE_TRQ;
		}
	}
	return enable;
}

void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake) {
	u32 mask = cpu_enter_critical();
	if (hw_brake != ctrl->b_hw_braker) {
		ctrl->b_hw_braker = hw_brake;
	}
	if (is_hw_brake_shutting_power(ctrl)) {
		if (!ctrl->b_ebrk_running && !mot_contrl_energy_recovery(ctrl, true)) {
			line_ramp_reset(&ctrl->input_torque, 0);
		}
	}
	cpu_exit_critical(mask);
}


static PI_Controller *_pid(mot_contrl_t *ctrl, u8 id) {
	PI_Controller *pi = NULL;
	if (id == PID_ID_ID) {
		pi = &ctrl->foc.daxis;
	}else if (id == PID_IQ_ID) {
		pi = &ctrl->foc.qaxis;
	}else if (id == PID_VelLim_ID) {
		pi = &ctrl->pi_vel_lim;
	}else if (id == PID_Vel_ID) {
		pi = &ctrl->pi_vel;
	}else if (id == PID_AutoHold_ID) {
		pi = &ctrl->pi_lock;
	}
	return pi;
}

void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd) {
	if (id > PID_Max_ID) {
		return;
	}
	PI_Controller *pi = _pid(ctrl, id);
	if (pi != NULL) {
		pi->kp = kp;
		pi->ki = ki;
		pi->kd = kd;
	}
}

void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd) {
	if (id > PID_Max_ID) {
		return;
	}
	PI_Controller *pi = _pid(ctrl, id);
	if (pi != NULL) {
		*kp = pi->kp;
		*ki = pi->ki;
		*kd = pi->kd;
	}
}

void mot_contrl_calc_current(mot_contrl_t *ctrl) {
	float vd = ctrl->foc.out.vol_dq.d;
	float vq = ctrl->foc.out.vol_dq.q;

	float id = ctrl->out_idq_filterd.d;
	float iq = ctrl->out_idq_filterd.q;
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 3/2(iq x vq + id x vd);
	*/
	float m_pow = (vd * id + vq * iq);
	float raw_idc = 0.0f;
	float v_dc = get_vbus_float();
	if (v_dc != 0.0f) {
		raw_idc = m_pow / v_dc;
	}
	LowPass_Filter(ctrl->dc_curr_calc, raw_idc, 0.02f);

	raw_idc = get_vbus_current();
	if (raw_idc != NO_VALID_CURRENT) {
		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.05f);
	}else {
		ctrl->dc_curr_filted = ctrl->dc_curr_calc;
	}
	ctrl->out_current_vec = sqrtf(SQ(id) + SQ(iq));
}