#include "ladrc_observer.h"
#include "app/nv_storage.h"
#include "math/fast_math.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 > 1) {
		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 = FOC_CTRL_US;
	observer.lpf_cutoff_freq = lpf_cut_off * 2 * M_PI;
	observer.ld = nv_get_motor_params()->ld;
	observer.lq = nv_get_motor_params()->lq;
	observer.r = nv_get_motor_params()->r;
	observer.poles = nv_get_motor_params()->poles;
	ladrc_observer_band(0);
}

void 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);
	/* est alpha emf */
	float F0 = -observer.r/observer.ld * ia;
	float e = observer.alpha.z1 - ia;

	observer.alpha.z2 += (-e * observer.B2);
	observer.alpha.z1 += (observer.alpha.z2 + F0 + va/observer.ld + e * observer.B1 * observer.Vel_El - induct * observer.beta.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.beta.z1 += (observer.beta.z2 + F0 + va/observer.ld - e * observer.B1 * observer.Vel_El + induct * observer.alpha.z1);
	observer.Ebeta = observer.beta.z2 * (-observer.ld);

	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
	/* 补偿ladrc相位延时， 同时电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
	float angle_comp = fast_atan_2(observer.Vel_El * Wo, SQ(Wo) - SQ(observer.Vel_El)) + observer.Vel_El * observer.ts;
	angle += angle_comp;
	angle_clamp(angle);
	float delta_angle = angle - observer.angle_last;
	float delta_angle_abs = ABS(delta_angle);
	if (delta_angle_abs >= M_PI) {
		delta_angle = 2 * M_PI - delta_angle_abs;
	}
	observer.angle_sum += 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);
	LowPass_Filter(observer.Vel_El, vel, (observer.lpf_cutoff_freq * observer.ts));
	observer.angle_last = angle;
}

float ladrc_observer_angle(void) {
	return observer.angle_last;
}

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