#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, 150},
	{5050, 110},
	{5200, 85},
	{5300, 70},
	//{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;
	}
	int map_size = ARRAY_SIZE(mot_map);
	for (int i = 1; i < map_size; 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;
			if (trq1 > trq2) {
				return (s16)f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
			}else {
				return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
			}
		}
	}
	return mot_map[map_size-1].torque;
}