#include "bsp/bsp.h"
#include "foc/core/foc_observer.h"
#include "foc/core/smo_observer.h"
#include "foc/motor/motor.h"

static foc_observer_t foc_obser;
void foc_observer_init(void) {
	foc_obser.smo_enabled = false;
	foc_obser.smo_used = false;
	foc_obser.enc_angle = INVALID_ANGLE;
	foc_obser.enc_est_angle = INVALID_ANGLE;
	foc_obser.enc_speed = 0;
	foc_obser.smo_angle = INVALID_ANGLE;
	foc_obser.smo_est_angle = INVALID_ANGLE;
	foc_obser.smo_speed = 0;
	foc_obser.fusion_ceof = 1.0f;
#ifdef CONFIG_SMO_OBSERVER
	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
	foc_obser.smo_enabled = true;
#endif
}

#define RPM_2_degree(rpm)  ((rpm) * 60.0f * nv_get_motor_params()->poles * FOC_CTRL_US)

float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
	float prev_enc_angle = foc_obser.enc_angle;
	float prev_enc_speed = foc_obser.enc_speed;
	foc_obser.enc_angle = motor_encoder_get_angle();
	foc_obser.enc_speed = motor_encoder_get_speed();

	if (!foc_obser.smo_enabled) {
		return foc_obser.enc_angle;
	}
	float est_enc_delta = RPM_2_degree(prev_enc_speed);
	float real_enc_delta = foc_obser.enc_angle - prev_enc_angle;
	if (real_enc_delta < 0) {
		real_enc_delta += 360.0f;
	}
	float est_ration = real_enc_delta/est_enc_delta;
	if (est_ration >= 1.5f || est_ration <= 0.5f) {
		foc_obser.fusion_ceof -= 0.1f;
		if (foc_obser.fusion_ceof < 0.0f) {
			foc_obser.fusion_ceof = 0.0f;
		}
		if (foc_obser.enc_est_angle == INVALID_ANGLE) {
			foc_obser.enc_est_angle = prev_enc_angle;
		}else {
			foc_obser.enc_est_angle += est_enc_delta;
			rand_angle(foc_obser.enc_est_angle);
		}
	}else {
		foc_obser.fusion_ceof += 0.1f;
		if (foc_obser.fusion_ceof > 1.0f) {
			foc_obser.fusion_ceof = 1.0f;
		}
		foc_obser.enc_est_angle = foc_obser.enc_angle;
	}
	foc_obser.smo_est_angle = foc_obser.smo_angle + RPM_2_degree(foc_obser.smo_speed);
	foc_obser.smo_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
	foc_obser.smo_speed = smo_observer_est_rpm();

	if (foc_obser.smo_used) {
		return foc_obser.smo_angle;
	}
#if 0
	return (foc_obser.enc_est_angle * foc_obser.fusion_ceof + foc_obser.smo_angle * (1.0f - foc_obser.fusion_ceof));
#else
	return foc_obser.enc_angle;
#endif
}

float foc_observer_speed(void) {
	if (foc_obser.smo_used) {
		return foc_obser.smo_speed;
	}
	return foc_obser.enc_speed;
}

bool  foc_observer_is_encoder(void) {
	return !foc_obser.smo_used;
}
void  foc_observer_use_smo(bool use_smo) {
	if (foc_obser.smo_enabled) {
		foc_obser.smo_used = use_smo;
	}else {
		foc_obser.smo_used = false;
	}
}
void  foc_observer_enable_smo(bool enable) {
	foc_obser.smo_enabled = enable;
}

float foc_observer_smo_angle(void) {
	return foc_obser.smo_angle;
}

float foc_observer_smo_speed(void) {
	return foc_obser.smo_speed;
}