/*=========================================================================
 *
 *  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
 *
 *         http://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.
 *
 *=========================================================================*/
#ifndef itkLandmarkBasedTransformInitializer_hxx
#define itkLandmarkBasedTransformInitializer_hxx

#include "itkLandmarkBasedTransformInitializer.h"
#include "itkMatrix.h"
#include "itkSymmetricEigenAnalysis.h"

#include <cmath>
#include "vnl/algo/vnl_qr.h"

namespace itk
{
template <typename TTransform, typename TFixedImage, typename TMovingImage>
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::LandmarkBasedTransformInitializer()
  : m_ReferenceImage(nullptr)
  , m_Transform(nullptr)
  , m_FixedLandmarks(0)
  , m_MovingLandmarks(0)
  , m_LandmarkWeight(0)

{}

/** default transform initializer, if transform type isn't
 * specifically handled.
 */
template <typename TTransform, typename TFixedImage, typename TMovingImage>
template <typename TTransform2>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InternalInitializeTransform(TTransform2 *)
{

  if (dynamic_cast<BSplineTransformType *>(this->m_Transform.GetPointer()) != nullptr)
  {
    this->InternalInitializeTransform(static_cast<BSplineTransformType *>(nullptr));
    return;
  }

  if (dynamic_cast<AffineTransformType *>(this->m_Transform.GetPointer()) != nullptr)
  {
    this->InternalInitializeTransform(static_cast<AffineTransformType *>(nullptr));
    return;
  }

  if (dynamic_cast<VersorRigid3DTransformType *>(this->m_Transform.GetPointer()) != nullptr)
  {
    this->InternalInitializeTransform(static_cast<VersorRigid3DTransformType *>(nullptr));
    return;
  }

  if (dynamic_cast<Rigid2DTransformType *>(this->m_Transform.GetPointer()) != nullptr)
  {
    this->InternalInitializeTransform(static_cast<Rigid2DTransformType *>(nullptr));
    return;
  }

  itkExceptionMacro(<< "Unsupported Transform Type " << this->m_Transform->GetNameOfClass());
}

template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InternalInitializeTransform(
  BSplineTransformType *)
{
  auto * transform = dynamic_cast<BSplineTransformType *>(this->m_Transform.GetPointer());
  if (transform == nullptr)
  {
    itkExceptionMacro(<< "BSplineTransform Expected but transform is " << this->m_Transform->GetNameOfClass());
  }
  if (m_ReferenceImage.IsNull())
  {
    itkExceptionMacro(
      << "Reference image required for BSplineTransform initialization is NULL (not set or set to NULL).");
  }

  const size_t numberOfLandMarks = m_MovingLandmarks.size();

  // Instantiating B-spline filter and creating B-spline domain
  //
  using VectorType = Vector<double, ImageDimension>;
  using VectorImageType = Image<VectorType, ImageDimension>;
  using PointSetType = PointSet<typename VectorImageType::PixelType, ImageDimension>;
  using FilterType = BSplineScatteredDataPointSetToImageFilter<PointSetType, VectorImageType>;

  using WeightsContainerType = typename FilterType::WeightsContainerType;
  typename WeightsContainerType::Pointer weights = WeightsContainerType::New();
  weights->Reserve(numberOfLandMarks);

  if (!m_LandmarkWeight.empty())
  {
    if (m_LandmarkWeight.size() != numberOfLandMarks)
    {
      itkExceptionMacro(<< "Size mismatch between number of landmarks pairs and weights");
    }
    auto weightIt = m_LandmarkWeight.begin();
    for (unsigned int i = 0; weightIt != m_LandmarkWeight.end(); ++i, ++weightIt)
    {
      weights->InsertElement(i, (*weightIt));
    }
  }
  else
  {
    // Set weights to identity
    for (unsigned int i = 0; i < numberOfLandMarks; ++i)
    {
      weights->InsertElement(i, 1);
    }
  }

  // Set a pointSet from the input landmarks
  typename PointSetType::Pointer pointSet = PointSetType::New();
  pointSet->Initialize();

  PointsContainerConstIterator fixedIt = m_FixedLandmarks.begin();
  PointsContainerConstIterator movingIt = m_MovingLandmarks.begin();
  for (size_t i = 0; fixedIt != m_FixedLandmarks.end(); ++i, ++fixedIt, ++movingIt)
  {
    pointSet->SetPoint(static_cast<typename PointSetType::PointIdentifier>(i), (*fixedIt));
    VectorType vectorTmp;
    for (unsigned int d = 0; d < ImageDimension; ++d)
    {
      vectorTmp[d] = (*movingIt)[d] - (*fixedIt)[d];
    }
    pointSet->SetPointData(static_cast<typename PointSetType::PointIdentifier>(i), vectorTmp);
  }

  const typename VectorImageType::SizeType      size = this->m_ReferenceImage->GetLargestPossibleRegion().GetSize();
  const typename VectorImageType::PointType     origin = this->m_ReferenceImage->GetOrigin();
  const typename VectorImageType::SpacingType   spacing = this->m_ReferenceImage->GetSpacing();
  const typename VectorImageType::DirectionType direction = this->m_ReferenceImage->GetDirection();

  typename FilterType::Pointer filter = FilterType::New();
  // Define the parametric domain.
  filter->SetOrigin(origin);
  filter->SetSpacing(spacing);
  filter->SetSize(size);
  filter->SetDirection(direction);
  filter->SetInput(pointSet);
  filter->SetPointWeights(weights);
  filter->SetGenerateOutputImage(false);
  filter->SetSplineOrder(SplineOrder);

  typename FilterType::ArrayType ncps;
  ncps.Fill(this->m_BSplineNumberOfControlPoints); // should be greater than SplineOrder
  filter->SetNumberOfControlPoints(ncps);

  filter->SetNumberOfLevels(3);

  typename FilterType::ArrayType close;
  close.Fill(0);
  filter->SetCloseDimension(close);

  filter->Update();

  // Set the BSpline transform
  //
  using CoefficientImageType = typename BSplineTransformType::ImageType;

  typename BSplineTransformType::CoefficientImageArray coefficientImages;
  for (unsigned int j = 0; j < ImageDimension; ++j)
  {
    using SelectorType = VectorIndexSelectionCastImageFilter<VectorImageType, CoefficientImageType>;
    typename SelectorType::Pointer selector = SelectorType::New();
    selector->SetInput(filter->GetPhiLattice());
    selector->SetIndex(j);

    coefficientImages[j] = selector->GetOutput();
    coefficientImages[j]->Update();
    coefficientImages[j]->DisconnectPipeline();
  }

  transform->SetCoefficientImages(coefficientImages);
}

/** Compute an affine transform from landmark set.
 *  This was implemented by Eun Young Kim based on method
 *  described in the paper "Closed-form solution of absolute
 *  orientation using orthonormal matrices" by Horn, Hilden, and
 *  Negahdaripour. See http://www.opticsinfobase.org/abstract.cfm?&id=3143
 */
template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InternalInitializeTransform(
  AffineTransformType *)
{
  auto * transform = dynamic_cast<AffineTransformType *>(this->m_Transform.GetPointer());
  if (transform == nullptr)
  {
    itkExceptionMacro(<< "AffineTransform Expected but transform is " << this->m_Transform->GetNameOfClass());
  }

  const auto numberOfLandmarks = static_cast<const unsigned int>(m_MovingLandmarks.size());

  if (numberOfLandmarks < LandmarkPointContainer::value_type::GetPointDimension() + 1)
  {
    itkExceptionMacro(<< " insufficient number of landmarks, expected "
                      << LandmarkPointContainer::value_type::GetPointDimension() + 1 << " got " << numberOfLandmarks);
  }


  //[Set Landmark Weight]
  //:: If no landmark weights are given, weight matrix is identity matrix
  vnl_matrix<ParametersValueType> vnlWeight(numberOfLandmarks, numberOfLandmarks, 0);
  vnlWeight.set_identity();

  if (!m_LandmarkWeight.empty())
  {
    if (m_LandmarkWeight.size() != numberOfLandmarks)
    {
      itkExceptionMacro(<< " size mismatch between number of landmars pairs and weights");
    }
    auto weightIt = m_LandmarkWeight.begin();
    for (unsigned int i = 0; weightIt != m_LandmarkWeight.end(); ++i, ++weightIt)
    {
      vnlWeight(i, i) = (*weightIt);
    }
  }
  // Normalize weights
  //
  vnlWeight = vnlWeight / vnlWeight.fro_norm();

  //[Convert Point to Matrix for Matrix Muptiplication]
  //

  // q
  // dim+1=4 * numberOfLandmarks matrix
  vnl_matrix<ParametersValueType> q(ImageDimension + 1, numberOfLandmarks, 0.0F);
  PointsContainerConstIterator    fixedIt = m_FixedLandmarks.begin();
  for (unsigned int i = 0; fixedIt != m_FixedLandmarks.end(); ++i, ++fixedIt)
  {
    for (unsigned int dim = 0; dim < ImageDimension; dim++)
    {
      q(dim, i) = (*fixedIt)[dim];
    }
    q(ImageDimension, i) = 1.0F;
  }
  q *= vnlWeight;

  // p
  // dim=3 * numberOfLandmarks matrix
  vnl_matrix<ParametersValueType> p(ImageDimension, numberOfLandmarks, 0.0F);
  PointsContainerConstIterator    movingIt = m_MovingLandmarks.begin();
  for (unsigned int i = 0; movingIt != m_MovingLandmarks.end(); ++i, ++movingIt)
  {
    for (unsigned int dim = 0; dim < ImageDimension; dim++)
    {
      p(dim, i) = (*movingIt)[dim];
    }
  }

  p *= vnlWeight;
  // EX) 3 dimensional case
  //          | sum( q(1,i)*q(1,i) sum( q(1,i)*q(2,i) sum( q(1,i)*q(3,i) sum( q(1,i) ) |
  // Q[4x4] = | sum( q(2,i)*q(1,i) sum( q(2,i)*q(2,i) sum( q(2,i)*q(3,i) sum( q(2,i) ) |
  //          | sum( q(3,i)*q(1,i) sum( q(3,i)*q(2,i) sum( q(3,i)*q(3,i) sum( q(3,i) ) |
  //          | sum( q(1,i) )      sum( q(2,i) )      sum( q(3,i) )      sum( q(4,i) ) |
  //
  //          | a(1,1) a(2,1) a(3,1) |
  // a[4x3] = | a(1,2) a(2,2) a(3,2) |
  //          | a(1,3) a(2,3) a(3,3) |
  //          | a(1,4) a(2,4) a(3,4) |
  //
  //          | sum( q(1,i)*p(1,i) )  sum( q(1,i)*p(2,i) ) sum( q(1,i)*p(3,i) )  |
  // C[4x3] = | sum( q(2,i)*p(1,i) )  sum( q(2,i)*p(2,i) ) sum( q(2,i)*p(3,i) )  |
  //          | sum( q(3,i)*p(1,i) )  sum( q(3,i)*p(2,i) ) sum( q(3,i)*p(3,i) )  |
  //          | sum( p(1,i) )         sum( p(2,i) )        sum( p(3,i) )         |
  //

  // [Q]
  //
  vnl_matrix<ParametersValueType> Q(ImageDimension + 1, ImageDimension + 1, 0.0F);
  for (unsigned int i = 0; i < numberOfLandmarks; i++)
  { // Iterate for the number of landmakrs
    vnl_matrix<ParametersValueType> qTemp(ImageDimension + 1, 1);
    // convert vector to colume matrix
    for (unsigned int k = 0; k < ImageDimension + 1; k++)
    {
      qTemp(k, 0) = q.get(k, i);
    }
    vnl_matrix<ParametersValueType> qTempT(1, ImageDimension + 1);
    qTempT = qTemp.transpose();
    Q = Q + qTemp * qTempT;
  }

  // [C]
  //
  vnl_matrix<ParametersValueType> C(ImageDimension + 1, ImageDimension, 0);
  for (unsigned int i = 0; i < numberOfLandmarks; i++)
  {
    vnl_matrix<ParametersValueType> qTemp(ImageDimension + 1, 1);
    vnl_matrix<ParametersValueType> pTemp(1, ImageDimension);
    // convert vector to colume matrix
    for (unsigned int k = 0; k < ImageDimension + 1; k++)
    {
      qTemp(k, 0) = q.get(k, i);
    }
    for (unsigned int k = 0; k < ImageDimension; k++)
    {
      pTemp(0, k) = p.get(k, i);
    }

    C = C + qTemp * pTemp;
  }

  // [Solve Qa=C]
  // :: Solving code is from
  // https://www.itk.org/pipermail/insight-users/2008-December/028207.html
  vnl_matrix<ParametersValueType> transposeAffine = vnl_qr<ParametersValueType>(Q).solve(C);
  vnl_matrix<ParametersValueType> Affine = transposeAffine.transpose();

  vnl_matrix<ParametersValueType> AffineRotation =
    vnl_matrix<ParametersValueType>(Affine.get_n_columns(0, ImageDimension));

  // [Convert ITK Affine Transformation from vnl]
  //
  // Define Matrix Type
  //
  itk::Matrix<ParametersValueType, ImageDimension, ImageDimension> mA =
    itk::Matrix<ParametersValueType, ImageDimension, ImageDimension>(AffineRotation);
  itk::Vector<ParametersValueType, ImageDimension> mT;
  for (unsigned int t = 0; t < ImageDimension; t++)
  {
    mT[t] = Affine(t, ImageDimension);
  }

  transform->SetMatrix(mA);
  transform->SetOffset(mT);
}

template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InternalInitializeTransform(
  VersorRigid3DTransformType *)
{
  itkDebugMacro("Internal Initialize VersorRigid3DTransformType");
  auto * transform = dynamic_cast<VersorRigid3DTransformType *>(this->m_Transform.GetPointer());
  if (transform == nullptr)
  {
    itkExceptionMacro(<< "VersorRigid3DTransformType Expected but transform is "
                      << this->m_Transform->GetNameOfClass());
  }

  // Sanity check
  if (ImageDimension != 3)
  {
    itkExceptionMacro("Transform is VersorRigid3DTransform and Fixed image dimension is not 3");
    return;
  }
  if (MovingImageType::ImageDimension != 3)
  {
    itkExceptionMacro("Transform is VersorRigid3DTransform and Moving image dimension is not 3");
    return;
  }

  // --- compute the necessary transform to match the two sets of landmarks
  // ---
  //
  //
  //    The solution is based on
  //    Berthold K. P. Horn (1987),
  //    "Closed-form solution of absolute orientation using unit
  // quaternions,"
  //    Journal of the Optical Society of America A, 4:629-642
  //
  //
  //    Original python implementation by David G. Gobbi
  //    Readpted from the code in VTK: Hybrid/vtkLandmarkTransform
  //
  //----------------------------------------------------------------------------

  using VectorType = typename VersorRigid3DTransformType::OutputVectorType;
  using PointType = typename VersorRigid3DTransformType::OutputPointType;

  // Compute the centroids
  PointType fixedCentroid;
  fixedCentroid.Fill(0.0);
  PointsContainerConstIterator fixedItr = m_FixedLandmarks.begin();
  while (fixedItr != m_FixedLandmarks.end())
  {
    fixedCentroid[0] += (*fixedItr)[0];
    fixedCentroid[1] += (*fixedItr)[1];
    fixedCentroid[2] += (*fixedItr)[2];
    ++fixedItr;
  }

  fixedCentroid[0] /= m_FixedLandmarks.size();
  fixedCentroid[1] /= m_FixedLandmarks.size();
  fixedCentroid[2] /= m_FixedLandmarks.size();

  PointsContainerConstIterator movingItr = m_MovingLandmarks.begin();
  PointType                    movingCentroid;
  movingCentroid.Fill(0.0);
  while (movingItr != m_MovingLandmarks.end())
  {
    movingCentroid[0] += (*movingItr)[0];
    movingCentroid[1] += (*movingItr)[1];
    movingCentroid[2] += (*movingItr)[2];
    ++movingItr;
  }

  movingCentroid[0] /= m_MovingLandmarks.size();
  movingCentroid[1] /= m_MovingLandmarks.size();
  movingCentroid[2] /= m_MovingLandmarks.size();

  itkDebugMacro(<< "fixed centroid  = " << fixedCentroid);
  itkDebugMacro(<< "moving centroid  = " << movingCentroid);

  using VersorType = typename VersorRigid3DTransformType::VersorType;

  VersorType versor;

  // If we have at least 3 landmarks, we can compute a rotation.
  // Otherwise the versor will be an identity versor.
  if (m_FixedLandmarks.size() >= ImageDimension)
  {
    itk::Matrix<ParametersValueType, ImageDimension, ImageDimension> M;

    fixedItr = m_FixedLandmarks.begin();
    movingItr = m_MovingLandmarks.begin();

    VectorType fixedCentered;
    VectorType movingCentered;

    fixedCentered.Fill(0.0);
    movingCentered.Fill(0.0);

    itkDebugStatement(int ii = 0);

    // Computations are relative to the Center of Rotation.
    while (movingItr != m_MovingLandmarks.end())
    {
      for (unsigned int i = 0; i < ImageDimension; i++)
      {
        fixedCentered[i] = (*fixedItr)[i] - fixedCentroid[i];
        movingCentered[i] = (*movingItr)[i] - movingCentroid[i];
      }

      for (unsigned int i = 0; i < ImageDimension; i++)
      {
        for (unsigned int j = 0; j < ImageDimension; j++)
        {
          // mmm this indices i,j may have to be reverted...
          M[i][j] += fixedCentered[i] * movingCentered[j];
        }
      }

      itkDebugStatement(++ii);
      itkDebugMacro(<< "f_" << ii << " = " << fixedCentered);
      itkDebugMacro(<< "m_" << ii << " = " << movingCentered);

      ++movingItr;
      ++fixedItr;
    }

    // -- build the 4x4 matrix N --

    itk::Matrix<ParametersValueType, 4, 4> N;

    // on-diagonal elements
    N[0][0] = M[0][0] + M[1][1] + M[2][2];
    N[1][1] = M[0][0] - M[1][1] - M[2][2];
    N[2][2] = -M[0][0] + M[1][1] - M[2][2];
    N[3][3] = -M[0][0] - M[1][1] + M[2][2];
    // off-diagonal elements
    N[0][1] = N[1][0] = M[1][2] - M[2][1];
    N[0][2] = N[2][0] = M[2][0] - M[0][2];
    N[0][3] = N[3][0] = M[0][1] - M[1][0];

    N[1][2] = N[2][1] = M[0][1] + M[1][0];
    N[1][3] = N[3][1] = M[2][0] + M[0][2];
    N[2][3] = N[3][2] = M[1][2] + M[2][1];

    itkDebugMacro(<< "For Closed form solution: ");
    itkDebugMacro(<< "M matrix " << M);
    itkDebugMacro(<< "N matrix " << N);

    vnl_matrix<ParametersValueType> eigenVectors(4, 4);
    vnl_vector<ParametersValueType> eigenValues(4);

    using SymmetricEigenAnalysisType = itk::SymmetricEigenAnalysisFixedDimension<4,
                                                                                 itk::Matrix<ParametersValueType, 4, 4>,
                                                                                 vnl_vector<ParametersValueType>,
                                                                                 vnl_matrix<ParametersValueType>>;
    SymmetricEigenAnalysisType symmetricEigenSystem;

    symmetricEigenSystem.ComputeEigenValuesAndVectors(N, eigenValues, eigenVectors);

    itkDebugMacro(<< "EigenVectors " << eigenVectors);
    itkDebugMacro(<< "EigenValues " << eigenValues);

    // By default eigen values are sorted in ascending order.  therefore the
    // maximum
    // eigen value is the one  in the fourth place = index 3. We need the
    // eigen
    // vector associated with the maximum eigenvalue, so we take the
    // eigenvector
    // from the last row, index=3.

    versor.Set(eigenVectors[3][1], eigenVectors[3][2], eigenVectors[3][3], eigenVectors[3][0]);
    itkDebugMacro(<< "Resulting versor" << versor);
  }
  else
  {
    // Remember..
    // Less than 3 landmarks available. Rotation is not computed
  }

  transform->SetCenter(fixedCentroid);
  transform->SetRotation(versor);

  VectorType translation = transform->GetTranslation();
  translation = movingCentroid - fixedCentroid;
  transform->SetTranslation(translation);
}

template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InternalInitializeTransform(
  Rigid2DTransformType *)
{
  itkDebugMacro("Internal Initialize VersorRigid3DTransformType");
  auto * transform = dynamic_cast<Rigid2DTransformType *>(this->m_Transform.GetPointer());

  if (transform == nullptr)
  {
    itkExceptionMacro(<< "VersorRigid3DTransformType Expected but transform is "
                      << this->m_Transform->GetNameOfClass());
  }
  // Sanity check
  if (ImageDimension != 2)
  {
    itkExceptionMacro("Transform is Rigid2DTransfrom and Fixed image dimension is not 2");
    return;
  }
  if (MovingImageType::ImageDimension != 2)
  {
    itkExceptionMacro("Transform is Rigid2DTransform and Moving image dimension is not 2");
    return;
  }

  const double PI = 4.0 * std::atan(1.0);
  using VectorType = typename Rigid2DTransformType::OutputVectorType;
  using PointType = typename Rigid2DTransformType::OutputPointType;

  // Initialize the transform to identity
  transform->SetIdentity();

  // Compute the centroids
  PointType fixedCentroid;
  fixedCentroid.Fill(0.0);
  PointsContainerConstIterator fixedItr = m_FixedLandmarks.begin();
  while (fixedItr != m_FixedLandmarks.end())
  {
    fixedCentroid[0] += (*fixedItr)[0];
    fixedCentroid[1] += (*fixedItr)[1];
    ++fixedItr;
  }

  fixedCentroid[0] /= m_FixedLandmarks.size();
  fixedCentroid[1] /= m_FixedLandmarks.size();

  PointsContainerConstIterator movingItr = m_MovingLandmarks.begin();
  PointType                    movingCentroid;
  movingCentroid.Fill(0.0);
  while (movingItr != m_MovingLandmarks.end())
  {
    movingCentroid[0] += (*movingItr)[0];
    movingCentroid[1] += (*movingItr)[1];
    ++movingItr;
  }

  movingCentroid[0] /= m_MovingLandmarks.size();
  movingCentroid[1] /= m_MovingLandmarks.size();

  itkDebugMacro(<< "fixed centroid  = " << fixedCentroid);
  itkDebugMacro(<< "moving centroid  = " << movingCentroid);

  double rotationAngle = 0.0;

  // If we have at least 2 landmarks, we can compute a rotation.
  // Otherwise the rotation matrix will be identity.
  //
  // For the Rigid2DTransform, the least squares error will be minimized
  // by choosing the offset as the distance between the two centroids,
  // fixed centroid (after having undergone the rotation transform, that
  // we must compute) and the moving centroid.
  // The rotation angle will be given by the cross and dot products of the
  // fixed and moving landmark vectors, the vectors being computed relative
  // to the fixed and moving centroids.
  if (m_FixedLandmarks.size() >= 2)
  {
    fixedItr = m_FixedLandmarks.begin();
    movingItr = m_MovingLandmarks.begin();

    VectorType fixedCentered;
    VectorType movingCentered;

    fixedCentered.Fill(0.0);
    movingCentered.Fill(0.0);

    itkDebugStatement(int ii = 0);
    double s_dot = 0;
    double s_cross = 0;
    // Computations are relative to the Center of Rotation.
    while (movingItr != m_MovingLandmarks.end())
    {
      fixedCentered[0] = (*fixedItr)[0] - fixedCentroid[0];
      movingCentered[0] = (*movingItr)[0] - movingCentroid[0];
      fixedCentered[1] = (*fixedItr)[1] - fixedCentroid[1];
      movingCentered[1] = (*movingItr)[1] - movingCentroid[1];

      s_dot += (movingCentered[0] * fixedCentered[0]) + (movingCentered[1] * fixedCentered[1]);
      s_cross += (movingCentered[1] * fixedCentered[0]) - (movingCentered[0] * fixedCentered[1]);

      itkDebugStatement(++ii);
      itkDebugMacro(<< "f_" << ii << " = " << fixedCentered);
      itkDebugMacro(<< "m_" << ii << " = " << movingCentered);

      ++movingItr;
      ++fixedItr;
    }

    itkDebugMacro(<< "Dot Product of landmarks: " << s_dot << " Cross Product: " << s_cross);
    if (std::fabs(s_dot) > 0.00005)
    {
      rotationAngle = std::atan2(s_cross, s_dot);
    }
    else
    {
      rotationAngle = -0.5 * PI;
    }
  }
  else
  {
    itkWarningMacro(<< "Less than 2 landmarks available. Rotation is not computed");
  }

  typename Rigid2DTransformType::Pointer t = Rigid2DTransformType::New();
  t->SetIdentity();
  t->SetAngle(rotationAngle);

  transform->SetCenter(fixedCentroid);
  transform->SetAngle(rotationAngle);

  VectorType translation = transform->GetTranslation();
  itkDebugMacro(<< "Initial transform translation: " << translation);
  translation = movingCentroid - fixedCentroid;
  itkDebugMacro(<< "translation computed as difference of centroids: " << translation);
  transform->SetTranslation(translation);
}

template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::InitializeTransform()
{
  // Sanity check
  if (!m_Transform)
  {
    itkExceptionMacro("Transform has not been set");
    return;
  }
  if (m_FixedLandmarks.size() != m_MovingLandmarks.size())
  {
    itkExceptionMacro("Different number of fixed and moving landmarks");
    return;
  }
  this->InternalInitializeTransform(static_cast<TTransform *>(nullptr));
}

template <typename TTransform, typename TFixedImage, typename TMovingImage>
void
LandmarkBasedTransformInitializer<TTransform, TFixedImage, TMovingImage>::PrintSelf(std::ostream & os,
                                                                                    Indent         indent) const
{
  Superclass::PrintSelf(os, indent);

  itkPrintSelfObjectMacro(Transform);
  itkPrintSelfObjectMacro(ReferenceImage);

  os << indent << "Fixed Landmarks: " << std::endl;
  auto fitr = m_FixedLandmarks.begin();
  while (fitr != m_FixedLandmarks.end())
  {
    os << indent << *fitr << std::endl;
    ++fitr;
  }
  os << indent << "Moving Landmarks: " << std::endl;
  auto mitr = m_MovingLandmarks.begin();
  while (mitr != m_MovingLandmarks.end())
  {
    os << indent << *mitr << std::endl;
    ++mitr;
  }
  os << indent << "Landmark Weight: " << std::endl;
  auto witr = m_LandmarkWeight.begin();
  while (witr != m_LandmarkWeight.end())
  {
    os << indent << *witr << std::endl;
    ++witr;
  }

  os << indent << "BSplineNumberOfControlPoints: " << m_BSplineNumberOfControlPoints << std::endl;
}
} // namespace itk

#endif /* itkLandmarkBasedTransformInitializer_hxx */