#ifndef _PMSM_FOC_Core_H__
#define _PMSM_FOC_Core_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.h"
typedef struct {
	s16q5_t a;
	s16q5_t b;
}AB_t;

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

typedef struct {
	u8 		n_poles;
	s16q5_t s_maxIdq;
	s16q5_t s_maxiDC;
	s32q14_t  s_maxRPM;
	s16q5_t s_iABC[3];
	s16     s_motRPM;   //from hall or encoder
	s16q5_t s_motAngle; //from hall or encoder
	s16     s_targetRPM;
	s16q5_t s_targetTrq;
	DQ_t    s_targetIdq;
	DQ_t    s_targetVdq;
	s16q5_t s_vDC;
	s16q14_t n_modulation;
	u8      n_ctlMode;
	bool    b_motEnable;
	bool    b_cruiseEna;
	bool    b_motLock;
	bool    b_eBrake;
	s16q5_t s_manualAngle; //mainly used when calibrate hall
}FOC_InP;

typedef struct {
	u16   n_Duty[3];
	u16   n_lowDuty;
	u16   n_midDuty;
	u8    n_Sector;
	u16   n_Sample1;
	u16   n_Sample2;
	u8    n_RunMode;
	DQ_t  s_OutVdq;
	DQ_t  s_RealIdq;
	s16q14_t f_vdqRation;
	u8    n_Error;
}FOC_OutP;

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

typedef struct {
	PI_Controller *id_ctl;
	PI_Controller *iq_ctl;
	PI_Controller *spd_ctl;
	PI_Controller *fw_ctl;
	FOC_InP       in;
	FOC_OutP      out;
	dq_Rctrl      idq_ctl[2];
	dq_Rctrl      vdq_ctl[2];
}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 OPEN_MODE                      ((u8)0U)
#define SPD_MODE                       ((u8)1U)
#define TRQ_MODE                       ((u8)2U)

#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(s16q5_t ibusLimit);
void PMSM_FOC_SpeedLimit(s32q4_t speedLimit);
s32q4_t PMSM_FOC_GetSpeedLimit(void);
void PMSM_FOC_VbusVoltage(s16q5_t vbusVol);
void PMSM_FOC_SetCtrlMode(u8 mode);
void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
bool PMSM_FOC_EnableCruise(bool enable);
bool PMSM_FOC_Set_Speed(s32q4_t rpm);
bool PMSM_FOC_Set_Trque(float current);
bool PMSM_FOC_Set_CruiseSpeed(float rpm);
s32q4_t PMSM_FOC_GetSpeed(void);
bool PMSM_FOC_Lock_Motor(bool lock);
void PMSM_FOC_Brake(bool brake);
s16q5_t 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(s16 angle);
bool PMSM_FOC_Is_Start(void);
void PMSM_FOC_SetErrCode(u8 error);
u8 PMSM_FOC_GetErrCode(void);

#endif /* _PMSM_FOC_Core_H__ */