#include "app/nv_storage.h"
#include "math/fast_math.h"
#include "PMSM_FOC_Core.h"
#include "smo_observer.h"

static smo_observer_t smo;

static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta);
static void smo_pll(void);
void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta) {
	smo.ts = 1.0f/(float)FOC_PWM_FS;
	smo.bandwith = pll_bandwith;
	smo.pll_kp = pll_bandwith * 2 * M_PI * 2;
	smo.pll_ki = 0.25f * SQ(smo.pll_kp);
	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI;
	smo.lpf_wc = lpf_wc;
	smo.lpf_ceof = (lpf_wc*2*M_PI/(float)FOC_PWM_FS);
	smo.Ksmo = Ksmo;
	smo.Ksta = Ksta;
	smo.motor_r = nv_get_motor_params()->r;
	smo.motor_ld = nv_get_motor_params()->ld;
	smo.motor_lq = nv_get_motor_params()->lq;
	smo.motor_poles = nv_get_motor_params()->poles;
}	

float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta) {

	smo_observer(uAlpha, uBeta, iAlpha, iBeta);
	smo_pll();
	return pi_2_degree(smo.est_angle_out);
}

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

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

	float err_iAlpha = smo.Ialpha_hat - iAlpha;
	
	smo.est_eAlpha = fclamp(err_iAlpha, -smo.Ksta, smo.Ksta) * smo.Ksmo;
	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);

	/* est beta back emf */
	float calc_beta = uBeta/smo.motor_ld - smo.IBeta_hat*(smo.motor_r/smo.motor_ld) - smo.est_eBeta / smo.motor_ld + est_ab * smo.Ialpha_hat;
	smo.IBeta_hat += calc_beta * smo.ts; //积分
	
	float err_iBeta = smo.IBeta_hat - iBeta;
	
	smo.est_eBeta = fclamp(err_iBeta, -smo.Ksta, smo.Ksta) * smo.Ksmo;
	smo.est_eBeta_Filted = do_lpf(smo.est_eBeta_Filted, smo.est_eBeta, smo.lpf_ceof);

}
#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};

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;
	smo.est_rad_pers = perr + smo.pll_integrator; //计算角速度
	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_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
	angle_clamp(smo.est_angle);
	/* 对低通进行角度补偿 */
	smo.est_angle_out = smo.est_angle + fast_arctan2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
	angle_clamp(smo.est_angle_out);
}


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

//机械角度 rpm
float smo_observer_est_rpm(void) {
	return (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
}