#include "os/os_task.h"
#include "bsp/bsp_driver.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 target_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_cmd.cmd = Foc_Start_Motor;
			if (PMSM_FOC_Is_Start()) {
				PMSM_FOC_Set_Torque(foc_current);
				PMSM_FOC_Set_Speed(0.0f);
			}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) {
			PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
			//PMSM_FOC_SpeedLimit(100);
			//PMSM_FOC_AutoHold(true);
		}
		key_value[KEY_STOP] = value;
	}

	value = gpio_funckey_value();
	if (value != key_value[KEY_FUNC]) {
		if (value) {
			if (ctrl_mode == CTRL_MODE_TRQ) {
				ctrl_mode = CTRL_MODE_SPD;
				PMSM_FOC_SetCtrlMode(ctrl_mode);
			}else if(ctrl_mode == CTRL_MODE_SPD){
				//ctrl_mode = CTRL_MODE_TRQ;
				//PMSM_FOC_EnableCruise(false);
				PMSM_FOC_SpeedLimit(10000);
				target_speed += 1000;
				PMSM_FOC_Set_Speed(target_speed);
				
			}else {
				//ctrl_mode = CTRL_MODE_TRQ;
			}
			
			//PMSM_FOC_SpeedLimit(max_speed);
			//max_speed += 1000;
		}
		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);
}