#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 "app/nv_storage.h"
//#include "foc/core/torque.h"
#include "foc/limit.h"

#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif

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 _autohold_beep_timer_handler(shark_timer_t *);
static shark_timer_t _autohold_beep_timer = TIMER_INIT(_autohold_beep_timer, _autohold_beep_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 motor_t motor = {
	.s_direction = POSITIVE,
	.n_gear = 0,
	.b_is96Mode = false,
	.mode = CTRL_MODE_OPEN,
	.mos_lim = 0,
	.motor_lim = 0,
	.idc_user_lim = IDC_USER_LIMIT_NONE,
};
static motor_err_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(abc);
	int vbus_vol = get_vbus_int();
	if (vbus_vol > nv_get_foc_params()->s_maxDCVol) {
		mc_set_critical_error(FOC_CRIT_OV_Vol_Err);
	}
	if (vbus_vol <= nv_get_foc_params()->s_minDCVol) {
		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 >= nv_get_foc_params()->s_maxDCVol) {
		mc_set_critical_error(FOC_CRIT_ACC_OV_Err);
	}
	if (vbus_vol <= nv_get_foc_params()->s_minDCVol) {
		mc_set_critical_error(FOC_CRIT_ACC_Un_Err);
	}
	if ((get_throttle_float() < nv_get_foc_params()->f_minThroVol) || (get_throttle_float() > nv_get_foc_params()->f_maxThroVol)) {
		if (!motor.b_ignor_throttle) {
			mc_set_critical_error(FOC_CRIT_THRO_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 u32 _self_check_task(void *p) {
	if (ENC_Check_error()) {
		err_add_record(FOC_CRIT_Encoder_Err, 0);
		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 (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;
}

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


static void mc_gear_vmode_changed(void) {
	mc_gear_t *gears = mc_get_gear_config();
	
	//sys_debug("limit %d-%d-%d, mode = %s\n", gears->n_max_speed, gears->n_max_idc, gears->n_max_trq, motor.b_is96Mode?"96V":"48V");
	PMSM_FOC_SpeedLimit(gears->n_max_speed);
	PMSM_FOC_DCCurrLimit(min(gears->n_max_idc, motor.idc_user_lim));
	PMSM_FOC_TorqueLimit(gears->n_max_trq);
}

static s16 mc_get_gear_idc_limit(void) {
	if (motor.b_is96Mode) {
		return nv_get_gear_configs()->gears_96[motor.n_gear].n_max_idc;
	}else {
		return nv_get_gear_configs()->gears_48[motor.n_gear].n_max_idc;
	}
}

void mc_init(void) {
	fan_pwm_init();
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	thro_torque_init();
	mc_detect_vbus_mode();
	PMSM_FOC_CoreInit();
	eCtrl_init(mc_get_gear_config()->n_zero_accl, nv_get_foc_params()->n_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);
}

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

mc_gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
	mc_gear_t *gears;

	if (motor.b_is96Mode) {
		gears = &nv_get_gear_configs()->gears_96[0];
	}else {
		gears = &nv_get_gear_configs()->gears_48[0];
	}
	return &gears[n_gear];
}

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

bool mc_critical_can_not_run(void) {
	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err);
	u32 err = motor.n_CritiCalErrMask & mask;
	return (err != 0);
}

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)));
	sys_debug("err=0x%x\n", err);
#endif
	if (motor.b_ignor_throttle) {
		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_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();

	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 (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;
	}

	pwm_up_enable(false);

	_mc_internal_init(mode, true);

	thro_torque_reset();
	mc_gear_vmode_changed();
	eCtrl_init(mc_get_gear_config()->n_zero_accl, nv_get_foc_params()->n_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 (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();
	_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) {
	motor.mos_lim = l;
}

void mc_set_motor_lim_level(u8 l) {
	motor.motor_lim = l;
}

bool mc_set_gear(u8 gear) {
	if (gear >= CONFIG_MAX_GEAR_NUM) {
		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_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_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.idc_user_lim) || (limit < 0)) {
		return;
	}
	motor.idc_user_lim = limit;
	s16 g_limit = mc_get_gear_idc_limit();
	limit = min(g_limit, limit);
	PMSM_FOC_DCCurrLimit(limit);
}

bool mc_set_foc_mode(u8 mode) {
	if (mode == motor.mode) {
		return true;
	}
	if (!motor.b_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(nv_get_foc_params()->s_maxEpmTorqueLimBck);
		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);
		}
		PMSM_FOC_TorqueLimit(motor.f_epm_trq);
		PMSM_FOC_Set_Speed(motor.f_epm_vel);
	}else {
		motor.b_epm_cmd_move = false;
		PMSM_FOC_Set_Speed(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);
	}
}

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.n_gear;
	data |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
	data |= (motor.b_break?1:0) << 3;
	data |= (motor.b_cruise?1:0) << 4;
	data |= (motor.b_start?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 |= (motor.b_is96Mode?1:0) << 14; //是否高压模式
	return data;
}


static float _force_angle = 0.0f;
static int   _force_wait = 2000;
/* 开环，强制给定电角度和DQ的电压 */
void mc_force_run_open(s16 vd, s16 vq) {
	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 * 1.0f, 0);
	_force_wait = 2000;
	motor.b_force_run = true;
}

void mc_encoder_off_calibrate(s16 vd) {
	if (motor.b_start || motor.b_calibrate) {
		return;
	}
	motor.b_calibrate = true;
	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();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_MotAngle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(2000);
	motor_encoder_set_direction(POSITIVE);
	for (int i = 0; i < 200; i++) {
		for (float angle = 0; angle < 360; angle++) {
			PMSM_FOC_Set_MotAngle(angle);
			delay_ms(1);
			if (i > 20) {
				motor_encoder_offset(angle);
			}
		}
		wdog_reload();
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	motor_encoder_set_direction(NEGATIVE);
	delay_ms(100);
	for (int i = 0; i < 200; i++) {
		for (float angle = 360; angle > 0; angle--) {
			PMSM_FOC_Set_MotAngle(angle);
			delay_ms(1);
			if (i > 10) {
				motor_encoder_offset(angle);
			}
		}
		wdog_reload();
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	wdog_reload();
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	pwm_up_enable(true);
	motor_encoder_data_upload();
	motor.b_calibrate = false;
}


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;
	if (phase != INVALID_ANGLE) {
		nv_save_angle_offset(phase);
	}
}

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_ZERO_SPEED_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 (nv_get_foc_params()->n_autoHold == 0) {
		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);
	}
	if (motor.b_auto_hold) {
		gpio_brk_light_enable(true);
	}else {
		gpio_brk_light_enable(false);
	}
	cpu_exit_critical(mask);
	return true;
}


void mc_set_critical_error(u8 err) {
	motor.n_CritiCalErrMask |= (1u << err);
}

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

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 get_throttle_float() <= nv_get_foc_params()->n_startThroVol;
}

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");
}

void MC_Protect_IRQHandler(void){
	pwm_brake_enable(false);
	shark_timer_post(&_brake_prot_timer, 1000);
	if (!motor.b_start) {
		return;
	}
	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_smo_running = !foc_observer_is_encoder();
	mc_error.mos_temp = get_mos_temp();
	mc_error.mot_temp = get_motor_temp();
	mc_error.enc_error = foc_observer_enc_errcount();

	mc_set_critical_error(FOC_CRIT_Phase_Err);
	_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.b_smo_running, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
}

int mc_get_phase_errinfo(u8 *data, int dlen) {
	int len = 0;
	encode_s16(data, mc_error.vbus_x10);
	len += 2;
	encode_s16(data, mc_error.ibus_x10);
	len += 2;
	encode_s16(data, mc_error.vacc_x10);
	len += 2;
	encode_s16(data, mc_error.id_ref_x10);
	len += 2;
	encode_s16(data, mc_error.iq_ref_x10);
	len += 2;
	encode_s16(data, mc_error.id_x10);
	len += 2;
	encode_s16(data, mc_error.iq_x10);
	len += 2;
	encode_s16(data, mc_error.vd_x10);
	len += 2;
	encode_s16(data, mc_error.vq_x10);
	len += 2;
	encode_s16(data, mc_error.torque_ref_x10);
	len += 2;
	encode_s16(data, mc_error.rpm);
	len += 2;
	encode_u8(data, mc_error.run_mode);
	len += 1;
	encode_u8(data, mc_error.b_smo_running);
	len += 1;
	encode_s16(data, mc_error.mos_temp);
	len += 2;
	encode_s16(data, mc_error.mot_temp);
	len += 2;
	return len;
}

void TIMER_UP_IRQHandler(void){
	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
		motor_encoder_update();
	}
}

measure_time_t g_meas_foc = {.exec_max_time = 20, .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)

/*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();
	PMSM_FOC_Schedule();
	TIME_MEATURE_END();
}


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 _autohold_beep_timer_handler(shark_timer_t *t) {
	gpio_beep(60);
}

static void mc_autohold_process(void) {
	if (nv_get_foc_params()->n_autoHold == 0) {
		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() < CONFIG_ZERO_SPEED_RPM)) {
		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;
					shark_timer_post(&_autohold_beep_timer, 0);
				}
			}
		}
	}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 = nv_get_foc_params()->s_maxEpmRPM;
	float target_trq = nv_get_foc_params()->s_maxEpmTorqueLim;
	float step = 0.2f;
	if (motor.epm_dir == EPM_Dir_Back) {
		target_vel = -nv_get_foc_params()->s_maxEpmRPMBck;
		target_trq = nv_get_foc_params()->s_maxEpmTorqueLimBck;
		step = 0.1f;
	}
	step_towards(&motor.f_epm_vel, target_vel, step);
	step_towards(&motor.f_epm_trq, target_trq, 0.05f);
	PMSM_FOC_TorqueLimit(motor.f_epm_trq);
	PMSM_FOC_Set_Speed(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) {
			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;
}
#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_MIN_CRUISE_RPM - 20)) {
			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_can_not_run()) {
		return true;
	}
	if (motor.mode == CTRL_MODE_CURRENT) {
		return false;
	}
	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() && PMSM_FOC_GetSpeed() < foc_observer_sensorless_working_speed()) {
		return true;
	}
	return false;
}

static bool mc_can_restart_foc(void) {
	return (!PMSM_FOC_Is_Start()) && (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_unsafe_critical_error());
}

#endif

static void mc_motor_runstop(void) {
	u32 mask;
	if (mc_can_stop_foc()) {
		if (PMSM_FOC_Is_Start()) {
			mask = cpu_enter_critical();
			PMSM_FOC_Stop();
			pwm_disable_channel();
			cpu_exit_critical(mask);
		}
	}
	if (mc_can_restart_foc()) {
		mask = cpu_enter_critical();
		PMSM_FOC_Start(motor.mode);
		mc_gear_vmode_changed();
		thro_torque_reset();
		pwm_enable_channel();
		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;
	mc_TaskStart;

	adc_vref_filter();

	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_MAX_VBUS_CURRENT * 1.1f)) || (PMSM_FOC_GetVbusCurrent() < CONFIG_MAX_CHRG_CURRENT)) {
		vbus_err_cnt ++;
		if (vbus_err_cnt > 10) {
			mc_error.ibus_x10 = (s16)(PMSM_FOC_GetVbusCurrent() * 10.0f);
			mc_set_critical_error(FOC_CRIT_IDC_OV);
		}
	}else {
		vbus_err_cnt = 0;
	}

	if (mc_process_force_running()) {
		mc_TaskEnd;
		return;
	}

	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L)) {
		mc_gear_vmode_changed();
		motor.b_limit_pending = false;
	}else if (limted == FOC_LIM_CHANGE_H) {
		motor.b_limit_pending = true;
	}
	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
	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();
		mc_TaskEnd;
		return;
	}

	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) && mc_is_epm()) {
				mc_process_throttle_epm();
				mc_process_epm_move();
			}else {
				float thro = get_throttle_float();
				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);
			}
			PMSM_FOC_Slow_Task();
		}
#endif
	}
	mc_TaskEnd;
}