#ifndef _PMSM_FOC_Core_H__
#define _PMSM_FOC_Core_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.h"
#include "foc/core/e_ctrl.h"
#include "foc/core/adrc.h"

typedef struct {
	float a;
	float b;
}AB_t;

typedef struct {
	float d;
	float q;
}DQ_t;

typedef enum {
	EPM_Dir_None,
	EPM_Dir_Back,
	EPM_Dir_Forward,
}EPM_Dir_t;

typedef enum {
	Plot_None,
	Plot_Phase_curr,
	Plot_DQ_Curr,
	Plot_Phase_vol,
	Plot_Spd_flow,
	Plot_D_flow,
	Plot_Q_flow,
	Plot_D_Step,
	Plot_Q_Step,
	Plot_SMO_OBS,
	Plot_t_Max,
}Plot_t;

typedef enum {
	FOC_Success = 0,
	FOC_NotAllowed = 1,
	FOC_Have_CritiCal_Err,
	FOC_Throttle_Err, //ready的时候检测到转把信号
	FOC_NowAllowed_With_Speed,
	FOC_Speed_TooLow,
	FOC_NotCruiseMode,
	FOC_Param_Err,
	FOC_MEM_Err,
	FOC_CRC_Err,
	FOC_Unknow_Cmd,
}FOC_ErrCode_t;

typedef enum {
	FOC_CRIT_OV_Vol_Err,
	FOC_CRIT_UN_Vol_Err,
	FOC_CRIT_ACC_OV_Err,
	FOC_CRIT_ACC_Un_Err,
	FOC_CRIT_Phase_Err,
	FOC_CRIT_Encoder_Err, /* 编码器错误，可能还是可以骑行，取决无感是否稳定 */
	FOC_CRIT_Angle_Err, /* FOC 角度错误，一般发生在编码器错误，同时无感没有稳定的情况下，必须要停机 */
	FOC_CRIT_CURR_OFF_Err,
	FOC_CRIT_H_MOS_Err,
	FOC_CRIT_L_MOS_Err,
	FOC_CRIT_Phase_Conn_Err,
	FOC_CRIT_MOTOR_TEMP_Err,
	FOC_CRIT_MOS_TEMP_Err,
	FOC_CRIT_Fan_Err,
	FOC_CRIT_IDC_OV,
	FOC_CRIT_THRO_Err,
	FOC_CRIT_ENC_AB_Err,
	FOC_CRIT_Vol_HW_Err, //17
	FOC_CRIT_PHASE_UNBalance_Err, /* 三相不平衡错误，比如相线螺丝松了 */
	FOC_CRIT_Err_Max = 32,
}FOC_CritiCal_Ebit_t;

typedef enum {
	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
	FOC_EV_MOS_Limit_L,
}FOC_EVENT_R_t;

#define FOC_Cri_Err_Mask(err) (1<<(err))

typedef struct {
	u8 	  n_poles;
	float n_modulation;
	float lq;
	float ld;
	float flux;
	float n_PhaseFilterCeof;
}FOC_Params;

typedef struct {
	float 	s_iABC[3];
	float 	s_iABCFilter[3];
	float   s_iABC_DT[3]; //abc phase current for deadtime compesition
	float   s_vABC[3];
	float   s_motVelocity;   //from hall or encoder
	float 	s_motAngle; //from hall or encoder
	float   s_angleLast;
	float   s_targetRPM;
	float   s_cruiseRPM;
	e_Ramp  cruiseRpmRamp;
	float 	s_targetCurrent;
	DQ_t    s_targetIdq;
	DQ_t    s_targetVdq;
	float   s_targetTorque; //对s_trqModeOutRaw进行扭矩过零处理
	float   s_trqModeOutLim;//扭矩模式下限速限流后的扭矩请求
	float 	s_vDC;
	u8      n_ctlMode;
	bool    b_motEnable;
	bool    b_cruiseEna;
	bool    b_AutoHold;
	bool    b_eBrake;
	bool    b_epmMode;
	bool    b_fwEnable;
	float   s_motVelocityFiltered; //电机滤波后的转速
	float   s_motVelRadusPers; //电机的电角速度
	volatile bool    b_MTPA_calibrate;
	float   s_manualAngle; //mainly used when calibrate hall/mtpa.
	float   s_dqAngle; //D轴电流超前角
}FOC_InP;

typedef struct {
	float s_motRPMLim;
	float s_torqueLim;
	float s_iDCLim;
	float s_PhaseCurrLim; //最大相电流
	float s_iDCeBrkLim; //最大母线回收电流
	float s_TorqueBrkLim;
	float s_PhaseVoleBrkLim;
	float s_vDCMinLim;
	float s_vDCMaxLim;
}FOC_UserLimit;

typedef struct {
	e_Ramp rpmLimRamp;
	e_Ramp torqueLimRamp;
	e_Ramp DCCurrLimRamp;
}FOC_RTLimit;

typedef struct {
	float s_motRPMMax;
	float s_PhaseCurrMax;
	float s_PhaseVolMax;
	float s_FWDCurrMax; //D轴最大退磁电流
	float s_iDCMax;
	float s_vDCMax;
	float s_torqueMax;
}FOC_HwLimit;

typedef struct {
	float s_iDCLim;
	float s_TorqueLim;
}FOC_ProtLimit;

typedef struct {
	u16   n_Duty[3];
	u16   n_lowDuty;
	u16   n_midDuty;
	u8    n_Sector;
	u8    n_CPhases;
	u16   n_Sample1;
	u16   n_Sample2;
	u8    n_RunMode;
	DQ_t  s_OutVdq;
	DQ_t  s_OutVdqDTC;
	AB_t  s_OutVAB;
	DQ_t  s_RealIdq;
	DQ_t  s_RealVdq;
	DQ_t  s_FilterIdq;
	float s_FilteriDC;
	float s_CalciDC;
	float s_CalciDC2;
	float s_RealCurrentFiltered;
	float f_vdqRation;
	float f_autohold_trq;
	s16   test_sample;
	float sin;
	float cos;
	u8    n_Error;
	bool  empty_load; //空载运行
}FOC_OutP;

typedef struct {
	float s_FinalTgt;
	float s_Cp;
	float s_Step;
	int      n_CtrlCount;
	int      n_StepCount;
}dq_Rctrl; //dq ramp ctrl

typedef struct {
	PI_Controller pi_id;
	PI_Controller pi_iq;
	PI_Controller pi_lock;
	PI_Controller pi_power;
#ifdef CONFIG_SPEED_LADRC
	ladrc_t        vel_lim_adrc;
	ladrc_t        vel_adrc;
#else
	PI_Controller pi_vel_lim;
	PI_Controller pi_vel;
#endif
	dq_Rctrl      idq_ctl[2];
	dq_Rctrl      vdq_ctl[2];	
	FOC_InP       in;
	FOC_OutP      out;
	FOC_Params    params;
	FOC_UserLimit userLim;
	FOC_HwLimit   hwLim;
	FOC_ProtLimit protLim;
	FOC_RTLimit   rtLim;
	Plot_t        plot_type;
	int           ctrl_count;
}PMSM_FOC_Ctrl;

#define CTRL_MODE_OPEN                      ((u8)0U)
#define CTRL_MODE_SPD                       ((u8)1U)
#define CTRL_MODE_TRQ                       ((u8)2U)
#define CTRL_MODE_CURRENT                   ((u8)3U)
#define CTRL_MODE_EBRAKE               ((u8)4U)

#define FOC_CALIMOD_HALL               ((u8) 1U)
#define FOC_CALIMOD_MTPA               ((u8) 2U)

#define FOC_LIM_NO_CHANGE  0
#define FOC_LIM_CHANGE_H   1
#define FOC_LIM_CHANGE_L   2

#if 1
#define SECTOR_1  0u
#define SECTOR_2  1u
#define SECTOR_3  2u
#define SECTOR_4  3u
#define SECTOR_5  4u
#define SECTOR_6  5u
#define SECTOR_UKNOW 0xFF
#else
#define SECTOR_1  3u
#define SECTOR_2  4u
#define SECTOR_3  5u
#define SECTOR_4  0u
#define SECTOR_5  1u
#define SECTOR_6  2u
#endif

typedef enum {
	PID_D_id,
	PID_Q_id,
	PID_Spd_id,
	PID_TRQ_id,
	PID_Pow_id,
	PID_Lock_id,
	PID_FW_id,
	PID_Max_id
}PID_id_t;

PMSM_FOC_Ctrl *PMSM_FOC_Get(void);
void PMSM_FOC_CoreInit(void);
bool PMSM_FOC_Schedule(void);
u8 PMSM_FOC_CtrlMode(void);
void PMSM_FOC_idqCalc(void);
void PMSM_FOC_Start(u8 nCtrlMode);
void PMSM_FOC_Stop(void);
void PMSM_FOC_DCCurrLimit(float ibusLimit);
void PMSM_FOC_SpeedLimit(float speedLimit);
float PMSM_FOC_GetSpeedLimit(void);
float PMSM_FOC_GetVbusVoltage(void);
float PMSM_FOC_GetVbusCurrent(void);
DQ_t  *PMSM_FOC_GetDQCurrent(void);
bool PMSM_FOC_SetCtrlMode(u8 mode);
u8 PMSM_FOC_GetCtrlMode(void);
void PMSM_FOC_SetOpenVdq(float vd, float vq);
void PMSM_FOC_SetOpenVdq_Immediate(float vd, float vq);
bool PMSM_FOC_EnableCruise(bool enable);
bool PMSM_FOC_Set_Speed(float rpm);
bool PMSM_FOC_Set_Torque(float trque);
bool PMSM_FOC_Set_Current(float current);
bool PMSM_FOC_Set_CruiseSpeed(float rpm);
float PMSM_FOC_GetSpeed(void);
bool PMSM_FOC_Lock_Motor(bool lock);
void PMSM_FOC_Brake(bool brake);
void PMSM_FOC_Calc_Current(void);
void PMSM_FOC_AutoHold(bool lock);
void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
void PMSM_FOC_Set_MotAngle(float angle);
void PMSM_FOC_Set_Dq_Angle(float angle);
bool PMSM_FOC_Is_Start(void);
void PMSM_FOC_SetErrCode(u8 error);
u8 PMSM_FOC_GetErrCode(void);
void PMSM_FOC_MTPA_Calibrate(bool enable);
void PMSM_FOC_Get_TgtIDQ(DQ_t * dq);
void PMSM_FOC_TorqueLimit(float torqueLimit);
float PMSM_FOC_GetTorqueLimit(void);
bool PMSM_FOC_Set_epmMode(bool epm);
bool PMSM_FOC_is_epmMode(void);
bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir);
EPM_Dir_t PMSM_FOC_Get_epmDir(void);
void PMSM_FOC_SetEbrkTorque(s16 torque);
float PMSM_FOC_GetEbrkTorque(void);
void PMSM_FOC_PhaseCurrLim(float lim);
float PMSM_FOC_GetPhaseCurrLim(void);
float PMSM_FOC_GetDCCurrLimit(void);
void PMSM_FOC_GetRunningStatus(u8 *data);
bool PMSM_FOC_Is_CruiseEnabled(void);
void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd);
bool PMSM_FOC_AutoHoldding(void);
void PMSM_FOC_Slow_Task(void);
void PMSM_FOC_Set_PlotType(Plot_t t);
u8   PMSM_FOC_RunTime_Limit(void);
void PMSM_FOC_RT_PhaseCurrLim(float lim);
void PMSM_FOC_RT_LimInit(void);
float PMSM_FOC_Get_Real_dqVector(void);
void PMSM_FOC_Reset_Torque(void);
void PMSM_FOC_SpeedDirectLimit(float limit);
bool PMSM_FOC_iDC_is_Limited(void);
bool PMSM_FOC_Torque_is_Limited(void);
bool PMSM_FOC_PauseCruise(void);
bool PMSM_FOC_ResumeCruise(void);
void PMSM_FOC_Change_VelLoop_Params(float wcv, float b0);
void PMSM_FOC_Change_TrqLoop_Params(float wcv, float b0);
void PMSM_FOC_ABC2Dq(float a, float b, float c, float *d, float *q);

#endif /* _PMSM_FOC_Core_H__ */