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

static foc_observer_t foc_obser = {
	.enc_err_cnt = 0,
	.b_force_sensorless = false,
};
void foc_observer_init(void) {
	foc_obser.is_sensorless_enable = false;
	foc_obser.is_sensorless_running = false;
	foc_obser.enc_angle = INVALID_ANGLE;
	foc_obser.enc_est_angle = INVALID_ANGLE;
	foc_obser.enc_speed = 0;
	foc_obser.sensorless_angle = INVALID_ANGLE;
	foc_obser.sensorless_est_angle = INVALID_ANGLE;
	foc_obser.sensorless_speed = 0;
	foc_obser.is_sensorless_stable = false;
	foc_obser.sensorless_stable_cnt = 0;
	foc_obser.sensorless_unstable_cnt = 0;
#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.is_sensorless_enable = true;
#endif
#ifdef CONFIG_LADRC_OBSERVER
	ladrc_observer_init(CONFIG_LADRC_OBSERVER_MIN_Wo, CONFIG_LADRC_OBSERVER_MIN_eVEL, CONFIG_LADRC_OBSERVER_LPF_FREQ);
	foc_obser.is_sensorless_enable = true;
#endif
}

#define RPM_2_degree(rpm)  ((rpm) * 6.0f * mc_conf()->m.poles * FOC_CTRL_US)

float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
	foc_obser.sensorless_est_angle = foc_obser.sensorless_angle + RPM_2_degree(foc_obser.sensorless_speed);
#ifdef CONFIG_SMO_OBSERVER
	foc_obser.sensorless_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
	foc_obser.sensorless_speed = smo_observer_vel();
#elif defined CONFIG_LADRC_OBSERVER
	foc_obser.sensorless_angle = ladrc_observer_update(uAlp, uBeta, iAlp, iBeta);
	foc_obser.sensorless_speed = ladrc_observer_vel();
#endif
	return foc_obser.sensorless_angle;
}

bool foc_observer_diagnostic(float enc_angle, float enc_vel) {
	if (foc_obser.b_force_sensorless) {
		if (foc_obser.is_sensorless_stable) {
			if (foc_obser.sensorless_speed <= foc_observer_sensorless_working_speed()/2.0f) {
				foc_obser.is_sensorless_stable = false;
			}
		}
		return false;
	}
	if (enc_vel <= (foc_observer_sensorless_working_speed() - 100.0f)) {
		if (!foc_obser.is_sensorless_running) {
			if (motor_encoder_may_error()) {
				foc_obser.enc_err_cnt++;
				foc_obser.is_sensorless_running = true;
			}
		}
		foc_obser.enc_angle = enc_angle;
		foc_obser.enc_speed = ABS(enc_vel);
		if (!foc_obser.is_sensorless_running) {
			return true;
		}
		if (foc_obser.is_sensorless_stable) {
			if (foc_obser.sensorless_speed <= foc_observer_sensorless_working_speed()/2.0f) {
				foc_obser.is_sensorless_stable = false;
			}
		}
		return false;
	}
	if (!foc_obser.is_sensorless_running) {
		/* 判断无感算法是否已经稳定跟踪速度 */
		if (!foc_obser.is_sensorless_stable) {
			float vel_diff_abs = ABS(enc_vel - foc_obser.sensorless_speed);
			if (vel_diff_abs < 50) {
				foc_obser.sensorless_stable_cnt++;
			}else {
				foc_obser.sensorless_stable_cnt = 0;
			}
			if (foc_obser.sensorless_stable_cnt >= 1000) {
				foc_obser.is_sensorless_stable = true;
			}
		}
		if (motor_encoder_may_error()) {
			foc_obser.enc_err_cnt++;
			foc_obser.is_sensorless_running = true;
		}
	}
	foc_obser.enc_angle = enc_angle;
	foc_obser.enc_speed = ABS(enc_vel);
	return !foc_obser.is_sensorless_running;
}

float foc_observer_speed(void) {
	return foc_obser.sensorless_speed;
}

bool  foc_observer_is_encoder(void) {
	if (foc_obser.b_force_sensorless) {
		return false;
	}
	return !foc_obser.is_sensorless_running;
}

void  foc_observer_use_sensorless(bool use_smo) {
	if (foc_obser.is_sensorless_enable && use_smo) {
		foc_obser.b_force_sensorless = true;
	}else {
		foc_obser.b_force_sensorless = false;
	}
}
void  foc_observer_enable_sensorless(bool enable) {
	foc_obser.is_sensorless_enable = enable;
}

float foc_observer_sensorless_angle(void) {
	return foc_obser.sensorless_angle;
}

float foc_observer_sensorless_speed(void) {
	return foc_obser.sensorless_speed;
}

u16 foc_observer_enc_errcount(void) {
	return foc_obser.enc_err_cnt;
}

float foc_observer_sensorless_diff(void) {
	return foc_obser.real_est_diff;
}

float foc_observer_sensorless_ration(void) {
	return foc_obser.real_est_ration;
}

bool foc_observer_sensorless_stable(void) {
	return foc_obser.is_sensorless_stable;
}

bool foc_observer_is_force_sensorless(void) {
	return foc_obser.b_force_sensorless;
}

float foc_observer_sensorless_working_speed(void) {
#ifdef CONFIG_SMO_OBSERVER
	return CONFIG_SMO_MIN_SPEED;
#elif defined CONFIG_LADRC_OBSERVER
	return CONFIG_LADRC_OBSERVER_MIN_SPEED;
#else
	return 20000.0f;
#endif
}