// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
#include "pid.h"
#include <stdint.h>
class FOC_Controlroller {
  public:
	FOC_Controlroller();

	u8 control_mode = 0;
	float limit_voltage = 0.0f;
	float limit_velocity = 0.0f;
	float target_angle = 0.0f;
	float target_velocity = 0.0f;
	float rt_angle = 0.0f;
	float rt_velocity = 0.0f;
	bool thermal_uncontrol = false;
	bool voltage_error = false;
	bool is_shut_down = false;
	float voltage_power_supply = 24.0f; // 电源电压

	float velocity_open_loop();
	float angle_control(float target_angle);
	float velocity_control(float target_velocity);
	float angle_control_NPID(float target_angle);

	void update_power(void);
	void DFOC_alignSensor(int _PP);
	void shut_down(void);
	void resume(void);
	void drv_stop(void);
	void drv_hold(float duty = 0.5f);

  protected:
	std::u64 last_time_us = 0;
	std::u64 now_us = 0;
	float shaft_angle = 0;
	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
	float zero_electric_angle = 0;
	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
	float v_safe = voltage_power_supply * 0.95f;

	// PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000,
	// voltage_power_supply};//8V
	// PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000,
	// 										 voltage_power_supply}; // 8V
	PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
										   voltage_power_supply}; // 12V
	PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
											 voltage_power_supply}; // 12V

	void setPhaseVoltage(float Uq, float Ud);
};

float _normalizeAngle(float angle);
void DFOC_Vbus(float power_supply);
inline float _electricalAngle();
float DFOC_M0_Angle(void);

#endif