#include "os/co_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_api.h"
#include "foc/core/foc_core.h"
#include "foc/motor/hall.h"
#include "foc/commands.h"
#include "prot/can_foc_msg.h"
#include "app/nv_storage.h"
extern motor_foc_t g_foc;

static void foc_cmd_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));
	co_task_create(foc_cmd_task, NULL, 512);
}

bool foc_send_command(foc_cmd_body_t *command) {
	return queue_put(_cmd_queue, command);
}

static void foc_cmd_task(void *args) {
	foc_cmd_body_t command;
	while(1) {
		if (queue_get(_cmd_queue, &command)) {
			process_foc_command(&command);
			if (command.data) {
				co_free(command.data);
			}
		}
		co_task_yield();
	}
}


static void do_hall_calibrate(u8 can_addr, float vd) {
	sys_debug("cali hall phase, %d, %f\n", g_foc.state, vd);
	if (g_foc.state == IDLE) {
		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Cali_Hall_Phase), 1);
		pwm_turn_on_low_side();
		delay_ms(10);
		int result = hall_sensor_calibrate(vd);
		if (result) {
			store_hall_table(hall_get_table());
		}
		sys_debug("hall phase cali %d\n", result);
		can_send_ack(can_addr, CMD_2_CAN_KEY(Foc_Hall_Phase_Cali_Result), result);
	}else {
		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:
		{
			foc_fault_t f;
			foc_start_stop_t start = (foc_start_stop_t)decode_u8((u8 *)command->data);
			if (start == Foc_Start) {
				f = foc_start_motor();
			}else if (start == Foc_Stop) {
				f = foc_stop_motor();
			}
			sys_debug("start motor %d\n", f);
			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Start_Motor), (u8)f);
			break;
		}
		case Foc_Cali_Hall_Phase:
			do_hall_calibrate(command->can_src, decode_s32((u8 *)command->data));
			break;
		case Foc_Cali_Hall_Offset:
		{
			int offset = 0;
			u8 inc = decode_u8((u8 *)command->data);
			if (inc) {
				offset = hall_offset_increase(1);
			}else {
				offset = hall_offset_increase(-1);
			}
			sys_debug("cali hall offset %d\n", offset);
			can_response_hall_offset(command->can_src, offset);
			break;
		}
		case Foc_Set_Open_Dq_Vol:
		{
			s32 v_q = decode_s32(((u8 *)command->data) + 4);
			sys_debug("set v_q %d\n", v_q);
			foc_set_voltage_ramp(v_q);
			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Set_Open_Dq_Vol), 1);
			break;
		}
		default:
			break;
	}
}