#include "foc/core/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 motor_map_t gear_torques[5][5] = {
	[0] = {{500,  100}, {1200, 100}, {1700, 80}, {2200, 75}, {2800, 50},},
	[1] = {{800,  130}, {1600, 120}, {2400, 110}, {3200, 80}, {4300, 60},},
	[2] = {{1200, 150}, {2200, 140}, {3200, 120}, {4200, 80}, {5300, 70},},
	[3] = {{3000, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
	[4] = {{4500, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
};

/*
通过查表获取对应扭矩和速度时的Id和IQ的分配
*/

static torque_manager_t torque_ctrl;
void torque_init(void) {
	trq2dq_lookup_init();
	torque_reset();
}

void torque_reset(void) {
	memset(&torque_ctrl, 0, sizeof(torque_ctrl));
}

float torque_max_from_gear_rpm(void) {
	u8 gear = mc_get_internal_gear();
	if (gear > 4) {
		gear = 0;
	}
	motor_map_t *map = &gear_torques[gear][0];
	s16 rpm = (s16)torque_ctrl.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 throttle_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 throttle_to_speed(float f_throttle) {
	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
}

float throttle_to_torque(float f_throttle) {
	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
}

#define FUNC_SEG1_X_END 0.5F
#define FUNC_SEG1_Y_END 0.25F
#define FUNC_SEG1_K    (FUNC_SEG1_Y_END/FUNC_SEG1_X_END)

#define FUNC_SEG2_X_OFF FUNC_SEG1_X_END
#define FUNC_SEG2_Y_OFF FUNC_SEG1_Y_END
#define FUNC_SEG2_K ((1.0F-FUNC_SEG1_Y_END)/(1.0F - FUNC_SEG1_X_END))

static float __INLINE unline_func(float x) {
	if (x <= FUNC_SEG1_X_END) {
		return x * FUNC_SEG1_K;
	}

	return (x - FUNC_SEG1_X_END) * FUNC_SEG2_K + FUNC_SEG2_Y_OFF;
} 

#define REAL_DQ_CEOF 0.9f
#define FINAL_DQ_CEFO 1.1F

static float get_throttle_torque(float trq_ration) {
	float curr_rpm = PMSM_FOC_GetSpeed();
	if (curr_rpm == 0) {
		torque_ctrl.spd_filted = 0;
	}else {
		LowPass_Filter(torque_ctrl.spd_filted, curr_rpm, 0.01f);
	}
	float torque_map = torque_max_from_gear_rpm();// (float)motor_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
	float torque_lim = PMSM_FOC_GetTorqueLimit();
	float max_torque = min(torque_map, torque_lim);

	return max_torque * unline_func(trq_ration);
}

float get_thro_request_torque(void) {
	return get_throttle_torque(torque_ctrl.thro_ration);
}

void request_torque(float thro_ration) {
	float curr_rpm = PMSM_FOC_GetSpeed();
	float ref_torque = get_throttle_torque(thro_ration);
#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 (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
			torque_ctrl.torque_req = eCtrl_get_FinalTorque() * FINAL_DQ_CEFO;
			ref_torque  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_torque);
		}
		if (ref_torque > torque_ctrl.torque_req) { //加扭矩step给定
			step_towards(&torque_ctrl.torque_req, ref_torque, 1.0f);
		}else { //减扭矩，直接给定
			torque_ctrl.torque_req = ref_torque;
		}
		PMSM_FOC_Set_Torque(torque_ctrl.torque_req);
	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
		float real_trq = PMSM_FOC_Get_Real_dqVector() * REAL_DQ_CEOF;
		float ref_now = min(real_trq, eCtrl_get_RefTorque());
		eCtrl_reset_Torque(ref_now);
		PMSM_FOC_Set_Torque(0);
	}
}

void request_speed(float thro_ration) {
	if (!mc_is_cruise_enabled()) {
		float speed_Ref = PMSM_FOC_GetSpeedLimit() * thro_ration;
		PMSM_FOC_Set_TgtSpeed(speed_Ref);
	}
}

#define THRO_REF_LP_CEOF 0.2f
void throttle_process(u8 run_mode, float f_throttle) {
	if (mc_throttle_released()){
		torque_ctrl.throttle_value = 0;
		torque_ctrl.torque_req = 0;
	}else {
		LowPass_Filter(torque_ctrl.throttle_value, f_throttle, THRO_REF_LP_CEOF);
	}
	torque_ctrl.thro_ration = throttle_ration(f_throttle);
	if (run_mode == CTRL_MODE_TRQ) {
		request_torque(torque_ctrl.thro_ration);
	}else if (run_mode == CTRL_MODE_SPD) {
		request_speed(torque_ctrl.thro_ration);
	}else if (run_mode == CTRL_MODE_EBRAKE) {
		eCtrl_reset_Torque(0);
		if (eCtrl_get_FinalCurrent() < 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);
		}
	}
}