#include "FOC.h"
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "config.h"
#include "motor.h"
#include "pid.h"
#include <arm_math.h>
#include <cmath>
#include <gd32c10x.h>

FOC_Controller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波

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

// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
									   float current_c) {
	// 计算电气角度
	float angle_el = fmod(shaft_angle * PP, 360.0f);

	float I_alpha, I_beta;
	// Clarke变换 (使用三相电流)
	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变换
	float sin_el, cos_el;
	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
	float I_q, I_d;
	I_q = I_beta * cos_el - I_alpha * sin_el;

	return I_q * 1000.0f -
		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
}

/**
 * @brief 电机控制类初始化函数
 *
 * @return u8 0表示初始化成功，1表示初始化失败
 */
u8 FOC_Controller::init() {
	motor_init(); // 电机驱动初始化
	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
					 vel_loop_Kd); // 设置电机速度控制参数
	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
					   0.2f); // 设置电机角度控制参数
	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
						 7e-8); // 设置电机电流控制参数

	vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
							 vel_angloop_Kd); // 设置电机速度控制参数
	ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
							 ang_angloop_Ki,
							 ang_angloop_Kd); // 设置电机角度控制参数

	CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
	return 0;
}

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

/**
 * @brief 设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 * @param angle_el: 电机轴角度
 */
inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
	// 计算电气角度
	float angle_el = fmod(shaft_angle * PP, 360.0f);

	// 帕克逆变换 (直接计算，避免函数调用开销)
	float sin_el, cos_el;
	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
	float Ualpha = Ud * cos_el - Uq * sin_el;
	float Ubeta = Ud * sin_el + Uq * cos_el;

	// 克拉克逆变换 (使用预计算常量优化)
	Ua = Ualpha + v_half;
	Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
	Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;

	if (Ua > v_safe)
		Ua = v_safe;
	else if (Ua < -v_safe)
		Ua = -v_safe;
	if (Ub > v_safe)
		Ub = v_safe;
	else if (Ub < -v_safe)
		Ub = -v_safe;
	if (Uc > v_safe)
		Uc = v_safe;
	else if (Uc < -v_safe)
		Uc = -v_safe;

	// 设置PWM输出
	float voltage_exchange = limit_voltage / v_safe;
	duty_a = Ua / voltage_power_supply * voltage_exchange;
	duty_b = Ub / voltage_power_supply * voltage_exchange;
	duty_c = Uc / voltage_power_supply * voltage_exchange;

	Set_3Pwm(duty_a, duty_b, duty_c);
}

/**
 * @brief 使用SVPWM算法设置电机输出（优化版）
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 */
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
	// 计算电气角度（保持原有角度单位和范围）
	float angle_el = fmod(shaft_angle * PP, 360.0f);

	// 帕克逆变换
	float sin_el, cos_el;
	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
	float Ualpha = Ud * cos_el - Uq * sin_el;
	float Ubeta = Ud * sin_el + Uq * cos_el;

	// 计算角度并归一化到 [0, 2π)
	float angle = atan2f(Ubeta, Ualpha);
	if (angle < 0)
		angle += 2.0f * PI;

	// 扇区与局部角度
	int sector = (int)(angle * INV_PI_3) + 1;
	if (sector < 1)
		sector = 1;
	else if (sector > 6)
		sector = 6;
	float local_angle = angle - (sector - 1) * PI_3;

	// 模长计算
	float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);

	// 计算电压调制比，同时限制最大输出电压
	float modulation_index =
		(magnitude / voltage_power_supply); // 90%作为安全裕量

	// 电压限制
	if (limit_voltage > voltage_power_supply) {
		limit_voltage = voltage_power_supply;
	}
	float limit_percent = limit_voltage / v_safe;
	if (modulation_index > limit_percent) {
		// 限制电压
		modulation_index = limit_percent;
		// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
		magnitude = voltage_power_supply * limit_percent;
	}

	// 使用arm_sin_cos_f32计算local_angle的sin和cos
	float sin_l, cos_l;
	arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
	float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
	float T2 = modulation_index * sin_l;
	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.0f;
		duty_b = 0.0f;
		duty_c = 0.0f;
		break;
	}

	// 设置PWM输出
	Set_3Pwm(duty_a, duty_b, duty_c);
}

/**
 * @brief 电机关机
 *
 */
void FOC_Controller::shut_down() {
	is_shut_down = 1;
	motor_sleep();
}
/**
 * @brief 占空比置0
 *
 */
void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
 * @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, hold_pwm_duty, hold_pwm_duty);
	return hold_pwm_duty;
}
/**
 * @brief 电机恢复
 *
 */
void FOC_Controller::resume() {
	over_current = false;
	voltage_error = false;
	is_shut_down = false;
	motor_resume();
}
/**
 * @brief 更新电源电压
 *
 */
void FOC_Controller::update_power(float voltage) {
	if (voltage < 36.0f and voltage > 8.0f) {
		voltage_error = false;
		voltage_power_supply = voltage;
		v_half = voltage * 0.5f;
		v_safe = voltage * 1.05f;
	} else {
		voltage_error = true;
	}
}

/**
 * @brief 速度开环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controller::velocity_open_loop() {
	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
										 TIMER2_FREQUENCY,
					   360.0f);

	// 因为在开环控制中，Ud始终为0，只控制Uq
	const float Uq = voltage_power_supply / 2.0f;

	setSVPWM_PhaseVoltage(Uq, 0.0f);
	// CAN0_transmiter.testfloat_union.f = Uq;
	// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
	// 		  sampler.get_current('W'));

	return limit_voltage;
}

/**
 * @brief 角度闭环控制
 *
 * @param target_angle: 目标角度
 * @return float: 当前角度
 */
float FOC_Controller::angle_control() {
	// PID 控制计算
	float Uq = angle_loop(target_angle - rt_angle);

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	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_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
float FOC_Controller::angle_control_with_velocity_limit() {
	// 计算角度误差
	float angle_error = target_angle - rt_angle;

	// 使用角度PID控制器计算目标速度
	float target_velocity = ang_inVelAngloop(angle_error);

	// 使用速度PID控制器计算最终输出电压
	// 根据目标速度大小，选择不同的PID参数
	if (target_velocity < -120.0f) {
		vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
	} else if (target_velocity < -30.0f) {
		vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
	} else {
		vel_inVelAngloop.set_P(vel_angloop_p);
	}
	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);

	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));
	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();

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

	return shaft_angle;
}

/**
 * @brief 带速度限制的角度闭环控制
 *
 * @param target_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
float FOC_Controller::MIT_Mode() {
	// 第一层：角度环（外环）
	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
	float angle_error = rt_angle - target_angle;
	float target_velocity =
		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射

	// 第二层：速度环（中环）
	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
	float velocity_error = target_velocity - rt_velocity;
	float target_current =
		vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射

	CAN0_transmiter.testfloat_union.f = target_current;
	// 第三层：电流环（内环）
	// 计算当前实际电流
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));

	// 通过电流环PID控制器计算输出电压
	float current_error = target_current - rt_current;
	float Uq = current_loop(current_error);

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();

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

	return shaft_angle;
}

/**
 * @brief 速度闭环控制
 *
 * @param target_velocity: 目标速度
 * @return float: 当前速度
 */
float FOC_Controller::velocity_control() {
	// PID 控制计算
	float Uq = vel_loop((rt_velocity - target_velocity) * 25);

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 设置相电压
	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;
}

float FOC_Controller::current_control() {
	// PID 控制计算
	float Iq = rt_current =
		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
				  sampler.get_current('W'));
	float Uq = current_loop(target_current - Iq);
	CAN0_transmiter.testfloat_union.f = Iq;
	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq, 0.0f);
	return 1;
}