/**** Includes ****/
#include "utils.h"
#include "interpolate.h"

using namespace util;

/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
uint16_t util::interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
{
	// validate axis length
	if(len_axis==0) return 0;			// Empty data set
	if(len_axis==1) return y_values[0];	// Only one data point

	uint16_t y;

	uint8_t i = find_interval_end_index(x, x_axis, len_axis);
	if(i==0)
	{
		//Less then start
		y = y_values[0];
	}
	else if(i==len_axis)
	{
		//More than end
		y = y_values[len_axis-1];
	}
	else
	{
		// Do interpolate
		y = interpolate(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
	}

	return y;
}

uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
{
	// validate axis length
	if((len_x_axis==0)&&(len_y_axis==0)) return 0;				// Empty data set
	if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0];	// Only one data point

	uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
	uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);

	// Check corners - easy answers
	if((ix==0)&&(iy==0))
	{
		return z_values[0];  //[0][0]  [Y][X]
	}
	else if((ix==len_x_axis)&&(iy==0))
	{
		return z_values[len_x_axis-1]; //[0][end]
	}
	else if((ix==0)&&(iy==len_y_axis))
	{
		uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
		return z_values[i]; //[end][0]
	}
	else if((ix==len_x_axis)&&(iy==len_y_axis))
	{
		uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
		return z_values[i];  //[end][end]
	};

	// Check boundaries - 1D interpolation
	if(ix==0)
    {
        // On ix=0 line
        uint16_t i = 0;
        uint16_t z0 = z_values[i];
        i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
        uint16_t z1 = z_values[i];
        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
    }
    else if(ix==len_x_axis)
    {
        // On ix=END line
        uint16_t i = len_x_axis-1;
        uint16_t z0 = z_values[i];
        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
        uint16_t z1 = z_values[i];
        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
    }
    else if(iy==0)
    {
        // On iy=0 line
        uint16_t i = 0;
        uint16_t z0 = z_values[i];
        i = len_x_axis-1;
        uint16_t z1 = z_values[i];
        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
    }
    else if(iy==len_y_axis)
    {
        // On iy=END line
        uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
        uint16_t z0 = z_values[i];
        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
        uint16_t z1 = z_values[i];
        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
    }

	// Do interpolation
	// Get axis values
	uint16_t x0 = x_axis[ix-1];
	uint16_t x1 = x_axis[ix];
	uint16_t y0 = y_axis[iy-1];
	uint16_t y1 = y_axis[iy];

	// Do y0 line calculation
	// Get z values at x0 and x1 points on y0 line
	uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
	uint16_t z0 = z_values[i];
	uint16_t z1 = z_values[i+1];
	// Interpolate z value on y0 line
	uint16_t zy0 = interpolate(x, x0, x1, z0, z1);

	// Do y1 line calculation
	// Get z values at x0 and x1 points on y1 line
	i = index2d_to_index1d(ix-1, iy, len_x_axis);
	z0 = z_values[i];
	z1 = z_values[i+1];
	// Interpolate z value on y0 line
	uint16_t zy1 = interpolate(x, x0, x1, z0, z1);

	// Do calculation in y axis on xz line
	return interpolate(y, y0, y1, zy0, zy1);
}

uint16_t  util::interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
{
	int32_t dy = (int32_t)y1 - (int32_t)y0;
	int32_t dx = (int32_t)x1 - (int32_t)x0;
	int32_t d = (int32_t)x - (int32_t)x0;

	int32_t y = dy * d;
	y /= dx;
	y += y0;

	return util::sat_cast(y);
}

uint8_t  util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
{
	for(uint8_t i=0; i<len_axis; i++)
	{
		if(val < axis_values[i]) return i;
		continue;
	}

	return len_axis;
}

uint16_t  util::index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
{
	return ((uint16_t)len_x * iy) + ix;
}

/**** Private function definitions ****/