#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) {
	return queue_put(_cmd_queue, command);
}

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(pid_conf_t *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;
			if (scmd->start_stop == Foc_Start) {
				success = mc_start(TRQ_MODE);
			}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_Current_Limit:
		{
			s16 current = decode_s16(((u8 *)command->data));
			PMSM_FOC_iBusLimit(current);
		}		
		case Foc_Cali_Hall_Phase:
		{
			mc_hall_calibrate(decode_s16((u8 *)command->data));
			break;
		}
		case Foc_Set_Open_Dq_Vol:
		{
			s32 vq = decode_s32(((u8 *)command->data) + 4);
			sys_debug("set v_q %d\n", vq);
			PMSM_FOC_SetOpenVdq(0, vq);
			break;
		}
		case Foc_Conf_Pid:
		{
			pid_conf_t pid[5];
			memcpy((char *)pid, (char *)command->data, 5 * sizeof(pid_conf_t));
			conf_foc_pid(pid);
		}
		default:
		{
			erroCode = FOC_Unknow_Cmd;
			break;
		}
	}
	can_send_ack(command->can_src, CMD_2_CAN_KEY(command->cmd), (u8)erroCode);
}