#include "health.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
#include <stdint.h>

extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
ThermalController thermalController; // 热量控制器初始化

#define max(a, b, c)                                                           \
	((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
/**
** @brief  热能计算
** @param  None
** @retval 热量值
*/
u32 ThermalController::update() {
	thermal_value = Q16_16_MUL(thermal_value, thermalRate);
	// 添加温度积累
	int drv_temp = (int)sampler.get_drv_temp();
	int mcu_temp = (int)sampler.get_mcu_temp();
	if (drv_temp == 255)
		drv_temp = 0;
	if (mcu_temp == 255)
		mcu_temp = 0;
	thermal_value += drv_temp * drv_temp + mcu_temp * mcu_temp;

	if (remainingTime10Ms) {
		// 过热停机状态
		// 检查是否可以恢复
		q16_16_t recovery_threshold =
			Q16_16_MUL(thermal_capacity, recoveryRate);
		if (thermal_value < recovery_threshold) {
			remainingTime10Ms = 1; // 可以恢复工作
		}
		if (remainingTime10Ms == 1) { // 过热恢复
			FOC.resume();
			CAN0_transmiter.ThermalStatus = 0;
			return --remainingTime10Ms;
		}
		return --remainingTime10Ms;
	} else {
		// 当前没有过热停机
		// 添加电流热积累
		u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
						   (u32)(sampler.get_current('V') * 1000),
						   (u32)(sampler.get_current('W') * 1000)};

		// 找最大电流（原始ADC值）
		current_max = max(currents[0], currents[1], currents[2]);
		if (current_max < 2000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.00007f; // 计算当前电流热积累值
		} else if (current_max > 3800) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.01f; // 添加当前电流热积累值
		} else if (current_max > 3400) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.005f; // 计算当前电流热积累值
		} else if (current_max > 3000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.0002f; // 计算当前电流热积累值
		} else if (current_max > 2000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.0001f; // 计算当前电流热积累值
		}
	}

	if (thermal_value > thermal_capacity) {
		FOC.shut_down();
		remainingTime10Ms = PROTECT_TIME; // 过热停机
		CAN0_transmiter.ThermalStatus = 1;
		return remainingTime10Ms;
	}
	return thermal_value;
}
/**
** @brief  温度检测
** @param  None
*/
void angsens_MG_check(void) {
	bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
	bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
	CAN0_transmiter.MagStatus = MGH or MGL;
}