#include "DengFOC.h"
#include "Timer.h"
#include "systick.h"
#include <cmath>
#include <cstdio>
#include <stdint.h>

constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11, DIR = 1;
// 初始变量及函数定义
#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))

float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }

/**
 * @brief 归一化角度
 *
 * @param angle
 * @return float
 */
float _normalizeAngle(float angle) {
	float a = fmod(angle, 2 * PI);
	// 取余运算可以用于归一化，列出特殊值例子算便知
	return a >= 0 ? a : (a + 2 * PI);
}
/**
 * @brief 电机控制类构造函数
 *
 */
FOC_Controlroller::FOC_Controlroller() { ; }
/**
 * @brief 获取电机 shaft_angle
 *
 * @return void
 */
void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {

	// 计算占空比
	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
	Set_3Pwm(dc_a, dc_b, dc_c);
	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}

/**
 * @brief 设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 * @param angle_el: 电机轴角度
 */
void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
	Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2);
	angle_el = _normalizeAngle(angle_el + zero_electric_angle);
	// 帕克逆变换
	Ualpha = -Uq * sin(angle_el);
	Ubeta = Uq * cos(angle_el);

	// 克拉克逆变换
	Ua = Ualpha + voltage_power_supply / 2;
	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
	driver_output(Ua, Ub, Uc);
}

/**
 * @brief 速度开环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controlroller::velocity_open_loop(float target_velocity) {
	now_us = get_system_runtime_us();
	float dt = (now_us - last_time_us) * 1e-6f;
	// 限制时间间隔
	if (dt <= 0.0f || dt > 0.5f)
		dt = 1e-3f;
	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * dt);

	float uq = limit_voltage; // 设定一个固定的q轴电压
	setPhaseVoltage(uq, 0, _electricalAngle(shaft_angle));
	last_time_us = now_us;
	return uq;
}

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