#include "os/os_task.h"
#include "os/queue.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "prot/can_message.h"
#include "bsp/bsp_driver.h"
#include "foc/motor/motor.h"
#include "foc/commands.h"
#include "prot/can_foc_msg.h"
#include "app/nv_storage.h"
#include "foc/core/foc_observer.h"
#include "foc/mc_error.h"
#include "foc/motor/mot_params_ind.h"

#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif
extern int plot_type;
static void _reboot_timer_handler(shark_timer_t *);
static shark_timer_t _reboot_timer = TIMER_INIT(_reboot_timer, _reboot_timer_handler);

static u32 foc_command_task(void *args);
static void process_foc_command(foc_cmd_body_t *command);

static co_queue_t _cmd_queue;

static bool _pc_connect = false;

bool can_is_connect_pc(void) {
	return _pc_connect;
}

void foc_command_init(void) {
	_cmd_queue = queue_create(16, sizeof(foc_cmd_body_t));
	shark_task_create(foc_command_task, NULL);
}

bool foc_send_command(foc_cmd_body_t *command) {
	if (!queue_put(_cmd_queue, command)) {
		if (command->data) {
			os_free(command->data);
		}
		return false;
	}
	return true;
}

static u32 foc_command_task(void *args) {
	foc_cmd_body_t command;
	if (queue_get(_cmd_queue, &command)) {
		process_foc_command(&command);
		if (command.data) {
			os_free(command.data);
		}
	}
	return 0;
}

static void process_ext_command(foc_cmd_body_t *command) {
	if (command->ext_key == 0x1A01) {
		return;
	}else if (command->ext_key == 0x1A02) {
		u8 b0 = decode_u8(command->data);
		u8 p_mode = decode_8bits(b0, 0, 1);
		if (p_mode == 1) {
			mc_start(CTRL_MODE_TRQ);
		}else if (p_mode == 2) {
			mc_stop();
		}
		s8 ext_gear = decode_8bits(b0, 5, 7);
		sys_debug("gear %d\n", ext_gear);
		if (ext_gear >= 1 && ext_gear <= 4) {
			if (ext_gear == 4) {
				mc_set_gear(3);
			}else {
				mc_set_gear(ext_gear - 1);
			}
		}
		u8 b1 = decode_u8((u8 *)command->data + 1);
		u8 cruise = decode_8bits(b1, 0, 1);
		if (cruise == 2) {
			mc_enable_cruise(true);
			sys_debug("cruise enable: %d\n", mc_is_cruise_enabled());
		}else if (cruise == 1) {
			mc_enable_cruise(false);
			sys_debug("cruise disable: %d\n", mc_is_cruise_enabled());
		}

		u8 epm = decode_8bits(b0, 2, 3);
		if (epm == 2) {
			mc_start_epm(true);
		}else if(epm == 1) {
			mc_start_epm(false);
		}

		u8 m_4896 = decode_8bits(b1, 4, 5);

		u8 epm_dir = decode_8bits(b1, 6, 7);
		if (epm_dir == 0) {
			mc_command_epm_move(EPM_Dir_None);
		}else if (epm_dir == 1) {
			mc_command_epm_move(EPM_Dir_Back);
		}else if (epm_dir == 2) {
			mc_command_epm_move(EPM_Dir_Forward);
		}

		u16 cruise_spd = decode_u16((u8 *)command->data + 3);
		sys_debug("crui spd %d\n", cruise_spd);
		if ((cruise_spd > 0) && (cruise_spd != 0xFFFF)) {
			mc_set_cruise_speed(true, (float)cruise_spd * 4.0f);
		}
		u8 response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
		response[0] &= 0xFC;
		response[0] |= (mc_is_start()?1:2);
		response[0] |= (mc_get_gear() << 5);
		
		response[1] &= 0xC0;
		response[1] |= (mc_is_cruise_enabled()?2:1);
		response[1] |= (mc_is_epm()?1:2) << 2;
		response[1] |= m_4896<<4;

		shark_can0_send_ext_message(0x1A024D43, response, sizeof(response));
	}else if (command->ext_key == 0x1A05) {
		u16 idc_lim = decode_u16((u8 *)command->data) / 10;
		sys_debug("idc %d\n", idc_lim);
		mc_set_idc_limit((s16)idc_lim);
		shark_can0_send_ext_message(0x1A054D43, command->data, command->len);
	}
	
}

static u8 ignore_with_speed[] = {Foc_Set_Adrc_Params, Foc_Set_Gear_Limit, Foc_Conf_Pid, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Set_Limiter_Config, Foc_End_Write_TRQ_Table, Foc_SN_Write};

static bool _can_process_with_speed(u8 cmd) {
	int size = ARRAY_SIZE(ignore_with_speed);
	if (!mc_is_start() || PMSM_FOC_GetSpeed() < 0.1f) {
		return true;
	}
	for (int i = 0; i < size; i++) {
		if (ignore_with_speed[i] == cmd) {
			return false;
		}
	}
	return true;
}

static void process_foc_command(foc_cmd_body_t *command) {
	u8 erroCode = 0;
	u8 response[128];
	int len = 3;
	if ((command->ext_key != 0) && (command->cmd == 0)) {
		process_ext_command(command);
		return;
	}
	if (!_can_process_with_speed(command->cmd)) {
		erroCode = FOC_NowAllowed_With_Speed;
		goto cmd_end;
	}
	switch (command->cmd) {
		case Foc_Start_Motor:
		{
			bool success;
			foc_start_cmd_t *scmd = (foc_start_cmd_t *)command->data;
			sys_debug("start cmd %d\n", scmd->start_stop);
			if (scmd->start_stop == Foc_Start) {
				success = mc_start(CTRL_MODE_TRQ);
			}else if (scmd->start_stop == Foc_Stop) {
				success = mc_stop();
			}
			if (!success) {
				erroCode = PMSM_FOC_GetErrCode();
			}else {
				if (command->len > sizeof(foc_start_cmd_t)) {
					u8 *p = (u8 *)command->data + sizeof(foc_start_cmd_t);
					s8 ext_gear = decode_u8(p);
					sys_debug("gear %d\n", ext_gear);
					if (ext_gear >= 1 && ext_gear <= 4) {
						if (ext_gear == 4) {
							mc_set_gear(3);
						}else {
							mc_set_gear(ext_gear - 1);
						}
					}
				}
			}
			sys_debug("start motor %d\n", erroCode);
			break;
		}
		case Foc_Set_DQ_Current:
		{
		#ifdef CONFIG_DQ_STEP_RESPONSE
			if (command->len == 2) {
				target_d = (float)decode_s08(command->data);
				target_q = (float)decode_s08((u8 *)command->data + 1);
				sys_debug("step res %f, %f\n", target_d, target_q);
			}else {
				erroCode = FOC_Param_Err;
			}
		#else
			erroCode = FOC_NotAllowed;
		#endif
			break;
		}
		case Foc_Set_Gear_Mode:
		{
			u8 gear = decode_u8(command->data);
			if (gear > 3) {
				erroCode = FOC_Param_Err;
			}else {
				sys_debug("set gear %d\n", gear);
				mc_set_gear(gear);
				response[3] = gear;
				len += 1;
			}
			break;
		}
		case Foc_Set_Cruise_Mode:
		{
			u8 enable = decode_u8(command->data);
			if (!mc_enable_cruise(enable)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			
			break;
		}
		case Foc_Set_Cruise_Speed:
		{
			u8 mode = decode_u8(command->data);
			float rpm = (float)decode_s16((u8 *)command->data + 1);
			if (!mc_set_cruise_speed(mode?true:false, rpm)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			sys_debug("Cruise RPM %d\n", (int)rpm);
			encode_u16(response + 3, (s16)rpm);
			len += 2;
			break;
		}
		case Foc_Set_Ctrl_Mode:
		{
			u8 mode = decode_u8(command->data);
			sys_debug("ctl mode = %d, len = %d\n", mode, command->len);
			if (!mc_set_foc_mode(mode)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			response[len++] = PMSM_FOC_GetCtrlMode();
			break;
		}
		case Foc_Set_Gear_Limit:
		{
			sys_debug("len = %d\n", command->len);
			u8 mode = decode_u8(command->data);
			if (!nv_set_gear_config(mode, (u8 *)command->data+1, command->len-1)){
				erroCode = FOC_Param_Err;
			}
			break;
		}
		case Foc_Get_Gear_Limit:
		{
			u8  mode = decode_u8(command->data);
			int config_len = 0;
			void *config = nv_get_gear_config(mode, &config_len);
			u8 *data = os_alloc(config_len + 3);
			data[0] = command->cmd;
			data[1] = CAN_MY_ADDRESS;
			data[2] = 0;
			memcpy(data + 3, config, config_len);
			can_send_response(command->can_src, data, config_len + 3);
			os_free(data);
			return;
		}
		case Foc_Set_Speed_Limit:
		{
			s16 speed = decode_s16(((u8 *)command->data));
			PMSM_FOC_SpeedLimit(speed);
			encode_u16(response + 3, (u16)PMSM_FOC_GetSpeedLimit());
			len += 2;
			break;
		}
		case Foc_Set_iDC_Limit:
		{
			u16 current = decode_u16(((u8 *)command->data));
			mc_set_idc_limit((float)current);
			encode_u16(response + 3, (u16)PMSM_FOC_GetDCCurrLimit());
			len += 2;
			break;
		}
		case Foc_Set_Phase_CurrLim:
		{
			s16 curr = decode_s16(((u8 *)command->data));
			PMSM_FOC_PhaseCurrLim((float)curr);
			encode_u16(response + 3, (u16)PMSM_FOC_GetPhaseCurrLim());
			len += 2;
			break;
		}
		case Foc_Cali_Hall_Phase:
		{
			s16 vd = decode_s16((u8 *)command->data);
			sys_debug("cali encoder %d\n", vd);
			erroCode = mc_encoder_zero_calibrate(vd)? FOC_Success:FOC_Param_Err;
			break;
		}
		case Foc_Enc_Zero_Cali_Result:
		{
			response[2] = encoder_get_cali_error()?1:0;
			u32 off = encoder_get_cnt_offset();
			encode_u32(response + 3, off);
			len += 4;
			break;
		}
		case Foc_Force_Open_Run:
		{
			s16 vd = decode_s16((u8 *)command->data);
			mc_force_run_open(vd, 0);
			break;
		}
		case Foc_Set_Open_Dq_Vol:
		{
			s16 vd = decode_s16(((u8 *)command->data));
			s16 vq = decode_s16(((u8 *)command->data) + 2);
			sys_debug("set v_q %d, %d\n", vd, vq);
			PMSM_FOC_SetOpenVdq(vd, (vq));
			break;
		}
		case Foc_Conf_Pid:
		{
			if (command->len < 13) {
				erroCode = FOC_Param_Err;
				break;
			}
			pid_conf_t pid;
			u8 id =   decode_u8((u8 *)command->data);
			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
			PMSM_FOC_SetPid(id, pid.kp, pid.ki, pid.kd);
			nv_set_pid(id, &pid);
			break;
		}
		case Foc_Get_Pid:
		{
			pid_conf_t pid;
			u8 id =   decode_u8((u8 *)command->data);
			if (id < PID_Max_id) {
				nv_get_pid(id, &pid);
				erroCode = id;
				memcpy(response+3, &pid, sizeof(pid));
				len = sizeof(pid) + 3;
				sys_debug("get id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
			}else {
				erroCode = 1;
				len = 3;
			}
			break;
		}
		case Foc_Set_Adrc_Params:
		{
			if (command->len < 24) {
				erroCode = FOC_Param_Err;
				break;
			}
			nv_get_foc_params()->f_adrc_vel_lim_Wo = decode_float(command->data);
			nv_get_foc_params()->f_adrc_vel_lim_Wcv = decode_float((u8 *)command->data + 4);
			nv_get_foc_params()->f_adrc_vel_lim_B0 = decode_float((u8 *)command->data + 8);
			nv_get_foc_params()->f_adrc_vel_Wo = decode_float((u8 *)command->data + 12);
			nv_get_foc_params()->f_adrc_vel_Wcv = decode_float((u8 *)command->data + 16);
			nv_get_foc_params()->f_adrc_vel_B0 = decode_float((u8 *)command->data + 20);
			nv_save_foc_params();
			break;
		}
		case Foc_Get_Adrc_Params:
		{
			encode_float(response+3, nv_get_foc_params()->f_adrc_vel_lim_Wo);
			encode_float(response+7, nv_get_foc_params()->f_adrc_vel_lim_Wcv);
			encode_float(response+11, nv_get_foc_params()->f_adrc_vel_lim_B0);
			encode_float(response+15, nv_get_foc_params()->f_adrc_vel_Wo);
			encode_float(response+19, nv_get_foc_params()->f_adrc_vel_Wcv);
			encode_float(response+23, nv_get_foc_params()->f_adrc_vel_B0);
			len += 24;
			break;
		}
		case Foc_Set_EPM_Mode:
		{
			bool mode = decode_u8((u8 *)command->data) == 0?false:true;
			if (!mc_start_epm(mode)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			break;
		}
		case Foc_Set_Thro_Ration:
		{
			if (command->len >= 2) {
				bool use = decode_u8(command->data)==0?false:true;
				u8 r = decode_u8((u8 *)command->data + 1);
				mc_set_throttle_r(use, r);
			}
			break;
		}
		case Foc_Lock_Motor:
		{
			u8 lock = decode_u8((u8 *)command->data);
			if (lock == Foc_Start) {
				mc_lock_motor(true);
			}else {
				mc_lock_motor(false);
			}
			erroCode = PMSM_FOC_GetErrCode();
			break;
		}
		case Foc_Auto_Hold:
		{
			u8 hold = decode_u8((u8 *)command->data);
			if (hold == Foc_Start) {
				mc_auto_hold(true);
			}else {
				mc_auto_hold(false);
			}
			erroCode = PMSM_FOC_GetErrCode();
			break;
		}
		case Foc_Start_EPM_Move:
		{
			EPM_Dir_t dir = (EPM_Dir_t)decode_u8((u8 *)command->data);
			if(!mc_command_epm_move(dir)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			break;
		}
		case Foc_Start_DQ_Calibrate:
		{
			u8 start = decode_u8((u8 *)command->data);
			if (start == Foc_Start) {
				sys_debug("start mpta cali\n");
				mc_set_foc_mode(CTRL_MODE_CURRENT);
				PMSM_FOC_MTPA_Calibrate(true);
			}else {
				PMSM_FOC_MTPA_Calibrate(false);
				mc_set_foc_mode(CTRL_MODE_TRQ);
			}
			break;
		}
		case Foc_Set_IS_Curr_Angle:
		{
			if (command->len != 4) {
				erroCode = FOC_Param_Err;
			}else {
				s16 is_curr = decode_s16((u8 *)command->data);
				s16 is_angle = decode_s16((u8 *)command->data + 2);
				sys_debug("curr %d, angle %d\n", is_curr, is_angle);
				PMSM_FOC_Set_Current(is_curr);
				PMSM_FOC_Set_Dq_Angle(is_angle);
			}
			break;
		}
		case Foc_Set_Plot_Type:
		{
			plot_type = (int)decode_u8((u8 *)command->data);
		#if (CONFIG_ENABLE_IAB_REC==1)
			if (plot_type == 6) {
				mc_start_current_rec(true);
			}else if (plot_type == 0) {
				mc_start_current_rec(false);
			}
		#endif
			sys_debug("plot type %d\n", plot_type);
			break;
		}
		case Foc_Set_Throttle_throld:
		{
			if (mc_is_start()) {
				erroCode = FOC_NotAllowed;
			}else {
				u16 start = decode_u16((u8 *)command->data);
				u16 end = decode_u16((u8 *)command->data + 2);
				nv_get_foc_params()->n_startThroVol = (float)start/100.0f;
				nv_get_foc_params()->n_endThroVol = (float)end/100.0f;
				nv_save_foc_params();
			}
			break;
		}
		case Foc_Get_Config:
		{
			int config_len = 128;
			int len = 0;
			u8 *config = os_alloc(config_len);
			if (config == NULL) {
				erroCode = FOC_MEM_Err;
				break;
			}
			config[0] = command->cmd;
			config[1] = CAN_MY_ADDRESS;
			config[2] = 0;
			len = 3;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_PhaseCurrLim);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_maxDCVol);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_minDCVol);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_maxRPM);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_maxEpmRPM);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_maxEpmTorqueLim);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_maxTorque);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_TorqueBrkLim);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_iDCeBrkLim);
			len += 2;
			encode_s16(config+len, (s16)nv_get_foc_params()->s_LimitiDC);
			len += 2;
			encode_s16(config+len, (s16)(nv_get_foc_params()->n_startThroVol * 100.0f));
			len += 2;
			encode_s16(config+len, (s16)(nv_get_foc_params()->n_endThroVol * 100.0f));
			len += 2;
			encode_u8(config+len, (u8)nv_get_foc_params()->n_brkShutPower);
			len += 1;
			encode_u8(config+len, (u8)nv_get_foc_params()->n_autoHold);
			len += 1;
			encode_u32(config+len, nv_get_foc_params()->n_dec_time);
			len += 4;
			encode_u32(config+len, nv_get_foc_params()->n_ebrk_time);
			len += 4;
			encode_s16(config+len, nv_get_foc_params()->s_maxEpmRPMBck);
			len += 2;
			encode_s16(config+len, nv_get_foc_params()->s_maxEpmTorqueLimBck);
			len += 2;
			can_send_response(command->can_src, config, len);
			os_free(config);
			return;
		}
		case Foc_Set_Config:
		{
			if (mc_is_start()) {
				erroCode = FOC_NotAllowed;
			}else if (command->len < 28) {
				erroCode = FOC_Param_Err;
			}else {
				nv_get_foc_params()->s_TorqueBrkLim = decode_s16((u8 *)command->data);
				nv_get_foc_params()->s_iDCeBrkLim = decode_s16((u8 *)command->data + 2);
				nv_get_foc_params()->s_LimitiDC = decode_s16((u8 *)command->data + 4);
				nv_get_foc_params()->n_startThroVol = (float)decode_s16((u8 *)command->data + 6)/100.0f;
				nv_get_foc_params()->n_endThroVol = (float)decode_s16((u8 *)command->data + 8)/100.0f;
				nv_get_foc_params()->s_maxEpmRPM = decode_s16((u8 *)command->data + 10);
				nv_get_foc_params()->s_maxEpmTorqueLim = decode_s16((u8 *)command->data + 12);
				nv_get_foc_params()->n_brkShutPower = decode_u8((u8 *)command->data + 14);
				nv_get_foc_params()->n_autoHold = decode_u8((u8 *)command->data + 15);
				nv_get_foc_params()->n_dec_time = decode_u32((u8 *)command->data + 16);
				nv_get_foc_params()->n_ebrk_time = decode_u32((u8 *)command->data + 20);
				nv_get_foc_params()->s_maxEpmRPMBck = decode_s16((u8 *)command->data + 24);
				nv_get_foc_params()->s_maxEpmTorqueLimBck = decode_s16((u8 *)command->data + 26);
				nv_save_foc_params();
				shark_timer_post(&_reboot_timer, 200);
			}
			break;
		}
		case Foc_Fan_Duty:
		{
			u8 duty = decode_u8(command->data);
			mc_set_fan_duty(duty);
			break;
		}
		case Foc_Set_eBrake_Throld:
		{
			if (command->len >= 4) {

				s16 torque = decode_s16((u8 *)command->data);
				u16 time = decode_u16((u8 *)command->data + 2);
				mc_set_ebrk_level(torque, time);
			}
			break;
		}
		case Foc_Use_SensorLess_Angle:
		{
			bool sensorless = decode_u8((u8 *)command->data)?true:false;
			sys_debug("use smo %d\n", sensorless);
#if 0
			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= foc_observer_sensorless_working_speed()) {
				sys_debug("use smo %d\n", sensorless);
				foc_observer_use_sensorless(sensorless);
			}else {
				sys_debug("unuse smo\n");
				foc_observer_use_sensorless(false);
			}
#else
			if (sensorless && mc_is_start()){
				motor_encoder_produce_error(sensorless);
			}else {
				motor_encoder_produce_error(false);
			}
#endif
			break;
		}
		case Foc_Set_Limiter_Config:
		{
			sys_debug("limter %d\n", command->len);
			if (!nv_set_limit_config((u8 *)command->data, command->len)) {
				erroCode = FOC_Param_Err;
			}
			break;
		}
		case Foc_Get_Limiter_Config:
		{
			int config_len;
			u8 *config = nv_get_limit_config(&config_len);
			u8 *data = os_alloc(config_len + 3);
			data[0] = command->cmd;
			data[1] = CAN_MY_ADDRESS;
			data[2] = 0;
			memcpy(data + 3, config, config_len);
			can_send_response(command->can_src, data, config_len+3);
			os_free(data);
			return;
		}
		case Foc_SN_Write:
		{
			if (command->len < 18) {
				erroCode = FOC_Param_Err;
			}else{
				erroCode = nv_write_sn((u8 *)command->data, command->len)>0?0:1 ;
			}
			break;
		}
		case Foc_SN_Read:
		{
			if (nv_read_sn(response + 3, 18) == 0) {
				memset(response + 3, '0', 18);
			}
			len += 18;
			break;
		}
		case Foc_Get_MC_NV_Crit_Err:
		{
			s16 offset = decode_s16((u8 *)command->data);
			len += mc_crit_err_get(offset, response+3, sizeof(response) - 3);
			break;
		}
		case Foc_Get_MC_NV_Err_RT:
		{
			s16 offset = decode_s16((u8 *)command->data);
			len += mc_err_runtime_get(offset, response+3, sizeof(response) - 3);
			break;
		}
		case Foc_Set_LogLevel:
		{
			u8 level = decode_u8((u8 *)command->data);
			_pc_connect = (level != 0)?true:false;
			set_log_level(MOD_SYSTEM, (level != 0)?L_debug:L_disable);
			break;
		}
		case Foc_MotPara_Ind:
		{
			_pc_connect = true;
			set_log_level(MOD_SYSTEM, L_debug);
			bool start = decode_u8((u8 *)command->data)?true:false;
			if (!start) {
				mc_ind_motor_start(start);
				mot_params_ind_stop();
				sys_debug("stop mot ind\n");
			}else {
				u8 type = decode_u8((u8 *)command->data + 1);
				sys_debug("start mot ind %d\n", type);
				if (type == R_TYPE) { // rs ind
					u8 v_max = decode_u8((u8 *)command->data + 2);
					u8 i_max = decode_u8((u8 *)command->data + 3);
					u8 time = decode_u8((u8 *)command->data + 4);
					sys_debug("rs ind %d, %d, %d\n", v_max, i_max, time);
					if (mc_ind_motor_start(true)) {
						mot_params_ind_rs((float)v_max, (float)i_max, (s32)time);
					}
				}else if (type == L_TYPE_D || type == L_TYPE_Q) { //ld/lq ind
					u8 v = decode_u8((u8 *)command->data + 2);
					u16 freq = decode_u16((u8 *)command->data + 3);
					sys_debug("ldq ind %d, %d\n", v, freq);
					if (mc_ind_motor_start(true)) {
						if (type == L_TYPE_D) {
							mot_params_ind_ld((float)v, (float)freq);
						}else {
							mot_params_ind_lq((float)v, (float)freq);
						}
					}
				}else if (type == FLUX_TYPE) {
					u8 iq = decode_u8((u8 *)command->data + 2);
					sys_debug("ind flux iq = %d\n", iq);
					if (mc_ind_motor_start(true)) {
						mot_params_ind_flux(0, (float)iq);
					}
				}else{ 
					erroCode = FOC_Param_Err;
				}
			}
			break;
		}
		default:
		{
			erroCode = FOC_Unknow_Cmd;
			break;
		}
	}
cmd_end:
	sys_debug("err = %d\n", erroCode);
	response[0] = command->cmd;
	response[1] = CAN_MY_ADDRESS;
	response[2] = erroCode;
	can_send_response(command->can_src, response, len);
}


static void _reboot_timer_handler(shark_timer_t *t) {
	system_reboot();
}