#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"

#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif
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_foc_command(foc_cmd_body_t *command) {
	u8 erroCode = 0;
	u8 response[32];
	int len = 3;
	sys_debug("command %d\n", command->cmd);
	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_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:
		{
			if (command->len < 5) {
				erroCode = FOC_Param_Err;
			}else {
				u16 maxRPM = decode_u16(command->data);
				u16 maxPhaseCurr = decode_u8((u8 *)command->data + 2);
				u8  maxiDC = decode_u8((u8 *)command->data + 4);
				PMSM_FOC_SpeedLimit((float)maxRPM);
				PMSM_FOC_PhaseCurrLim((float)maxPhaseCurr);
				PMSM_FOC_DCCurrLimit((float)maxiDC);
				encode_u16(response + 3, (u16)PMSM_FOC_GetSpeedLimit());
				encode_u16(response + 5, (u16)PMSM_FOC_GetPhaseCurrLim());
				encode_u8(response + 7, (u8)PMSM_FOC_GetDCCurrLimit());
				len += 5;
			}
			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);
			mc_encoder_off_calibrate((vd));
			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:
		{
			pid_conf_t pid;
			u8 id =   decode_u8((u8 *)command->data);
			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
			sys_debug("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_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_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) {
				PMSM_FOC_MTPA_Calibrate(true);
			}else {
				PMSM_FOC_MTPA_Calibrate(false);
			}
			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);
				PMSM_FOC_Set_Current(is_curr);
				PMSM_FOC_Set_Angle(is_angle);
			}
			break;
		}
		default:
		{
			erroCode = FOC_Unknow_Cmd;
			break;
		}
	}
	response[0] = command->cmd;
	response[1] = CAN_MY_ADDRESS;
	response[2] = erroCode;
	can_send_response(command->can_src, response, len);
}