#include "kit_table.h"


//根据数据类型(1字节 2字节 4字节)来查表
KitResult kit_table_get_data(TabItem* item, uint8_t idx, int32_t * value)
{
	KitResult res = kKitResult_ParamErr;

	if ((item != NULL) && (item->tab != NULL) && (idx < item->len))
	{
		if (item->type == kTabDataType_8Bit)
		{
			*value = ((int8_t *)item->tab)[idx];
		}
		else if (item->type == kTabDataType_16Bit)
		{
			*value = ((int16_t*)item->tab)[idx];
		}
		else
		{
			*value = ((int32_t*)item->tab)[idx];
		}
		res = kKitResult_Ok;
	}
	return res;
}

//根据数据类型(1字节 2字节 4字节)来写数据
KitResult kit_table_set_data(TabItem* item, uint8_t idx, int32_t value)
{
	KitResult res = kKitResult_ParamErr;

	if ((item != NULL) && (item->tab != NULL) && (idx < item->len))
	{
		if (item->type == kTabDataType_8Bit)
		{
			((int8_t*)item->tab)[idx] = value;
		}
		else if (item->type == kTabDataType_16Bit)
		{
			((int16_t*)item->tab)[idx] = value;
		}
		else
		{
			((int32_t*)item->tab)[idx] = value;
		}
		res = kKitResult_Ok;
	}

	return res;
}

//返回kKitResult_OutRange 数据超出上下限
KitResult kit_table_search_index(TabItem *item, int32_t input_value, uint8_t * left_idx, uint8_t * right_idx)
{
	KitResult res = kKitResult_ParamErr;
	uint8_t  end;
	int32_t i, v1, v2;
	if ((item != NULL) && (item->len > 1))
	{
		end = item->len - 1;
		//查找两端数据
		if ((kit_table_get_data(item, 0, &v1) == kKitResult_Ok)
		 && (kit_table_get_data(item, end, &v2) == kKitResult_Ok))
		{
			if (((v1 < v2) && (input_value < v1)) || ((v1 > v2) && (input_value > v1)))
			{
				*left_idx = *right_idx = 0;
				res = kKitResult_OutRange;
			}
			else if (((v1 < v2) && (input_value > v2)) || ((v1 > v2) && (input_value < v2)))
			{
				*left_idx = *right_idx = end;
				res = kKitResult_OutRange;
			}
			else
			{
				*left_idx = 0;
				*right_idx = 1;
				for (i = 0; i < end; i++)
				{
					kit_table_get_data(item, i, &v1);
					kit_table_get_data(item, i + 1, &v2);
					if ((input_value >= v1 && input_value <= v2) || (input_value <= v1 && input_value >= v2))
					{
						*left_idx = i;
						*right_idx = i + 1;
						break;
					}
				}
				res = kKitResult_Ok;
			}
		}
	}
	return res;
}



KitResult kit_table_linear_search(TwoDTabItem *item, int32_t input_value, uint32_t factor, int32_t *output_value)
{
	KitResult res = kKitResult_ParamErr;
	uint8_t left_idx, right_idx;
	int32_t dat_x, dat_y, x_left_value, x_right_value, y_left_value, y_right_value;

	*output_value = 0;
	if ((item != NULL) && (item->x.tab != NULL) && (item->y.tab != NULL))
	{			
		res = kit_table_search_index(&item->x, input_value, &left_idx, &right_idx);

		kit_table_get_data(&item->y, left_idx, &y_left_value);
		kit_table_get_data(&item->y, right_idx, &y_right_value);

		if((res == kKitResult_OutRange) || (y_left_value == y_right_value))
		{
			*output_value = y_left_value * factor;
		}
		else
		{
			kit_table_get_data(&item->y, left_idx, &y_left_value);
			kit_table_get_data(&item->y, right_idx, &y_right_value);

			kit_table_get_data(&item->x, left_idx, &x_left_value);
			kit_table_get_data(&item->x, right_idx, &x_right_value);

			dat_x = x_right_value - x_left_value;
			dat_y = y_right_value - y_left_value;

			*output_value = ((int64_t)dat_y * factor * ((int32_t)input_value - x_left_value) / dat_x) + y_left_value * factor;

			res = kKitResult_Ok;
		}
	}
	return res;
}


KitResult kit_table_bilinear_search(ThreeDTabItem *item, int32_t x_input, int32_t y_input, int32_t *z_output)
{
	KitResult res = kKitResult_Ok;
	uint8_t x_left_idx, x_right_idx, y_left_idx, y_right_idx;
	int32_t data_x1_y1, data_x1_y2, data_x2_y1, data_x2_y2;
	int32_t x1, x2, y1, y2, dat_x;


	res &= kit_table_search_index(&item->x, x_input, &x_left_idx, &x_right_idx);
	res &= kit_table_search_index(&item->y, y_input, &y_left_idx, &y_right_idx);

	if (res == kKitResult_Ok)
	{
		kit_table_get_data(&item->z, item->x.len * y_left_idx + x_left_idx, &data_x1_y1);
		kit_table_get_data(&item->z, item->x.len * y_right_idx + x_left_idx, &data_x1_y2);
		kit_table_get_data(&item->z, item->x.len * y_left_idx + x_right_idx, &data_x2_y1);
		kit_table_get_data(&item->z, item->x.len * y_right_idx + x_right_idx, &data_x2_y2);

		kit_table_get_data(&item->x, x_left_idx, &x1);
		kit_table_get_data(&item->x, x_right_idx, &x2);
		kit_table_get_data(&item->y, y_left_idx, &y1);
		kit_table_get_data(&item->y, y_right_idx, &y2);

		dat_x = (x2 - x1) * (y2 - y1);

		*z_output = data_x1_y1 * (x2 - (int32_t)x_input) * (y2 - (int32_t)y_input) + data_x2_y1 * ((int32_t)x_input - x1) * (y2 - (int32_t)y_input)
			+ data_x1_y2 * (x2 - (int32_t)x_input) * ((int32_t)y_input - y1) + data_x2_y2 * ((int32_t)x_input - x1) * ((int32_t)y_input - y1);

		*z_output = (*z_output + (dat_x >> 1)) / dat_x;
	}

	return res;
}

KitResult kit_table_bilinear_search_by_yz(ThreeDTabItem* item, int32_t y_input, int32_t z_input, uint32_t factor, int32_t* x_output)
{
	KitResult res = kKitResult_ParamErr;
	uint8_t y_left_idx, y_right_idx, value[200];
	int32_t i, tmp, dat_x, dat_y;
	int32_t x_left_value, x_right_value, y_left_value, y_right_value;
	TwoDTabItem table;

	if (item != NULL)
	{
		res = kit_table_search_index(&item->y, y_input, &y_left_idx, &y_right_idx);

		if ((res == kKitResult_Ok) || (res == kKitResult_OutRange))
		{
			//创建二维表，x轴数据为拟合出来新数据，y轴为原来三维表的x轴数据
			table.x.len = item->x.len;
			table.x.tab = (uint8_t*)value;
			table.x.type = item->z.type;

			table.y.len = item->x.len;
			table.y.tab = item->x.tab;
			table.y.type = item->x.type;

			kit_table_get_data(&item->y, y_left_idx, &y_left_value);
			kit_table_get_data(&item->y, y_right_idx, &y_right_value);

			dat_x = y_input - y_left_value;
			//数据不用插值，直接取
			if ((dat_x == 0) || (res == kKitResult_OutRange))
			{
				for (i = 0; i < item->x.len; i++)
				{
					kit_table_get_data(&item->z, item->x.len * y_left_idx + i, &x_left_value);
					kit_table_set_data(&table.x, i, x_left_value);
				}
			}
			//数据不用插值，直接取
			else if (y_input == y_right_value)
			{
				for (i = 0; i < item->x.len; i++)
				{
					kit_table_get_data(&item->z, item->x.len * y_right_idx + i, &x_left_value);
					kit_table_set_data(&table.x, i, x_left_value);
				}
			}
			else
			{
				dat_y = y_right_value - y_left_value;
				for (i = 0; i < item->x.len; i++)
				{
					kit_table_get_data(&item->z, item->x.len * y_left_idx + i, &x_left_value);
					kit_table_get_data(&item->z, item->x.len * y_right_idx + i, &x_right_value);

					tmp = (x_right_value - x_left_value) * dat_x / dat_y + x_left_value;
					kit_table_set_data(&table.x, i, tmp);
				}
			}

            res = kit_table_linear_search(&table, z_input, factor, x_output);
		}
	}

	return res;
}