/*=========================================================================
 *
 *  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 itkWarpMeshFilter_hxx
#define itkWarpMeshFilter_hxx

#include "itkWarpMeshFilter.h"
#include "itkMacro.h"

namespace itk
{
/**
 *
 */
template <typename TInputMesh, typename TOutputMesh, typename TDisplacementField>
WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::WarpMeshFilter()
{
  // Setup the number of required inputs.
  // This filter requires as input one Mesh and one Vector image.
  this->SetNumberOfRequiredInputs(2);
}

template <typename TInputMesh, typename TOutputMesh, typename TDisplacementField>
const typename WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::DisplacementFieldType *
WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::GetDisplacementField() const
{
  return itkDynamicCastInDebugMode<const DisplacementFieldType *>(this->ProcessObject::GetInput(1));
}

template <typename TInputMesh, typename TOutputMesh, typename TDisplacementField>
void
WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::SetDisplacementField(const DisplacementFieldType * field)
{
  // const cast is needed because the pipeline is not const-correct.
  auto * input = const_cast<DisplacementFieldType *>(field);

  this->ProcessObject::SetNthInput(1, input);
}

/**
 *
 */
template <typename TInputMesh, typename TOutputMesh, typename TDisplacementField>
void
WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
}

/**
 * This method causes the filter to generate its output.
 */
template <typename TInputMesh, typename TOutputMesh, typename TDisplacementField>
void
WarpMeshFilter<TInputMesh, TOutputMesh, TDisplacementField>::GenerateData()
{
  using InputPointsContainer = typename TInputMesh::PointsContainer;
  using OutputPointsContainer = typename TOutputMesh::PointsContainer;

  using OutputPointsContainerPointer = typename TOutputMesh::PointsContainerPointer;

  const InputMeshType *    inputMesh = this->GetInput();
  OutputMeshPointer        outputMesh = this->GetOutput();
  DisplacementFieldPointer fieldPtr = this->GetDisplacementField();

  if (!inputMesh)
  {
    itkExceptionMacro(<< "Missing Input Mesh");
  }

  if (!outputMesh)
  {
    itkExceptionMacro(<< "Missing Output Mesh");
  }

  outputMesh->SetBufferedRegion(outputMesh->GetRequestedRegion());

  const InputPointsContainer * inPoints = inputMesh->GetPoints();
  OutputPointsContainerPointer outPoints = outputMesh->GetPoints();

  outPoints->Reserve(inputMesh->GetNumberOfPoints());
  outPoints->Squeeze(); // in case the previous mesh had
                        // allocated a larger memory

  typename InputPointsContainer::ConstIterator inputPoint = inPoints->Begin();
  typename OutputPointsContainer::Iterator     outputPoint = outPoints->Begin();

  using InputPointType = typename InputMeshType::PointType;
  using OutputPointType = typename OutputMeshType::PointType;

  OutputPointType displacedPoint;

  DisplacementType displacement;

  const unsigned int Dimension = fieldPtr->GetImageDimension();

  while (inputPoint != inPoints->End())
  {
    const InputPointType & originalPoint = inputPoint.Value();
    const auto             index = fieldPtr->TransformPhysicalPointToIndex(originalPoint);
    displacement = fieldPtr->GetPixel(index);

    for (unsigned int i = 0; i < Dimension; i++)
    {
      displacedPoint[i] = originalPoint[i] + displacement[i];
    }

    outputPoint.Value() = displacedPoint;

    ++inputPoint;
    ++outputPoint;
  }

  // Create duplicate references to the rest of data on the mesh

  this->CopyInputMeshToOutputMeshPointData();
  this->CopyInputMeshToOutputMeshCells();
  this->CopyInputMeshToOutputMeshCellLinks();
  this->CopyInputMeshToOutputMeshCellData();

  unsigned int maxDimension = TInputMesh::MaxTopologicalDimension;

  for (unsigned int dim = 0; dim < maxDimension; dim++)
  {
    outputMesh->SetBoundaryAssignments(dim, inputMesh->GetBoundaryAssignments(dim));
  }
}
} // end namespace itk

#endif
