#include "foc/foc_config.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/bsp_driver.h"
#include "libs/logger.h"
#include "foc/mc_config.h"
#include "foc/motor/throttle.h"
#include "foc/mc_error.h"

static u8 err_mask;
static float _start_v, _end_v;
static bool _auto_detect_sv = true;
static int  _auto_detect_sv_cnt = 0;
static float _auto_detect_sv_totle = 0;
static int _detect_release_cnt = 0;
#define CONFIG_SAFE_INV_V   0.06f

void throttle_init(void) {
	_start_v = mc_conf()->c.thro_start_vol;
	_end_v   = mc_conf()->c.thro_end_vol;
}

bool throttle1_is_error(void) {
	if (err_mask & (THRO1_5V_ERR_BIT | THRO1_SIG_ERR_BIT)) {
		return true;
	}
	return false;
}

bool throttle2_is_error(void) {
	if (err_mask & (THRO2_5V_ERR_BIT | THRO2_SIG_ERR_BIT)) {
		return true;
	}
	return false;
}

u8 throttle_get_errors(void) {
	return err_mask;
}

bool throttle_is_all_error(void) {
#if CONFIG_DAUL_THROTTLE==1
	return throttle1_is_error() && throttle2_is_error();
#else
	return throttle1_is_error();
#endif
}

float throttle_start_vol(void) {
	return _start_v;
}

float throttle_end_vol(void) {
	return _end_v;
}

float throttle_vol_range(void) {
	return (_end_v - _start_v);
}

float throttle_get_signal(void) {
#if CONFIG_DAUL_THROTTLE==1
	if (throttle1_is_error() && throttle2_is_error()) {
		return 0.0f;
	}else if (throttle1_is_error() && !throttle2_is_error()) {
		float thr = get_thro2_5v_float() - get_throttle2_float();
		return fclamp(thr, _start_v, _end_v);
	}else if (!throttle1_is_error() && throttle2_is_error()) {
		return get_throttle_float();
	}else {
		float thr1 = get_throttle_float();
		float thr2 = get_thro2_5v_float() - get_throttle2_float();
		return (thr1+thr2)/2.0f;
	}
#else
	return get_throttle_float();
#endif
}

bool throttle_is_released(void) {
#if CONFIG_DAUL_THROTTLE==1
	float signal = 0;
	if (throttle1_is_error() && !throttle2_is_error()) {
		float thr = get_thro2_5v_float() - get_throttle2_float();
		signal = fclamp(thr, _start_v, _end_v);
	}else if (!throttle1_is_error() && throttle2_is_error()) {
		signal = get_throttle_float();
	}else {
		float thr1 = get_throttle_float();
		float thr2 = get_thro2_5v_float() - get_throttle2_float();
		signal = (thr1+thr2)/2.0f;
	}
	return signal <= _start_v;
#else
	return get_throttle_float() <= _start_v;
#endif
}

bool throttle_not_released_err(void)
{
	return ((err_mask & THRO_NOT_RELEASED) != 0);
}

void throttle_force_detect(void) {
	u32 mask = cpu_enter_critical();
	throttle_init();
	_auto_detect_sv = true;
	_auto_detect_sv_cnt = 0;
	_auto_detect_sv_totle = 0;
	_detect_release_cnt = 0;
	err_mask = 0; //clear err mask
	cpu_exit_critical(mask);
}

void throttle_detect(bool ready) {
	float thr_5v, thr_sig;

	sample_throttle();

	thr_5v = get_thro_5v_float();
	thr_sig = get_throttle_float();
	if (thr_sig <= mc_conf()->c.thro_min_vol || thr_sig >= mc_conf()->c.thro_max_vol) {
		err_mask |= THRO1_SIG_ERR_BIT;
	}
	if (thr_5v <= 4.5f || thr_5v >= 5.5f) {
		err_mask |= THRO1_5V_ERR_BIT;
	}
#if CONFIG_DAUL_THROTTLE==1
	thr_5v = get_thro2_5v_float();
	if (thr_5v <= 4.5f || thr_5v >= 5.5f) {
		err_mask |= THRO2_5V_ERR_BIT;
	}else {
		float thr2_sig = get_thro2_5v_float() - get_throttle2_float();
		if (thr2_sig <= mc_conf()->c.thro_min_vol || thr2_sig >= mc_conf()->c.thro_max_vol) {
			err_mask |= THRO2_SIG_ERR_BIT;
		}else {
			if (ABS(thr2_sig - thr_sig) > 0.5f) {
				err_mask |= THRO2_SIG_ERR_BIT;
				err_mask |= THRO1_SIG_ERR_BIT;
			}
		}
	}
#endif
	if (!ready && throttle_get_signal() > _start_v) {
		if (_detect_release_cnt < 500) {
			_detect_release_cnt ++;
		}else {
			err_mask |= THRO_NOT_RELEASED;
		}
	}
	if (ready) {
		_auto_detect_sv = false;
	}else if (!throttle_is_all_error() && !throttle_not_released_err() && !ready && _auto_detect_sv) {
		float v = throttle_get_signal();
		if (v < _start_v) {
			_auto_detect_sv_totle += v;
			_auto_detect_sv_cnt ++;
			if (_auto_detect_sv_cnt == 200) {
				_start_v = _auto_detect_sv_totle / (float)_auto_detect_sv_cnt + CONFIG_SAFE_INV_V;
				_auto_detect_sv = false;
				mc_crit_err_add_s16(FOC_EV_THRO_START_V, (s16)(_start_v * 100.0f));
			}
		}
	}
}