#include "ladrc_observer.h"
#include "foc/mc_config.h"
#include "math/fast_math.h"
#include "libs/logger.h"

static ladrc_observer observer;

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

static __inline float ladrc_observer_band(float vel) {
	float ration = vel / observer.vel_min;
	float Wo = observer.Wo;
	if (ration > 7.0f) { //限制最高带宽，实际调试
		ration = 7.0f;
	}else if (ration < 0.1f) {
		ration = 0.1f;
	}
	Wo = ration * Wo;
	observer.B1 = 2 * Wo;
	observer.B2 = SQ(Wo);
	return Wo;
}

void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
	observer.Wo = Wo;
	observer.vel_min = vel_min;
	observer.ts = CONFIG_SENSORLESS_TS;
	observer.lpf_ceof = lpf_cut_off * observer.ts;
	observer.ld = mc_conf()->m.ld;
	observer.lq = mc_conf()->m.lq;
	observer.r = mc_conf()->m.rs;
	observer.poles = mc_conf()->m.poles;
	observer.max_eVel = CONFIG_HW_MAX_MOTOR_RPM/30.0f * M_PI * mc_conf()->m.poles;
	observer.max_z1 = CONFIG_HW_MAX_PHASE_CURR * 10.0f;
	observer.max_z2 = CONFIG_MAX_ACTIVE_EMF / observer.ld;
	observer.Vel_El = 0;
	observer.Vel_El_filter = 0;
	observer.angle_atan = 0;
	observer.angle_out = 0;
	observer.alpha.z1 = 0;
	observer.alpha.z2 = 0;
	observer.beta.z1 = 0;
	observer.beta.z2 = 0;
	observer.Ealpha = 0;
	observer.Ebeta = 0;
	observer.angle_idx = 0;
	observer.angle_sum = 0;
	for (int i = 0; i < ANGLE_BUF_NUM; i++) {
		observer.angle_array[i] = 0;
	}
	ladrc_observer_band(0);
}

float ladrc_observer_update(float va, float vb, float ia, float ib) {
	float induct = observer.Vel_El * (observer.ld - observer.lq) / observer.ld;
	/* update Wc for current est vel */
	float Wo = ladrc_observer_band(observer.Vel_El_filter);
	/* est alpha emf */
	float F0 = -observer.r/observer.ld * ia;
	float e = observer.alpha.z1 - ia;
	
	float alpha_z1 = observer.alpha.z1;
	observer.alpha.z2 += (-e * observer.B2) * observer.ts;
	observer.alpha.z2 = fclamp(observer.alpha.z2, -observer.max_z2, observer.max_z2);
	observer.alpha.z1 += (observer.alpha.z2 + F0 + va/observer.ld - e * observer.B1 - induct * observer.beta.z1) * observer.ts;
	observer.alpha.z1 = fclamp(observer.alpha.z1, -observer.max_z1, observer.max_z1);

	observer.Ealpha = observer.alpha.z2 * (-observer.ld);

	/* est beta emf */
	F0 = -observer.r/observer.ld * ib;
	e = observer.beta.z1 - ib;

	observer.beta.z2 += (-e * observer.B2) * observer.ts;
	observer.beta.z2 = fclamp(observer.beta.z2, -observer.max_z2, observer.max_z2);
	observer.beta.z1 += (observer.beta.z2 + F0 + vb/observer.ld - e * observer.B1 + induct * alpha_z1) * observer.ts;
	observer.beta.z1 = fclamp(observer.beta.z1, -observer.max_z1, observer.max_z1);
	
	observer.Ebeta = observer.beta.z2 * (-observer.ld);

	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
	UTILS_NAN_ZERO(angle);
	angle_clamp(angle);

	/* 速度计算 */
	float delta_angle = angle - observer.angle_atan;
	float delta_angle_abs = ABS(delta_angle);
	if (delta_angle_abs >= M_PI) {
		delta_angle = 2 * M_PI - delta_angle_abs;
	}
	observer.angle_atan = angle;
	observer.angle_sum += delta_angle;
	observer.angle_sum -= observer.angle_array[observer.angle_idx];
	observer.angle_array[observer.angle_idx] = delta_angle;
	if (++observer.angle_idx == ANGLE_BUF_NUM) {
		observer.angle_idx = 0;
	}
 	float vel = observer.angle_sum / (ANGLE_BUF_NUM * observer.ts);
	if (vel > observer.max_eVel) {
		vel = observer.max_eVel;
	}else if (vel < -observer.max_eVel) {
		vel = -observer.max_eVel;
	}
	LowPass_Filter(observer.Vel_El, vel, observer.lpf_ceof);
	/* 补偿ladrc相位延时,LADRC等效截止频率为Wo/2pi的两个低通滤波器串联 */
	angle = fast_atan_2(observer.Vel_El, Wo) * 2.0f;
	/* 电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
	observer.angle_out = observer.angle_atan + (angle + 0/*observer.Vel_El * observer.ts*/);
	angle_clamp(observer.angle_out);

	LowPass_Filter(observer.Vel_El_filter, observer.Vel_El, 0.01f); //需要再加一级低通滤波，给计算Wo和输出使用
	
	return pi_2_degree(observer.angle_out);
}

float ladrc_observer_angle(void) {
	return pi_2_degree(observer.angle_out);
}

float ladrc_observer_vel(void) {
	return (observer.Vel_El_filter * (30.0f / M_PI) / observer.poles);
}

ladrc_observer *ladrc_observer_get(void) {
	return &observer;
}