#ifndef _CONTROLLER_H__
#define _CONTROLLER_H__
#include "math/fix_math.h"
#include "foc/core/PI_Controller.h"
#include "foc/core/foc.h"
#include "foc/core/etcs.h"
#include "foc/limit.h"

#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_LIM_NO_CHANGE  0
#define FOC_LIM_CHANGE_H   1
#define FOC_LIM_CHANGE_L   2

#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

typedef enum {
	EPM_Dir_None,
	EPM_Dir_Back,
	EPM_Dir_Forward,
}epm_dir_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,
}error_code_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_Lim,
	FOC_CRIT_MOS_TEMP_Lim,
	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_THRO2_Err,
	FOC_CRIT_MOT_TEMP_Sensor,
	FOC_CRIT_MOS_TEMP_Sensor,  //21
	FOC_CRIT_BRK_Err,
	FOC_CRIT_Err_Max = 32,
}mctrl_critical_ebit_t;

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

typedef enum {
	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
	FOC_EV_MOS_Limit_L,
	FOC_EV_THRO_START_V,
	FOC_START_Err_Code,
	FOC_Reset_Reson,
	FOC_NV_Err,
}ctrl_event_r_t;


typedef struct {
	float mot_vel;
	float torque;
	float dc_curr;
	float phase_curr; //最大相电流
	float ebrk_dc_curr; //最大母线回收电流
	float ebrk_torque;
	float dc_vol_min;
	float dc_vol_max;
}user_limit;

typedef struct {
	lineramp_t vel;
	lineramp_t torque;
	lineramp_t dc_curr;
}rt_lim_ramper;

typedef struct {
	float mot_vel;
	float phase_curr;
	//float phase_vol;
	float fw_id; //D轴最大退磁电流
	float dc_curr;
	//float dc_vol;
	float torque;
}hw_limit;

typedef struct {
	float dc_curr;
	float torque;
}prot_limit;

#if 0
typedef struct {
	bool b_sample;
	s16  v_inj;
	float v_alpha_inj;
	float v_beta_inj;
	float sample_prev;
	float sample_now;
	float vel_integrator;
	float angle_rad;
	float angle_deg;
	float elec_vel;
	float max_vel;
	float int_gain2;
	float int_gain;
	float inv_ld_lq;
}hfi_t;
#else
typedef struct {
	float sign;
	s16  v_inj;
	float v_alpha_inj;
	float v_beta_inj;
	albt_t sample_prev;
	float rad_integrator;
	float angle_deg;
	float elec_vel;
	float rpm_vel;
	PI_Controller pi;
}hfi_t;

#endif
typedef struct{
	bool 			b_start;
	bool			b_ebrk_running;
	bool			b_hw_braker;
	bool 			b_mtpa_calibrate;
	bool    		b_cruiseEna;
	bool    		b_AutoHold;
	float 			adv_angle; //dq 分配超前角
	float 			force_angle;
	float 			angle_last;
	float   		target_torque;
	float           target_torque_raw; //扭矩过零点处理前的扭矩
	dq_t			target_idq;
	dq_t            out_idq_filterd;
	float 			out_current_vec;
	u32				ebrk_ramp_time;
	u32				torque_acc_time;
	u32				torque_dec_time;
	float   		dc_curr_filted;
	float			dc_curr_calc;
	float   		phase_curr_filted[3];
	float           autohold_torque;
	u8 				mode_req;
	u8 				mode_running;
	u8      		error;
	foc_t 			foc;
	etcs_t          etcs;
	PI_Controller 	pi_lock;
	PI_Controller 	pi_power;
	PI_Controller 	pi_vel_lim;
	PI_Controller 	pi_vel;
	user_limit 		userlim;
	hw_limit   		hwlim;
	prot_limit 		protlim;
	lineramp_t   	ramp_target_current;
	lineramp_t 		ramp_cruise_vel;
	lineramp_t 		ramp_target_vd;
	lineramp_t 		ramp_target_vq;
	lineramp_t		ramp_target_vel;
	lineramp_t 		ramp_vel_lim;
	lineramp_t 		ramp_torque_lim;
	lineramp_t 		ramp_dc_curr_lim;
	lineramp_t		ramp_input_torque;
	lineramp_t		ramp_adv_angle;
	float 			phase_a_max;
	float 			phase_b_max;
	float 			phase_c_max;
	u32 			phase_unbalance_cnt;
	bool 			b_hfi;
	hfi_t           hfi;
}mot_contrl_t;

void mot_contrl_init(mot_contrl_t *ctrl);
bool mot_contrl_enable(mot_contrl_t *ctrl, bool start);
bool mot_contrl_update(mot_contrl_t *ctrl);
u8   mot_contrl_mode(mot_contrl_t *ctrl);
bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode);
void mot_contrl_slow_task(mot_contrl_t *ctrl);
u8   mot_contrl_protect(mot_contrl_t *ctrl);
float mot_contrl_get_speed(mot_contrl_t *ctrl);
void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0);
void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0);
void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit);
void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel);
void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque);
float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl);
void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time);
void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq);
void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq);
bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable);
bool mot_contrl_resume_cruise(mot_contrl_t *ctrl);
bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm);
bool mot_contrl_set_current(mot_contrl_t *ctrl, float is);
void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec);
void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc);
bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque);
void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable);
void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock);
bool mot_contrl_set_ebreak(mot_contrl_t *ctrl, bool start);
void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake);
void mot_contrl_calc_current(mot_contrl_t *ctrl);
void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd);
void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd);
void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time);
void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time);
bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque);
void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable);

static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
	mot_contrl_enable(ctrl, true);
	return mot_contrl_request_mode(ctrl, mode);
}

static __INLINE bool mot_contrl_stop(mot_contrl_t *ctrl) {
	return mot_contrl_enable(ctrl, false);
}

static __INLINE bool mot_contrl_is_start(mot_contrl_t *ctrl) {
	return ctrl->b_start;
}
static __INLINE bool mot_contrl_dccurr_is_protected(mot_contrl_t *ctrl) {
	return (ctrl->protlim.dc_curr != HW_LIMIT_NONE);
}

static __INLINE bool mot_contrl_torque_is_protected(mot_contrl_t *ctrl) {
	return (ctrl->protlim.torque != HW_LIMIT_NONE);
}

static __INLINE void mot_contrl_set_ebrk_torquer(mot_contrl_t *ctrl, s16 torque) {
	ctrl->userlim.ebrk_torque = torque;
}

static __INLINE void mot_contrl_set_error(mot_contrl_t *ctrl, u8 error) {
	ctrl->error = error;
}

static __INLINE u8 mot_contrl_get_errcode(mot_contrl_t *ctrl) {
	return ctrl->error;
}

static __INLINE void mot_contrl_pause_cruise(mot_contrl_t *ctrl) {
	ctrl->b_cruiseEna = false;
}

static __INLINE bool mot_contrl_is_cruise_enabled(mot_contrl_t *ctrl) {
	return (ctrl->b_cruiseEna && (ctrl->mode_running == CTRL_MODE_SPD));
}

static __INLINE bool mot_contrl_set_target_vel(mot_contrl_t *ctrl, float vel) {
	if (ctrl->b_cruiseEna) {
		return false;
	}
	line_ramp_set_target(&ctrl->ramp_target_vel ,min(ABS(vel), ctrl->userlim.mot_vel)*SIGN(vel));
	return true;
}

static __INLINE void mot_contrl_set_vel_rttime(mot_contrl_t *ctrl, u32 time) {
	line_ramp_set_time(&ctrl->ramp_target_vel, (float)time);
	line_ramp_update(&ctrl->ramp_target_vel);
}


static __INLINE void mot_contrl_set_angle(mot_contrl_t *ctrl, float angle) {
	ctrl->force_angle = (angle);
}

static __INLINE void mot_contrl_set_adv_angle(mot_contrl_t *ctrl, float angle) {
	ctrl->adv_angle = (angle);
	line_ramp_set_target(&ctrl->ramp_adv_angle, angle);
}

static __INLINE bool mot_contrl_is_auto_holdding(mot_contrl_t *ctrl) {
	return ctrl->b_AutoHold;
}

static __INLINE float mot_contrl_get_current_vector(mot_contrl_t *ctrl) {
	return ctrl->out_current_vec;
} 

static __INLINE float mot_contrl_get_dc_current(mot_contrl_t *ctrl) {
	return ctrl->dc_curr_filted;
} 

static __INLINE bool mot_contrl_ebrk_is_enabled(mot_contrl_t *ctrl) {
	return mot_contrl_get_ebrk_torque(ctrl) != 0;
}

static __INLINE bool mot_contrl_ebrk_is_running(mot_contrl_t *ctrl) {
	return ctrl->mode_running == CTRL_MODE_EBRAKE;
}

static __INLINE float mot_contrl_get_final_torque(mot_contrl_t *ctrl) {
	return ctrl->ramp_input_torque.target;
}

static __INLINE u32 mot_contrl_get_torque_acc_time(mot_contrl_t *ctrl) {
	return ctrl->ramp_input_torque.time;
}

static __INLINE s16 mot_contrl_get_speed_abs(mot_contrl_t *ctrl) {
	s16 speed = (s16)mot_contrl_get_speed(ctrl);
	return ABS(speed);
}

#endif /* _CONTROLLER_H__ */