#include "os/os_types.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/motor/motor_param.h"
#include "libs/logger.h"

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

extern torque_map_t table_map[14][10];
//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;                //两行插值
	
	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;
}

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 < table_map[index][0].torque) {
		low_right = low_left = 0;
		_DEBUG("---%d, %d--%d\n", z, table_map[index][0].torque, table_map[0][0].torque);
	}else if (z > table_map[index][max_index - 1].torque) {
		low_right = low_left = max_index - 1;
	}else {
		for (int i = 0; i < max_index; i++) {
			//_DEBUG("index %d, trq %d\n", i, table_map[index][i].torque);
			if (z >= table_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 trq2dq_lookup_init(void) {
	//if (table_map == NULL) {
		//table_map = get_torque2dq_maps();
	//}
}

void trq2dq_lookup(int rpm, s16 torque, DQ_t *dq_out) {
	//if (table_map == NULL) {
	//	step_towards(&dq_out->d, 0, 10.0f);
	//	dq_out->q = torque;
	//	return;
	//}
	if (torque < 0.0f) {
		dq_out->d = 0.0f;
		dq_out->q = torque;
		return;
	}

	int low = 0, high = 0;
	s16 x1 = 0, x2 = 0;
	rpm = motor_map_rpm_idx(rpm, &low, &high, &x1, &x2);

	_DEBUG("speed %d-%d, %d-%d\n", low, high, x1, x2);
	int max_trq_idx = motor_map_torque_max_count();
	int low_left = max_trq_idx - 1, low_right = max_trq_idx - 1;
	get_torque_range(torque, 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(torque, 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] = &table_map[low][low_left];
	maps[1] = &table_map[low][low_right];
	maps[2] = &table_map[high][high_left];
	maps[3] = &table_map[high][high_right];
	float frac_x = 0, d = 0, q = 0;
	if (x1 != x2) {
		frac_x = (float)(rpm - x1)/(x2 - x1);
	}
	intp_line2(frac_x, torque, maps, &d, &q);
	//step_towards(&dq_out->d, d, 10.0f);
	dq_out->d = d;
	dq_out->q = q;
}