#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.h"
#include "bsp/pwm.h"
#include "bsp/adc.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"

#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif
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;

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);
		if (ext_gear >= 0 && ext_gear <= 5) {
			sys_debug("gear %d\n", ext_gear);
			if (ext_gear == 0) {
				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) {
			PMSM_FOC_EnableCruise(true);
		}else if (cruise == 1) {
			PMSM_FOC_EnableCruise(false);
		}

		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);
		if ((cruise_spd > 0) && (cruise_spd != 0xFFFF)) {
			PMSM_FOC_Set_CruiseSpeed((float)cruise_spd);
		}
		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] |= (PMSM_FOC_Is_CruiseEnabled()?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) {
		shark_can0_send_ext_message(0x1A054D43, command->data, command->len);
	}
	
}

static u8 ignore_with_speed[] = {Foc_Set_Gear_Limit, Foc_Conf_Pid, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Start_Write_TRQ_Table, Foc_Write_TRQ_Table, 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[32];
	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();
			}
			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_Cruise_Mode:
		{
			u8 enable = decode_u8(command->data);
			if (!PMSM_FOC_EnableCruise(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 (mode == 0) {
				rpm = PMSM_FOC_GetSpeed() + rpm;	
			}
			if (!PMSM_FOC_Set_CruiseSpeed(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("mode = %d\n", mode);
			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);
			if (command->len < 9) {
				erroCode = FOC_Param_Err;
			}else {
				u8  gear = decode_u8(command->data);
				u16 maxRPM = decode_u16((u8 *)command->data + 1);
				u16 maxPhaseCurr = decode_u16((u8 *)command->data + 3);
				u16 maxiDC = decode_u16((u8 *)command->data + 5);
				u16 accline = decode_u16((u8 *)command->data + 7);
				sys_debug("gear 0x%x, rpm %d, phase %d idc %d acc %d\n", gear, maxRPM, maxPhaseCurr, maxiDC, accline);
				if (!nv_set_gear_config((gear>>4 & 0xF), (gear & 0xF), maxRPM, maxPhaseCurr, maxiDC, accline)){
					erroCode = FOC_Param_Err;
				}
			}
			break;
		}
		case Foc_Get_Gear_Limit:
		{
			u8  gear = decode_u8(command->data);
			u16 maxRPM;
			u16 maxPhaseCurr;
			u16 maxiDC;
			u16 accline;
			sys_debug("gear = 0x%x\n", gear);
			if (!nv_get_gear_config((gear>>4 & 0xF), (gear & 0xF), &maxRPM, &maxPhaseCurr, &maxiDC, &accline)){
				erroCode = FOC_Param_Err;
			}else {
				encode_u8(response + 3, gear);
				encode_u16(response + 4, maxRPM);
				encode_u16(response + 6, maxPhaseCurr);
				encode_u16(response + 8, maxiDC);
				encode_u16(response + 10, accline);
				len += 9;
			}
			break;
		}
		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:
		{
			u8 current = decode_u8(((u8 *)command->data));
			PMSM_FOC_DCCurrLimit((float)current);
			encode_u8(response + 3, (u8)PMSM_FOC_GetDCCurrLimit());
			len += 1;
			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, kb = %f\n", id, pid.kp, pid.ki, pid.kb);
			PMSM_FOC_SetPid(id, pid.kp, pid.ki, pid.kb);
			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, kb = %f\n", id, pid.kp, pid.ki, pid.kb);
			}else {
				erroCode = 1;
				len = 3;
			}
			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:
		{
			u8 r = decode_u8(command->data);
			mc_set_throttle_r(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_Angle(is_angle);
			}
			break;
		}
		case Foc_Set_Plot_Type:
		{
			u8 plot = decode_u8((u8 *)command->data);
			if (plot >= Plot_t_Max) {
				erroCode = FOC_Param_Err;
			}else {
				PMSM_FOC_Set_PlotType((Plot_t)plot);
			}
			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:
		{
			len = sizeof(foc_params_t) + 2 - sizeof(pid_conf_t) * PID_Max_id - 2;
			u8 *config = os_alloc(len);
			if (config == NULL) {
				erroCode = FOC_MEM_Err;
				break;
			}
			memcpy((void *)(config + 2), (void *)nv_get_foc_params(), sizeof(foc_params_t) - sizeof(pid_conf_t) * PID_Max_id - 2);
			config[0] = command->cmd;
			config[1] = CAN_MY_ADDRESS;
			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 < 32) {
				erroCode = FOC_Param_Err;
			}else {
				nv_get_foc_params()->s_PhaseCurrLim = decode_s16((u8 *)command->data);
				nv_get_foc_params()->s_maxRPM = decode_s16((u8 *)command->data + 2);
				nv_get_foc_params()->s_PhaseCurreBrkLim = decode_s16((u8 *)command->data + 4);
				nv_get_foc_params()->s_iDCeBrkLim = decode_s16((u8 *)command->data + 6);
				nv_get_foc_params()->s_LimitiDC = decode_s16((u8 *)command->data + 8);
				nv_get_foc_params()->n_startThroVol = (float)decode_s16((u8 *)command->data + 10)/100.0f;
				nv_get_foc_params()->n_endThroVol = (float)decode_s16((u8 *)command->data + 12)/100.0f;
				nv_get_foc_params()->s_maxEpmRPM = decode_s16((u8 *)command->data + 14);
				nv_get_foc_params()->s_maxEpmTorqueLim = decode_s16((u8 *)command->data + 16);
				nv_get_foc_params()->n_brkShutPower = decode_u8((u8 *)command->data + 18);
				nv_get_foc_params()->n_autoHold = decode_u8((u8 *)command->data + 19);
				nv_get_foc_params()->n_acc_time = decode_u32((u8 *)command->data + 20);
				nv_get_foc_params()->n_dec_time = decode_u32((u8 *)command->data + 24);
				nv_get_foc_params()->n_ebrk_time = decode_u32((u8 *)command->data + 28);
				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) {
				nv_get_foc_params()->s_iDCeBrkLim = decode_s16((u8 *)command->data);
				nv_get_foc_params()->s_LimitiDC = decode_s16((u8 *)command->data + 2);
				nv_save_foc_params();
			}
			break;
		}
		case Foc_Use_SensorLess_Angle:
		{
			bool sensorless = decode_u8((u8 *)command->data)?true:false;
			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= CONFIG_SMO_MIN_SPEED) {
				sys_debug("use smo %d\n", sensorless);
				foc_observer_use_smo(sensorless);
			}else {
				sys_debug("unuse smo\n");
				foc_observer_use_smo(false);
			}
			break;
		}
		case Foc_Start_Write_TRQ_Table:
		{
			nv_write_trq_table_begin(0);
			break;
		}
		case Foc_Write_TRQ_Table:
		{
			int pos = nv_write_trq_table_continue((u8 *)command->data, command->len);
			if (pos < 0) {
				erroCode = FOC_NotAllowed;
			}else {
				encode_u24(response+3, pos);
				len += 3;
			}
			break;
		}
		case Foc_End_Write_TRQ_Table:
		{
			erroCode = nv_write_trq_table_check((u8 *)command->data, command->len, 0)?FOC_CRC_Err:FOC_Success;
			break;
		}
		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;
		}
		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();
}