#include "DengFOC.h"
#include "Can.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "initial.h"
#include "pid.h"
#include <cmath>
#include <gd32c10x.h>
#include <stdint.h>
#include "motor.h"

constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f;	   // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));

FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;

// 初始变量及函数定义
#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, 2 * PI);
	// 取余运算可以用于归一化，列出特殊值例子算便知
	return a >= 0 ? a : (a + 2 * PI);
}

// 通过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);

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

	// Park变换
	float sin_el, cos_el;
	cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
	sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
	float I_q = I_beta * cos_el - I_alpha * sin_el;

	return I_q*1000.0f; // 返回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;
	sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
	cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
	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);
	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_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 电机锁止
 */
void FOC_Controlroller::drv_hold(float duty) {
	if (duty > 0.95f) {
		duty = 0.95f;
	} else if (duty < 0.0f) {
		duty = 0.0f;
	}
	Set_3Pwm(duty, duty, 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中，在此之前，还需要对轴角度归一化
	// 预计算常量 1e-4 = 100微秒控制周期
	shaft_angle =
		_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);

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

	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 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(float target_angle) {
	// PID 控制计算
	float Uq = angle_loop(target_angle - rt_angle);

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

/**
 * @brief 角度闭环控制
 *
 * @param target_angle: 目标角度
 * @return float: 当前角度
 */
float FOC_Controlroller::angle_control_NPID(float target_angle) {
	static float Kp = 20.0f;
	// 控制力矩计算
	float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
						  voltage_power_supply);

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

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

	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
	// 设置相电压
	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(float target_current) {
	// 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() * DEG2RAD;
	// 设置相电压
	setPhaseVoltage(Uq, 0.0f);
	return 1;
}