#include "app/app.h"
#include "bsp/bsp.h"
#include "os/os_task.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "foc/motor/motor.h"
#include "foc/samples.h"
#include "prot/can_foc_msg.h"
#include "prot/can_message.h"
#include "libs/time_measure.h"
#include "app/nv_storage.h"
#include "foc/commands.h"


static u32 _app_low_task(void *args);
extern void PMSM_FOC_LogDebug(void);
extern measure_time_t g_meas_hall;
extern measure_time_t g_meas_foc;
extern measure_time_t g_meas_timeup;

#ifdef JTAG_DEBUG
int jtag_cmd = 0;
int jtag_data = 0;
void fetch_jtag_cmd(void) {
	foc_cmd_body_t foc_cmd;
	u8 cmd_data[32];
	if (jtag_cmd == 1 || jtag_cmd == 2) {
		foc_cmd.cmd = Foc_Start_Motor;
		foc_cmd.data = cmd_data;
		cmd_data[0] = jtag_cmd;
		foc_send_command(&foc_cmd);
		jtag_cmd = 0;
	}else if (jtag_cmd == 3) {
		float vq = (float)jtag_data/10.0f;
		PMSM_FOC_SetOpenVdq(0, (vq));
		jtag_cmd = 0;
	}else if (jtag_cmd == 4) {
		foc_cmd.cmd = Foc_Cali_Hall_Phase;
		foc_cmd.data = cmd_data;
		encode_s16(cmd_data, jtag_data);
		foc_send_command(&foc_cmd);
		jtag_cmd = 0;
	}else if (jtag_cmd == 5) {
		PMSM_FOC_Set_Current(0.5f);
		jtag_cmd = 0;
	}else if (jtag_cmd == 6) {
		PMSM_FOC_EnableCruise(true);
	}else if (jtag_cmd == 7) {
		PMSM_FOC_EnableCruise(false);
	}
}
#else
void fetch_jtag_cmd(void){
}
#endif

static void mc_exec_log(void) {
//	sys_debug("intval = %d, exec = %d, err = %d, %d, %d, count = %d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_time_error, g_meas_foc.intval_low_err, g_meas_foc.intval_hi_err, g_meas_foc.exec_count);
//	sys_debug("timeup intval = %d, err = %d\n\n", g_meas_timeup.intval_time, g_meas_timeup.intval_time_error);
	PMSM_FOC_LogDebug();
}

void app_start(void){
	set_log_level(MOD_SYSTEM, L_debug);
	can_message_init();
	nv_storage_init();
	mc_init();
#ifdef GD32_FOC_DEMO	
	extern void key_init(void);
	key_init();
#endif
	gpio_led1_enable(true);
	shark_task_create(_app_low_task, NULL);

	sys_debug("mc start\n");
	shark_task_run();
}

static void _can_report_info(void) {
	//can_report_speed(0x45, PMSM_FOC_GetSpeed());
}


static u32 _app_low_task(void *args) {
	wdog_reload();
	_can_report_info();
	mc_exec_log();
	fetch_jtag_cmd();
	return 1000;
}