#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[] = {
	{4500, 200},
	{4740, 170},
	{5050, 125},
	{5400, 85},
	{5740, 85},//5
	{6050, 90},//10
	{6430, 107},//16
};
#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 trq1 = mot_map[i-1].torque;
			float min_rpm = mot_map[i-1].rpm;
			float trq2 = mot_map[i].torque;
			float max_rpm = mot_map[i].rpm;

			return (s16)f_map(rpm, min_rpm, max_rpm, min(trq1, trq2), MAX(trq1, trq2));
		}
	}
	return mot_map[ARRAY_SIZE(mot_map)-1].torque;
}