// SPDX-FileCopyrightText: Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
// SPDX-License-Identifier: BSD-3-Clause

#include "vtkDataArray.h"
#include "vtkImageData.h"
#include "vtkImageImport.h"
#include "vtkImageInterpolator.h"
#include "vtkMathUtilities.h"
#include "vtkMatrix3x3.h"
#include "vtkNew.h"
#include "vtkPointData.h"

#include <iostream>

// Test functions
namespace
{

// Print "<text>: (a, b, c) != (x, y, z)"
template <class T>
void PrintError(const char* text, const T* x, const T* y, int size)
{
  std::cout << text << " ";

  const T* v = x;
  for (int j = 0; j < 2; ++j)
  {
    if (j != 0)
    {
      std::cout << " != ";
    }

    const char* delim = "";
    std::cout << "(";
    for (int i = 0; i < size; ++i)
    {
      std::cout << delim << v[i];
      delim = ", ";
    }
    std::cout << ")";
    v = y;
  }

  std::cout << std::endl;
}

// Compare vectors with a small tolerance
template <class T>
bool CompareVectorFuzzy(const char* text, const T* x, const T* y, int size, T tol)
{
  for (int i = 0; i < size; ++i)
  {
    if (!vtkMathUtilities::FuzzyCompare(x[i], y[i], tol))
    {
      PrintError(text, x, y, size);
      return false;
    }
  }

  return true;
}

bool TestImageNoDirection()
{
  std::cout << "Testing with no image direction:" << std::endl;

  // basic image information
  constexpr double origin[3] = { 5.1234165, -12.09375, 0.857643 };
  constexpr double spacing[3] = { 5.0, 2.0, 3.0 };
  int extent[6] = { 0, 19, 0, 49, 0, 29 };

  // the input image
  vtkNew<vtkImageData> input;
  input->SetSpacing(spacing);
  input->SetOrigin(origin);
  input->SetExtent(extent);
  input->AllocateScalars(VTK_UNSIGNED_CHAR, 3);
  // set the data values to index times spacing for testing
  vtkDataArray* pixels = input->GetPointData()->GetScalars();
  vtkIdType idx = 0;
  for (int k = extent[4]; k <= extent[5]; ++k)
  {
    for (int j = extent[2]; j <= extent[3]; ++j)
    {
      for (int i = extent[0]; i <= extent[1]; ++i)
      {
        pixels->SetTuple3(idx++, i * spacing[0], j * spacing[1], k * spacing[2]);
      }
    }
  }

  vtkNew<vtkImageInterpolator> interpolator;
  interpolator->Initialize(input);
  interpolator->SetInterpolationModeToLinear();
  interpolator->SetOutValue(255);

  bool success = true;

  double points[20][3] = {
    // random points within image bounds
    { 80.6616917, 16.2843800, 58.7851199 },
    { 62.5605082, 47.4324900, 51.7575806 },
    { 90.4047476, -5.3094740, 9.7738484 },
    { 87.2008769, 14.7361779, 45.2379583 },
    { 44.8079918, 33.3200863, 87.2547544 },
    { 11.4262999, 53.5768517, 87.0814281 },
    { 64.8173899, 2.8272480, 45.1864982 },
    { 90.5444488, 22.7720092, 70.7924360 },
    { 97.1644100, 66.8605872, 54.7898647 },
    { 44.9673259, 60.1069581, 44.9178906 },
    // some out-of-bounds points
    { 88.9490496, 55.7590724, 89.1766919 },
    { 103.9643647, 50.7450499, 79.7674468 },
    { 97.7313546, 34.6269689, 88.6995403 },
    { 101.9604439, 46.7481909, 67.1910364 },
    { 77.4775538, 84.4625972, -1.9124923 },
    { 103.5592146, 82.3749221, 10.9448482 },
    { 102.0220226, 3.3229726, 38.7948792 },
    { 49.7995352, 72.2975327, 0.6357556 },
    { 40.6762731, 57.7901959, 88.4700388 },
    { 0.8091914, -5.9155755, 39.3323177 },
  };

  for (int i = 0; i < 20; ++i)
  {
    // expected value if point is out of bounds
    double expectedValue[3] = { 255.0, 255.0, 255.0 };
    if (i < 10)
    {
      // expected interpolated value if point is within bounds
      expectedValue[0] = points[i][0] - origin[0];
      expectedValue[1] = points[i][1] - origin[1];
      expectedValue[2] = points[i][2] - origin[2];
    }

    double value[3];
    bool inBounds = interpolator->Interpolate(points[i], value);
    success &= CompareVectorFuzzy("Value:", value, expectedValue, 3, 1e-4);
    if (!inBounds && i < 10)
    {
      std::cout << "Point " << i << " incorrectly marked out-of-bounds\n";
      success = false;
    }
    else if (inBounds && i >= 10)
    {
      std::cout << "Point " << i << " incorrectly marked in-bounds\n";
      success = false;
    }
  }

  if (success)
  {
    std::cout << "Success!" << std::endl;
  }

  return success;
}

bool TestImageWithDirection()
{
  std::cout << "Testing with image direction:" << std::endl;

  // basic image information
  constexpr double origin[3] = { 5.1234165, -12.09375, 0.857643 };
  constexpr double direction[9] = {
    // 3x3 matrix
    -0.5618556200580342, -0.34610201570703625, 0.7513532171573689, // 1st row
    0.826672975724893, -0.2014274000446417, 0.525393941454797,     // 2nd row
    -0.030496777130583796, 0.9163189385987733, 0.39928629997767906 // 3rd row
  };
  constexpr double spacing[3] = { 5.0, 2.0, 3.0 };
  int extent[6] = { 0, 19, 0, 49, 0, 29 };

  // the direction matrix
  vtkNew<vtkMatrix3x3> matrix;
  matrix->DeepCopy(direction);
  vtkNew<vtkMatrix3x3> matrixInverse;
  vtkMatrix3x3::Invert(matrix, matrixInverse);

  // the input image
  vtkNew<vtkImageData> input;
  input->SetSpacing(spacing);
  input->SetDirectionMatrix(matrix);
  input->SetOrigin(origin);
  input->SetExtent(extent);
  input->AllocateScalars(VTK_FLOAT, 3);
  // set the data values to index times spacing for testing
  vtkDataArray* pixels = input->GetPointData()->GetScalars();
  vtkIdType idx = 0;
  for (int k = extent[4]; k <= extent[5]; ++k)
  {
    for (int j = extent[2]; j <= extent[3]; ++j)
    {
      for (int i = extent[0]; i <= extent[1]; ++i)
      {
        pixels->SetTuple3(idx++, i * spacing[0], j * spacing[1], k * spacing[2]);
      }
    }
  }

  vtkNew<vtkImageInterpolator> interpolator;
  interpolator->Initialize(input);
  interpolator->SetInterpolationModeToLinear();

  bool success = true;

  double points[20][3] = {
    // random points within image bounds
    { 2.8462919, -8.5164647, 12.7463881 },
    { 9.9924921, 72.6033040, 61.1140928 },
    { -31.8801448, 59.3476671, 43.7503173 },
    { 39.0528309, 15.6922992, 109.4925365 },
    { -60.8563933, 50.6743971, 92.7543887 },
    { -39.5351161, 64.8472584, 89.8646631 },
    { -26.7609181, 33.8420170, 31.1569328 },
    { -18.8116334, 76.8430662, 79.6248037 },
    { 17.6741224, 57.6621854, 94.5859167 },
    { -11.5610305, 90.3415364, 61.0285089 },
    // some out-of-bounds points
    { 34.5208733, 66.5281775, 62.0378498 },
    { 39.0771521, 12.3222314, 61.3746313 },
    { 60.0838912, 46.9133954, 44.1492156 },
    { -22.9413822, 72.5683548, 116.1745177 },
    { -0.5906894, 91.1229086, 94.1050301 },
    { -22.6683535, 37.8556764, 110.2892534 },
    { -8.0201237, 93.3770987, 17.0129858 },
    { 35.5092073, 50.1582868, 80.5899676 },
    { 29.7921164, 30.2326700, 17.7492048 },
    { -21.4728238, 90.5506128, 54.2677831 },
  };

  for (int i = 0; i < 20; ++i)
  {
    // expected value if point is out of bounds
    double expectedValue[3] = { 0.0, 0.0, 0.0 };
    if (i < 10)
    {
      // expected interpolated value if point is in bounds,
      // pixel values are equal to image coordinate with no
      // orientation (direction) or offset (origin)
      expectedValue[0] = points[i][0] - origin[0];
      expectedValue[1] = points[i][1] - origin[1];
      expectedValue[2] = points[i][2] - origin[2];
      matrixInverse->MultiplyPoint(expectedValue, expectedValue);
    }

    double value[3];
    bool inBounds = interpolator->Interpolate(points[i], value);
    success &= CompareVectorFuzzy("Value:", value, expectedValue, 3, 1e-4);
    if (!inBounds && i < 10)
    {
      std::cout << "Point " << i << " incorrectly marked out-of-bounds\n";
      success = false;
    }
    else if (inBounds && i >= 10)
    {
      std::cout << "Point " << i << " incorrectly marked in-bounds\n";
      success = false;
    }
  }

  if (success)
  {
    std::cout << "Success!" << std::endl;
  }

  return success;
}

bool TestInterpolateRow()
{
  std::cout << "Test interpolating a row of pixels:" << std::endl;

  // basic image information
  constexpr double origin[3] = { 0.0, 0.0, 0.0 };
  constexpr double spacing[3] = { 1.0, 1.0, 1.0 };
  int extent[6] = { 0, 1, 2, 3, 0, 9 };

  // the input image
  vtkNew<vtkImageData> input;
  input->SetSpacing(spacing);
  input->SetOrigin(origin);
  input->SetExtent(extent);
  input->AllocateScalars(VTK_FLOAT, 3);

  // set the data values to index times spacing for testing
  vtkDataArray* pixels = input->GetPointData()->GetScalars();
  vtkIdType idx = 0;
  for (int k = extent[4]; k <= extent[5]; ++k)
  {
    for (int j = extent[2]; j <= extent[3]; ++j)
    {
      for (int i = extent[0]; i <= extent[1]; ++i)
      {
        pixels->SetTuple3(idx++, i * spacing[0], j * spacing[1], k * spacing[2]);
      }
    }
  }

  constexpr double matrix[16] = {
    // matrix exchanges x and z
    0.0, 0.0, 0.25, 0.0, //
    0.0, 0.25, 0.0, 0.0, //
    0.25, 0.0, 0.0, 0.0, //
    0.0, 0.0, 0.0, 1.0,  //
  };
  constexpr int outExtent[6] = { -2, 37, 0, 15, 2, 2 };

  vtkNew<vtkImageInterpolator> interpolator;
  interpolator->SetInterpolationModeToCubic();
  interpolator->Initialize(input);

  double interpolatedData[114];
  int checkExtent[6];
  vtkInterpolationWeights* weights;
  interpolator->PrecomputeWeightsForExtent(matrix, outExtent, checkExtent, weights);
  interpolator->InterpolateRow(weights, -1, 7, 2, interpolatedData, 38);
  interpolator->FreePrecomputedWeights(weights);

  bool success = true;
  constexpr int expectedCheck[6] = { 0, 36, 8, 12, 2, 2 };
  constexpr double expectedData[114] = {
    // 3 components per pixel
    0.500000, 1.929688, -0.070312, //
    0.500000, 1.929688, 0.000000,  //
    0.500000, 1.929688, 0.179688,  //
    0.500000, 1.929688, 0.437500,  //
    0.500000, 1.929688, 0.726562,  //
    0.500000, 1.929688, 1.000000,  //
    0.500000, 1.929688, 1.250000,  //
    0.500000, 1.929688, 1.500000,  //
    0.500000, 1.929688, 1.750000,  //
    0.500000, 1.929688, 2.000000,  //
    0.500000, 1.929688, 2.250000,  //
    0.500000, 1.929688, 2.500000,  //
    0.500000, 1.929688, 2.750000,  //
    0.500000, 1.929688, 3.000000,  //
    0.500000, 1.929688, 3.250000,  //
    0.500000, 1.929688, 3.500000,  //
    0.500000, 1.929688, 3.750000,  //
    0.500000, 1.929688, 4.000000,  //
    0.500000, 1.929688, 4.250000,  //
    0.500000, 1.929688, 4.500000,  //
    0.500000, 1.929688, 4.750000,  //
    0.500000, 1.929688, 5.000000,  //
    0.500000, 1.929688, 5.250000,  //
    0.500000, 1.929688, 5.500000,  //
    0.500000, 1.929688, 5.750000,  //
    0.500000, 1.929688, 6.000000,  //
    0.500000, 1.929688, 6.250000,  //
    0.500000, 1.929688, 6.500000,  //
    0.500000, 1.929688, 6.750000,  //
    0.500000, 1.929688, 7.000000,  //
    0.500000, 1.929688, 7.250000,  //
    0.500000, 1.929688, 7.500000,  //
    0.500000, 1.929688, 7.750000,  //
    0.500000, 1.929688, 8.000000,  //
    0.500000, 1.929688, 8.273438,  //
    0.500000, 1.929688, 8.562500,  //
    0.500000, 1.929688, 8.820312,  //
    0.500000, 1.929688, 9.000000,  //
  };

  std::cout << "CheckExtent:";
  for (int i = 0; i < 6; i++)
  {
    std::cout << " " << checkExtent[i];
  }

  // check extent should be input extent divided by matrix
  if (checkExtent[0] != expectedCheck[0] || checkExtent[1] != expectedCheck[1] ||
    checkExtent[2] != expectedCheck[2] || checkExtent[3] != expectedCheck[3] ||
    checkExtent[4] != expectedCheck[4] || checkExtent[5] != expectedCheck[5])
  {
    std::cout << " !=";
    for (int i = 0; i < 6; i++)
    {
      std::cout << " " << expectedCheck[i];
    }
    success = false;
  }
  std::cout << std::endl;

  std::ios_base::fmtflags savedFlags = std::cout.flags();
  std::cout << "OutputData:" << std::endl;
  std::cout << std::fixed << std::setprecision(6);
  for (int i = 0; i < 38; i++)
  {
    std::cout << "    "                              //
              << interpolatedData[3 * i] << ", "     //
              << interpolatedData[3 * i + 1] << ", " //
              << interpolatedData[3 * i + 2] << ","; //

    bool different = false;
    for (int j = 0; j < 3; j++)
    {
      if (!vtkMathUtilities::FuzzyCompare(
            interpolatedData[3 * i + j], expectedData[3 * i + j], 1e-6))
      {
        different = true;
        break;
      }
    }

    if (different)
    {
      std::cout << " // expected: "                //
                << expectedData[3 * i] << ", "     //
                << expectedData[3 * i + 1] << ", " //
                << expectedData[3 * i + 2];        //
      success = false;
    }
    std::cout << std::endl;
  }
  std::cout.flags(savedFlags);

  if (success)
  {
    std::cout << "Success!" << std::endl;
  }

  return success;
}
}

// Driver Function
int ImageInterpolator(int, char*[])
{
  bool success = true;

  success &= TestImageNoDirection();
  success &= TestImageWithDirection();
  success &= TestInterpolateRow();

  return success ? EXIT_SUCCESS : EXIT_FAILURE;
}