#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/core/foc_api.h"
#include "bsp/timer_count32.h"
#include "app/nv_storage.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();
	restore_config();
	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;
}

static void print_backtrace(void){
    if (gd32_bkp_btrace_valid()){
		uint32_t bt[16];
		uint32_t bt_over;
		uint32_t bt_dep;
		uint16_t line;
		gd32_bkp_get_backtrace(bt, &bt_over, &bt_dep, &line);
		sys_error("system backtrace:\n");
		sys_error("stack overflow %d, stack dep = %d, line = %d\n", bt_over, bt_dep, line);
		for(bt_over = 0; bt_over < bt_dep; bt_over++){
			sys_error("0x%x ", bt[bt_over]);
		}
		sys_error("system backtrace end!\n");
	}else{
		sys_error("no backtrace\n");
	}
}

static void _app_low_task(void *args) {
	sys_debug("reset src: 0x%x\n", gd32_get_reset_source());
	print_backtrace();
	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);
	}
}