#include "os/os_task.h"
#include "bsp/gpio.h"
#include "bsp/bsp.h"
#include "os/os_task.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "foc/commands.h"
#include "foc/core/PMSM_FOC_Core.h"

#define KEY_START 0
#define KEY_STOP  1
#define KEY_FUNC  2

static u8 key_value[3];
static float foc_current = 0.0f;
static u8 ctrl_mode = CTRL_MODE_OPEN;
static float max_speed = 2000;
static u32 key_task(void *p) {
	foc_cmd_body_t foc_cmd;
	u8 cmd_data[16];
	foc_cmd.data = cmd_data;
	int value = gpio_startkey_value();
	if (value != key_value[KEY_START]) {
		if (value) {
			foc_current = 0.0f;
			ctrl_mode = CTRL_MODE_OPEN;
			foc_cmd.cmd = Foc_Start_Motor;
			if (PMSM_FOC_Is_Start()) {
				cmd_data[0] = Foc_Stop; 
			}else {
				cmd_data[0] = Foc_Start; 
			}
			foc_send_command(&foc_cmd);
		}
		key_value[KEY_START] = value;
	}

	value = gpio_stopkey_value();
	if (value != key_value[KEY_STOP]) {
		if (value) {
			if (foc_current < 2.0f) {
				foc_current += 0.1f;
			}
			PMSM_FOC_Set_Torque(foc_current);
		}
		key_value[KEY_STOP] = value;
	}

	value = gpio_funckey_value();
	if (value != key_value[KEY_FUNC]) {
		if (value) {
			if (ctrl_mode == CTRL_MODE_OPEN) {
				ctrl_mode = CTRL_MODE_TRQ;
			}else {
				ctrl_mode = CTRL_MODE_OPEN;
			}
			PMSM_FOC_SetCtrlMode(ctrl_mode);
			PMSM_FOC_SpeedLimit(max_speed);
			//max_speed += 10;
		}
		key_value[KEY_FUNC] = value;
	}
	return 0;
}

void key_init(void) {
	key_value[KEY_START] = gpio_startkey_value();
	key_value[KEY_STOP] = gpio_stopkey_value();
	key_value[KEY_FUNC] = gpio_funckey_value();

	shark_task_create(key_task, NULL);
}