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

#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
#define MOT_HAVE_MAPS
#define MOT_USE_PHASE_I //表示使用电流矢量和RPM查表，获取D轴电流，iq = 开根号(电流矢量的平方 + D轴电流的平方)
#define RPM_MAX_IDX 10
#define TRQ_MAX_IDX 10
static int map_rpm[] = {3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000};
#include "foc/motor/A1_motor_config.c"

#endif

/* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
s16 get_max_torque_for_rpm(s16 rpm) {
#ifdef MOT_HAVE_MAPS
	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;
#else
	return CONFIG_MAX_MOTOR_TORQUE;
#endif
}

s16 motor_map_rpm_idx(float rpm, int *ilow, int *ihigh, s16 *lowrpm, s16 *highrpm) {
	*ilow = *ihigh = 0xFF;
	s16 irpm = (s16)rpm;
#ifdef MOT_HAVE_MAPS
	if (irpm >= map_rpm[RPM_MAX_IDX-1]) {
		irpm = map_rpm[RPM_MAX_IDX-1];
		*ilow = RPM_MAX_IDX-1;
		*ihigh = RPM_MAX_IDX-1;
	}else {
		for (int i = 0; i < RPM_MAX_IDX; i++) {
			if (irpm <= map_rpm[i]) {
				*ihigh = i;
				if (*ilow == 0xFF) {
					*ilow = 0;
				}
				break;
			}
			*ilow = i;
		}
	}
	*lowrpm = map_rpm[*ilow];
	*highrpm = map_rpm[*ihigh];
#endif
	return irpm;
}

int motor_map_torque_max_count(void) {
#ifdef MOT_HAVE_MAPS
	return TRQ_MAX_IDX;
#else
	return 0;
#endif
}

#define _DEBUG(fmt, args...) no_debug(fmt, ##args)

#ifdef MOT_HAVE_MAPS

//x -> rpm
//z -> torque
static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {
	float frac_z1 = 0; //对应x1索引的t_maps
	float frac_z2 = 0; //对应x2索引的t_maps

	_DEBUG("low --> %d %d\n", map[1]->torque, map[0]->torque);
	if ((map[1]->torque != map[0]->torque)) {
		frac_z1 = (float)(z - map[0]->torque)/(map[1]->torque - map[0]->torque);
	}
	_DEBUG("high --> %d %d\n", map[3]->torque, map[2]->torque);
	if ((map[3]->torque != map[2]->torque)) {
		frac_z2 = (float)(z - map[2]->torque)/(map[3]->torque - map[2]->torque);
	}
	
	_DEBUG("%f -- %f -- %f\n", frac_x, frac_z1, frac_z2);

	float c1 = (1.0f - frac_z1) * map[0]->d + frac_z1 * map[1]->d; //第一行插值
	float c2 = (1.0f - frac_z2) * map[2]->d + frac_z2 * map[3]->d; //第二行插值
	*d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
#ifdef MOT_USE_PHASE_I
	*q = sqrtf(z*z - (*d)*(*d));
#else
	c1 = (1.0f - frac_z1) * map[0]->q + frac_z1 * map[1]->q;
	c2 = (1.0f - frac_z2) * map[2]->q + frac_z2 * map[3]->q;
	*q = c1 * (1.0f - frac_x) + c2 * frac_x;
#endif
}

static void get_torque_range(s16 z, int index, int max_index, int *left, int *right) {
	int low_left = max_index - 1, low_right = max_index - 1;
	if (z < mtpa_fw_map[index][0].torque) {
		low_right = low_left = 0;
		_DEBUG("---%d, %d--%d\n", z, mtpa_fw_map[index][0].torque, mtpa_fw_map[0][0].torque);
	}else if (z > mtpa_fw_map[index][max_index - 1].torque) {
		low_right = low_left = max_index - 1;
	}else {
		for (int i = 0; i < max_index; i++) {
			if (z >= mtpa_fw_map[index][i].torque) {
				low_left = i;
				low_right = i + 1;
				if (i == max_index - 1) {
					low_right = low_left;
					break;
				}
			}
		}
	}
	*left = low_left;
	*right = low_right;
}

void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
	bool neg_trq = false;
	s16 itorque = torque * 10;
	if (itorque < 0) {
		neg_trq = true;
		itorque = -itorque;
	}
	int low = 0, high = 0;
	s16 x1 = 0, x2 = 0;
	rpm = ABS(rpm);
	s16 irpm = motor_map_rpm_idx(rpm, &low, &high, &x1, &x2);

	_DEBUG("speed %d-%d, %d-%d\n", low, high, x1, x2);
	int max_trq_idx = TRQ_MAX_IDX;
	int low_left = max_trq_idx - 1, low_right = max_trq_idx - 1;
	get_torque_range(itorque, low, max_trq_idx, &low_left, &low_right);
	_DEBUG("low speed torque %d-%d\n", low_left, low_right);
	
	int high_left = max_trq_idx - 1, high_right = max_trq_idx - 1;
	get_torque_range(itorque, high, max_trq_idx, &high_left, &high_right);
	_DEBUG("high speed torque %d-%d\n", high_left, high_right);
	torque_map_t *maps[4];
	maps[0] = &mtpa_fw_map[low][low_left];
	maps[1] = &mtpa_fw_map[low][low_right];
	maps[2] = &mtpa_fw_map[high][high_left];
	maps[3] = &mtpa_fw_map[high][high_right];
	float frac_x = 0, d = 0, q = 0;
	if (x1 != x2) {
		frac_x = (float)(irpm - x1)/(x2 - x1);
	}
	intp_line2(frac_x, itorque, maps, &d, &q);
	dq_out->d = d / 10.0f;
	dq_out->q = q / 10.0f;

	if (neg_trq) {
		dq_out->d = -dq_out->d;
		dq_out->q = -dq_out->q;
	}
}
#else
void mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
	dq_out->d = 0;
	dq_out->q = torque;
}

#endif