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

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 conf_foc_pid(u8 id, pid_conf_t *pid) {
	nv_set_pid(id, pid);
}

static void process_foc_command(foc_cmd_body_t *command) {
	u8 erroCode = 0;
	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_Speed_Limit:
		{
			s16 speed = decode_s16(((u8 *)command->data));
			PMSM_FOC_SpeedLimit(speed);
		}
		case Foc_Set_iDC_Limit:
		{
			s16 current = decode_s16(((u8 *)command->data));
			PMSM_FOC_iBusLimit(current);
		}
		case Foc_Set_Phase_Current:
		{
			s16 curr = decode_s16(((u8 *)command->data));
			PMSM_FOC_PhaseCurrLim((float)curr);
		}
		case Foc_Cali_Hall_Phase:
		{
			s16 vd = decode_s16((u8 *)command->data);
			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\n", 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));
			conf_foc_pid(id, &pid);
		}
		case Foc_Start_EPM_Move:
		{
			bool move = decode_u8((u8 *)command->data) == 0?false:true;
			EPM_Dir_t dir = (EPM_Dir_t)decode_u8((u8 *)command->data + 1);
			if(!PMSM_FOC_Start_epmMove(move, dir)) {
				erroCode = PMSM_FOC_GetErrCode();
			}
			break;
		}
		default:
		{
			erroCode = FOC_Unknow_Cmd;
			break;
		}
	}
	//can_send_ack(command->can_src, CMD_2_CAN_KEY(command->cmd), (u8)erroCode);
}