/**
 * @file Algorithm/FOC.cpp
 * @brief 算法模块源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

#include "FOC.h"
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "config.h"
#include "flash.h"
#include "motor.h"
#include "pid.h"
#include "run_inhibit.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <gd32c10x.h>

// 全局 FOC 控制器单例：系统只有一个电机控制对象。
// 初始化顺序由 main::hardware_init() 明确调用 FOC.init() 完成；
// 运行期主要由主循环控制任务访问，CAN 命令只更新目标/参数。
FOC_Controller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
extern FlashEEPROM flashEEP;

// 初始变量及函数定义
#define _constrain(amt, low, high)                                             \
	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
/** @brief constrain 宏定义 */
#define constrain(value, min, max, target_min, target_max)                     \
	((value - min) / (max - min) * (target_max - target_min))

// 校准步骤（代码模板）
/**
 * @brief calibrate_zero_position 函数
 * @return void
 */
void calibrate_zero_position(void) {
	// 运行中禁止校准：要求控制模式为停止(0)且基本静止，
	// 否则转子无法对齐到电气 0°，标定结果错误，甚至因电角度突变导致失控。
	// 被禁止运行时同样拒绝：本函数会直接 motor_resume() 唤醒驱动，
	// 不判闸门就成了停机态下的旁路通道。
	if (FOC.control_mode != 0 || !run_inhibit.permitted() ||
		fabsf(FOC.rt_velocity) > CALIB_MAX_SPEED) {
		return;
	}

	// 使能电机驱动器，准备对齐
	motor_resume();

	// 施加固定电压矢量（A 相高电平），让转子对齐到电气 0° 位置
	Set_3Pwm(ALIGN_PWM_DUTY, 0.0f, 0.0f);
	delay_1ms(ALIGN_SETTLE_MS);

	// 对齐稳定后重新采样传感器角度，再计算零位补偿角，使当前位置对应电气 0°
	KTH7823.update();
	float current_sensor_angle = KTH7823.getcurrent_sensorangle();
	float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f);
	KTH7823.set_calibration(-electrical_angle);

	// 停止施加电压
	Set_3Pwm(0.0f, 0.0f, 0.0f);

	// 保存校准角到 Flash，掉电不丢
	flashEEP.save_calibration_angle(KTH7823.calibration_angle);
}

// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
/**
 * @brief cal_Iq_Id 函数
 * @param[in] current_a 参数
 * @param[in] current_b 参数
 * @param[in] current_c 参数
 * @param[in] angle_el 参数
 * @return float 返回值
 */
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
								   float current_c) {
	if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
		current_c == ERROR_ADC_VAL) {
		return Current_kalman.getCurrent();
	}

	// 去除三相公共模偏置，使 Ia + Ib + Ic 更接近 0
	const float current_avg = (current_a + current_b + current_c) / 3.0f;
	current_a -= current_avg;
	current_b -= current_avg;
	current_c -= current_avg;

	// Clarke变换 (使用三相电流)
	float I_alpha, I_beta;
	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
			  (1.0f / 3.0f) * current_c;
	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;

	// Park变换：sin/cos 由 update_el_sincos() 每周期算一次，
	// 与 setSVPWM_PhaseVoltage 的逆 Park 共用，省一次三角函数
	float I_q, I_d;
	I_q = I_beta * el_cos - I_alpha * el_sin;
	I_d = I_beta * el_sin + I_alpha * el_cos;

	// 返回Iq值，乘以1000以转换为mA
	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
	// return I_q * 1000 - KE_FACTOR * rt_velocity;
}


/**
 * @brief 角度同步 Uq 前馈更新/输出。
 * @note 只在稳定同向巡航时输出和学习；停止、低速、反向、明显刹车/超速时立即退出。
 */
float FOC_Controller::update_angle_sync_uq_feedforward(
	u8 mode, float table[], u16 table_size, float mech_angle,
	float target_vel, float actual_vel, float velocity_error) {
	if (table_size == 0U) return 0.0f;

	bool enable = false;
	bool learn_enable = false;
	float min_target = 0.0f;
	float stop_speed = 0.0f;
	float brake_err = 0.0f;
	float learn_err_max = 0.0f;
	float learn_alpha = 0.0f;
	float gain = 0.0f;
	float max_ff = 0.0f;
	float dir_sign = 1.0f;

	switch (mode) {
	case 2:
		enable = MODE2_UQ_FF_ENABLE;
		learn_enable = MODE2_UQ_FF_LEARN_ENABLE;
		min_target = MODE2_UQ_FF_MIN_TARGET;
		stop_speed = MODE2_UQ_FF_STOP_SPEED;
		brake_err = MODE2_UQ_FF_BRAKE_ERR;
		learn_err_max = MODE2_UQ_FF_LEARN_ERR_MAX;
		learn_alpha = MODE2_UQ_FF_LEARN_ALPHA;
		gain = MODE2_UQ_FF_GAIN;
		max_ff = MODE2_UQ_FF_MAX;
		dir_sign = 1.0f;
		break;
	case 3:
		enable = MODE3_UQ_FF_ENABLE;
		learn_enable = MODE3_UQ_FF_LEARN_ENABLE;
		min_target = MODE3_UQ_FF_MIN_TARGET;
		stop_speed = MODE3_UQ_FF_STOP_SPEED;
		brake_err = MODE3_UQ_FF_BRAKE_ERR;
		learn_err_max = MODE3_UQ_FF_LEARN_ERR_MAX;
		learn_alpha = MODE3_UQ_FF_LEARN_ALPHA;
		gain = MODE3_UQ_FF_GAIN;
		max_ff = MODE3_UQ_FF_MAX;
		dir_sign = DIR;
		break;
	default:
		return 0.0f;
	}

	// 超速/制动判定必须沿"运动方向"：velocity_error×dir_sign 是 PID 系误差，
	// 再乘运动方向符号后，< -brake_err 才表示"沿运动方向明显超速"。
	// 原实现只乘 dir_sign：反转(负速度)时判定极性恰好相反——反转超速时
	// 前馈继续输出、反转欠速时前馈反而被切断，已学到的电压(可达±MAX)
	// 随误差符号通断，形成正转没有、反转独有的 bang-bang 震荡。
	const float motion_sign = (target_vel * dir_sign >= 0.0f) ? 1.0f : -1.0f;
	const bool active = (fabsf(target_vel) > stop_speed) &&
					(fabsf(actual_vel) > stop_speed) &&
					((target_vel * actual_vel) > 0.0f) &&
					((velocity_error * dir_sign * motion_sign) > -brake_err);

	float angle = normalizeAngle360(mech_angle);
	float index_f = angle / 360.0f * (float)table_size;
	u16 idx0 = (u16)index_f;
	if (idx0 >= table_size) idx0 = table_size - 1U;
	u16 idx1 = (idx0 + 1U) % table_size;
	float frac = index_f - (float)idx0;

	float ff = table[idx0] * (1.0f - frac) + table[idx1] * frac;
	if (ff > max_ff) ff = max_ff;
	else if (ff < -max_ff) ff = -max_ff;

	// 学习仅在误差带内进行：启停/换向瞬态误差可达上百 °/s，直接学习会把
	// ±MAX 钳位值写进途经的角度桶，下一轮运动经过该角度时突加满幅前馈
	// 电压，本身就是一次转矩扰动。
	if (learn_enable && active && fabsf(target_vel) > min_target &&
		fabsf(velocity_error * dir_sign) < learn_err_max) {
		float ff_target = velocity_error * dir_sign * gain;
		if (ff_target > max_ff) ff_target = max_ff;
		else if (ff_target < -max_ff) ff_target = -max_ff;
		table[idx0] += learn_alpha * (ff_target - table[idx0]);
	}

	return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
}
/**
 * @brief mode3 低速缩放因子：静止附近降到 min_scale，速度升高后线性恢复到 1。
 * @note P/I 使用各自的地板（I 地板更低）：低速段保持"比例主导"，
 *       积分充电足够慢，压制静摩擦 hunting（停下瞬间小幅来回抖动）。
 *       D 不衰减，保持阻尼。
 */
float FOC_Controller::get_mode3_velocity_scale(float target_vel,
											   float actual_vel,
											   float min_scale) {
	if (!MODE3_LOW_SPEED_P_SCALE_ENABLE) {
		return 1.0f;
	}

	float abs_target = fabsf(target_vel);
	float abs_speed = fabsf(actual_vel);
	float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
	if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
		return min_scale;
	}
	if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
		return min_scale +
			   (1.0f - min_scale) * (speed_ref - MODE3_LOW_SPEED_P_LOW) /
				   (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
	}
	return 1.0f;
}

/**
 * @brief mode3 速度内环 P：低速缩放因子（方向无关）。
 * @note 历史上曾有"反转 P 提升到 ×2"的增益调度（先是 −120/−30 硬台阶，
 *       后改为连续线性过渡）：连续化后反转仍剧烈震荡，说明问题不在
 *       台阶突跳，而在增益本身——×2 只对反转生效，把反转环路增益整体
 *       翻倍而正转不变，正是"正转正常、反转震荡"的直接来源之一。
 *       已移除。反转阻力差异由 Uq 前馈学习承担；
 *       若实测反转欠速/偏慢，优先增大前馈学习限幅，不要恢复非对称 P。
 */
float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
	return vel_angloop_p * get_mode3_velocity_scale(target_vel, actual_vel,
													MODE3_LOW_SPEED_P_MIN_SCALE);
}

/**
 * @brief 电机控制类初始化函数
 *
 * @return u8 0表示初始化成功，1表示初始化失败
 */
u8 FOC_Controller::init() {
	motor_init(); // 电机驱动初始化

	// limit_voltage 从 Flash/CAN 恢复，可能为 0：分母下限保护，防止 P=inf
	const float denom = (limit_voltage > MIN_LIMIT_VOLTAGE)
							? limit_voltage
							: voltage_power_supply;
	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / denom;
	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / denom;
	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / denom;
	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / denom;
	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / denom;
	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / denom;

	// 运行期不每周期 set_P 的环路（mode1/mode5）在此同步一次有效增益，
	// 避免沿用构造期的临时值
	angle_loop.set_P(ang_loop_p);
	current_loop.set_P(cur_loop_p);
	return 0;
}

/**
 * @brief 电机控制类构造函数
 *
 */
FOC_Controller::FOC_Controller() { ; }

/**
 * @brief 每控制周期调用一次：计算当前电角度的 sin/cos。
 * @note cal_Iq_Id 的 Park 变换与 setSVPWM_PhaseVoltage 的逆 Park 变换
 *       使用同一电角度，三角函数每周期只算这一次，两处共用。
 */
inline void FOC_Controller::update_el_sincos(float angle_el) {
	arm_sin_cos_f32(angle_el, &el_sin, &el_cos);
}

/**
 * @brief 使用SVPWM算法设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 *
 * 优化：atan2f → X/Y/Z 符号法判扇区（~15周期 vs atan2f ~300周期）
 *       边界上扇区编号可能与 atan2f 差1，但 duty 输出完全一致（已数值验证）
 *       逆 Park 所需 sin/cos 由 update_el_sincos() 预计算，不再重复计算
 */
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
	// ① 逆Park变换（sin/cos 为本周期预计算值）
	float Ualpha = Ud * el_cos - Uq * el_sin;
	float Ubeta  = Ud * el_sin + Uq * el_cos;

	// ② 扇区判定（替代 atan2f）
	float X =  Ubeta;
	float Y =  SQRT3DV2 * Ualpha - 0.5f * Ubeta;
	float Z = -SQRT3DV2 * Ualpha - 0.5f * Ubeta;

	u8 sector_code = (X > 0.0f ? 1u : 0u)
	              + (Y > 0.0f ? 2u : 0u)
	              + (Z > 0.0f ? 4u : 0u);

	// 编码 → 扇区 查找表（由 atan2f 扇区定义反推，全角度数值验证通过）
	static const u8 sector_lut[8] = {0, 2, 6, 1, 4, 3, 5, 0};
	u8 sector = sector_lut[sector_code];

	if (sector == 0u) {
		Set_3Pwm(0.5f, 0.5f, 0.5f);
		return;
	}

	// ③ 模长与调制比（arm_sqrt_f32 替代 sqrtf，~14周期 vs ~100周期）
	float magnitude;
	arm_sqrt_f32(Ualpha * Ualpha + Ubeta * Ubeta, &magnitude);
	float inv_Udc = 1.0f / voltage_power_supply;
	float modulation_index = magnitude * inv_Udc;

	// 电压限制
	if (limit_voltage > voltage_power_supply) {
		limit_voltage = voltage_power_supply;
	}
	float limit_percent = limit_voltage / v_safe;

	// ④ T1/T2 计算（替代 atan2f + 第二次 arm_sin_cos_f32）
	//    用预计算系数表直接从 Ualpha/Ubeta 算出
	static const float svpwm_coeff[6][4] = {
		{  SQRT3DV2, -0.5f,    0.0f,    1.0f},  // sector 1
		{  SQRT3DV2,  0.5f, -SQRT3DV2,  0.5f},  // sector 2
		{  0.0f,      1.0f, -SQRT3DV2, -0.5f},  // sector 3
		{ -SQRT3DV2,  0.5f,    0.0f,   -1.0f},  // sector 4
		{ -SQRT3DV2, -0.5f,  SQRT3DV2, -0.5f},  // sector 5
		{  0.0f,     -1.0f,  SQRT3DV2,  0.5f},  // sector 6
	};

	float T1 = (svpwm_coeff[sector-1][0] * Ualpha + svpwm_coeff[sector-1][1] * Ubeta) * inv_Udc;
	float T2 = (svpwm_coeff[sector-1][2] * Ualpha + svpwm_coeff[sector-1][3] * Ubeta) * inv_Udc;

	// 过调制限制：等比缩放 T1/T2
	if (modulation_index > limit_percent) {
		float scale = limit_percent / modulation_index;
		T1 *= scale;
		T2 *= scale;
	}
	float T0 = 1.0f - T1 - T2;

	// ⑤ 占空比计算（不变）
	switch (sector) {
	case 1:
		duty_a = 0.5f * T0 + T1 + T2;
		duty_b = 0.5f * T0 + T2;
		duty_c = 0.5f * T0;
		break;
	case 2:
		duty_a = 0.5f * T0 + T1;
		duty_b = 0.5f * T0 + T1 + T2;
		duty_c = 0.5f * T0;
		break;
	case 3:
		duty_a = 0.5f * T0;
		duty_b = 0.5f * T0 + T1 + T2;
		duty_c = 0.5f * T0 + T2;
		break;
	case 4:
		duty_a = 0.5f * T0;
		duty_b = 0.5f * T0 + T1;
		duty_c = 0.5f * T0 + T1 + T2;
		break;
	case 5:
		duty_a = 0.5f * T0 + T2;
		duty_b = 0.5f * T0;
		duty_c = 0.5f * T0 + T1 + T2;
		break;
	case 6:
		duty_a = 0.5f * T0 + T1 + T2;
		duty_b = 0.5f * T0;
		duty_c = 0.5f * T0 + T1;
		break;
	default:
		duty_a = 0.5f;
		duty_b = 0.5f;
		duty_c = 0.5f;
		break;
	}
	Set_3Pwm(duty_a, duty_b, duty_c);
}

/**
 * @brief 电机锁止
 */
float FOC_Controller::drv_hold() {
	float hold_pwm_duty = HOLD_PWM_DUTY;
	if (HOLD_PWM_DUTY > 0.95f) {
		hold_pwm_duty = 0.95f;
	} else if (HOLD_PWM_DUTY < 0.0f) {
		hold_pwm_duty = 0.0f;
	}
	Set_3Pwm(hold_pwm_duty, 0, 0);
	return hold_pwm_duty;
}


/**
 * @brief PWM模式（调试）
 */
float FOC_Controller::PWM_out() {
	float hold_pwm_duty = target_velocity/100;
	Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
	return hold_pwm_duty;
}


/**
 * @brief 更新电源电压
 * @note  读数不在合理区间（含采样链给的 ERROR_ADC_VAL 哨兵值）时保留上次值，
 *        不更新。这里不再置"电压故障"标志：欠压判定与停机都在
 *        fast_voltage_check() → run_inhibit 那条路上做。
 */
void FOC_Controller::update_power(float voltage) {
	if (voltage < 36.0f and voltage > 8.0f) {
		voltage_power_supply = voltage;
		v_half = voltage * 0.5f;
		v_safe = voltage * 1.1f;
	}
}

/**
 * @brief 快速电压检测（在电机控制循环中每100us调用一次）
 *
 * 直接从DMA缓冲区读取最新ADC值，检测电压跌幅
 * 响应时间 < 200us
 *
 * 算法：
 * 1. 使用峰值检测算法跟踪当前额定电压（vbus_reference）
 *    - 电压上升时，基准电压跟随
 *    - 电压下跌时，基准电压保持（不跟随下跌）
 * 2. 计算电压跌幅 = 基准电压 - 当前电压
 * 3. 关机条件（满足任一即可）：
 *    - 电压跌幅超过 VBUS_DROP_THRESHOLD
 *    - 电压低于绝对下限 VBUS_ABSOLUTE_MIN
 *
 * 参数定义在 config.h 中:
 * - VBUS_DROP_THRESHOLD: 电压跌幅阈值 (默认5V)
 * - VBUS_ABSOLUTE_MIN: 绝对最低电压 (默认8V)
 * - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次)
 */
/**
 * @brief fast_voltage_check 函数
 * @return void
 */
void FOC_Controller::fast_voltage_check(void) {
	// ADC1 连续模式零等待读最新 VBUS（ch6，11:1 分压）。
	// 旧实现 adc1_read_raw 阻塞式"触发+等EOC"：10kHz 调用每次 ~1.5μs 死等。
	// 通道顺序问题的历史注释已随 ADC1 连续模式改造失效（本函数只读 DR）。
	float current_vbus =
		adc1_read_vbus_latest() * (3.3f / 4096.0f) * 11.0f;

	// 更新基准电压（峰值检测）
	// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
	if (current_vbus > vbus_reference) {
		vbus_reference = current_vbus;
	}
	// 电压下跌时不更新基准电压，保持之前的峰值

	// 更新上次电压值
	last_vbus_fast = current_vbus;

	// 计算电压跌幅（相对于基准电压）
	float voltage_drop = vbus_reference - current_vbus;

	// 关机条件（满足任一即可）：
	// 1. 电压跌幅超过阈值
	// 2. 电压低于绝对下限
	bool drop_exceeded = (voltage_drop > VBUS_DROP_THRESHOLD);
	bool below_absolute = (current_vbus < VBUS_ABSOLUTE_MIN);

	// 只看本来源是否已经申请过：不查别的故障源，避免欠压判定被过热/nFAULT 挡住
	if ((drop_exceeded || below_absolute) &&
		(run_inhibit.sources() & INHIBIT_UNDERVOLTAGE) == 0U) {
		// 电压跌落计数（防抖）
		vbus_drop_count++;

		if (vbus_drop_count >= VBUS_DEBOUNCE_COUNT) {
			// 确认需要关机：只申请禁止位，停机动作由 run_inhibit 在控制任务里执行
			run_inhibit.request(INHIBIT_UNDERVOLTAGE);
			emergency_shutdown = true; // 标记紧急关机，低频任务会执行保存
			vbus_drop_count = 0;	 // 重置计数器
		}
	} else {
		// 电压正常，重置计数器
		vbus_drop_count = 0;
	}
}

/**
 * @brief 速度开环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controller::velocity_open_loop() {
	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
	shaft_angle =
		normalizeAngle360(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY);
	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));

	// 因为在开环控制中，Ud 始终为 0，只控制 Uq
	// 幅度限幅必须保留符号：原 _constrain 下限为 +2V，负速度会被钳到 +2V，
	// 磁场角度反向而 Uq 仍为正，导致反转时电机原地抖动。
	float Uq = target_velocity * REDUCTION_RATIO * 0.005f;
	const float UQ_MIN_MAG = 2.0f;
	const float UQ_MAX_MAG = voltage_power_supply * 0.5f;
	// 零速不输出，避免 2V 最小幅值导致静止时缓慢爬行
	if (Uq != 0.0f) {
		float Uq_mag = fabsf(Uq);
		if (Uq_mag < UQ_MIN_MAG) {
			Uq_mag = UQ_MIN_MAG;
		} else if (Uq_mag > UQ_MAX_MAG) {
			Uq_mag = UQ_MAX_MAG;
		}
		Uq = (Uq < 0.0f) ? -Uq_mag : Uq_mag;
	}
	setSVPWM_PhaseVoltage(Uq, 0.0f);
	return limit_voltage;
}

/**
 * @brief 角度闭环控制
 *
 * @param target_angle: 目标角度
 * @return float: 当前角度
 */
float FOC_Controller::angle_control() {
	// PID 控制计算（D 项对测量微分+低通，目标阶跃无微分踢）
	angle_loop.set_P(ang_loop_p);
	float Uq = angle_loop(target_angle - rt_angle, rt_angle);

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);
	return shaft_angle;
}

/**
 * @brief 速度闭环控制
 *
 * @param target_velocity: 目标速度
 * @return float: 当前速度
 */
float FOC_Controller::velocity_control() {
	// 获取电机轴角度：mode2 Uq 前馈补偿需要用物理 360° 角度查表。
	shaft_angle = KTH7823.getcurrent_sensorangle();

	// mode2 速度环：真实速度波动保留在反馈里，PID 继续看真实 rt_velocity。
	float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;

	// mode2 使用独立速度环参数。
	vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
	vel_loop.set_limit(limit_voltage);

	// 仅"实质性换向"时清积分：目标速度变号且新幅值够大（真实反向指令）才清。
	// 最终贴合阶段误差过零引起的微小变号（|target|=150×0.01°≈1.5°/s）不清——
	// 否则积分被反复清空，低速段缩小的 P 项（0.12V@0.01°）推不过静摩擦，
	// 永远停在差 ~0.01° 的位置
	static float last_mode2_target_vel = 0.0f;
	if (last_mode2_target_vel * target_velocity < 0.0f &&
		fabsf(target_velocity) > 5.0f) {
		vel_loop.reset_integral();
	}
	last_mode2_target_vel = target_velocity;

	float Uq_pid =
		vel_loop(velocity_error, rt_velocity * DIR * REDUCTION_RATIO);

	static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f};
	float Uq_ff = update_angle_sync_uq_feedforward(
		2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle,
		target_velocity, rt_velocity, velocity_error);

	// PID 输出已内部钳位；前馈叠加后超限时泄放积分，
	// 让 PID 感知真实饱和（否则重载下积分继续累积→释载超调振荡）
	float Uq = Uq_pid + Uq_ff;
	if (Uq > limit_voltage) {
		vel_loop.bleed_integral(Uq - limit_voltage);
		Uq = limit_voltage;
	} else if (Uq < -limit_voltage) {
		vel_loop.bleed_integral(Uq + limit_voltage);
		Uq = -limit_voltage;
	}
	CAN0_transmiter.testfloat_union.f = Uq;

	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));

	return rt_velocity;
}

/**
 * @brief current_control 函数
 * @return float 返回值
 */
float FOC_Controller::current_control() {
	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	CAN0_transmiter.testfloat_union.f = shaft_angle;
	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);
	// PID 控制计算
	float Iq = rt_current =
		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
				  sampler.get_current('W'));
	current_loop.set_P(cur_loop_p);
	float Uq = current_loop(target_current - Iq, Iq);
	if (Uq > limit_voltage) {
		Uq = limit_voltage;
	} else if (Uq < -limit_voltage) {
		Uq = -limit_voltage;
	}
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);
	return 1;
}

/**
 * @brief 带速度限制的角度闭环控制
 *
 * @param target_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
float FOC_Controller::angle_control_with_velocity_limit() {
	// 计算角度误差
	float angle_error = target_angle - rt_angle;

	ang_inVelAngloop.set_P(ang_angloop_p);
	// 使用角度PID控制器计算目标速度（D 项对测量微分+低通）
	float target_velocity = ang_inVelAngloop(angle_error, rt_angle);

	// 使用速度PID控制器计算最终输出电压
	// 低速段 P/I 各自按地板衰减（D 保持满值）：I 地板更低（0.15），
	// 停止瞬间积分充电速率仅为全速的 15%，进一步压制静摩擦 hunting
	float i_scale = get_mode3_velocity_scale(target_velocity, rt_velocity,
											 MODE3_LOW_SPEED_I_MIN_SCALE);
	vel_inVelAngloop.set_PID(get_mode3_velocity_p(target_velocity, rt_velocity),
							 vel_angloop_Ki * i_scale, vel_angloop_Kd);
	float velocity_error = target_velocity - rt_velocity;

	// 仅"实质性换向"时清积分（变号且新幅值>5°/s）。制动段不清（防欠程）；
	// 最终贴合阶段误差过零的微小变号也不清（防积分反复被清、停在差0.01°）
	static float last_vang_target_vel = 0.0f;
	if (last_vang_target_vel * target_velocity < 0.0f &&
		fabsf(target_velocity) > 5.0f) {
		vel_inVelAngloop.reset_integral();
	}
	last_vang_target_vel = target_velocity;

	float Uq_pid = vel_inVelAngloop(velocity_error, rt_velocity) * DIR;

	// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
	shaft_angle = KTH7823.getcurrent_sensorangle();
	static float mode3_uq_ff_table[MODE3_UQ_FF_TABLE_SIZE] = {0.0f};
	float Uq_ff = update_angle_sync_uq_feedforward(
		3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle,
		target_velocity, rt_velocity, velocity_error);

	// 前馈叠加后超限时泄放积分（×DIR 补偿：Uq = PID输出×DIR + 前馈）
	float Uq = Uq_pid + Uq_ff;
	if (Uq > limit_voltage) {
		vel_inVelAngloop.bleed_integral(DIR * (Uq - limit_voltage));
		Uq = limit_voltage;
	} else if (Uq < -limit_voltage) {
		vel_inVelAngloop.bleed_integral(DIR * (Uq + limit_voltage));
		Uq = -limit_voltage;
	}
	CAN0_transmiter.testfloat_union.f = shaft_angle;

	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);
	// 获取当前实际电流值
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);

	return shaft_angle;
}

/**
 * @brief MIT控制
 *
 * @param target_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
float FOC_Controller::MIT_Mode() {
	// 第一层：角度环（外环）
	float angle_error = target_angle - rt_angle;
	ang_inMITloop.set_P(ang_MIT_p);
	float target_velocity =
		ang_inMITloop(angle_error, rt_angle);

	// 第二层：速度环（中环）
	float velocity_error = (target_velocity - rt_velocity)* DIR;

	// 仅"实质性换向"时清积分（变号且新幅值>5°/s，理由同 mode3）
	static float last_mit_target_vel = 0.0f;
	if (last_mit_target_vel * target_velocity < 0.0f &&
		fabsf(target_velocity) > 5.0f) {
		vel_inMITloop.reset_integral();
	}
	last_mit_target_vel = target_velocity;

	vel_inMITloop.set_P(vel_MIT_p);
	float target_current = vel_inMITloop(velocity_error, rt_velocity * DIR);

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 电气角度（算一次，供 cal_Iq_Id / setSVPWM 共用）
	float angle_el = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
	update_el_sincos(angle_el);

	// 第三层：电流环（内环）
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));

	float current_error = target_current - rt_current;
	current_inMITloop.set_P(cur_MIT_p);
	float Uq = current_inMITloop(current_error, rt_current);

	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);

	return shaft_angle;
}