#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;
static motor_map_t gear_torques[4][5] = {
#if 0
	[0] = {{500,  10}, {1200, 10}, {1700, 10}, {2200, 10}, {8000, 10},},
	[1] = {{800,  10}, {1600, 10}, {2400, 10}, {3200, 10}, {8000, 10},},
	[2] = {{1200, 10}, {2200, 10}, {3200, 10}, {4200, 10}, {8000, 10},},
	[3] = {{3000, 10}, {4740, 10}, {5050, 11}, {5200, 10}, {8000, 10},},
#else
	[0] = {{3000,  400}, {5000, 246}, {6000, 220}, {7000, 208}, {8000, 193},},
	[1] = {{3000,  400}, {5000, 246}, {6000, 220}, {7000, 208}, {8000, 193},},
	[2] = {{3000,  400}, {5000, 246}, {6000, 220}, {7000, 208}, {8000, 193},},
	[3] = {{3000,  400}, {5000, 246}, {6000, 220}, {7000, 208}, {8000, 193},},
#endif
};

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 float thro_torque_gear_map(void) {
	u8 gear = mc_get_gear();
	if (gear > 3) {
		gear = 0;
	}
	motor_map_t *map = &gear_torques[gear][0];
	s16 rpm = (s16)_torque.spd_filted;
	if (rpm <= map[0].rpm) {
		return (float)map[0].torque;
	}
	int map_size = 5;
	for (int i = 1; i < map_size; i++) {
		if (rpm <= map[i].rpm) { //线性插值
			float trq1 = map[i-1].torque;
			float min_rpm = map[i-1].rpm;
			float trq2 = map[i].torque;
			float max_rpm = map[i].rpm;
			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 (float)map[map_size-1].torque;	
}

/* 获取油门开度 */
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());
}