#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/foc_config.h"
#include "foc/mc_error.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/bsp_driver.h"
#include "libs/time_measure.h"
#include "foc/commands.h"
#include "libs/logger.h"
#include "foc/core/e_ctrl.h"
#include "foc/samples.h"
#include "foc/motor/motor_param.h"
#include "foc/core/foc_observer.h"
#include "foc/core/thro_torque.h"
#include "foc/core/F_Calc.h"
#include "foc/core/etcs.h"
#include "app/nv_storage.h"
#include "foc/motor/mot_params_ind.h"
#include "foc/motor/throttle.h"
#include "foc/limit.h"

#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif
extern u32 enc_pwm_err_ms;
extern s16 enc_delta_err1, enc_delta_err2;

static bool mc_detect_hwbrake(void);
static bool mc_is_gpio_mlock(void);
static void _pwm_brake_prot_timer_handler(shark_timer_t *);
static shark_timer_t _brake_prot_timer = TIMER_INIT(_brake_prot_timer, _pwm_brake_prot_timer_handler);
static void _fan_det_timer_handler(shark_timer_t *);
static shark_timer_t _fan_det_timer1 = TIMER_INIT(_fan_det_timer1, _fan_det_timer_handler);
static shark_timer_t _fan_det_timer2 = TIMER_INIT(_fan_det_timer2, _fan_det_timer_handler);
static void _encoder_zero_off_timer_handler(shark_timer_t *);
static shark_timer_t _encoder_zero_off_timer = TIMER_INIT(_encoder_zero_off_timer, _encoder_zero_off_timer_handler);
static void _led_off_timer_handler(shark_timer_t *);
static shark_timer_t _led_off_timer = TIMER_INIT(_led_off_timer, _led_off_timer_handler);
static m_contrl_t motor = {
	.s_direction = POSITIVE,
	.n_gear = 0,
	.b_is96Mode = false,
	.mode = CTRL_MODE_OPEN,
	.mos_lim = 0,
	.motor_lim = 0,
	.b_ind_start = false,
	.s_target_speed = MAX_S16,
	.u_set.idc_lim = IDC_USER_LIMIT_NONE,
	.u_set.rpm_lim = RPM_USER_LIMIT_NONE,
	.u_set.ebrk_lvl = 0,
	.u_set.n_brkShutPower = CONFIG_Settings_BrkShutPower,
	.u_set.b_tcs = CONFIG_Settings_TcsEnable,
};
/* 无感运行的挡位，限制速度，母线电流，最大扭矩 */
static gear_t sensorless_gear = {
	.max_speed = CONFIG_SENSORLESS_MAX_SPEED,
	.max_torque = CONFIG_SENSORLESS_MAX_TORQUE,
	.max_idc = CONFIG_SENSORLESS_MAX_IDC,
	.zero_accl = 1500,
	.accl_time = 1500,
	.torque = {100, 80, 60, 50, 40, 30, 0, 0, 0, 0},
};
static runtime_node_t mc_error;

static void MC_Check_MosVbusThrottle(void) {
	int count = 1000;
	float ibus_adc = 0;
	float vref_adc = 0;
	float vref_5v_adc = 0;
	gpio_phase_u_detect(true);
	while(count-- > 0) {
		task_udelay(20);
		sample_uvw_phase();
		sample_throttle();
		sample_vbus();
		vref_adc += adc_get_vref();
		vref_5v_adc += adc_get_5v_ref();
	}

	adc_set_vref_calc(vref_adc/1000.0f);
	adc_set_5vref_calc(vref_5v_adc/1000.0f);
	sys_debug("5v ref %f\n", vref_5v_adc/1000.0f);
	count = 50;
	while(count-- >0) {
		task_udelay(300);
		ibus_adc += adc_get_ibus();
	}

	u16 offset = ((float)ibus_adc)/50.0f;
	sys_debug("ibus offset %d\n", offset);
	sample_ibus_offset(offset);

	gpio_phase_u_detect(false);
	float abc[3];
	get_phase_vols_filtered(abc);
	int vbus_vol = get_vbus_int();
	if (vbus_vol > mc_conf()->c.max_dc_vol) {
		mc_set_critical_error(FOC_CRIT_OV_Vol_Err);
	}
	if (vbus_vol <= mc_conf()->c.min_dc_vol) {
		mc_set_critical_error(FOC_CRIT_UN_Vol_Err);
	}

	vbus_vol = get_acc_vol();
	sys_debug("acc vol %d\n", vbus_vol);
	if (vbus_vol >= mc_conf()->c.max_dc_vol) {
		mc_set_critical_error(FOC_CRIT_ACC_OV_Err);
	}
	if (vbus_vol <= CONFIG_ACC_MIN_VOL) {
		mc_set_critical_error(FOC_CRIT_ACC_Un_Err);
	}
	if (throttle_is_all_error()) {
		if (!motor.b_ignor_throttle) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}
	}
	if (abc[0] > vbus_vol/2 || abc[1] > vbus_vol/2 || abc[2] > vbus_vol/2) {
		mc_set_critical_error(FOC_CRIT_H_MOS_Err);
	}else if (abc[0] < 0.001f){
		mc_set_critical_error(FOC_CRIT_L_MOS_Err);
	}else if ((abc[0] > 0.5f) && (abc[1] < 0.001f || abc[2] < 0.001f)) {
		mc_set_critical_error(FOC_CRIT_Phase_Conn_Err);
	}
	sys_debug("phase vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
}

static int hw_brk_err_cnt = 0;
static int hw_brk_no_err_cnt = 0;
static u32 _self_check_task(void *p) {
	if (get_tick_ms() < 500) { //启动500ms内检测刹车状态是否正常，应该是没有刹车
		if (mc_detect_hwbrake() && hw_brk_err_cnt++ >= 60) {
			mc_set_critical_error(FOC_CRIT_BRK_Err);
		}
	}else {
		if (mc_critical_err_is_set(FOC_CRIT_BRK_Err) && !mc_detect_hwbrake()) {
			if (hw_brk_no_err_cnt++ >= 100) {
				mc_clr_critical_error(FOC_CRIT_BRK_Err);
			}
		}else {
			hw_brk_no_err_cnt = 0;
		}
	}

	if (ENC_Check_error()) {
		mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, -1);
		mc_set_critical_error(FOC_CRIT_Encoder_Err);
	}
	if (get_tick_ms() < 5000) { //启动后5s内检测锁电机线
		if (mc_is_gpio_mlock()) {
			mc_lock_motor(true);
		}
	}
	if (motor.b_lock_motor) {
		if (!mc_is_gpio_mlock()) {
			mc_lock_motor(false);
		}
	}
	if (!motor.b_ignor_throttle) {
		if (throttle_is_all_error()) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}else if (throttle1_is_error()) {
			mc_set_critical_error(FOC_CRIT_THRO_Err);
		}else if (throttle2_is_error()) {
			mc_set_critical_error(FOC_CRIT_THRO2_Err);
		}
	}
	if (fan_pwm_is_running()) {
	#ifdef GPIO_FAN1_IN_GROUP
		if ((get_delta_ms(motor.fan[0].start_ts) >= 5000) && (motor.fan[0].rpm == 0)) {
			mc_set_critical_error(FOC_CRIT_Fan_Err);
		}else if (motor.fan[0].rpm > 0) {
			mc_clr_critical_error(FOC_CRIT_Fan_Err);
		}
	#endif
	}
	return 5;
}

static bool mc_detect_vbus_mode(void) {
#ifdef CONFIG_FORCE_96V_MODE
	motor.b_is96Mode = true;
	return false;
#else
	bool is_96mode = motor.b_is96Mode;
	motor.b_is96Mode = get_vbus_int() >= CONFIG_96V_MODE_VOL;
	return (is_96mode != motor.b_is96Mode);
#endif
}

static void _mc_internal_init(u8 mode, bool start) {
	motor.mode = mode;
	motor.throttle = 0;
	motor.b_start = start;
	motor.b_runStall = false;
	motor.runStall_time = 0;
	motor.b_epm = false;
	motor.b_epm_cmd_move = false;
	motor.epm_dir = EPM_Dir_None;
	motor.n_autohold_time = 0;
	motor.b_auto_hold = 0;
	motor.b_break = false;
	motor.b_wait_brk_release = false;
	motor.b_force_run = false;
	motor.b_cruise = false;
	motor.b_limit_pending = false;
	motor.f_epm_trq = 0;
	motor.f_epm_vel = 0;
	motor.s_vbus_hw_min = mc_conf()->c.min_dc_vol;
}

static void _led_off_timer_handler(shark_timer_t *t) {
	gpio_led_enable(false);
}


static void mc_gear_vmode_changed(void) {
	gear_t *gears = mc_get_gear_config();
	if (gears != &sensorless_gear) {
		sensorless_gear.max_torque = gears->max_torque;
	}else { //slowly changed
		eRamp_set_time(&(PMSM_FOC_Get()->rtLim.rpmLimRamp), CONFIG_SENSORLESS_RAMP_TIME, CONFIG_SENSORLESS_RAMP_TIME);
		eRamp_set_time(&(PMSM_FOC_Get()->rtLim.DCCurrLimRamp), CONFIG_SENSORLESS_RAMP_TIME, CONFIG_SENSORLESS_RAMP_TIME);
	}
	PMSM_FOC_SpeedLimit((float)min(gears->max_speed, motor.u_set.rpm_lim));
	PMSM_FOC_DCCurrLimit((float)min(gears->max_idc, motor.u_set.idc_lim));
	PMSM_FOC_TorqueLimit((float)gears->max_torque);
	sys_debug("change %d, %d, %d\n", gears->max_idc, gears->max_speed, gears->max_torque);
}

void mc_init(void) {
	fan_pwm_init();
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	throttle_init();
	thro_torque_init();
	mc_detect_vbus_mode();
	PMSM_FOC_CoreInit();
	eCtrl_init(mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
	mc_gpio_init();
	MC_Check_MosVbusThrottle();
	sched_timer_enable(CONFIG_SPD_CTRL_US);
	shark_task_create(_self_check_task, NULL);
	pwm_up_enable(true);
	gpio_led_enable(true);
	shark_timer_post(&_led_off_timer, 5000);
}

m_contrl_t * mc_params(void) {
	return &motor;
}

gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
	gear_t *gears;
	if (!foc_observer_is_encoder()) { //无感模式，受限运行
		return &sensorless_gear;
	}
	if (motor.b_is96Mode) {
		gears = &mc_conf()->g_n[0];
	}else {
		gears = &mc_conf()->g_l[0];
	}
	return &gears[n_gear];
}

gear_t *mc_get_gear_config(void) {
	return mc_get_gear_config_by_gear(motor.n_gear);
}

/* 必须立即停机 */
bool mc_critical_need_stop(void) {
	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err);
	u32 err = motor.n_CritiCalErrMask & mask;
	return (err != 0);
}

bool mc_critical_can_not_run(void) {
	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Lim) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Lim);
	u32 err = motor.n_CritiCalErrMask & mask;
	bool crit_err = (err != 0) || mc_critical_need_stop();
	if (!motor.b_ignor_throttle) {
		crit_err = crit_err || throttle_is_all_error();
	}
	return crit_err;
}

bool mc_unsafe_critical_error(void) {
	u32 err = motor.n_CritiCalErrMask & (~(FOC_Cri_Err_Mask(FOC_CRIT_Fan_Err)));
#ifdef CONFIG_DQ_STEP_RESPONSE
	sys_debug("err=0x%x\n", err);
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_Encoder_Err)));
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
	sys_debug("err=0x%x\n", err);
#else
	if (!throttle_is_all_error()) {//不是两个转把都有问题，忽略出问题的转把错误
		if (throttle1_is_error()) {
			err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
		}else if (throttle2_is_error()) {
			err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
		}
	}
#endif
	if (motor.b_ignor_throttle) {
		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO2_Err)));
	}
	return (err != 0);
}

bool mc_start(u8 mode) {
	if (motor.b_start) {
		return true;
	}
#ifdef CONFIG_DQ_STEP_RESPONSE
	mode = CTRL_MODE_CURRENT;
	target_d = 0.0f;
	target_q = 0.0f;
#endif
	mc_detect_vbus_mode();
	etcs_enable(motor.u_set.b_tcs);
	if (motor.b_lock_motor) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	MC_Check_MosVbusThrottle();

	if (mc_unsafe_critical_error()) {
		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
		return false;
	}
	if (mode > CTRL_MODE_CURRENT) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}

	pwm_up_enable(false);

	_mc_internal_init(mode, true);

	thro_torque_reset();
	mc_gear_vmode_changed();
	eCtrl_init(mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
	motor_encoder_start(true);
	PMSM_FOC_Start(mode);
	PMSM_FOC_RT_LimInit();
	pwm_turn_on_low_side();
	delay_ms(10);
	phase_current_offset_calibrate();
	pwm_start();
	delay_us(10); //wait for ebrake error
	if (mc_unsafe_critical_error()) {
		mc_stop();
		return false;
	}
	adc_start_convert();
	phase_current_calibrate_wait();
	if (phase_curr_offset_check()) {
		mc_set_critical_error(FOC_CRIT_CURR_OFF_Err);
		mc_stop();
		return false;
	}
	if (mc_detect_hwbrake()) {
		PMSM_FOC_Brake(true);
	}
	gpio_beep(200);

	return true;
}

bool mc_stop(void) {
	if (!motor.b_start) {
		return true;
	}

	if (motor.b_lock_motor) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}

	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}

	u32 mask = cpu_enter_critical();
	_mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor_encoder_start(false);
	pwm_up_enable(true);
	cpu_exit_critical(mask);

	return true;
}

void mc_set_mos_lim_level(u8 l) {
	if (motor.mos_lim != l) {
		mc_crit_err_add(FOC_EV_MOS_Limit_L, l, get_mos_temp_raw());
	}
	motor.mos_lim = l;
}

void mc_set_motor_lim_level(u8 l) {
	if (motor.motor_lim != l) {
		mc_crit_err_add(FOC_EV_MOT_Limit_L, l, get_motor_temp_raw());
	}
	motor.motor_lim = l;
}

bool mc_set_gear(u8 gear) {
	if (gear >= CONFIG_MAX_GEARS) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (motor.n_gear != gear) {
		u32 mask = cpu_enter_critical();
		motor.n_gear = gear;
		mc_gear_vmode_changed();
		cpu_exit_critical(mask);
	}
	return true;
}

u8 mc_get_gear(void) {
	if (motor.n_gear == 3){
		return 0;
	}
	return motor.n_gear + 1;
}

u8 mc_get_internal_gear(void) {
	return motor.n_gear;
}


bool mc_hwbrk_can_shutpower(void) {
	if (motor.u_set.n_brkShutPower != MAX_U8) {
		return (motor.u_set.n_brkShutPower != 0);
	}
	return (mc_conf()->s.brk_shut_power != 0);
}

bool mc_enable_cruise(bool enable) {
	if (enable == motor.b_cruise) {
		return true;
	}
	if (PMSM_FOC_EnableCruise(enable)) {
		motor.b_cruise = enable;
		motor.cruise_time = enable?shark_get_seconds():0;
		motor.cruise_torque = 0.0f;
		return true;
	}
	return false;
}

bool mc_set_target_vel(s16 vel) {
	if (motor.mode != CTRL_MODE_SPD || mc_is_epm()) {
		return false;
	}
	motor.s_target_speed = vel;
	return true;
}

bool mc_is_cruise_enabled(void) {
	return motor.b_cruise;
}

bool mc_set_cruise_speed(bool rpm_abs, float target_rpm) {
	bool ret;
	if (rpm_abs) {
		ret = PMSM_FOC_Set_CruiseSpeed(target_rpm);
	}else {
		ret = PMSM_FOC_Set_CruiseSpeed(PMSM_FOC_GetSpeed() + target_rpm);
	}
	if (ret) {
		motor.cruise_time = shark_get_seconds();
		motor.cruise_torque = 0.0f;
	}
	return ret;
}

void mc_set_idc_limit(s16 limit) {
	if ((limit == motor.u_set.idc_lim) || (limit < 0)) {
		return;
	}
	motor.u_set.idc_lim = limit;
	mc_gear_vmode_changed();
}

void mc_set_rpm_limit(s16 limit) {
	if ((limit == motor.u_set.rpm_lim) || (limit < 0)) {
		return;
	}
	motor.u_set.rpm_lim = limit;
	mc_gear_vmode_changed();
}


bool mc_set_ebrk_level(u8 level) {
	if (level < CONFIG_EBRK_LVL_NUM) {
		motor.u_set.ebrk_lvl = level;
		eCtrl_Set_eBrk_RampTime(mc_get_ebrk_time());
		PMSM_FOC_SetEbrkTorque(mc_get_ebrk_torque());
		return true;
	}
	return false;
}

void mc_enable_brkshutpower(u8 shut) {
	motor.u_set.n_brkShutPower = shut;
}

void mc_enable_tcs(bool enable) {
	motor.u_set.b_tcs = enable;
	etcs_enable(enable);
}

s16 mc_get_ebrk_torque(void) {
	if (motor.u_set.ebrk_lvl < CONFIG_EBRK_LVL_NUM) {
		return min(mc_conf()->e_r[motor.u_set.ebrk_lvl].torque, mc_conf()->c.max_ebrk_torque);
	}
	return 0;
}

u16 mc_get_ebrk_time(void) {
	if (motor.u_set.ebrk_lvl < CONFIG_EBRK_LVL_NUM) {
		return (u16)mc_conf()->e_r[motor.u_set.ebrk_lvl].time;
	}
	return 0xFFFF;
}

bool mc_set_foc_mode(u8 mode) {
	if (mode == motor.mode) {
		return true;
	}
	if (!motor.b_start && !motor.b_ind_start) {
		return false;
	}
	if (mc_critical_can_not_run()) {
		return false;
	}
	if ((mode == CTRL_MODE_OPEN) && (ABS(PMSM_FOC_GetSpeed()) > CONFIG_ZERO_SPEED_RPM)) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}

	u32 mask = cpu_enter_critical();
	bool ret = false;
	if (PMSM_FOC_SetCtrlMode(mode)) {
		motor.mode = mode;
		if (mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) {
			PMSM_FOC_Start(motor.mode);
			pwm_enable_channel();
		}
		ret = true;
	}
	cpu_exit_critical(mask);
	return ret;
}

bool mc_start_epm(bool epm) {
	if (motor.b_epm == epm) {
		return true;
	}
	if (!motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_epm = epm;
	motor.f_epm_vel = 0.0f;
	motor.f_epm_trq = 0.0f;
	motor_encoder_band_epm(epm);
	if (epm) {
		eCtrl_set_TgtSpeed(0);
		motor.mode = CTRL_MODE_SPD;
		motor.epm_dir = EPM_Dir_None;
		PMSM_FOC_TorqueLimit(mc_conf()->c.max_epm_torque);
		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
	}else {
		motor.epm_dir = EPM_Dir_None;
		motor.mode = CTRL_MODE_TRQ;
		motor.b_epm_cmd_move = false;
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
		mc_gear_vmode_changed();
	}
	cpu_exit_critical(mask);
	
	return false;
}

bool mc_is_epm(void) {
	return motor.b_epm;
}

bool mc_is_start(void) {
	return (motor.b_start || PMSM_FOC_Is_Start());
}

bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
	if (!motor.b_epm || !motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if ((dir == motor.epm_dir) && (is_command == motor.b_epm_cmd_move)) {
		return true;
	}
	u32 mask = cpu_enter_critical();
	if (motor.epm_dir != dir) {
		motor.f_epm_vel = 0.0f;
		motor.f_epm_trq = 0.0f;
	}
	motor.epm_dir = dir;
	if (dir != EPM_Dir_None) {
		motor.b_epm_cmd_move = is_command;
		
		if (!PMSM_FOC_Is_Start()) {
			PMSM_FOC_Start(motor.mode);
			pwm_enable_channel();
		}else if (PMSM_FOC_AutoHoldding()) {
			mc_auto_hold(false);
		}
		if (dir == EPM_Dir_Back) {
#ifdef CONFIG_SPEED_LADRC
			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPMBK_B0);
#else
			PMSM_FOC_Change_VelLoop_Params(0.2f, 7.5f);
#endif
		}else {
#ifdef CONFIG_SPEED_LADRC
			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPM_B0);
#else
			PMSM_FOC_Change_VelLoop_Params(0.2f, 7.5f);
#endif
		}
		PMSM_FOC_TorqueLimit(motor.f_epm_trq);
		PMSM_FOC_Set_TgtSpeed(motor.f_epm_vel);
	}else {
		motor.b_epm_cmd_move = false;
#ifdef CONFIG_SPEED_LADRC
		PMSM_FOC_Change_VelLoop_Params(nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
#else
		PMSM_FOC_Change_VelLoop_Params(mc_conf()->c.pid[PID_Vel_ID].kp, mc_conf()->c.pid[PID_Vel_ID].ki);
#endif
		PMSM_FOC_Set_TgtSpeed(0);
	}
	cpu_exit_critical(mask);
	return true;
}

void mc_set_fan_duty(u8 duty) {
	sys_debug("fan duty %d\n", duty);
	if (!fan_pwm_is_running() && duty > 0) {
		motor.fan[0].start_ts = get_tick_ms();
		motor.fan[1].start_ts = get_tick_ms();
		shark_timer_post(&_fan_det_timer1, 5000);
		shark_timer_post(&_fan_det_timer2, 5000);
	}else if (duty == 0) {
		shark_timer_cancel(&_fan_det_timer1);
		shark_timer_cancel(&_fan_det_timer2);
	}
	fan_set_duty(duty);
}

bool mc_command_epm_move(EPM_Dir_t dir) {
	return mc_start_epm_move(dir, true);
}

bool mc_throttle_epm_move(EPM_Dir_t dir) {
	return mc_start_epm_move(dir, false);
}

void mc_set_throttle_r(bool use, u8 r) {
	motor.u_throttle_ration = r;
	motor.b_ignor_throttle = use;
	if (motor.b_ignor_throttle) {
		mc_clr_critical_error(FOC_CRIT_THRO_Err);
		mc_clr_critical_error(FOC_CRIT_THRO2_Err);
	}
}

void mc_use_throttle(void) {
	motor.b_ignor_throttle = false;
}

void mc_get_running_status(u8 *data) {
	data[0] = motor.mode;
	data[0] |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
	data[0] |= (motor.b_break?1:0) << 3;
	data[0] |= (motor.b_cruise?1:0) << 4;
	data[0] |= (motor.b_start?1:0) << 5;
	data[0] |= (mc_is_epm()?1:0) << 6;
	data[0] |= (motor.b_lock_motor) << 7; //motor locked
}

u16 mc_get_running_status2(void) {
	u16 data = 0;
	data = motor.b_start?1:0;
	data |= (motor.n_gear & 0x7) << 1;
	data |= (PMSM_FOC_AutoHoldding()?1:0) << 3;
	data |= (motor.b_break?1:0) << 4;
	data |= (motor.b_cruise?1:0) << 5;
	data |= (mc_is_epm()?1:0) << 6;
	data |= (motor.b_lock_motor) << 7; //motor locked
	data |= (eCtrl_is_eBrk_Running()?1:0) << 8; //能量回收运行标志
	data |= ((motor.n_CritiCalErrMask != 0)?1:0) << 9;
	data |= (fan_pwm_is_running()?1:0) << 10; //风扇是否运行
	data |= (motor.b_runStall?1:0) << 11; //是否堵转
	data |= (PMSM_FOC_iDC_is_Limited()?1:0) << 12; //是否欠压限制母线电流
	data |= (PMSM_FOC_Torque_is_Limited()?1:0) << 13; //是否高温限扭矩
	data |= (etcs_is_running()?1:0) << 14; //电子tcs是否正在工作
	data |= (throttle_not_released_err()?1:0) << 15;
	return data;
}


static float _force_angle = 0.0f;
static int   _force_wait = 2000;
/* 开环，强制给定电角度和DQ的电压 */
void mc_force_run_open(s16 vd, s16 vq, bool align) {
	if (motor.b_start || motor.b_force_run) {
		if (vd == 0 && vq == 0) {
			PMSM_FOC_SetOpenVdq(0, 0);
			delay_ms(500);
			wdog_reload();
			adc_stop_convert();
			pwm_stop();
			PMSM_FOC_Stop();
			pwm_up_enable(true);
			motor.b_force_run = false;
			motor.b_ignor_throttle = false;
		}
		return;
	}
	if (vd == 0 && vq == 0) {
		return;
	}
	motor.b_ignor_throttle = true;
	MC_Check_MosVbusThrottle();
	if (mc_unsafe_critical_error()) {
		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
	}
	
	pwm_up_enable(false);
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	pwm_enable_channel();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_MotAngle(0);
	PMSM_FOC_SetOpenVdq((float)vd, 0);
	if (align) {
		_force_wait = 2000 + 1;
	}else {
		_force_wait = 2000;
	}
	motor.b_force_run = true;
}

bool mc_ind_motor_start(bool start) {
	if (start == motor.b_ind_start) {
		return true;
	}
	if (start) {
		motor.b_ignor_throttle = true;
		MC_Check_MosVbusThrottle();
		if (mc_unsafe_critical_error() || mot_params_flux_pending()) {
			PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
			return false;
		}
		pwm_up_enable(false);
		pwm_turn_on_low_side();
		task_udelay(500);
		PMSM_FOC_Start(CTRL_MODE_OPEN);
		phase_current_offset_calibrate();
		pwm_start();
		adc_start_convert();
		pwm_enable_channel();
		phase_current_calibrate_wait();
		PMSM_FOC_SetOpenVdq_Immediate(0, 0);
		motor.b_ind_start = start;
	}else {
		u32 mask = cpu_enter_critical();
		motor.b_ind_start = start;
		PMSM_FOC_SetOpenVdq(0, 0);
		cpu_exit_critical(mask);
		delay_us(500);
		wdog_reload();
		adc_stop_convert();
		pwm_stop();
		PMSM_FOC_Stop();
		motor.mode = CTRL_MODE_OPEN;
		pwm_up_enable(true);
		motor.b_ignor_throttle = false;
	}
	
	return true;
}

static void _encoder_zero_off_timer_handler(shark_timer_t *t){
	if (!motor.b_calibrate) {
		return;
	}
	float enc_off = 0.0f;
	float phase = motor_encoder_zero_phase_detect(&enc_off);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(50);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	_mc_internal_init(CTRL_MODE_OPEN, false);
	motor.b_calibrate = false;
}

bool mc_encoder_zero_calibrate(s16 vd) {
	if (motor.b_calibrate) {
		if (vd == 0) {
			encoder_clear_cnt_offset();
			shark_timer_cancel(&_encoder_zero_off_timer);
			PMSM_FOC_SetOpenVdq(0, 0);
			delay_ms(500);
			adc_stop_convert();
			pwm_stop();
			PMSM_FOC_Stop();
			_mc_internal_init(CTRL_MODE_OPEN, false);
			motor.b_calibrate = false;
			motor.b_ignor_throttle = false;
		}
		return true;
	}
	encoder_clear_cnt_offset();
	motor.b_ignor_throttle = true;
	MC_Check_MosVbusThrottle();
	if (mc_unsafe_critical_error()) {
		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
		return false;
	}
	_mc_internal_init(CTRL_MODE_OPEN, true);
	motor.b_calibrate = true;
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	pwm_enable_channel();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_MotAngle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);

	shark_timer_post(&_encoder_zero_off_timer, 6*1000);

	return true;
}


bool mc_current_sensor_calibrate(float current) {
	if (!mc_start(CTRL_MODE_OPEN)) {
		return false;
	}
	phase_current_sensor_start_calibrate(current);
	phase_current_calibrate_wait();
	return true;
}

bool mc_lock_motor(bool lock) {
	if (motor.b_lock_motor == lock) {
		return true;
	}
	int ret = true;
	u32 mask = cpu_enter_critical();
	if (motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		ret = false;
		goto ml_ex_cri;
	}
	if (lock && (motor_encoder_get_speed() >= CONFIG_LOCK_MOTOR_MIN_RPM)) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		ret = false;
		goto ml_ex_cri;
	}
	motor.b_lock_motor = lock;
	if (lock) {
		pwm_start();
		pwm_update_duty(0, 0, 0);
		pwm_enable_channel();
	}else {
		pwm_stop();
	}
ml_ex_cri:
	cpu_exit_critical(mask);
	return ret;
}

bool mc_auto_hold(bool hold) {
	if (motor.b_auto_hold == hold) {
		return true;
	}
	if (!mc_conf()->s.auto_hold) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (!motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (hold && !mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_auto_hold = hold;
	if (!PMSM_FOC_Is_Start()) {
		PMSM_FOC_Start(motor.mode);
		PMSM_FOC_AutoHold(hold);
		pwm_enable_channel();
	}else {
		PMSM_FOC_AutoHold(hold);
	}
	cpu_exit_critical(mask);
	return true;
}


bool mc_set_critical_error(u8 err) {
	if (mc_critical_err_is_set(err)) {
		return false;
	}
	motor.n_CritiCalErrMask |= (1u << err);

	return true;
}

void mc_clr_critical_error(u8 err) {
	motor.n_CritiCalErrMask &= ~(1u << err);
}

bool mc_critical_err_is_set(u8 err) {
	u32 mask = (1u << err);
	return (motor.n_CritiCalErrMask & mask) != 0;
}

u32 mc_get_critical_error(void) {
	return motor.n_CritiCalErrMask;
}

bool mc_throttle_released(void) {
	if (motor.b_ignor_throttle) {
		return motor.u_throttle_ration == 0;
	}
	return throttle_is_released();
}

static bool mc_is_gpio_mlock(void) {
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = gpio_motor_locked();
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
		return false;
	}else if (settimes == 50) {
		return true;
	}	
	//有干扰，do nothing
	return false;
}

static bool _mc_is_hwbrake(void) {
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = mc_get_gpio_brake() || mc_get_gpio_brake1();
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return true;
#else
		return false;
#endif
	}else if (settimes == 50) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return false;
#else
		return true;
#endif
	}
	//有干扰，do nothing
	motor.n_brake_errors++;
	return false;
}

static bool mc_detect_hwbrake(void) {
	motor.b_break = _mc_is_hwbrake();
	return motor.b_break;
}

static void _fan_det_timer_handler(shark_timer_t *t) {
	if (t == &_fan_det_timer1) {
		motor.fan[0].rpm = 0;
		motor.fan[0].det_ts = 0;
	}else {
		motor.fan[1].rpm = 0;
		motor.fan[1].det_ts = 0;
	}
}

void Fan_IRQHandler(int idx) {
	fan_t *fan = motor.fan + idx;
	u32 pre_ts = fan->det_ts;
	u32 delta_ts = get_delta_ms(pre_ts);
	fan->det_ts = get_tick_ms();
	float rpm = 60.0f * 1000 / (float)delta_ts;
	
	LowPass_Filter(fan->rpm, rpm, 0.1f);
	if (idx == 0) {
		shark_timer_post(&_fan_det_timer1, 100);
	}else {
		shark_timer_post(&_fan_det_timer2, 100);
	}
}

void MC_Brake_IRQHandler(void) {
	mc_detect_hwbrake();

	if (!motor.b_start) {
		return;
	}
	if (motor.b_break) {
		mc_enable_cruise(false);
		PMSM_FOC_Brake(true);
	}else {
		PMSM_FOC_Brake(false);
	}
}

static void _pwm_brake_prot_timer_handler(shark_timer_t *t){
	pwm_brake_enable(true);
	sys_debug("MC protect error\n");
}

static void mc_save_err_runtime(void) {
	mc_error.vbus_x10 = (s16)(sample_vbus_raw() * 10.0f); 
	mc_error.ibus_x10 = (s16)(sample_ibus_raw() * 10.0f);
	mc_error.vacc_x10 = (s16) (sample_acc_vol_raw() * 10.0f);
	mc_error.id_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[0].s_Cp * 10.0f);
	mc_error.iq_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[1].s_Cp * 10.0f);
	mc_error.id_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f);
	mc_error.iq_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f);
	mc_error.vd_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.d * 10.0f);
	mc_error.vq_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.q * 10.0f);
	mc_error.torque_ref_x10 = (s16)(PMSM_FOC_Get()->in.s_targetTorque * 10.0f);
	mc_error.run_mode = PMSM_FOC_Get()->out.n_RunMode;
	mc_error.rpm = (s16)motor_encoder_get_speed();
	mc_error.b_sensorless = !foc_observer_is_encoder();
	mc_error.b_sensorless_stable = foc_observer_sensorless_stable();
	mc_error.mos_temp = get_mos_temp_raw();
	mc_error.mot_temp = get_motor_temp_raw();
	mc_error.enc_error = motor_encoder_may_error();
	mc_error.sensorless_rpm = (s16)foc_observer_sensorless_speed();

	mc_err_runtime_add(&mc_error);
}

void MC_Protect_IRQHandler(void){
	pwm_brake_enable(false);
	shark_timer_post(&_brake_prot_timer, 1000);
	if (!motor.b_start) {
		return;
	}
	mc_set_critical_error(FOC_CRIT_Phase_Err);
	mc_save_err_runtime();
	_mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	pwm_up_enable(true);
}

void motor_debug(void) {
	if (!mc_unsafe_critical_error()) {
		return;
	}
	sys_debug("err1: %f, %f, %f, %d\n", (float)mc_error.vbus_x10/10.0f, (float)mc_error.id_ref_x10/10.0f, (float)mc_error.iq_ref_x10/10.0f, mc_error.run_mode);
	sys_debug("err2: %f, %f, %f, %f\n", (float)mc_error.id_x10/10.0f, (float)mc_error.iq_x10/10.0f, (float)mc_error.vd_x10/10.0f, (float)mc_error.vq_x10/10.0f);
	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.sensorless_rpm, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
}

static void motor_vbus_crit_low(s16 curr_vbus) {
	static u16 _vbus_e_count = 0;

	if (curr_vbus < motor.s_vbus_hw_min) {
		_vbus_e_count ++;
		if (_vbus_e_count >= 2) {
			if (PMSM_FOC_Is_Start()) {
				pwm_disable_channel();
				mc_save_err_runtime();
				PMSM_FOC_Stop();
			}
			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
					mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
				}
			}
		}
	}else {
		_vbus_e_count = 0;
	}
}

void TIMER_UP_IRQHandler(void){
	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
		motor_encoder_update(false);
		motor_vbus_crit_low((s16)get_vbus_int());
	}
}

measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
#if (CONFIG_ENABLE_IAB_REC==1)
#define CONFIG_IAB_REC_COUNT 1000
static s16 ia[CONFIG_IAB_REC_COUNT], ib[CONFIG_IAB_REC_COUNT];
static int iab_w_count = 0, iab_r_count = 0;
static bool b_iab_rec = false;
extern void can_plot2(s16 v1, s16 v2);
#endif
/*ADC 电流采集中断，调用FOC的核心处理函数*/
void ADC_IRQHandler(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	if (phase_current_sensor_do_calibrate()){
		pwm_update_duty(100, FOC_PWM_Half_Period-100, 100);
		pwm_update_sample(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1, PHASE_BC);
		return;
	}
	TIME_MEATURE_START();

#if (CONFIG_ENABLE_IAB_REC==1)
	if (b_iab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
		ia[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[0];
		ib[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[1];
		iab_w_count ++;
	}
#endif
	motor_vbus_crit_low((s16)sample_vbus_raw()); //need fast detect vbus very low, to stop the motor
	float vd, vq;
	if (motor.b_ind_start) {
		mot_params_high_freq_inject();
		vd = PMSM_FOC_Get()->out.s_OutVdq.d;
		vq = PMSM_FOC_Get()->out.s_OutVdq.q;
	}
	if (!PMSM_FOC_Schedule()) {/* FOC 角度错误，立即停机 */
		if (PMSM_FOC_Is_Start()) {
			pwm_disable_channel();
			/* 记录错误 */
			if (!foc_observer_is_force_sensorless()) {
				mc_save_err_runtime();
			}
			PMSM_FOC_Stop();
			g_meas_foc.first = true;
			if (!foc_observer_is_force_sensorless()) {
				if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
					mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
				}
				if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
					if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
						mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
					}
				}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
					if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
						mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
					}
				}
			}
		}
	}
	if (motor.b_ind_start) {
		float id = PMSM_FOC_Get()->out.s_RealIdq.d;
		float iq = PMSM_FOC_Get()->out.s_RealIdq.q;
		mot_params_hj_sample_vi(vd, vq, id, iq);
	}
	TIME_MEATURE_END();
}

#if (CONFIG_ENABLE_IAB_REC==1)
static void _iab_plot_timer_handler(shark_timer_t *t) {
	if (!b_iab_rec) {
		return;
	}
	if (iab_r_count < iab_w_count) {
		can_plot2(ia[iab_r_count], ib[iab_r_count]);
		iab_r_count ++;
		shark_timer_post(t, 10);
	}
}
static shark_timer_t _iab_plot_timer = TIMER_INIT(_iab_plot_timer, _iab_plot_timer_handler);
void mc_start_current_rec(bool rec) {
	if (b_iab_rec == rec) {
		return;
	}
	if (!rec) {
		b_iab_rec = false;
		shark_timer_cancel(&_iab_plot_timer);
		return;
	}
	iab_w_count = 0;
	iab_r_count = 0;
	b_iab_rec = true;
	shark_timer_post(&_iab_plot_timer, 100);
}
#endif
static bool mc_run_stall_process(u8 run_mode) {
	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
		//堵转判断
		if (motor.b_runStall) {
			if (!mc_throttle_released()) {
				return true;
			}
			motor.runStall_time = 0;
			motor.b_runStall = false; //转把释放，清除堵转标志
		}else if (PMSM_FOC_Get_Real_dqVector() >= CONFIG_STALL_MAX_CURRENT){
			if (ABS(PMSM_FOC_GetSpeed()) < 1.0f && (motor.runStall_time == 0)) {
				motor.runStall_time = get_tick_ms();
				motor.runStall_pos = motor_encoder_get_position();
			}
			if (motor.runStall_time > 0) {
				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
					motor.b_runStall = true;
					motor.runStall_time = 0;
					PMSM_FOC_Set_Torque(0);
					thro_torque_reset();
					return true;
				}
				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
					motor.runStall_time = 0;
				}
			}
		}else {
			motor.runStall_time = 0;
		}
	}
	return false;
}

static void mc_autohold_process(void) {
	if (!mc_conf()->s.auto_hold) {
		if (PMSM_FOC_AutoHoldding()) {
			mc_auto_hold(false);
		}
		return;
	}
	if (PMSM_FOC_AutoHoldding()) {
		if (!mc_throttle_released()) {
			mc_auto_hold(false);
			motor.b_wait_brk_release = false;
		}else if (!motor.b_break && motor.b_wait_brk_release) {
			motor.b_wait_brk_release = false;
		}else if (motor.b_break && !motor.b_wait_brk_release) {
			mc_auto_hold(false);
		}
	}
	if (!PMSM_FOC_AutoHoldding() && motor.b_break && (motor_encoder_get_speed() == 0)) {
		if (motor.n_autohold_time == 0) {
			motor.n_autohold_time = get_tick_ms();
		}else {
			if (get_delta_ms(motor.n_autohold_time) >= CONFIG_AUTOHOLD_DETECT_TIME) {
				if (mc_auto_hold(true)) {
					motor.b_wait_brk_release = true;
				}
			}
		}
	}else {
		motor.n_autohold_time = 0;
	}
}

static void mc_process_throttle_epm(void) {
	if (!motor.b_epm_cmd_move) {//通过命令前进后退，不处理转把
		if (mc_throttle_released()) {
			mc_throttle_epm_move(EPM_Dir_None);
		}else {
			mc_throttle_epm_move(EPM_Dir_Forward);
		}
	}
}

static void mc_process_epm_move(void) {
	if (!motor.b_epm || (motor.epm_dir == EPM_Dir_None)){
		return;
	}
	float target_vel = mc_conf()->c.max_epm_rpm;
	float target_trq = mc_conf()->c.max_epm_torque;
	float step = 0.05f;
	if (motor.epm_dir == EPM_Dir_Back) {
		target_vel = -mc_conf()->c.max_epm_back_rpm;
		target_trq = mc_conf()->c.max_epm_back_torque;
	}else if (!motor.b_epm_cmd_move) {
		target_vel = get_throttle_ration(throttle_get_signal()) * 2.0f * (float)target_vel;
		step = 0.07f;
	}
	step_towards(&motor.f_epm_vel, target_vel, step);
	motor.f_epm_trq = target_trq;
	PMSM_FOC_TorqueLimit(motor.f_epm_trq);
	PMSM_FOC_Set_TgtSpeed(motor.f_epm_vel);
}

static bool mc_process_force_running(void) {
	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
		if (motor.b_force_run && _force_wait <= 2000) {
			if (_force_wait > 0) {
				--_force_wait;
			}else {
				_force_angle += 1.5f;
				rand_angle(_force_angle);
				PMSM_FOC_Set_MotAngle(_force_angle);
			}
		}
		return true;
	}
	return false;
}

static void mc_process_brake_light(void) {
	bool can_lighting = false;
	if (motor.b_break || motor.b_auto_hold || eCtrl_is_eBrk_Running() || ((!mc_critical_can_not_run()) && motor_encoder_get_speed() > 2000)) {
		can_lighting = true;
	}
	gpio_brk_light_enable(can_lighting);
}

#ifdef CONFIG_CRUISE_ENABLE_ACCL
static void mc_process_curise(void) {
	static bool can_pause_resume = false;
	if (motor.b_cruise) {
		if (PMSM_FOC_GetSpeed() < CONFIG_CRUISE_EXIT_RPM) {
			mc_enable_cruise(false);
			return;
		}
		/* 定速巡航模式下，必须转把归位后才能开始通过拧动转把加速 */
		if (mc_throttle_released() && !can_pause_resume) {
			can_pause_resume = true;
		}
		if (!can_pause_resume) {
			return;
		}
		if (PMSM_FOC_Is_CruiseEnabled()) {
			u32 cruise_time = shark_get_seconds() - motor.cruise_time;
			if ((cruise_time >= 3) && motor.cruise_torque == 0.0f) {
				motor.cruise_torque = PMSM_FOC_Get()->in.s_targetTorque;
			}else if ((cruise_time >= 3) && (motor.cruise_torque > 0.0f)){
				float trq_req = get_user_request_torque();
				if (trq_req > motor.cruise_torque * 1.2f) {
					PMSM_FOC_PauseCruise(); //需要加速,暂停定速巡航
				}
			}
		}else {
			float trq_req = get_user_request_torque();
			if (trq_req <= motor.cruise_torque * 1.1f) {
				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
				//motor.cruise_time = shark_get_seconds();
			}
		}
	}else {
		PMSM_FOC_EnableCruise(false);
		can_pause_resume = false;
	}
}
#endif

#ifndef CONFIG_DQ_STEP_RESPONSE
static bool mc_can_stop_foc(void) {
	if (mc_critical_need_stop()) {
		return true;
	}
	if (motor.mode == CTRL_MODE_CURRENT) {
		return false;
	}
	if (!motor.b_cruise && !motor.b_epm && motor.mode == CTRL_MODE_SPD) {
		if (motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) {
			return false;
		}else {
			return true;
		}
	}
	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
		if (!PMSM_FOC_AutoHoldding() && motor.epm_dir == EPM_Dir_None) {
			return true;
		}
	}
	/* 启用无感观测器，但是观测器未稳定，关闭输出，滑行 */
	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()) {
		return true;
	}
	return false;
}

static bool mc_can_restart_foc(void) {
	bool can_start = (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_critical_can_not_run());
	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()){
		return false;
	}
	if ((motor.s_target_speed != MAX_S16 && motor.s_target_speed != 0) && (!mc_critical_can_not_run()) && motor.mode == CTRL_MODE_SPD) {
		return true;
	}
	return can_start;
}

#endif

static void mc_motor_runstop(void) {
	u32 mask;
	if (PMSM_FOC_Is_Start() && mc_can_stop_foc()) {
		mask = cpu_enter_critical();
		PMSM_FOC_Stop();
		pwm_disable_channel();
		g_meas_foc.first = true;
		cpu_exit_critical(mask);
	}
	if (!PMSM_FOC_Is_Start() && mc_can_restart_foc()) {
		mask = cpu_enter_critical();
		PMSM_FOC_Start(motor.mode);
		mc_gear_vmode_changed();
		thro_torque_reset();
		pwm_enable_channel();
		g_meas_foc.first = true;
		cpu_exit_critical(mask);
	}
}

/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
measure_time_t g_meas_MCTask;
#define mc_TaskStart time_measure_start(&g_meas_MCTask)
#define mc_TaskEnd time_measure_end(&g_meas_MCTask)
void Sched_MC_mTask(void) {
	static int vbus_err_cnt = 0;
	static bool _sensorless_run = false;
	mc_TaskStart;

	adc_vref_filter();

	throttle_detect(motor.b_start);

	F_all_Calc();

#ifdef CONFIG_CRUISE_ENABLE_ACCL
	mc_process_curise();
#endif
	u8 runMode = PMSM_FOC_CtrlMode();

	/*保护功能*/
	u8 limted = PMSM_FOC_RunTime_Limit();
	/* 母线电流，实际采集的相电流矢量大小的计算 */
	PMSM_FOC_Calc_Current();

	if ((PMSM_FOC_GetVbusCurrent() > (CONFIG_HW_MAX_DC_CURRENT * 1.1f)) || (PMSM_FOC_GetVbusCurrent() < CONFIG_HW_MAX_CHRG_CURRENT)) {
		vbus_err_cnt ++;
		if (vbus_err_cnt >= 5) {
			if (mc_set_critical_error(FOC_CRIT_IDC_OV)) {
				mc_crit_err_add(FOC_CRIT_IDC_OV, (s16)sample_vbus_raw(), (s16)sample_ibus_raw());
				mc_save_err_runtime();
			}
		}
	}else {
		vbus_err_cnt = 0;
	}

	if (mc_process_force_running()) {
		mc_TaskEnd;
		return;
	}
	bool sensor_less = !foc_observer_is_encoder();
	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (_sensorless_run != sensor_less)) {
		mc_gear_vmode_changed();
		if (sensor_less && foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
				mc_set_critical_error(FOC_CRIT_Encoder_Err);
				mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
				mc_set_critical_error(FOC_CRIT_ENC_AB_Err);
				mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
			}
		}
		motor.b_limit_pending = false;
	}else if (limted == FOC_LIM_CHANGE_H) {
		motor.b_limit_pending = true;
	}
	_sensorless_run = sensor_less;

	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
	if (motor.b_limit_pending && mc_throttle_released()) {
		motor.b_limit_pending = false;
		mc_gear_vmode_changed();
	}
	/* 堵转处理 */
	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {
		eCtrl_Running();
		PMSM_FOC_Slow_Task();
		mc_motor_runstop();
		if (motor.b_ind_start) {
			mot_params_flux_stop();
		}
		mc_TaskEnd;
		return;
	}

	mc_process_brake_light();

	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
#ifndef CONFIG_DQ_STEP_RESPONSE
		mc_autohold_process();
		if (motor.mode != CTRL_MODE_OPEN) {
			mc_motor_runstop();
		}
		if (runMode != CTRL_MODE_OPEN) {
			eCtrl_Running();
			if (runMode == CTRL_MODE_SPD) {
				if (mc_is_epm()) {
					mc_process_throttle_epm();
					mc_process_epm_move();
				}else if (motor.s_target_speed != MAX_S16) {
					PMSM_FOC_Set_TgtSpeed(motor.s_target_speed);
				}
			}else {
				float thro = throttle_get_signal();
				if (motor.b_ignor_throttle) {
					float r = (float)motor.u_throttle_ration/100.0f;
					thro = thro_ration_to_voltage(r);
				}
				thro_torque_process(runMode, thro);
			}
			etcs_process();
			PMSM_FOC_Slow_Task();
		}
#endif
	}
	mc_TaskEnd;
}