#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();
	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));
}

static void _app_low_task(void *args) {
	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);
		co_task_delay(500);
	}
}