#include "foc/foc_config.h"
#include "foc/core/controller.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/motor/motor.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.1f
static throttle_torque_t _throttle;

void throttle_torque_reset(void) {
	_throttle.accl = false;
	_throttle.pedal_opening = _throttle.prev_pedal_opening = 0.0f;
	_throttle.torque_req = _throttle.torque_real = _throttle.torque_compensation = 0.0f;
	_throttle.gear = mc_get_internal_gear();
}


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

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()) {
		return true;
	}else 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);
}

/* 获取转把电压对应的油门开度 */
float throttle_vol_to_opening(float thro_val) {
	if (thro_val <= throttle_start_vol()) {
		return 0;
	}
	float delta = thro_val - throttle_start_vol();
	int ration = (delta * 100.0f) / throttle_vol_range();
	return ((float)ration)/100.0f;
}

/* 获取油门开度 */
float throttle_get_open_ration(void) {
	float thro_val = throttle_get_signal();
	return throttle_vol_to_opening(thro_val);
}

/* 获取油门开度对应的转把电压 */
float throttle_opening_to_vol(float r) {
	if (r == 0) {
		return 0;
	}else if (r > 1.0f) {
		r = 1.0f;
	}
	return (throttle_start_vol() + r * throttle_vol_range());
}


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));
			}
		}
	}
}

static float _throttle_torque_for_accelerate(float ration) {
	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
	float thro_torque = max_torque * ration;

	float pedal_acc = 1.0f;
	if (_throttle.prev_pedal_opening < 1.0f) {
		pedal_acc = (ration - _throttle.prev_pedal_opening)/ (1.0f - _throttle.prev_pedal_opening);
	}
	pedal_acc = fclamp(pedal_acc, 0, 1.0f);
	float acc_torque = _throttle.torque_real + pedal_acc * (max_torque - _throttle.torque_real);
	if (acc_torque < 0) {
		acc_torque = 0;
	}
	/*
	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
	*/
	float torque_compensation = thro_torque - acc_torque;
	float step = 0.0f;
	if (torque_compensation > 0) {
		float acc_t = mc_gear_conf()->accl_time;
		step = torque_compensation / (acc_t + 0.00001f);
	}else {
		torque_compensation = 0;
	}
	step_towards(&_throttle.torque_compensation, torque_compensation, step);
	return (acc_torque + _throttle.torque_compensation);
}


static float throttle_torque_for_accelerate(void) {
	return _throttle_torque_for_accelerate(_throttle.pedal_opening);
}

static float throttle_torque_for_decelerate(void) {
	if (_throttle.prev_pedal_opening == 0.0f) {
		return 0;
	}
	float pedal_dec = _throttle.pedal_opening / _throttle.prev_pedal_opening;
	pedal_dec = fclamp(pedal_dec, 0.0f, 1.0f);
	return pedal_dec * _throttle.torque_real;
}

float throttle_get_open_ration_filted(void) {
	return _throttle.pedal_opening;
}

#define THRO_RPM_LP_CEOF 0.01f
float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
	float pedal_opening = throttle_vol_to_opening(vol);
	float vel = mot_contrl_get_speed(ctrl);
	_throttle.gear = mc_get_internal_gear();
	LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);

	if (pedal_opening > _throttle.pedal_opening) {
		if (!_throttle.accl) {
			_throttle.prev_pedal_opening = _throttle.pedal_opening;
			_throttle.torque_real = _throttle.torque_req;
			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
				_throttle.torque_real = 0;
			}
			_throttle.torque_compensation = 0;
		}
		_throttle.accl = true;
	}else if (pedal_opening < _throttle.pedal_opening) {
		if (_throttle.accl) {
			_throttle.prev_pedal_opening = _throttle.pedal_opening;
			_throttle.torque_real = ctrl->target_torque_raw;
			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->ramp_input_torque) >= 10.0f ) {
				_throttle.torque_real = _throttle.torque_req;
			}
			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
				_throttle.torque_real = 0;
			}
		}
		_throttle.accl = false;
	}
	_throttle.pedal_opening = pedal_opening;
	if (_throttle.accl) {
		return throttle_torque_for_accelerate();
	}else {
		return throttle_torque_for_decelerate();
	}
}

void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
	float curr_vel = mot_contrl_get_speed(ctrl);
	float ref_torque = torque;
	if (_throttle.accl) { //加速需求		
		float hold_torque = ctrl->autohold_torque;
		if (hold_torque < 0) { //下坡驻车,最小给0扭矩
			hold_torque = 0;
		}
		ref_torque	= MAX(hold_torque, ref_torque);
		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
			if (_throttle.torque_req < hold_torque) {
				_throttle.torque_req = hold_torque;
				line_ramp_reset(&ctrl->ramp_input_torque, hold_torque);
			}
		}else {
			ctrl->autohold_torque = 0;
		}
		/* 处理加速ramp时间的变化，需要缓慢变小，变大可以立即处理，
		*  加速时间缓慢变小可以防止突然大扭矩加速
		*/
		u16 now_ramp_time = mot_contrl_get_torque_acc_time(ctrl);
		u16 next_ramp_time = mc_gear_conf()->accl_time;
		if (curr_vel < CONFIG_ZERO_SPEED_RAMP_RMP) {
			next_ramp_time = mc_gear_conf()->zero_accl;
		}

		if (now_ramp_time != next_ramp_time) {
			if (next_ramp_time > now_ramp_time) {
				mot_contrl_set_torque_acc_time(ctrl, next_ramp_time);
			}else {
				float f_now = (float)now_ramp_time;
				float f_next = (float)next_ramp_time;
				step_towards(&f_now, f_next, 5.0f);
				mot_contrl_set_torque_acc_time(ctrl ,(u16)f_now);
			}
		}
		_throttle.torque_req = ref_torque;
	}else {
		float ref_torque = throttle_torque_for_decelerate();
		/* autohold 启动的情况下，转把在0位置附近小幅抖动 */
		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {
			float hold_torque = ctrl->autohold_torque;
			ref_torque = MAX(hold_torque, ref_torque);
		}
		_throttle.torque_req = ref_torque;
	}
	mot_contrl_set_torque(ctrl, _throttle.torque_req);
}

/* 定速巡航需要判断是否需要加速 */
float get_user_request_torque(void) {
	if (_throttle.accl) {
		return throttle_torque_for_accelerate();
	}
	return throttle_torque_for_decelerate();
}


void throttle_log(void) {
	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.pedal_opening, _throttle.prev_pedal_opening, _throttle.torque_real, _throttle.torque_req);
	sys_debug("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
}