#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/motor/current.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);
static u32 _app_report_task(void *args);
static u32 _app_plot_task(void *args);
extern void PMSM_FOC_LogDebug(void);
extern void err_code_log(void);
extern void encoder_log(void);
extern measure_time_t g_meas_hall;
extern measure_time_t g_meas_foc;
extern measure_time_t g_meas_MCTask;
#ifdef JTAG_DEBUG
int jtag_cmd = 0;
int jtag_data = 0;
int jtag_plot = 0;
void fetch_jtag_cmd(void) {
	foc_cmd_body_t foc_cmd;

	if (jtag_cmd == 1 || jtag_cmd == 2) {
		jtag_plot = 2;
		foc_cmd.cmd = Foc_Start_Motor;
		foc_cmd.data = (void *)os_alloc(4);
		encode_u8(foc_cmd.data, 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) {
		jtag_plot = 1;
		foc_cmd.cmd = Foc_Cali_Hall_Phase;
		foc_cmd.data = (void *)os_alloc(4);;
		encode_s16(foc_cmd.data, jtag_data);
		foc_send_command(&foc_cmd);
		jtag_cmd = 0;
	}else if (jtag_cmd == 5) {
		PMSM_FOC_Set_Torque((float)jtag_data/10.0f);
		jtag_cmd = 0;
	}else if (jtag_cmd == 6) {
		PMSM_FOC_EnableCruise(true);
	}else if (jtag_cmd == 7) {
		PMSM_FOC_EnableCruise(false);
	}else if (jtag_cmd == 8) {
		//mc_current_sensor_calibrate((float)jtag_data/10.0f);
		jtag_cmd = 0;
	}else if (jtag_cmd == 9) {
		mc_encoder_zero_calibrate(jtag_data);
		jtag_cmd = 0;
	}
	jtag_cmd = 0;
}
#else
void fetch_jtag_cmd(void){
}
#endif

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);
	shark_task_create(_app_report_task, NULL);
	shark_task_create(_app_plot_task, NULL);
	sys_debug("mc start\n");
	shark_task_run();
}

static u32 _app_report_task(void *p) {
	static u32 loop = 0;
	can_report_power(0x45);
	can_report_dq_current(0x45);
	can_report_foc_status(0x45);
	can_report_phase_voltage(0x45);
	can_report_mpta_values(0x45);
	can_report_ext_status(0x43);
	
	if (++loop % 10 == 0) {
		//sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation);
		sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
		sys_debug("Fast: %d - %d, err: %d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err);
		sys_debug("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
		sys_debug("throttle %f\n", get_throttle_float());
		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
		encoder_log();
		//err_code_log();
	}
	return 200;
}
//static bool _mc_start = false;
//static float _angle = 0.0f;
static u32 _app_plot_task(void * args) {
	//can_report_plot_values(0x45);
	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
	//can_plot2(PMSM_FOC_Get()->in.s_motAngle, PMSM_FOC_Get()->in.s_smoAngle + 360.0f);
	can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
#if 0
	if (!_mc_start) {
		_mc_start = true;
		mc_start(CTRL_MODE_OPEN);
		PMSM_FOC_SetOpenVdq(0, 60);
	}
	_angle++;
	if (_angle >= 360.0f) {
		_angle = 0.0f;
	}
	PMSM_FOC_Set_Angle(_angle);
#endif
	return 20;
}
static u32 _app_low_task(void *args) {
	wdog_reload();
	fetch_jtag_cmd();
	return 1;
}