#include "main.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "flash.h"
#include "health.h"
#include "initial.h"
#include "motor.h"
#include "sys/flash.h"
#include "sys/initial.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <cstdio>
#include <stdint.h>
extern CAN_transmiter CAN0_transmiter;		// CAN初始化
extern sampler sampler;						// 采样器
extern angle_sensor KTH7823;				// 角度传感器
extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
extern EncoderAngleKalman Angle_kalman;		// 角度滤波
extern ThermalController thermalController; // 热量控制器
extern FOC_Controlroller FOC;				// FOC控制
extern FlashEEPROM flashEEP;				// EEPROM类

TaskScheduler scheduler = {0};
volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
volatile u32 cnt_test = 0;			 // 负载检测
int main(void) {
	u8 init_status = hardware_init();

	while (init_status) { // 初始化检测
		std::printf("初始化失败\r\n");
	}
	OTA_initial(); //  OTA初始化,A/B分区处理

	angsens_MG_check(); // 角度传感器磁感应强度测试
	watchdog_init(50);	// 看门狗初始化
	while (1) {
		__disable_irq();
		u32 currentTasks =
			scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性
		scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
		__enable_irq();

		do_task(currentTasks);
	}
	return 0;
}

u8 hardware_init(void) {
	__enable_irq();
	systick_config();  // 系统时钟初始化
	unused_pin_init(); // 未使用引脚初始化
	USART0_init();	   // 调试串口初始化
	motor_init();	   // 电机驱动初始化
	if (get_reset_cause() ==
		RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠
		printf("continue sleep\r\n");
		watchdog_init(5000); // 看门狗初始化
		prepare_deep_sleep(false);
	}
	User_timer_init(); // 任务Timer初始化
	u8 init_status = 0;
	init_status += KTH7823.init(); // 角度传感器初始化
	init_status += sampler.init(); // 采样器初始化
	flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后

	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
	init_status += CAN0_transmiter.init();				  // CAN初始化
	return init_status;
}

void do_task(u32 currentTasks) {
	cnt_test++;
	// 按照优先级和重要性处理任务
	if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
		sampler.update_current();
	}
	if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值
		KTH7823.update();
		FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
	}
	if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
	}
	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
		motor_runtime_50us++;

		// 函数指针数组，包含各种控制模式对应的函数
		static FOCFunctionPtr control_functions[] = {
			&FOC_Controlroller::drv_hold, // 0: 占空比置
			&FOC_Controlroller::
				angle_control_with_velocity_limit, // 1:
												   // 角度闭环控制，带速度限制
			&FOC_Controlroller::velocity_control,  // 2: 速度闭环控制
			&FOC_Controlroller::current_control,   // 3: 电流闭环控制
			&FOC_Controlroller::velocity_open_loop // 4: 速度开环控制
		};

		// 边界检查，防止数组越界
		if (FOC.control_mode >= 0 && FOC.control_mode < 5) {
			// 调用对应的控制函数
			(FOC.*control_functions[FOC.control_mode])();
		}
	}
	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值
		sampler.update_mcu_temp();
	}
	if (currentTasks & (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值
		sampler.update_drv_temp();
	}
	if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制
		thermalController.update();
	}
	if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测
		angsens_MG_check();
	}
	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
		power_manage();
	}
	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
		CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
		cnt_test = 0;
		CAN0_transmiter.can0_send_flag = SET;
		CAN0_transmiter.communication_check();
		fwdgt_counter_reload(); // 看门狗复位
	}
}