/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         https://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include <iostream>

#include "itkCenteredAffineTransform.h"
#include "itkImage.h"

using MatrixType = itk::Matrix<double, 2, 2>;
using VectorType = itk::Vector<double, 2>;

namespace
{

void
PrintVector(const VectorType & v)
{
  for (unsigned int i = 0; i < VectorType::Dimension; ++i)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}

} // namespace

int
itkCenteredAffineTransformTest(int, char *[])
{

  int any = 0; // Any errors detected in testing?

  MatrixType matrix2;
  VectorType vector2;

  int i, j;

  /* FIXME: This code exercises most of the methods but doesn't
     actually check that the results are correct. */

  /* Create a 2D identity transformation and show its parameters */
  using Affine2DType = itk::CenteredAffineTransform<double, 2>;
  auto id2 = Affine2DType::New();
  matrix2 = id2->GetMatrix();
  vector2 = id2->GetOffset();
  std::cout << "Matrix from instantiating an identity transform:" << std::endl << matrix2;
  std::cout << "Vector from instantiating an identity transform:" << std::endl;
  PrintVector(vector2);

  /* Create and show a simple 2D transform from given parameters */
  matrix2[0][0] = 1;
  matrix2[0][1] = 2;
  matrix2[1][0] = 3;
  matrix2[1][1] = 4;
  vector2[0] = 5;
  vector2[1] = 6;

  auto aff2 = Affine2DType::New();
  auto inverse2 = Affine2DType::New();
  aff2->SetMatrix(matrix2);
  aff2->SetOffset(vector2);
  for (i = 0; i < 2; ++i)
  {
    for (j = 0; j < 2; ++j)
    {
      matrix2[i][j] = 0.0;
    }
    vector2[i] = 0.0;
  }
  std::cout << "Instantiation of a given 2D transform:" << std::endl;
  aff2->Print(std::cout);

  aff2->GetInverse(inverse2);
  std::cout << "Inverse matrix for the given transform:" << std::endl << inverse2->GetMatrix();

  /* Set parameters of a 2D transform */
  matrix2[0][0] = 6;
  matrix2[0][1] = 5;
  matrix2[1][0] = 4;
  matrix2[1][1] = 3;
  vector2[0] = 2;
  vector2[1] = 1;
  aff2->SetMatrix(matrix2);
  aff2->SetOffset(vector2);
  for (i = 0; i < 2; ++i)
  {
    for (j = 0; j < 2; ++j)
    {
      matrix2[i][j] = 0.0;
    }
    vector2[i] = 0.0;
  }
  matrix2 = aff2->GetMatrix();
  vector2 = aff2->GetOffset();
  std::cout << "Setting the matrix in an existing transform:" << std::endl << matrix2;
  std::cout << "Setting the offset in an existing  transform:" << std::endl;
  PrintVector(vector2);

  /* Try composition of two transformations */
  aff2->Compose(aff2);
  std::cout << "Result of a composition:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with a translation */
  VectorType trans;
  trans[0] = 1;
  trans[1] = 2;
  aff2->Translate(trans);
  std::cout << "Result of a translation:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with an isotropic scaling */
  aff2->Scale(.3, true);
  std::cout << "Result of isotropic scaling:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with an anisotropic scaling */
  VectorType scale;
  scale[0] = .3;
  scale[1] = .2;
  aff2->Scale(scale);
  std::cout << "Result of anisotropic scaling:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with a general N-D rotation */
  aff2->Rotate(0, 1, 0.57, true);
  std::cout << "Result of general rotation:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with a 2-D rotation */
  aff2->Rotate(0, 1, -0.57, true);
  std::cout << "Result of 2-D rotation:" << std::endl;
  aff2->Print(std::cout);

  /* Compose with a shear */
  aff2->Shear(1, 0, .2);
  std::cout << "Result of shear:" << std::endl;
  aff2->Print(std::cout);

  /* Transform a point */
  itk::Point<double, 2> u2, v2;
  u2[0] = 3;
  u2[1] = 5;
  v2 = aff2->TransformPoint(u2);
  std::cout << "Transform a point:" << std::endl << v2[0] << " , " << v2[1] << std::endl;

  // /* Back transform a point */
  // v2 = aff2->BackTransform(u2);
  // std::cout << "Back transform a point:" << std::endl
  // << v2[0] << " , " << v2[1] << std::endl;

  /* Transform a vnl_vector */
  vnl_vector_fixed<double, 2> x2, y2;
  x2[0] = 1;
  x2[1] = 2;
  y2 = aff2->TransformVector(x2);
  std::cout << "Transform a vnl_vector:" << std::endl << y2[0] << " , " << y2[1] << std::endl;

  // /* Back transform a vector */
  // y2 = aff2->BackTransform(x2);
  // std::cout << "Back transform a vnl_vector:" << std::endl
  // << y2[0] << " , " << y2[1] << std::endl;

  /* Transform a vector */
  itk::Vector<double, 2> u3, v3;
  u3[0] = 3;
  u3[1] = 5;
  v3 = aff2->TransformVector(u3);
  std::cout << "Transform a vector:" << std::endl << v3[0] << " , " << v3[1] << std::endl;

  // /* Back transform a vector */
  // v3 = aff2->BackTransform(u3);
  // std::cout << "Back transform a vector :" << std::endl
  // << v3[0] << " , " << v3[1] << std::endl;

  /* Transform a Covariant vector */
  itk::Vector<double, 2> u4, v4;
  u4[0] = 3;
  u4[1] = 5;
  v4 = aff2->TransformVector(u4);
  std::cout << "Transform a Covariant vector:" << std::endl << v4[0] << " , " << v4[1] << std::endl;

  // /* Back transform a vector */
  // v4 = aff2->BackTransform(u4);
  // std::cout << "Back transform a vector :" << std::endl
  // << v4[0] << " , " << v4[1] << std::endl;

  /* Create a 3D transform and rotate in 3D */
  using Affine3DType = itk::CenteredAffineTransform<double, 3>;
  auto                   aff3 = Affine3DType::New();
  itk::Vector<double, 3> axis;
  axis[0] = .707;
  axis[1] = .707;
  axis[2] = .707;
  aff3->Rotate3D(axis, 1.0, true);
  std::cout << "Create and rotate a 3D transform:" << std::endl;
  aff3->Print(std::cout);

  /* Generate inverse transform */
  auto inv3 = Affine3DType::New();
  if (!aff3->GetInverse(inv3))
  {
    std::cout << "Cannot create inverse transformation" << std::endl;
  }
  std::cout << "Create an inverse transformation:" << std::endl;
  inv3->Print(std::cout);

  Affine3DType::Pointer inv4 = dynamic_cast<Affine3DType *>(aff3->GetInverseTransform().GetPointer());
  if (!inv4)
  {
    std::cout << "Cannot compute inverse transformation" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "Create an inverse transformation:" << std::endl;
  inv4->Print(std::cout);

  /* Create an image for testing index<->physical transforms */
  std::cout << "Creating image for testing index<->physical transforms" << std::endl;
  double                                spacing[3] = { 1.0, 2.0, 3.0 };
  double                                origin[3] = { 4.0, 5.0, 6.0 };
  itk::Image<unsigned char, 3>::Pointer image = itk::Image<unsigned char, 3>::New();
  image->SetOrigin(origin);
  image->SetSpacing(spacing);

  /* Test output of ComputeJacobianWithRespectToParameters */
  auto                           jaff = Affine3DType::New();
  const Affine3DType::MatrixType jaffMatrix = jaff->GetMatrix();
  std::cout << "GetMatrix:" << std::endl;
  std::cout << jaffMatrix << std::endl;

  const Affine3DType::OffsetType jaffVector = jaff->GetOffset();
  std::cout << "GetOffset:" << std::endl;
  std::cout << jaffVector << std::endl;

  Affine3DType::InputPointType jpoint;
  jpoint[0] = 5.0;
  jpoint[1] = 10.0;
  jpoint[2] = 15.0;
  Affine3DType::JacobianType jaffJacobian;
  jaff->ComputeJacobianWithRespectToParameters(jpoint, jaffJacobian);

  std::cout << "ComputeJacobianWithRespectToParameters: " << std::endl;
  std::cout << jaffJacobian << std::endl;

  /* Get the parameters */
  Affine3DType::ParametersType parameters3D;
  parameters3D = aff3->GetParameters();

  std::cout << "Parameters 3D: " << parameters3D << std::endl;

  /* Now set the parameters of another matrix */
  jaff->SetParameters(parameters3D);

  std::cout << "A transform after SetParameters:" << std::endl;
  jaff->Print(std::cout);

  return any;
}
