// 函数声明
#ifndef _FOC_H
#define _FOC_H
#include "config.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 target_current = 0.0f;
	float rt_angle = 0.0f;
	float rt_velocity = 0.0f;
	bool over_current = false;
	bool voltage_error = false;
	bool is_shut_down = false;
	float voltage_power_supply = 24.0f; // 电源电压
	float v_safe = voltage_power_supply * 1.05f;
	float duty_a = 0, duty_b = 0, duty_c = 0;
	float Ua = 0, Ub = 0, Uc = 0;
	u8 init(void);
	float velocity_open_loop(void);
	float angle_control(void);
	float angle_control_with_velocity_limit(void);
	float velocity_control(void);
	float current_control(void);

	void update_power(void);
	void DFOC_alignSensor(int _PP);
	void shut_down(void);
	void resume(void);
	void drv_stop(void);
	float drv_hold(void);
	PIDController vel_loop = PIDController{
		vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
	PIDController angle_loop =
		PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000,
					  voltage_power_supply}; // 12V
	PIDController current_loop = PIDController{
		current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
	PIDController vel_inVelAngloop = PIDController{
		vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
	PIDController ang_inVelAngloop =
		PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999};

  protected:
	float shaft_angle = 0;
	float Ualpha, Ubeta = 0;
	float zero_electric_angle = 0;
	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
	void setPhaseVoltage(float Uq, float Ud);
	float cal_Iq_Id(float current_a, float current_b, float current_c);
};

// 定义函数指针类型
typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);

inline float _normalizeAngle(float angle);
inline float _electricalAngle();

#endif