#include "FOC.h"
#include "Can.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>
#include "Filter.h"
#include <stdint.h>

FOC_Controlroller 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))

/**@
 * @brief 归一化角度
 *
 * @param angle
 * @return float
 */
inline float _normalizeAngle(float angle) {
	float a = fmod(angle, 360.0f);
	// 取余运算可以用于归一化，列出特殊值例子算便知
	return a >= 0 ? a : (a + 360.0f);
}

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

	float I_alpha, I_beta;
	// Clarke变换 (使用三相电流)
	// arm_clarke_f32(current_a, current_b, &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变换
	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;
	// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);

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

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

/**
 * @brief 设置电机输出，使用优化的SVPWM算法
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 * @param angle_el: 电机轴角度
 */
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
	// 计算电气角度
	float angle_el = _normalizeAngle(shaft_angle * PP);

	// 帕克逆变换 (直接计算，避免函数调用开销)
	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;

	// 计算占空比并限制范围
	duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
	duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
	duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
	// 设置PWM输出
	float voltage_exchange = limit_voltage / v_safe;
	duty_a = duty_a * voltage_exchange;
	duty_b = duty_b * voltage_exchange;
	duty_c = duty_c * voltage_exchange;
	Set_3Pwm(duty_a, duty_b, duty_c);
}

/**
 * @brief 电机关机
 *
 */
void FOC_Controlroller::shut_down() {
	is_shut_down = 1;
	motor_sleep();
}
/**
 * @brief 占空比置0
 *
 */
void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
 * @brief 电机锁止
 */
float FOC_Controlroller::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_Controlroller::resume() {
	over_current = false;
	voltage_error = false;
	is_shut_down = false;
	motor_resume();
}
/**
 * @brief 更新电源电压
 *
 */
void FOC_Controlroller::update_power(void) {
	voltage_power_supply = sampler.get_vbus_voltage();
	if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) {
		voltage_error = false;
		v_half = voltage_power_supply * 0.5f;
		v_safe = voltage_power_supply * 0.95f;
	} else {
		voltage_error = true;
	}
}

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

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

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

	return limit_voltage;
}

/**
 * @brief 电机初始化
 *
 * @param _PP: 电机极队数
 */
// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
// 	u8 PP = _PP;
// 	setPhaseVoltage(3, 0, _3PI_2);
// 	delay_1ms(3000);
// 	zero_electric_angle = _electricalAngle();
// 	setPhaseVoltage(0, 0, _3PI_2);
// 	std::printf("0电角度:%f", zero_electric_angle);
// }

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

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 设置相电压
	setPhaseVoltage(Uq, 0.0f);
	return shaft_angle;
}

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

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

	// 对目标速度进行限幅
	target_velocity =
		_constrain(target_velocity, -limit_velocity, limit_velocity);

	// 使用速度PID控制器计算最终输出电压
	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
	CAN0_transmiter.testfloat_union.f = shaft_angle;

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

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

	return shaft_angle;
}

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

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	// 设置相电压
	setPhaseVoltage(Uq, 0.0f);
	CAN0_transmiter.testfloat_union.f =
		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
				  sampler.get_current('W'));

	return rt_velocity;
}

float FOC_Controlroller::current_control() {
	// PID 控制计算
	float Iq = 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();
	// 设置相电压
	setPhaseVoltage(Uq, 0.0f);
	return 1;
}