#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/core/foc_core.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 do_hall_calibrate(u8 can_addr, float vd) {
	sys_debug("cali hall phase, %f\n", vd);
	PMSM_FOC_HallCalibrate(true, vd);
	can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 0);
}


static void process_foc_command(foc_cmd_body_t *command) {
	sys_debug("command %d\n", command->cmd);
	switch (command->cmd) {
		case Foc_Start_Motor:
		{
			u8 erroCode = 0;
			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);
			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)erroCode);
			break;
		}
		case Foc_Set_Cruise_Mode:
		{
			u8 enable = decode_u8(command->data);
			PMSM_FOC_EnableCruise(enable);
			break;
		}
		case Foc_Cali_Hall_Phase:
		{
			do_hall_calibrate(command->can_src, decode_s32((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);
			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Set_Open_Dq_Vol), 1);
			break;
		}
		default:
			break;
	}
}