#include "os/os_types.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"

#define RPM_I_MAX MAX_SPD_POINTS
#define TRQ_I_MAX MAX_TRQ_POINTS
#define RPM_INTVAL TBL_SPD_INTVAL
#define IDX2RPM(I) ((I)*RPM_INTVAL + RPM_INTVAL)

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

static trq2dq_t **table_map = NULL;
//x -> rpm
//z -> torque
static void intp_line2(float frac_x, float z, trq2dq_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(float 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;
	}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) {
		trq2dq_table_t *table = nv_get_trq2dq_table();
		if (table != NULL) {
			table_map = (trq2dq_t **)&table->tdq[0][0];
		}
	}
}

void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
	if (table_map == NULL) {
		step_towards(&dq_out->d, 0, 1.0f);
		dq_out->q = torque;
		return;
	}

	int low = 0, high = 0;

	low = rpm / RPM_INTVAL - 1;

	if (low >= RPM_I_MAX) {
		low = RPM_I_MAX - 1;
	}
	if (low < 0)  {
		high = low = 0;
	}else if (low == (RPM_I_MAX-1)) {
		high = low;
	}else {
		high = low + 1;
	}

	_DEBUG("speed %d-%d\n", low, high);

	int low_left = TRQ_I_MAX - 1, low_right = TRQ_I_MAX - 1;
	get_torque_range(torque, low, TRQ_I_MAX, &low_left, &low_right);
	_DEBUG("low speed torque %d-%d\n", low_left, low_right);
	
	int high_left = TRQ_I_MAX - 1, high_right = TRQ_I_MAX - 1;
	get_torque_range(torque, high, TRQ_I_MAX, &high_left, &high_right);
	_DEBUG("high speed torque %d-%d\n", high_left, high_right);
	trq2dq_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;
	int x1 = IDX2RPM(low);
	int x2 = IDX2RPM(high);
	if (x1 != x2) {
		frac_x = (float)(rpm - x1)/(x2 - x1);
	}
	intp_line2(frac_x, torque, maps, &d, &q);
	step_towards(&dq_out->d, d, 1.0f);
	dq_out->q = q;
}