#include "foc/core/thro_torque.h"
#include "foc/foc_config.h"
#include "foc/motor/motor.h"
#include "foc/motor/motor_param.h"
#include "foc/core/e_ctrl.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"
#include "prot/can_foc_msg.h"

static thro_torque_t _torque;

void thro_torque_reset(void) {
	_torque.accl = false;
	_torque.thro_filted = 0.0f;
	_torque.thro_ration = _torque.thro_ration_last = 0.0f;
	_torque.torque_req = _torque.torque_real = 0.0f;
}

void thro_torque_init(void) {
	thro_torque_reset();
	_torque.spd_filted = 0.0f;
}

static __INLINE float gear_rpm_torque(u8 trq, s16 max) {
	return (float)trq/100.0f * max;
}

static float thro_torque_gear_map(void) {
	mc_gear_t *_current_gear = mc_get_gear_config();
	if (_current_gear == NULL) {
		return 0;
	}

	s16 rpm = (s16)_torque.spd_filted;
	if (rpm > _current_gear->n_max_speed) {
		rpm = _current_gear->n_max_speed;
	}
	if (rpm <= 1000) {
		return gear_rpm_torque(_current_gear->n_torque[0], _current_gear->n_max_trq);
	}

	for (int i = 1; i < GEAR_SPEED_TRQ_NUM; i++) {
		if (rpm <= 1000 * (i + 1)) { //线性插值
			float trq1 = gear_rpm_torque(_current_gear->n_torque[i-1], _current_gear->n_max_trq);
			float min_rpm = (i * 1000);
			float trq2 = gear_rpm_torque(_current_gear->n_torque[i], _current_gear->n_max_trq);
			float max_rpm = (i + 1) * 1000;
			if (trq1 > trq2) {
				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
			}else {
				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
			}
		}
	}
	return gear_rpm_torque(_current_gear->n_torque[GEAR_SPEED_TRQ_NUM-1], _current_gear->n_max_trq);
}

/* 获取油门开度 */
static float thro_ration(float f_throttle) {
	if (f_throttle <= nv_get_foc_params()->n_startThroVol) {
		return 0;
	}
	float delta = f_throttle - (nv_get_foc_params()->n_startThroVol);
	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);	
	return ((float)ration)/100.0f;
}

float thro_ration_to_voltage(float r) {
	if (r == 0) {
		return 0;
	}
	float vol = nv_get_foc_params()->n_startThroVol + r * (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
	return fclamp(vol, 0, nv_get_foc_params()->n_endThroVol);
}

static float thro_torque_for_accelerate(void) {
	float max_torque = thro_torque_gear_map();
	float acc_r = 1.0f;
	if (_torque.thro_ration_last < 1.0f) {
		acc_r = (_torque.thro_ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
	}
	acc_r = fclamp(acc_r, 0, 1.0f);
	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);

	return acc_torque;
}

static float thro_torque_for_decelerate(void) {
	if (_torque.thro_ration_last == 0.0f) {
		return 0;
	}
	float dec_r = _torque.thro_ration / _torque.thro_ration_last;
	dec_r = fclamp(dec_r, 0.0f, 1.0f);
	return dec_r * _torque.torque_real;
}

static void thro_torque_request(void) {
#ifdef CONFIG_CRUISE_ENABLE_ACCL
	if (mc_is_cruise_enabled() && mc_throttle_released()) {
		return;
	}
#endif
	if (mc_throttle_released() && eCtrl_enable_eBrake(true)) {
		return;
	}
	if (!mc_throttle_released()) {
		if (_torque.accl) { //加速需求
			float ref_torque = thro_torque_for_accelerate();
			float curr_rpm = PMSM_FOC_GetSpeed();
			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
				float hold_torque = eCtrl_get_FinalTorque();
				ref_torque	= MAX(hold_torque, ref_torque);
			}
			_torque.torque_req = ref_torque;
		}else {
			float ref_torque = thro_torque_for_decelerate();
			_torque.torque_req = ref_torque;
		}
		PMSM_FOC_Set_Torque(_torque.torque_req);
	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
		_torque.torque_req = 0.0f;
		PMSM_FOC_Set_Torque(_torque.torque_req);
	}
}

#define THRO_REF_LP_CEOF 0.2f
static void thro_torque_filter(float f_throttle) {
	if (mc_throttle_released()){
		thro_torque_reset();
	}else {
		LowPass_Filter(_torque.thro_filted, f_throttle, THRO_REF_LP_CEOF);
	}
	float curr_rpm = PMSM_FOC_GetSpeed();
	if (curr_rpm == 0) {
		_torque.spd_filted = 0;
	}else {
		LowPass_Filter(_torque.spd_filted, curr_rpm, 0.01f);
	}
}

void thro_torque_process(u8 run_mode, float f_throttle) {

	thro_torque_filter(f_throttle);

	float thro_r = thro_ration(_torque.thro_filted);
	if (thro_r > _torque.thro_ration) {
		if (!_torque.accl) {
			_torque.thro_ration_last = _torque.thro_ration;
			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
		}
		_torque.accl = true;
	}else if (thro_r < _torque.thro_ration) {
		if (_torque.accl) {
			_torque.thro_ration_last = _torque.thro_ration;
			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
		}
		_torque.accl = false;
	}
	_torque.thro_ration = thro_r;
	if (run_mode == CTRL_MODE_TRQ) {
		thro_torque_request();
	}else if (run_mode == CTRL_MODE_SPD) {
		if (!mc_is_cruise_enabled()) {
			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.thro_ration;
			PMSM_FOC_Set_Speed(speed_Ref);
		}
	}else if (run_mode == CTRL_MODE_EBRAKE) {
		if (eCtrl_get_FinalTorque() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			eCtrl_enable_eBrake(false);	
		}
		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
			eCtrl_enable_eBrake(false);
		}
	}
}

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

void thro_torque_log(void) {
	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
	sys_debug("ration %f - %f - %f\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate());
}