#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <vector>

#include "base/helper.hpp"
#include "base/index.hpp"
#include "base/interpolate_polynomial.hpp"
#include "base/vector.hpp"
#include "grid/grid.hpp"

using namespace VMG;

InterpolatePolynomial::InterpolatePolynomial(const unsigned int& degree) :
  deg_1(degree+1),
  pos_begin(0.0)
{
  coeff = new vmg_float[Helper::intpow(deg_1, 3)];
}

InterpolatePolynomial::~InterpolatePolynomial()
{
  delete [] coeff;
}

void InterpolatePolynomial::ComputeCoefficients(const Grid& grid, const Index& index)
{
  Index i;

  const Index begin = index - (static_cast<int>(deg_1)-1)/2 - 1;

  h = grid.Extent().MeshWidth();

  for (i[0]=0; i[0]<deg_1; ++i[0])
    for (i[1]=0; i[1]<deg_1; ++i[1])
      for (i[2]=0; i[2]<deg_1; ++i[2])
        _access_coeff(i) = grid.GetVal(i+begin);

  pos_begin = grid.Extent().Begin()
    + (begin - grid.Local().Begin() + grid.Global().LocalBegin()) * grid.Extent().MeshWidth();

  // compute coefficients x-direction
  for (i=0; i[1]<deg_1; ++i[1])
    for (i[2]=0; i[2]<deg_1; ++i[2])
      _compute_coefficients_1d(i, 0);

  // compute coefficients y-direction
  for (i=0; i[0]<deg_1; ++i[0])
    for (i[2]=0; i[2]<deg_1; ++i[2])
      _compute_coefficients_1d(i, 1);

  // compute coefficients z-direction
  for (i=0; i[0]<deg_1; ++i[0])
    for (i[1]=0; i[1]<deg_1; ++i[1])
      _compute_coefficients_1d(i, 2);
}

void InterpolatePolynomial::_compute_coefficients_1d(const Index& index, const unsigned int& direction)
{
  vmg_float temp_array[deg_1];
  vmg_float power = 1.0;
  unsigned long faculty = 1;
  unsigned int c;
  Index i;

  for (i=index, c=0; c<deg_1; ++i[direction], ++c)
    temp_array[c] = _access_coeff(i);

  i=index;
  ++i[direction];
  for (c=1; c<deg_1; ++i[direction], ++c) {
    for (unsigned int j=0; j<deg_1-c; ++j)
      temp_array[j] = temp_array[j+1] - temp_array[j];
    faculty *= c;
    power *= h[direction];
    _access_coeff(i) = temp_array[0] / (faculty*power);
  }
}

vmg_float InterpolatePolynomial::Evaluate(const Vector& pos)
{
  vmg_float result = 0.0;
  Vector prod, offset;
  Index i;

  prod[0] = 1.0;
  offset[0] = pos_begin[0];
  for (i[0]=0; i[0]<deg_1; ++i[0]) {
    prod[1] = prod[0];
    offset[1] = pos_begin[1];
    for (i[1]=0; i[1]<deg_1; ++i[1]) {
      prod[2] = prod[1];
      offset[2] = pos_begin[2];
      for (i[2]=0; i[2]<deg_1; ++i[2]) {
        result += _access_coeff(i) * prod[2];
        prod[2] *= pos[2] - offset[2];
        offset[2] += h[2];
      }
      prod[1] *= pos[1] - offset[1];
      offset[1] += h[1];
    }
    prod[0] *= pos[0] - offset[0];
    offset[0] += h[0];
  }

  return result;
}

Vector InterpolatePolynomial::EvaluateGradient(const Vector& pos)
{
  Vector buffer[deg_1];
  std::vector<Vector> buffer_diff[deg_1-1];
  Vector offset, result;

  buffer[0] = 1.0;
  offset = pos - pos_begin;
  for (int i=1; i<deg_1; ++i) {
    buffer[i] = buffer[i-1] * offset;
    offset += h;
  }

  offset = pos - pos_begin;
  for (int i=0; i<deg_1-1; ++i) {
    buffer_diff[i].resize(i+1);
    offset += h;
    for (int j=0; j<i; ++j)
      buffer_diff[i][j] = buffer_diff[i-1][j] * offset;
    buffer_diff[i][i] = buffer[i+1];
  }

  for (int i=0; i<deg_1; ++i)
    for (int j=0; j<deg_1; ++j)
      for (int k=0; k<deg_1-1; ++k) {
	Vector sum;
	for (int l=0; l<=k; ++l)
	  sum += buffer_diff[k][l];
	result[0] += _access_coeff(k+1, i, j) * buffer[i][1] * buffer[j][2] * sum[0];
	result[1] += _access_coeff(i, k+1, j) * buffer[i][0] * buffer[j][2] * sum[1];
	result[2] += _access_coeff(i, j, k+1) * buffer[i][0] * buffer[j][1] * sum[2];
      }

  return result;
}