#include "app/app.h"
#include "bsp/bsp.h"
#include "os/timer.h"
#include "os/co_task.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "prot/can_foc_msg.h"
#include "prot/can_message.h"
#include "foc/foc_api.h"
#include "bsp/timer_count32.h"

static void _app_low_task(void *args);
extern measure_time_t g_meas_hall;
extern measure_time_t g_meas_foc;
void app_start(void){
	set_log_level(MOD_SYSTEM, L_debug);
	can_message_init();
	foc_init();
	log_start_task();
	co_task_create(_app_low_task, NULL, 512);
	co_task_schedule();
}

static void _can_report_info(void) {
	can_report_speed(0x45, foc_get_speed());
	current_samp_t *s = foc_get_current_sample();
	can_report_phase_current(0x45, F2I(s->Ia * 1000), F2I(s->Ib * 1000), F2I(s->Ic * 1000));
	//sys_debug("phase current %f %f %f %f\n", s->Ia, s->Ib, s->Ic, s->max_Ia);
	sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
	s->max_Ia = 0.0f;
}

extern void hall_debug_log(void);
static void _app_low_task(void *args) {
	sys_debug("reset src: 0x%x\n", gd32_get_reset_source());
	while(1) {
		wdog_reload();
		_can_report_info();
		///sys_debug("foc exec time %d, intval %d, max %d, error %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time, g_meas_foc.exec_max_error_time, g_meas_foc.intval_time_error);
		//sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
		//sys_debug("vbus voltage: %f\n", foc_get_vbus_voltage());
		//hall_debug_log();
		co_task_delay(1000);
	}
}