#include "Interpolation.hpp"
#include "Common.hpp"

namespace Math {

float linear_interpolation(Vector<2> val, float left, float right, float pos) {
    return val.x() + (pos - left) * (val.y() - val.x()) / (right - left);
}

float bilinear_interpolation(Matrix<2, 2> val, GridCellBoundaries cell, Vector<2> pos) {
    auto r1 = linear_interpolation(val.row(0), cell.x1, cell.x2, pos.x());
    auto r2 = linear_interpolation(val.row(1), cell.x1, cell.x2, pos.x());

    return linear_interpolation({r1, r2}, cell.y1, cell.y2, pos.y());
}

float trilinear_interpolation(Matrix<2, 2> val_front, Matrix<2, 2> val_back, const CubeCellBoundaries& cell, Vector<3> pos) {
    auto r1 = bilinear_interpolation(val_front, cell.grid_cell(), {pos.x(), pos.y()});
    auto r2 = bilinear_interpolation(val_back, cell.grid_cell(), {pos.x(), pos.y()});

    return linear_interpolation({r1, r2}, cell.z1, cell.z2, pos.z());
}

}