#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"
#include "foc/mc_config.h"
#include "foc/motor/throttle.h"


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 can_debug(bool enable) {
	_pc_connect = enable;
	set_log_level(MOD_SYSTEM, enable?L_debug:L_disable);
}

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) {
			if (!mc_start(CTRL_MODE_TRQ)) {
				mc_crit_err_add(FOC_START_Err_Code, (s16)(throttle_get_signal()*100.0f), (s16)mot_contrl_get_speed(&motor.controller));
			}
		}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_Write_Config, Foc_SN_Write};

static bool _can_process_with_speed(u8 cmd) {
	int size = ARRAY_SIZE(ignore_with_speed);
	if (!mc_is_start() || mot_contrl_get_speed(&motor.controller) < 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 = mot_contrl_get_errcode(&motor.controller);
			}else {
				if (command->len > sizeof(foc_start_cmd_t)) {
					u8 *p = (u8 *)command->data + sizeof(foc_start_cmd_t);
					u8 config = decode_u8(p);
					u8 ext_gear = config & 0x0f;
					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);
						}
					}
					config = (config >> 4);
					mc_enable_brkshutpower(config & 0x01);
					mc_enable_tcs((config & 0x02)?true:false);
				}
			}
			sys_debug("start motor %d\n", erroCode);
			break;
		}
		case Foc_Set_DQ_Current:
		{
		#ifdef CONFIG_DQ_STEP_RESPONSE
			if (command->len == 2) {
				dq_t tgt_dq;
				tgt_dq.d = (float)decode_s08(command->data);
				tgt_dq.q = (float)decode_s08((u8 *)command->data + 1);
				foc_set_target_idq(foc(), &tgt_dq);
				sys_debug("step res %f, %f\n", tgt_dq.d, tgt_dq.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 = mot_contrl_get_errcode(&motor.controller);
			}
			
			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 = mot_contrl_get_errcode(&motor.controller);
			}
			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 = mot_contrl_get_errcode(&motor.controller);
			}
			response[len++] = motor.controller.mode_req;
			break;
		}
		case Foc_Set_Gear_Limit:
		{
			sys_debug("len = %d\n", command->len);
			u8 mode = decode_u8(command->data);
			if (!mc_conf_set_gear(mode, (u8 *)command->data+1, command->len-1)){
				erroCode = FOC_Param_Err;
			}
			break;
		}
		case Foc_Get_Gear_Limit:
		{
			u8  mode = decode_u8(command->data);
			u8 *data = os_alloc(256 + 3);
			int config_len = mc_conf_get_gear(mode, data + 3);
			data[0] = command->cmd;
			data[1] = CAN_MY_ADDRESS;
			data[2] = mode;
			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));
			mot_contrl_set_vel_limit(&motor.controller ,speed);
			encode_u16(response + 3, (u16)motor.controller.userlim.mot_vel);
			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)motor.controller.userlim.dc_curr);
			len += 2;
			break;
		}
		case Foc_Set_Phase_CurrLim:
		{
			s16 curr = decode_s16(((u8 *)command->data));
			mot_contrl_set_torque_limit(&motor.controller, (float)curr);
			encode_u16(response + 3, (u16)motor.controller.userlim.torque);
			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);
			bool align = false;
			if (command->len > 2) {
				align = decode_u8((u8 *)command->data + 2)==1?true:false;
			}
			mc_force_run_open(vd, 0, align);
			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);
			mot_contrl_set_vdq(&motor.controller, vd, vq);
			break;
		}
		case Foc_Conf_Pid:
		{
			if (command->len < 13) {
				erroCode = FOC_Param_Err;
				break;
			}
			pid_t pid;
			u8 id = decode_u8((u8 *)command->data);
			mc_conf_decode_pid(&pid, (u8 *)command->data + 1);
			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
			mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
			mc_conf_set_pid(id, &pid);
			break;
		}
		case Foc_Get_Pid:
		{
			pid_t pid;
			u8 id =   decode_u8((u8 *)command->data);
			if (id < PID_Max_ID) {
				mc_conf_get_pid(id, &pid);
				erroCode = id;
				len += mc_conf_encode_pid(&pid, response + 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 = mot_contrl_get_errcode(&motor.controller);
			}
			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);
				sys_debug("set thro %d, r: %d\n", use, r);
				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 = mot_contrl_get_errcode(&motor.controller);
			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 = mot_contrl_get_errcode(&motor.controller);
			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 = mot_contrl_get_errcode(&motor.controller);
			}
			break;
		}
		case Foc_Start_DQ_Calibrate:
		{
			u8 start = decode_u8((u8 *)command->data);
			if (start == 1) {
				sys_debug("start mpta cali\n");
				if (mc_set_foc_mode(CTRL_MODE_CURRENT)) {
					mot_contrl_mtpa_calibrate(&motor.controller, true);
				}
			}else {
				mot_contrl_mtpa_calibrate(&motor.controller, 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);
				mot_contrl_set_current(&motor.controller, is_curr);
				mot_contrl_set_adv_angle(&motor.controller, 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);
				mc_conf()->c.thro_start_vol = (float)start/100.0f;
				mc_conf()->c.thro_end_vol = (float)end/100.0f;
				mc_conf_save();
			}
			break;
		}
		case Foc_Set_Config:
		{
			u8 conf_cmd = decode_u8(command->data);
			sys_debug("cmd %d\n", conf_cmd);
			if (conf_cmd == 1) { //start
				int len = decode_u16((u8 *)command->data+1);
				if (!mc_conf_begin_recv(len)) {
					erroCode = FOC_MEM_Err;
				}
			}else if (conf_cmd == 2) { //finish
				u16 crc = decode_u16((u8 *)command->data+1);
				if (!mc_conf_finish_recv(crc)) {
					erroCode = FOC_CRC_Err;
				}else {
					shark_timer_post(&_reboot_timer, 200);
				}
			}else if (conf_cmd == 0) { //recv config data
				int offset = decode_u16((u8 *)command->data+1);
				sys_debug("offset = %d\n", offset);
				if (!mc_conf_recv_data((u8 *)command->data + 3, offset, command->len - 3)){
					erroCode = FOC_Param_Err;
				}
			}else {
				erroCode = FOC_Unknow_Cmd;
			}
			break;
		}
		case Foc_Get_Config:
		{
			
			int offset = decode_u16((u8 *)command->data);
			if (offset == 0) {
				response[3] = 1;
			}else {
				response[3] = 0;
			}
			len += 1;
			int ret = mc_conf_send_data(response + 4, offset, sizeof(response) - 4);
			if (ret == -2) {
				erroCode = FOC_MEM_Err;
			}else if (ret == -1) {
				erroCode = FOC_Param_Err;
			}else if (ret == 0){
				response[3] = 2;
				len += mc_conf_finish_send(response + 4);
			}else {
				len += ret;
			}
			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 >= 1) {

				u8 level = decode_u8((u8 *)command->data);
				mc_set_ebrk_level(level);
			}
			break;
		}
		case Foc_Use_SensorLess_Angle:
		{
			bool sensorless = decode_u8((u8 *)command->data)?true:false;
			sys_debug("use smo %d\n", sensorless);
#if 1
			if (sensorless && mc_is_start() && foc_observer_sensorless_stable()) {
				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 (!mc_conf_set_limter((u8 *)command->data, command->len)) {
				erroCode = FOC_Param_Err;
			}
			break;
		}
		case Foc_Get_Limiter_Config:
		{
			u8 *data = os_alloc(256 + 3);
			int config_len = mc_conf_get_limter(data + 3);
			data[0] = command->cmd;
			data[1] = CAN_MY_ADDRESS;
			data[2] = 0;
			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;
		}
		case Foc_Set_Speed_Target:
		{
			if (command->len == 2) {
				s16 tgt_speed = decode_s16((u8 *)command->data);
				mc_set_target_vel(tgt_speed);
			}
			break;
		}
		case Foc_Set_Force_Torque:
		{
			if (command->len == 2) {
				s16 tgt_torque = decode_s16((u8 *)command->data);
				mc_set_force_torque(tgt_torque);
				sys_debug("torque:%d-%d\n", tgt_torque, motor.s_force_torque);
			}
			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();
}