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

#if defined(CONFIG_MOTOR_TORQUE_CONF)
#define MOT_HAVE_MAPS
#define MOT_USE_PHASE_I //表示使用电流矢量和RPM查表，获取D轴电流，iq = 开根号(电流矢量的平方 - D轴电流的平方)
#ifndef MOTOR_STATOR_5N
#define RPM_MAX_IDX 11
#define TRQ_MAX_IDX 10
#define MOT_LQ_LOOKUP 
static int map_rpm[] = {2000, 3000, 4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000};
#else
#define RPM_MAX_IDX 11
#define TRQ_MAX_IDX 10
static int map_rpm[] = {4000, 4500, 5000, 5500, 6000, 6500, 7000, 7500, 8000, 8500, 9000};
#endif
#include CONFIG_MOTOR_TORQUE_CONF
#endif

/* 根据电机外特性map，获取当前转速下的最大扭矩，主要给计算当前扭矩需求使用 */
s16 motor_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 mc_conf()->m.max_torque;
#endif
}

float motor_get_lq_from_iq(s16 iq) {
#ifdef MOT_LQ_LOOKUP
	iq = ABS(iq);
	int map_size = ARRAY_SIZE(iq_lq_map);
	for (int i = map_size-1; i >= 0; i--) {
		if (iq >= iq_lq_map[i].iq) {
			return iq_lq_map[i].lq;
		}
	}
	return iq_lq_map[0].lq;
#else
	return mc_conf()->m.lq;
#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("z: %d, low --> %d %d\n", z, 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
	if (z != 0) {
		if (z >= ABS(*d)) {
			*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;
		}
	}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;
	}
#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 motor_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);
	if (itorque != 0) {
		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 {
		step_towards(&dq_out->d, d/10.0f, 0.5f);
		step_towards(&dq_out->q, q/10.0f, 0.5f);
	}
}
#else
void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
	float d = 0;
	float q = 0;
#if defined(CONFIG_MOT_ADV_ANGLE)
	if (torque != 0) {
		float advanced_angle = CONFIG_MOT_ADV_ANGLE;
		float s, c;
		SinCos_Lut(advanced_angle + 90.0f, &s, &c);
		d = ABS(torque) * c;
		d = fclamp(d, -mc_conf()->m.max_fw_id, mc_conf()->m.max_fw_id);
		q = sqrtf(SQ(torque) - SQ(d));
		if (torque < 0) {
			q = -q;
		}
	}else {
		if (ABS(rpm) < 1000) {
			d = 0;
		}else if (ABS(rpm) < 3000) {
			d = -5;
		}else if (ABS(rpm) < 5000) {
			d = -10;
		}else {
			d = -20;
		}
	}
#else
	q = torque;
#endif
	step_towards(&dq_out->d, d, 1.0f);
	step_towards(&dq_out->q, q, 0.7f);
}

#endif


float motor_get_ebreak_toruqe(float rpm) {
	float max_e_trq = PMSM_FOC_GetEbrkTorque();
	if (rpm >= 2000) {
		return -max_e_trq;
	}else if (rpm >= 1000) {
		return -max_e_trq * ((rpm - 1000.0f) / 1000.0f * 0.25f + 0.75f);
	}else if (rpm > CONFIG_MIN_RPM_EXIT_EBRAKE) {
		return -max_e_trq * 0.75f * (rpm - CONFIG_MIN_RPM_EXIT_EBRAKE)/((float)(1000 - CONFIG_MIN_RPM_EXIT_EBRAKE));
	}

	return 0.0f;
}