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

#if defined(CONFIG_MOTOR_TORQUE_CONF)
#include 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
#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;
			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

vol_rpm_torque_map vol_rpm_curr_map[CONFIG_MAX_VOL_COUNT];
u8 vol_n = CONFIG_MAX_VOL_COUNT;
static void motor_vol_lookup(s16 vol, vol_rpm_torque_map *table, vol_rpm_torque_map **out_mapl, vol_rpm_torque_map **out_maph) {
	if (vol_n == 1) {
		*out_mapl = table;
        *out_maph = table;
		return;
	}
	if (vol <= table[0].vol) {
		*out_mapl = table;
        *out_maph = table + 1;
		return;
	}else if (vol >= table[vol_n - 1].vol){
		*out_mapl = table + vol_n - 2;
        *out_maph = table + vol_n - 1;
		return;
	}
	for (int i = 1; i < vol_n; i++) {
		if ((vol <= table[i].vol)) {
			*out_mapl = table + i - 1;
			*out_maph = table + i;
			break;
		}
	}
}

static void motor_rpm_lookup(s16 rpm, vol_rpm_torque_map *map, rpm_torque_map **out_map1, rpm_torque_map **out_maph) {
	rpm_torque_map *table = map->rpm_torque;
	if (rpm <= table[0].rpm) {
		*out_map1 = table;
        *out_maph = table + 1;
		return;
	}else if (rpm >= table[map->n - 1].rpm) {
		*out_map1 = *out_maph = table + map->n - 1;
		return;
	}
	for (int i = 1; i < map->n; i++) {
		if (rpm <= table[i].rpm) {
			*out_map1 = table + i - 1;
			*out_maph = table + i;
			break;
		}
	}
}

static void motor_current_lookup(s16 torque, rpm_torque_map *map,  torque2dq_t**currl, torque2dq_t **currh) {
	torque2dq_t *table = map->dqmap;
	if (torque <= table[0].torque) {
		*currl = table;
        *currh = table + 1;
	}else if (torque >= table[map->n - 1].torque) {
		*currl = table + map->n - 2;
        *currh = table + map->n - 1;
	}
	for (int i = 1; i < map->n; i++) {
		if (torque <= table[i].torque) {
			*currl = table + i - 1;
			*currh = table + i;
			break;
		}
	}
}

static void motor_torque_lookup(s16 vel, s16 torque, rpm_torque_map *mapl, rpm_torque_map *maph, dq_t *dq_out) {
	torque2dq_t *currl, *currh;
	motor_current_lookup(torque, mapl, &currl, &currh);
	float dl = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", mapl->rpm, currl->torque, currh->torque, dl);
	motor_current_lookup(torque, maph, &currl, &currh);
	float dh = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, dh);
	float d = line_intp(vel, mapl->rpm, maph->rpm, dl, dh);
    _DEBUG("id = %f\n", d);

	float ql = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
    _DEBUG("rpm %d -> curr %d : %d, iq=%f\n", mapl->rpm, currl->torque, currh->torque, ql);
	motor_current_lookup(torque, maph, &currl, &currh);
	float qh = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, qh);
	float q = line_intp(vel, mapl->rpm, maph->rpm, ql, qh);
    _DEBUG("id = %f\n", d);
	dq_out->d = d;
	dq_out->q = q;
}

#define SCALE(X) (X/10)

void motor_mpta_fw_lookup2(float rpm, float vdc, float torque, dq_t *dq_out) {
	vol_rpm_torque_map *vmapl, *vmaph;
	rpm_torque_map *rmapl, *rmaph;
	dq_t dql, dqh;
	s16 vel = ABS(((s16)rpm));
	s16 bus_vol = vdc;
    torque = torque * 10;
	/* 通过当前电压查找 对应的转速扭矩 */
	motor_vol_lookup(bus_vol, vol_rpm_curr_map, &vmapl, &vmaph);
	_DEBUG("vol %d : %d\n", vmapl->vol, vmaph->vol);
	/* 通过当前转速查找对应的扭矩 */
	motor_rpm_lookup(vel, vmapl, &rmapl, &rmaph);
    _DEBUG("vol %d -> rpm %d : %d\n", vmapl->vol, rmapl->rpm, rmaph->rpm);
	/* 通过当前扭矩需求插值计算dq电流 */
	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dql);

	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);

	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dqh);

	float d = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.d, dqh.d);
	float q = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.q, dqh.q);

	dq_out->d = SCALE(d);
	dq_out->q = SCALE(q) * SIGN(torque);
}


/* 通过电机外特性曲线获取当前转速和母线电压下的最大扭矩 */
s16 motor_torque_ext_char_curve(s16 rpm, s16 vdc) {
	vol_rpm_torque_map *vmapl, *vmaph;
	rpm_torque_map *rmapl, *rmaph;
	s16 vel = ABS(rpm);
	/* 通过当前电压查找 对应的转速扭矩 */
	motor_vol_lookup(vdc, vol_rpm_curr_map, &vmapl, &vmaph);
	_DEBUG("vol %d : %d\n", vmapl->vol, vmaph->vol);
	/* 获取低电压对应当前转速的最大扭矩 */
	motor_rpm_lookup(vel, vmapl, &rmapl, &rmaph);
    _DEBUG("vol %d -> rpm %d : %d\n", vmapl->vol, rmapl->rpm, rmaph->rpm);
	s16 max_t1 = rmapl->dqmap[rmapl->n - 1].torque;
	s16 max_t2 = rmaph->dqmap[rmaph->n - 1].torque;
	s16 low_vel_max_t = line_intp(vel, rmapl->rpm, rmaph->rpm, max_t1, max_t2);

	/* 获取高电压对应当前转速的最大扭矩 */
	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);
	max_t1 = rmapl->dqmap[rmapl->n - 1].torque;
	max_t2 = rmaph->dqmap[rmaph->n - 1].torque;
	s16 high_vel_max_t = line_intp(vel, rmaph->rpm, rmaph->rpm, max_t1, max_t2);

	/* 对两个电压的扭矩插值，获取最终的最大扭矩 */
	return line_intp(vdc, vmapl->vol, vmaph->vol, low_vel_max_t, high_vel_max_t);
}

//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 = sqrtsub2_f(z, (*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;
		arm_sin_cos(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 = sqrtsub2_f(torque, d);
		if (torque < 0) {
			q = -q;
		}
	}else {
		if (ABS(rpm) < 1000) {
			d = 0;
		}else if (ABS(rpm) < 3000) {
			d = -10;
		}else if (ABS(rpm) < 5000) {
			d = -30;
		}else {
			d = -50;
		}
	}
#else
	float fw_start_duty = (float)mc_conf()->m.fw_duty_start/100.0f;
	if (mc_conf()->m.fw_enable && (mot_contrl()->duty_filterd >= fw_start_duty)) {
		d = -f_map(mot_contrl()->duty_filterd, fw_start_duty, CONFIG_SVM_MODULATION, 0, mc_conf()->m.max_fw_id);
	}
	q = torque;
#endif
 	step_towards(&dq_out->d, d, 10.0f);
	step_towards(&dq_out->q, q, 6.0f);
}

#endif


float motor_get_ebreak_toruqe(float rpm) {
	float max_e_trq = mot_contrl_get_ebrk_torque(&motor.controller);
	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;
}