#include "main.h"
#include "Can.h"
#include "DengFOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "core_cm4.h"
#include "core_cmFunc.h"
#include "health.h"
#include "initial.h"
#include "sys/health.h"
#include "sys/systick.h"
#include "systick.h"

TaskScheduler scheduler = {0};

int main(void) {
	systick_config();							// 系统时钟初始化
	USART0_init();								// 调试串口初始化
	User_timer_init();							// Timer初始化
	driver_init();								// 电机驱动初始化
	extern FOC_Controlroller FOC;				// FOC控制
	extern sampler sampler;						// 采样器
	extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
	extern EncoderAngleKalman Angle_kalman;		// 角度滤波
	extern angle_sensor KTH7823;				// 角度传感器
	extern CAN_transmiter CAN0_transmiter;		// CAN
	extern ThermalController thermalController; // 热量控制器

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

		// 按照优先级和重要性处理任务
		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)) { // 电机控制任务(最高优先级)
			switch (FOC.control_mode) {
			case 0: {
				FOC.drv_hold(0.95); // 占空比置0
				break;
			}
			case 1: {
				FOC.angle_control(FOC.target_angle); // 角度闭环控制
				break;
			}
			case 2: {
				FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
				break;
			}
			case 3: {
				FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
				break;
			}
			case 4: {
				FOC.velocity_open_loop(); // 速度开环控制
				break;
			}
			}
		}
		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电压值
			sampler.update_vbus_voltage();
			FOC.update_power();
		}
		if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
			CAN0_transmiter.communication_check();
			fwdgt_counter_reload(); // 看门狗复位
		}
	}
}