#include "bsp/bsp.h"
#include "foc/motor/motor_param.h"
#include "math/fast_math.h"

#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
static motor_map_t mot_map[] = {
	{1500, 200},
	{2500, 130},
	{3000, 110},
	{3500, 100},
	{4000, 90},
	{4500, 80},
	{5000, 70},
	{5500, 60},
};
#endif

/* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
s16 get_max_torque_for_rpm(s16 rpm) {
	if (rpm <= mot_map[0].rpm) {
		return mot_map[0].torque;
	}
	for (int i = 1; i < ARRAY_SIZE(mot_map); i++) {
		if (rpm <= mot_map[i].rpm) { //线性插值
			float max_trq = mot_map[i-1].torque;
			float min_rpm = mot_map[i-1].rpm;
			float min_trq = mot_map[i].torque;
			float max_rpm = mot_map[i].rpm;

			return (s16)f_map(rpm, min_rpm, max_rpm, min_trq, max_trq);
		}
	}
	return mot_map[ARRAY_SIZE(mot_map)-1].torque;
}