/*=========================================================================
 *
 *  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.
 *
 *=========================================================================*/
#ifndef itkBSplineTransformInitializer_hxx
#define itkBSplineTransformInitializer_hxx


#include "itkContinuousIndex.h"
#include "itkPointSet.h"
#include "itkBoundingBox.h"

namespace itk
{

template <typename TTransform, typename TImage>
void
BSplineTransformInitializer<TTransform, TImage>::SetTransformDomainMeshSize(const MeshSizeType meshSize)
{
  itkDebugMacro("setting m_TransformDomainMeshSize to " << meshSize);
  if (this->m_SetTransformDomainMeshSizeViaInitializer == false || this->m_TransformDomainMeshSize != meshSize)
  {
    this->m_SetTransformDomainMeshSizeViaInitializer = true;
    this->m_TransformDomainMeshSize = meshSize;
    this->Modified();
  }
}

template <typename TTransform, typename TImage>
void
BSplineTransformInitializer<TTransform, TImage>::InitializeTransform() const
{
  if (!this->m_Transform)
  {
    itkExceptionMacro("Transform has not been set.");
  }
  if (!this->m_Image)
  {
    itkExceptionMacro("Image has not been set.");
  }
  if (TImage::GetImageDimension() != SpaceDimension)
  {
    itkExceptionMacro("Image dimensionality does not match the transform.");
  }

  OriginType             transformDomainOrigin;
  PhysicalDimensionsType transformDomainPhysicalDimensions;
  DirectionType          transformDomainDirection;

  // Determine the image corners. We keep track of the relative location of
  // the corners using a binary labeling system. For example, in a 3-D
  // coordinate system aligned with the x,y,z axes, we have 8 points labeled as
  // follows:
  //
  //  1. 000  min_x, min_y, min_z
  //  2. 001  max_x, min_y, min_z
  //  3. 010  min_x, max_y, min_z
  //  4. 011  max_x, max_y, min_z
  //  5. 100  min_x, min_y, max_z
  //  6. 101  max_x, min_y, max_z
  //  7. 110  min_x, max_y, max_z
  //  8. 111  max_x, max_y, max_z
  //
  // We use this binary description of the corners in n-dimensions because it
  // allows us to know the adjacent neighbors of an arbitrary image corner. For
  // example, suppose we locate the transform domain origin at the corner 011
  // the adjacent neighbors which form the rotated coordinate system are
  // 111, 001, and 010. Notice that we just change 1 bit at a time from the
  // origin to determine these axes. Thus bitwise operators are used
  // throughout the code so that the initializer is generalized to n-dimensions.

  using CoordRepType = typename ImagePointType::CoordRepType;

  using PointSetType = PointSet<CoordRepType, SpaceDimension>;
  auto cornerPoints = PointSetType::New();
  cornerPoints->Initialize();

  using PointType = typename PointSetType::PointType;
  using PointIdentifier = typename PointSetType::PointIdentifier;
  using RealType = typename PointType::RealType;
  using VectorType = typename PointType::VectorType;

  using ContinuousIndexType = ContinuousIndex<CoordRepType, SpaceDimension>;

  // We first convert the image corners into points which reside in physical
  // space and label them as indicated above.  Note that the corners reside
  // at the extreme corners of the image and not just at the voxel centers.
  // We also store the corners using the point set class which gives us easy
  // access to the bounding box.

  const CoordRepType BSplineTransformDomainEpsilon = std::pow(2.0, -3);

  ContinuousIndexType startIndex;
  for (unsigned int i = 0; i < SpaceDimension; ++i)
  {
    startIndex[i] = this->m_Image->GetLargestPossibleRegion().GetIndex()[i] - 0.5 - BSplineTransformDomainEpsilon;
  }

  for (unsigned int d = 0, N = 1 << SpaceDimension; d < N; ++d)
  {
    ContinuousIndexType whichIndex;
    for (unsigned int i = 0; i < SpaceDimension; ++i)
    {
      whichIndex[i] = startIndex[i] + static_cast<CoordRepType>(
                                        ((d >> i) & 1) * (this->m_Image->GetLargestPossibleRegion().GetSize()[i] +
                                                          2.0 * BSplineTransformDomainEpsilon));
    }
    ImagePointType point;
    this->m_Image->TransformContinuousIndexToPhysicalPoint(whichIndex, point);
    PointType corner;
    corner.CastFrom(point);
    cornerPoints->SetPoint(d, corner);
  }

  // We next determine which corner is the transform domain origin by which
  // point is closest to the minimum of the bounding box.

  using BoundingBoxType = BoundingBox<unsigned int,
                                      SpaceDimension,
                                      typename PointSetType::CoordRepType,
                                      typename PointSetType::PointsContainer>;
  auto bbox = BoundingBoxType::New();
  bbox->SetPoints(cornerPoints->GetPoints());
  bbox->ComputeBoundingBox();

  transformDomainOrigin.Fill(0);
  PointIdentifier transformDomainOriginId = 0;
  RealType        minDistance = NumericTraits<RealType>::max();

  for (unsigned int d = 0; d < cornerPoints->GetNumberOfPoints(); ++d)
  {
    PointType corner;
    corner.Fill(0.0);
    cornerPoints->GetPoint(d, &corner);

    RealType distance = corner.SquaredEuclideanDistanceTo(bbox->GetMinimum());
    if (distance < minDistance)
    {
      transformDomainOrigin.CastFrom(corner);
      minDistance = distance;
      transformDomainOriginId = static_cast<PointIdentifier>(d);
    }
  }

  // Now we need to find the transform direction matrix. This is done
  // by using the domain origin and its adjacent neighbors to determine a new
  // rotated coordinate system.

  transformDomainDirection.SetIdentity();

  // We first determine which image axis is the most aligned with each physical
  // axis.

  PointIdentifier minCornerId[SpaceDimension];
  double          minAngle[SpaceDimension];

  for (unsigned int d = 0; d < SpaceDimension; ++d)
  {
    minAngle[d] = NumericTraits<double>::max();

    VectorType vectorAxis{};
    vectorAxis[d] = 1.0;

    for (unsigned int i = 0; i < SpaceDimension; ++i)
    {
      PointIdentifier oppositeCornerId =
        (static_cast<PointIdentifier>(1) << static_cast<PointIdentifier>(i)) ^ transformDomainOriginId;

      PointType corner;
      corner.Fill(0.0);
      cornerPoints->GetPoint(oppositeCornerId, &corner);

      VectorType vector = corner - transformDomainOrigin;
      vector.Normalize();

      double theta = angle(vectorAxis.GetVnlVector(), vector.GetVnlVector());

      if (theta < minAngle[d])
      {
        bool alreadyFound = false;
        for (unsigned int j = 0; j < d; ++j)
        {
          if (minCornerId[j] == oppositeCornerId)
          {
            alreadyFound = true;
            break;
          }
        }
        if (!alreadyFound)
        {
          minCornerId[d] = oppositeCornerId;
          minAngle[d] = theta;
        }
      }
    }
  }

  // Now that we know which image axes corresponds to the unrotated coordinate
  // axes in physical space, we can easily construct the rotation matrix which
  // rotates a point from the unrotated coordinate system to the rotated
  // coordinate system. This is done by placing the rotated axis vectors as
  // columns in the rotation matrix.

  for (unsigned int d = 0; d < SpaceDimension; ++d)
  {
    PointType corner;
    corner.Fill(0.0);
    cornerPoints->GetPoint(minCornerId[d], &corner);

    VectorType vector = corner - transformDomainOrigin;

    // Note that specifying the size and spacing separately doesn't matter in
    // the case of the B-spline transform since the B-spline transform is a
    // continuous object over its finite domain.

    transformDomainPhysicalDimensions[d] = vector.GetNorm();
    vector.Normalize();

    for (unsigned int i = 0; i < SpaceDimension; ++i)
    {
      transformDomainDirection[i][d] = vector[i];
    }
  }

  this->m_Transform->SetTransformDomainOrigin(transformDomainOrigin);
  this->m_Transform->SetTransformDomainPhysicalDimensions(transformDomainPhysicalDimensions);
  this->m_Transform->SetTransformDomainDirection(transformDomainDirection);
  if (this->m_SetTransformDomainMeshSizeViaInitializer)
  {
    this->m_Transform->SetTransformDomainMeshSize(this->m_TransformDomainMeshSize);
  }
}

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

  itkPrintSelfObjectMacro(Image);

  itkPrintSelfObjectMacro(Transform);

  if (this->m_SetTransformDomainMeshSizeViaInitializer)
  {
    os << indent << "Transform domain mesh size: " << this->m_TransformDomainMeshSize << std::endl;
  }
}

} // namespace itk

#endif