#include "foc/foc_config.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/bsp_driver.h"
#include "libs/logger.h"
#include "app/nv_storage.h"
#include "foc/motor/throttle.h"

static u8 err_mask;

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_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, nv_get_foc_params()->n_startThroVol, nv_get_foc_params()->n_endThroVol);
	}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, nv_get_foc_params()->n_startThroVol, nv_get_foc_params()->n_endThroVol);
	}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 <= nv_get_foc_params()->n_startThroVol;
#else
	return get_throttle_float() <= nv_get_foc_params()->n_startThroVol;
#endif
}

void throttle_detect(void) {
	float thr_5v = get_thro_5v_float();
	float thr_sig = get_throttle_float();
	if (thr_sig <= nv_get_foc_params()->f_minThroVol || thr_sig >=nv_get_foc_params()->f_maxThroVol) {
		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 <= nv_get_foc_params()->f_minThroVol || thr2_sig >=nv_get_foc_params()->f_maxThroVol) {
			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
}