#ifndef _PMSM_FOC_Core_H__
#define _PMSM_FOC_Core_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.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 struct {
	u8 	  n_poles;
	float n_modulation;
	float n_PhaseFilterCeof;
	//float n_TrqVelLimGain;
	DQ_t  maxvDQ;
	DQ_t  minvDQ;
}FOC_Params;

typedef struct {
	float 	s_iABC[3];
	float 	s_iABCFilter[3];
	float   s_vABC[3];
	float   s_motRPM;   //from hall or encoder
	float 	s_motAngle; //from hall or encoder
	float 	s_hallAngle;//from hall or encoder
	float   s_targetRPM;
	float   s_cruiseRPM;
	float 	s_targetCurrent;
	DQ_t    s_targetIdq;
	DQ_t    s_targetVdq;
	float   s_targetTorque; //限速后的实际扭矩
	float 	s_vDC;
	EPM_Dir_t epmDirection;
	u8      n_ctlMode;
	bool    b_motEnable;
	bool    b_cruiseEna;
	bool    b_motLock;
	bool    b_eBrake;
	bool    b_epmMode;
	bool    b_MTPA_calibrate;
	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
}FOC_InP;

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

typedef struct {
	float s_motRPMMax;
	float s_PhaseCurrMax;
	float s_PhaseVolMax;
	float s_iDCMax;
	float s_vDCMax;
	float s_torqueMax;
}FOC_HwLimit;


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_preOutVdq;
	DQ_t  s_RealIdq;
	DQ_t  s_RealVdq;
	DQ_t  s_FilterIdq;
	float s_FilteriDC;
	float f_vdqRation;
	s16   test_sample;
	float test_targetIQ;
	float sin;
	float cos;
	u8    n_Error;
	u32   n_CritiCalErrMask;
}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_speed;
	PI_Controller *pi_fw;
	PI_Controller *pi_torque;
	PI_Controller *pi_lock;
	PI_Controller *pi_power;
	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;
	int           ctrl_count;
}PMSM_FOC_Ctrl;

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_Unknow_Cmd,
}FOC_ErrCode_t;

typedef enum {
	FOC_CRIT_OV_Vol_Err,
	FOC_CRIT_UN_Vol_Err,
	FOC_CRIT_DC_Curr_OV_Err,
	FOC_CRIT_Phase_Curr_OV_Err,
	FOC_CRIT_Phase_Err,
	FOC_CRIT_Encoder_Err,
	FOC_CRIT_Brake_Err,
	FOC_CRIT_CURR_OFF_Err,
	FOC_CRIT_H_MOS_Err,
	FOC_CRIT_L_MOS_Err,
	FOC_CRIT_Phase_Conn_Err,
	FOC_CRIT_Err_Max = 32,	
}FOC_CritiCal_Ebit_t;

#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_CURRENT_BRK               ((u8)4U)

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

#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);
void 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_iBusLimit(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);
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);
float PMSM_FOC_Calc_iDC(void);
void PMSM_FOC_LockMotor(bool lock);
void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
void PMSM_FOC_Set_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_SeteBrkPhaseCurrent(float curr);
float PMSM_FOC_GeteBrkPhaseCurrent(void);
void PMSM_FOC_SetCriticalError(u8 err) ;
void PMSM_FOC_ClrCriticalError(u8 err);
u32 PMSM_FOC_GetCriticalError(void);
void PMSM_FOC_PhaseCurrLim(float lim);
float PMSM_FOC_GetPhaseCurrLim(void);
float PMSM_FOC_GetiBusLimit(void);
void PMSM_FOC_GetRunningStatus(u8 *data);
bool PMSM_FOC_Is_CruiseEnabled(void);
void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb);
bool PMSM_FOC_MotorLocking(void);

#endif /* _PMSM_FOC_Core_H__ */