#include "vtkPatchInterpolation.h"

#include "vtkDataArray.h"
#include "vtkDataArrayIteratorMacro.h"
#include "vtkDoubleArray.h"
#include "vtkIntArray.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include "vtkSmartPointer.h"

#include <math.h>

vtkStandardNewMacro(vtkPatchInterpolation);

vtkPatchInterpolation::vtkPatchInterpolation()
{

}

vtkPatchInterpolation::~vtkPatchInterpolation()
{

}

void vtkPatchInterpolation::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

/**\brief Interpolate \a r on a patch defined by \a dim, \a P_i, and \a degree.
  *
  * Given parametric coordinates \a r, append the world
  * coordinates for the given patch to the \a outputPt array.
  * The control point array \a P_i is ordered as a regular
  * grid of dimension \a dim whose sides have \a degree[i]
  * points along each dimension i.
  */
void vtkPatchInterpolation::InterpolateOnPatch(
  vtkDataArray* outputPt,
  int dim, vtkDataArray* P_i, int* degree, double* r)
{
  double output_pt[4] = {0.0, 0.0, 0.0, 0.0};

  if (P_i->GetNumberOfComponents() != 4)
    {
    std::cout
      << "Number of components: " << P_i->GetNumberOfComponents() << "\n";
    }

  switch (P_i->GetDataType())
    {
    vtkDataArrayIteratorMacro(
      P_i, this->EvalCoord(vtkDABegin, vtkDAEnd, dim, degree, r, output_pt));
    }

  std::cout
    << "Point Coordinates: " <<output_pt[0]
    << "\t" << output_pt[1]
    << "\t" << output_pt[2]
    << "\t" << output_pt[3]
    << "\n";

  switch (outputPt->GetNumberOfComponents())
    {
  case 3:
    outputPt->InsertNextTuple3(
      output_pt[0]/output_pt[3],
      output_pt[1]/output_pt[3],
      output_pt[2]/output_pt[3]);
    break;
  case 4:
    outputPt->InsertNextTuple4(
      output_pt[0], output_pt[1], output_pt[2], output_pt[3]);
    break;
  default:
    vtkErrorMacro(
      "Number of components should be 3 or 4, not "
      << outputPt->GetNumberOfComponents() << ".");
    break;
    }
}

/**\brief Evaluate the contribution of a control point to a world coordinate.
  *
  */
template<typename Iterator>
void vtkPatchInterpolation::EvalCoord(
  Iterator data_iter, const Iterator data_end,
  const int& dim, const int* const degree, const double* const r,
  double* coord)
{
  // idx keeps the current k for binomial n in each dimension
  // for example if idxs = {2, 1, 0}, it means we need to evaluate
  // the coefficient for control point P_{2, 1, 0}.
  // Hence we need to evaluate the following Bernstein polynomials
  // B_{2, degree[0]}(r[0]), B_{1, degree[1]}(r[1]), B_{0, degree[2]}(r[2])
  // We assume that control points P are stored as 
  // P_{0, 0, 0}, ..., P_{degree[0], 0, 0};
  // P_{0, 1, 0}, ..., P_{degree[0], 1, 0};
  // ...
  // P_{0, degree[1], 0}, ..., P_{degree[0], degree[1], 0};
  // ...
  // ...
  // P_{0, degree[1], degree[2]}, ..., P_{degree[0], degree[1], degree[2]};
  std::vector<int> idxs(dim, 0);
  double coef = 1.0;
  coord[0] = coord[1] = coord[2] = coord[3] = 0.0;

  while (data_iter != data_end)
    {
    coef = 1.0;
    double coef_per_dim = 0.0;
    for (size_t i = 0; i < dim; ++i)
      {
      this->EvalBernstein(degree[i], idxs[i], r[i], coef_per_dim);
      coef *= coef_per_dim;
      }

    coord[0] += (*data_iter) * coef;
    ++data_iter;
    coord[1] += (*data_iter) * coef;
    ++data_iter;
    coord[2] += (*data_iter) * coef;
    ++data_iter;
    coord[3] += (*data_iter) * coef;
    ++data_iter;

    for (int j = 0; j < dim; ++j)
      {
      ++idxs[j];
      if (idxs[j] < degree[j]+1)
        {
        break;
        }
      idxs[j] = 0;
      }
    }

  //for (; data_iter != data_end; ++data_iter)
  //{
  //    std::cout<<(*data_iter)<<"\n";
  //}
}

/**\brief Evaluate a Bernstein polynomial.
  *
  */
template<typename T_i, typename T_d>
void vtkPatchInterpolation::EvalBernstein(
  const T_i& n, const T_i& v, const T_d& x, T_d& b)
{
  b = (vtkMath::Factorial(n) / vtkMath::Factorial(v) / vtkMath::Factorial(n - v)) * pow(x, v) * pow(1 - x, n - v);
}
