#pragma once

#include <crates/ulib/includes.hpp>
#include "lib.hpp"

template<typename T>
__ulib_inline static void lut_find_seg(const T *xs, int x_l, int x_r, T x, int &xi, int &xj) {
  xi = 0;
  if(x_l + 1 == x_r) {
    xj = 0;
    return;
  }
  while(xi + 1 + x_l < x_r && xs[xi + 1 + x_l] <= x) ++xi;
  xj = xi;
  if(xj + 1 + x_l < x_r) ++xj;
  else --xi;
}

template<typename T>
__ulib_inline static T interpolate(T f1, T f2, T x1, T x2, T x) {
  if(x1 == x2) return f1;
  return ((x2 - x) * f1 + (x - x1) * f2) / (x2 - x1);
}

template<typename T>
__ulib_inline static T lut_query(
	const T *lib_tables,
	LUTReference lutref,
  T x, T y)
{
	const T *tbl_x = lib_tables + lutref.data_start;
	const T *tbl_y = tbl_x + lutref.axis1;
	const T *tbl_d = tbl_y + lutref.axis2;
	int xi, xj, yi, yj;
	lut_find_seg(tbl_x, 0, lutref.axis1, x, xi, xj);
	lut_find_seg(tbl_y, 0, lutref.axis2, y, yi, yj);
#define FT(i, j) tbl_d[(i) * lutref.axis2 + (j)]
#define XT(i) tbl_x[i]
#define YT(i) tbl_y[i]
  T rxi = interpolate(FT(xi, yi), FT(xi, yj), YT(yi), YT(yj), y);
  T rxj = interpolate(FT(xj, yi), FT(xj, yj), YT(yi), YT(yj), y);
  T r = interpolate(rxi, rxj, XT(xi), XT(xj), x);
  return r;
#undef FT
#undef XT
#undef YT
}