#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/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"
#include "foc/mc_config.h"
#include "foc/motor/throttle.h"

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 throttle_log(void);
extern bool can_is_connect_pc(void);
extern void hall_debug_log(void);
extern measure_time_t g_meas_hall;
extern measure_time_t g_meas_foc;
extern measure_time_t g_meas_MCTask;

void app_start(void){
	can_debug(true);
	can_message_init();
	nv_storage_init();
	mc_conf_init();
	mc_err_block_init(FOC_Reset_Reson);
	mc_init();
	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();
}

#if 0
static void test_motor_temp(void) {
	static s16 temp = 100;
	static bool add = true;
	sys_debug("mot: %d->%f\n", temp, motor_temp_high_limit_test(temp));
	if (temp > 90 && temp != 300) {
		temp += add?1:-1;
		if (temp == 133) {
			add = false;
		}
	}else {
		temp = 300;
	}
}

static void test_mos_temp(void) {
	static s16 temp = 88;
	static bool add = true;
	sys_debug("mos: %d->%f\n", temp, mos_temp_high_limit_test(temp));
	if (temp > 85 && temp != -40) {
		temp += add?1:-1;
		if (temp == 123) {
			add = false;
		}
	}else {
		temp = -40;
	}

}

static void test_vbus_vol(void) {
	static s16 vol = 65;
	static bool add = false;
	sys_debug("vbus: %d->%d\n", vol, vbus_voltage_low_limit_test(vol));
	vol += add?1:-1;
	if (vol < 40) {
		add = true;
	}else if (vol >= 65) {
		add = false;
	}
}

#endif

static void app_print_log(void) {
	//sys_debug("Slow: %d - %d, err:%d, %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time, g_meas_MCTask.exec_max_error_time, g_meas_MCTask.exec_time_error);
	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 %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_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("curr A:%f,B:%f,C:%f\n", foc()->in.curr_abc[0], foc()->in.curr_abc[1], foc()->in.curr_abc[2]);
	//sys_debug("dead time 0x%x\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();
	hall_debug_log();
	//motor_debug();
	//sample_log();
	//throttle_log();
	//sys_debug("Trq: %f-%f-%f\n", motor.controller.ramp_input_torque.target, motor.controller.ramp_input_torque.interpolation, motor.controller.ramp_input_torque.step);
	sys_debug("FOC is %s, %d,%d, pwm:%s\n", mot_contrl_is_start(mot_contrl())?"start":"stop", motor.foc_start_cnt, motor.foc_stop_cnt, pwm_channel_is_enabled()?"start":"stop");
	sys_debug("curr: %f,%f\n", mot_contrl()->dc_curr_calc, mot_contrl()->dc_curr_filted);
	float phase_vol[3];
	get_uvw_phases_raw(phase_vol);
	sys_debug("phvol: %f,%f,%f\n", phase_vol[0], phase_vol[1], phase_vol[2]);
	//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");
	//test_mos_temp();
	//test_mos_temp();
	//test_vbus_vol();
}

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_foc_status(0x45);
	can_report_dq_voltage(0X45);
	if (motor.controller.b_mtpa_calibrate) {
		can_report_mpta_values(0x45);
	}else {
		can_report_dq_current(0x45);
	}

	if (mot_params_rs_ested()) {
		can_report_mot_params_ested(mot_params_get_est_rs(), R_TYPE);
	}
	if (mot_params_ld_ested()) {
		can_report_mot_params_ested(mot_params_get_est_ld(), L_TYPE_D);
	}
	if (mot_params_lq_ested()) {
		can_report_mot_params_ested(mot_params_get_est_lq(), L_TYPE_Q);
	}
	if (mot_params_flux_ested()) {
		can_report_mot_params_ested(mot_params_get_est_flux(), FLUX_TYPE);
	}

	if (++loop % 10 == 0) {
		app_print_log();
	}
	return 200;
}
int plot_type = 0;
static void plot_smo_angle(void) {
	u32 mask = cpu_enter_critical();
	float smo_angle = foc_observer_sensorless_angle();
	float mot_angle = foc()->in.mot_angle;
	cpu_exit_critical(mask);
	float delta = smo_angle - mot_angle;
	float s, c;
	arm_sin_cos(delta, &s, &c);
	delta = fast_atan2(s, c)/PI*180.0f;
	can_plot3(mot_angle, smo_angle, delta);
}

#ifdef CONFIG_USE_ENCODER_HALL
static void plot_hall_angle(void) {
	u32 mask = cpu_enter_critical();
	float hall_angle = hall_get_elec_angle();
	float mot_angle = foc()->in.mot_angle;
	if (mot_contrl()->if_ctl.b_ena) {
		mot_angle += 90;
	}
	norm_angle_deg(mot_angle);
	cpu_exit_critical(mask);
	float delta = hall_angle - mot_angle;
	float s, c;
	arm_sin_cos(delta, &s, &c);
	delta = fast_atan2(s, c)/PI*180.0f;
	can_plot3(mot_angle, hall_angle, delta);
}
#endif

static u32 app_plot_task(void * args) {
	if (plot_type == 1) {
		s16 plot_arg1 = (s16)foc_observer_sensorless_speed();
		s16 plot_arg2 = (s16)mot_contrl_get_speed(&motor.controller);
		if (mot_contrl()->mode_running == CTRL_MODE_SPD) {
			if (mc_is_cruise_enabled()) {
				plot_arg1 = mot_contrl()->ramp_cruise_vel.target;
			}else {
				plot_arg1 = mot_contrl()->ramp_target_vel.target;
			}
		}
		can_plot2(plot_arg1, plot_arg2);
	}else if (plot_type == 2) {
#ifdef CONFIG_USE_ENCODER_HALL
		if (mot_contrl()->if_ctl.b_ena || motor.b_force_run) {
			plot_hall_angle();
		}else {
			can_plot2(mot_contrl_get_final_torque(&motor.controller), mot_contrl()->target_torque);
		}
#else
		can_plot2(mot_contrl_get_final_torque(&motor.controller), mot_contrl()->target_torque);
#endif
	}else if (plot_type == 3) {
		plot_smo_angle();
	}else if (plot_type == 4) {
		can_plot2((s16)foc()->out.curr_dq.d, (s16)foc()->out.curr_dq.q);
	}else if (plot_type == 5) {
		can_plot2((s16)foc()->in.target_id.interpolation , (s16)foc()->in.target_iq.interpolation);
	}else if (plot_type == 6) {
		//do it in other place
	}else if (plot_type == 7) {
		can_plot2((s16)(foc()->in.target_id.target*10.0f), (s16)(foc()->out.curr_dq.d * 10.0f));
	}else if (plot_type == 8) {
		can_plot2((s16)(foc()->in.target_iq.target*10.0f), (s16)(foc()->out.curr_dq.q * 10.0f));
	}else if (plot_type == 9) {
		s16 thro_v = get_throttle_float() * 100.0f;
		s16 thro2_v = get_throttle2_float() * 100.0f;
		s16 sig = throttle_get_signal();
		can_plot3(sig, thro_v, thro2_v);
	}else if (plot_type == 10) {
		s16 duty = (s16)(motor.controller.duty_raw * 100);
		s16 duty_filterd = (s16)(motor.controller.duty_filterd * 100);
		can_plot2(duty, duty_filterd);
	}
	return 10;
}
static u32 app_low_task(void *args) {
	wdog_reload();
	if (!mot_contrl_is_start(&motor.controller)) {
		mc_err_block_save();
	}
	return 1;
}