#include "foc/mc_config.h"
#include "math/fast_math.h"
#include "controller.h"
#include "smo_observer.h"

#define USE_ARCTAN 0
static smo_observer_t smo;
static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta);
#ifdef USE_ARCTAN
static void smo_arctan(void);
#else
static void smo_pll(void);
#endif
void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta) {
	smo.ts = CONFIG_SENSORLESS_TS;
	smo.bandwith = pll_bandwith;
	smo.pll_kp = pll_bandwith * 2;
	smo.pll_ki = SQ(pll_bandwith);
	smo.pll_max_rad_pers = CONFIG_HW_MAX_MOTOR_RPM/30.0f * M_PI * mc_conf()->m.poles;
	smo.lpf_wc = lpf_wc;
	smo.lpf_ceof = (lpf_wc*smo.ts);
	smo.Ksmo = Ksmo;
	smo.Ksta = Ksta;
	smo.motor_r = mc_conf()->m.rs;
	smo.motor_ld = mc_conf()->m.ld;
	smo.motor_lq = mc_conf()->m.lq;
	smo.motor_poles = mc_conf()->m.poles;
	smo.dir_ccw = true;
	PLL_Reset(&smo.pll, 0);
	smo.pll.ts = smo.ts;
	smo.pll.kp = pll_bandwith * 2;
	smo.pll.ki = 0.25f * SQ(smo.pll.kp);
}	

float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta) {
	smo_observer(uAlpha, uBeta, iAlpha, iBeta);
#ifdef USE_ARCTAN
	smo_arctan();
#else
	smo_pll();
#endif
	return pi_2_degree(smo.est_angle_out);
}

smo_observer_t *get_smo(void) {
	return &smo;
}

static __INLINE float line_func(float err, float max, float slide) {
	float err_abs = ABS(err);

	if (err_abs > max) {
		return (err>0)?slide:-slide;
	}else {
		return (err * slide)/max;
	}
}

static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
	float est_ab = smo.est_rad_pers_filted * (smo.motor_ld - smo.motor_lq);

	/* est alpha back emf */
	float Ialpha_hat = smo.Ialpha_hat;
	float calc_alpha = (uAlpha - smo.Ialpha_hat*smo.motor_r - smo.est_eAlpha - est_ab * smo.IBeta_hat) / smo.motor_ld ;
	smo.Ialpha_hat += calc_alpha * smo.ts; //积分

	float err_iAlpha = smo.Ialpha_hat - iAlpha;
	
	smo.est_eAlpha = line_func(err_iAlpha, smo.Ksta, smo.Ksmo);//fclamp(err_iAlpha, -smo.Ksta, smo.Ksta) * smo.Ksmo;
#ifdef USE_ARCTAN
	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);
#else
	smo.est_eAlpha_Filted = smo.est_eAlpha;
#endif
	/* est beta back emf */
	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * Ialpha_hat) / smo.motor_ld;
	smo.IBeta_hat += calc_beta * smo.ts; //积分
	
	float err_iBeta = smo.IBeta_hat - iBeta;
	
	smo.est_eBeta = line_func(err_iBeta, smo.Ksta, smo.Ksmo);//fclamp(err_iBeta, -smo.Ksta, smo.Ksta) * smo.Ksmo;
#ifdef USE_ARCTAN
	smo.est_eBeta_Filted = do_lpf(smo.est_eBeta_Filted, smo.est_eBeta, smo.lpf_ceof);
#else
	smo.est_eBeta_Filted = smo.est_eBeta;
#endif
}
#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
#ifdef USE_ARCTAN
static void smo_arctan(void) {
	float ealpha_in = -smo.est_eAlpha_Filted;
	float ebeta_in  =  smo.est_eBeta_Filted;

	float angle = fast_atan_2(ealpha_in, ebeta_in); //通过反正切获取电角度
	UTILS_NAN_ZERO(angle);
	angle_clamp(angle);
	float prev_angle = smo.est_angle;
	float comp_angle = 0.0f;
	if (smo.dir_ccw) {
		if ((prev_angle > angle) && (prev_angle > M_PI && angle < M_PI)) {//wrapper
			comp_angle = 2 * M_PI;
		}
	}else {
		if ((prev_angle < angle) && (prev_angle < M_PI && angle > M_PI)) {//wrapper
			comp_angle = -2 * M_PI;
		}
	}
	smo.est_angle = angle;
	smo.est_rad_pers = PLL_run(&smo.pll, angle, comp_angle);
	if (smo.est_rad_pers > smo.pll_max_rad_pers) {
		smo.est_rad_pers = smo.pll_max_rad_pers;
		smo.pll.out = smo.est_rad_pers;
	}
	smo.est_rad_pers_filted = smo.est_rad_pers;
	/*低通滤波有相位滞后，需要补偿，同时电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc) + smo.est_rad_pers_filted * smo.ts;
	angle_clamp(smo.est_angle_out);
	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
	}else if (smo.est_rpm < 0) {
		smo.est_rpm = 0;
	}
}
#else
static void smo_pll(void) {
	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
	float ebeta_in  =  smo.est_eBeta_Filted/eab_sqr;
	float sin, cos;
	float angle_rad = pi_2_degree(smo.est_angle);
	
	arm_sin_cos_f32(angle_rad, &sin, &cos);
	float pi_err = cos * ealpha_in - sin * ebeta_in;
	float perr = pi_err * smo.pll_kp;
	if (smo.pll_integrator < smo.pll_max_rad_pers) {
		smo.pll_integrator += smo.ts * pi_err * smo.pll_ki; //更新pll的pi的积分xiang
	}
	smo.est_rad_pers = perr + smo.pll_integrator; //计算角速度
	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
	
	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
	angle_clamp(smo.est_angle);
	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
	}else if (smo.est_rpm < 0) {
		smo.est_rpm = 0;
	}
	/* 电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
	smo.est_angle_out = smo.est_angle + smo.est_rad_pers_filted * smo.ts;
	angle_clamp(smo.est_angle_out);
}
#endif

float smo_observer_angle(void) {
	return pi_2_degree(smo.est_angle_out);
}

//机械角度 rpm
float smo_observer_vel(void) {
	return smo.est_rpm;
}