#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 struct {
	u8 	  n_poles;
	float s_maxIdq;
	float s_minIdq;
	float s_maxiDC;
	float s_maxvDC;
	DQ_t  maxvDQ;
	DQ_t  minvDQ;
	float s_maxRPM;
	float n_modulation;
	float n_PhaseFilterCeof;
}FOC_Params;

typedef struct {
	float 	s_iABC[3];
	float 	s_iABCFilter[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_targetCurrent;
	DQ_t    s_targetIdq;
	DQ_t    s_targetVdq;
	float   s_targetTorque; //限速后的实际扭矩
	float 	s_vDC;
	u8      n_ctlMode;
	bool    b_motEnable;
	bool    b_cruiseEna;
	bool    b_motLock;
	bool    b_eBrake;
	bool    b_MTPA_calibrate;
	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
}FOC_InP;

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_RealIdq;
	float f_vdqRation;
	u8    n_Error;
}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_ctl_id;
	PI_Controller *pi_ctl_iq;
	PI_Controller *pi_ctl_spd;
	PI_Controller *pi_ctl_fw;
	PI_Controller *pi_ctl_trq;
	dq_Rctrl      idq_ctl[2];
	dq_Rctrl      vdq_ctl[2];	
	FOC_InP       in;
	FOC_OutP      out;
	FOC_Params    params;
	int           ctrl_count;
}PMSM_FOC_Ctrl;

typedef enum {
	FOC_Success = 0,
	FOC_NotAllowed = 1,
	FOC_Throttle_Err, //ready的时候检测到转把信号
	FOC_NowAllowed_With_Speed,
	FOC_Speed_TooLow,
	FOC_NotCruiseMode,
	FOC_Param_Err,
	FOC_Phase_Err,
	FOC_Hall_Err,
	FOC_Brake_Err = 100,
	FOC_Unknow_Cmd = 0xFF,
}foc_fault_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 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


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);
s32q4_t PMSM_FOC_GetSpeedLimit(void);
void PMSM_FOC_VbusVoltage(float vbusVol);
void PMSM_FOC_SetCtrlMode(u8 mode);
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_Get_iDC(void);
void PMSM_FOC_LockMotor(bool lock);
void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min);
void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min);
void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min);
void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min);
void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min);
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);
#endif /* _PMSM_FOC_Core_H__ */