#include "bsp/bsp_driver.h"
#include "app/app.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/core/foc_observer.h"
#include "foc/core/ladrc_observer.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"
#include "foc/core/thro_torque.h"
#include "foc/core/F_Calc.h"
#include "foc/motor/motor_param.h"
#include "foc/motor/mot_params_ind.h"
#include "foc/limit.h"
#include "foc/mc_error.h"


#ifdef CONFIG_DQ_STEP_RESPONSE
extern float target_d;
extern float target_q;
#endif
static u32 _app_low_task(void *args);
static u32 _app_report_task(void *args);
static u32 _app_plot_task(void *args);
//static u32 _app_trq_test_task(void *args);
extern void PMSM_FOC_LogDebug(void);
extern void mc_err_code_log(void);
extern void encoder_log(void);
extern void sample_log(void);
extern void thro_torque_log(void);
extern void eCtrl_debug_log(void);
extern bool can_is_connect_pc(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

#if 0
static s16 test_rpm = 0000;
static float test_trq = 0;
static float test_id = 0;
static float test_iq = 0;
static u32 _app_trq_test_task(void *args) {
	DQ_t dq;
	motor_mpta_fw_lookup(test_rpm, test_trq, &dq);
	test_id = dq.d;
	test_iq = dq.q;
	sys_debug("lookup: %d, %f, %f, %f\n", test_rpm, test_trq, test_id, test_iq);
	test_rpm = 8000;
	test_trq += 5.0f;
	return 500;
}
#endif
void app_start(void){
	set_log_level(MOD_SYSTEM, L_disable);
	can_message_init();
	nv_storage_init();
	mc_err_block_init();
	mc_init();
#ifdef GD32_FOC_DEMO	
	extern void key_init(void);
	key_init();
#endif
	shark_task_create(_app_low_task, NULL);
	shark_task_create(_app_report_task, NULL);
	shark_task_create(_app_plot_task, NULL);
	//shark_task_create(_app_trq_test_task, NULL);
	sys_debug("mc start\n");
	shark_task_run();
}

static u32 _app_report_task(void *p) {
	static u32 loop = 0;

	can_report_ext_status(0x43);
	can_mcast_foc_status2();
	if (!can_is_connect_pc()) {
		return 200;
	}
	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_phase_current(0x45);
	if (mot_params_rs_ested()) {
		can_report_motparam(mot_params_get_est_rs(), R_TYPE);
	}
	if (mot_params_ld_ested()) {
		can_report_motparam(mot_params_get_est_ld(), L_TYPE_D);
	}
	if (mot_params_lq_ested()) {
		can_report_motparam(mot_params_get_est_lq(), L_TYPE_Q);
	}
	if (mot_params_flux_ested()) {
		can_report_motparam(mot_params_get_est_flux(), FLUX_TYPE);
	}

	if (++loop % 10 == 0) {
		//sys_debug("rst 0x%x\n", get_mcu_reset_source());
		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
		//sys_debug("Fast: %d - %d, err: %d-%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, g_meas_foc.exec_max_error_time);
		//sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
		//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
		//sys_debug("throttle %f\n", get_throttle_float());
		//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
		//sys_debug("dead time %d\n", get_deadtime());
		//thro_torque_log();
		//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
		encoder_log();
		//motor_debug();
		//sample_log();
		PMSM_FOC_LogDebug();
		//F_debug();
		//eCtrl_debug_log();
		//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
		//mc_err_code_log();
		sys_debug("=====\n");
	}
	return 200;
}
int plot_type = 0;
static void plot_smo_angle(void) {
#if 1
	float smo_angle = foc_observer_sensorless_angle();
	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
	float s, c;
	arm_sin_cos_f32(delta, &s, &c);
	delta = fast_atan2(s, c)/PI*180.0f;
	can_plot3(PMSM_FOC_Get()->in.s_motAngle, smo_angle, delta);
#else
	can_plot2((s16)(foc_observer_sensorless_diff() * 100.0f), (s16)(foc_observer_sensorless_ration() * 100.0f));
#endif
}
static u32 _app_plot_task(void * args) {
	if (plot_type == 1) {
		can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
		//can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
	}else if (plot_type == 2) {
		can_plot2(eCtrl_get_FinalTorque(), PMSM_FOC_Get()->in.s_targetTorque);
	}else if (plot_type == 3) {
		plot_smo_angle();
	}else if (plot_type == 4) {
		can_plot2((s16)PMSM_FOC_Get()->out.s_RealIdq.d, (s16)PMSM_FOC_Get()->out.s_RealIdq.q);
	}else if (plot_type == 5) {
		can_plot2((s16)PMSM_FOC_Get()->idq_ctl[0].s_Cp, (s16)PMSM_FOC_Get()->idq_ctl[1].s_Cp);
	}else if (plot_type == 6) {
		//can_plot2((s16)(PMSM_FOC_Get()->in.s_iABC[0]), (s16)(PMSM_FOC_Get()->in.s_iABC[1]));
	}else if (plot_type == 7) {
		#ifdef CONFIG_DQ_STEP_RESPONSE
		can_plot2((s16)(target_d*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f));
		#endif
	}else if (plot_type == 8) {
		#ifdef CONFIG_DQ_STEP_RESPONSE
		can_plot2((s16)(target_q*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f));
		#endif
	}else if (plot_type == 9) {
		can_plot3((s16)F_get_air(), (s16)F_get_accl(), (s16)F_get_Te());
	}
	
	return 20;
}
static u32 _app_low_task(void *args) {
	wdog_reload();
	if (!PMSM_FOC_Is_Start()) {
		mc_err_block_save();
	}
	fetch_jtag_cmd();
	return 1;
}