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

#include "itkMacro.h"
#include <cmath>
#include "itkMath.h"

namespace itk
{
template <unsigned int VDimension>
auto
PolyLineParametricPath<VDimension>::Evaluate(const InputType & input) const -> OutputType
{
  // Handle the endpoint carefully, since there is no following vertex
  const auto endPoint = static_cast<InputType>(m_VertexList->Size() - 1);
  if (input > endPoint || itk::Math::FloatAlmostEqual(input, endPoint))
  {
    return static_cast<const VertexListType *>(this->m_VertexList)
      ->ElementAt(m_VertexList->Size() - 1); // the last vertex
  }

  const VertexType vertex0 =
    static_cast<const VertexListType *>(this->m_VertexList)->ElementAt(static_cast<int>(input));
  const VertexType vertex1 =
    static_cast<const VertexListType *>(this->m_VertexList)->ElementAt(static_cast<int>(input) + 1);

  const double fractionOfLineSegment = input - static_cast<int>(input);

  const PointType outputPoint = vertex0 + (vertex1 - vertex0) * fractionOfLineSegment;

  // For some stupid reason, there is no easy way to cast from a point to a
  // continuous index.
  OutputType output;
  for (unsigned int i = 0; i < VDimension; ++i)
  {
    output[i] = outputPoint[i];
  }

  return output;
}

template <unsigned int VDimension>
auto
PolyLineParametricPath<VDimension>::EvaluateDerivative(const InputType & input) const -> VectorType
{
  // Get next integral time-point
  const InputType nextTimepoint = std::min(std::floor(input + 1.0), this->EndOfInput());

  // Get previous integral time-point
  const InputType previousTimepoint = nextTimepoint - 1.0;

  // Calculate the continuous index for both points
  const ContinuousIndexType nextIndex = this->Evaluate(nextTimepoint);
  const ContinuousIndexType prevIndex = this->Evaluate(previousTimepoint);

  // For some reason, there's no way to convert ContinuousIndexType to VectorType
  VectorType partialDerivatives;
  for (unsigned int i = 0; i < VDimension; ++i)
  {
    partialDerivatives[i] = nextIndex[i] - prevIndex[i];
  }

  return partialDerivatives;
}

template <unsigned int VDimension>
auto
PolyLineParametricPath<VDimension>::IncrementInput(InputType & input) const -> OffsetType
{
  // Save this input index since we will use it to calculate the offset
  const IndexType originalIndex = this->EvaluateToIndex(input);

  InputType potentialTimestep{};
  bool      timeStepSmallEnough = false;
  while (!timeStepSmallEnough)
  {
    if (Math::ExactlyEquals(input, this->EndOfInput()))
    {
      const IndexType finalIndex = this->EvaluateToIndex(this->EndOfInput());
      OffsetType      finalOffset;
      for (unsigned int i = 0; i < VDimension; ++i)
      {
        finalOffset[i] = finalIndex[i] - originalIndex[i];
      }
      return finalOffset;
    }

    // Check to make sure we aren't already at a place with an offset of 1 pixel
    const IndexType potentialIndex = this->EvaluateToIndex(input);
    // For some reason, there's no way to convert OutputType to OffsetType
    OffsetType offset;
    for (unsigned int i = 0; i < VDimension; ++i)
    {
      offset[i] = potentialIndex[i] - originalIndex[i];
    }

    if (offset != this->GetZeroOffset())
    {
      return offset;
    }
    // Get the derivative at the current input
    VectorType partialDerivatives = this->EvaluateDerivative(input);

    // Find the largest of all the partial derivatives
    unsigned int maxPartialDerivativeIndex = 0;
    for (unsigned int i = 1; i < VDimension; ++i)
    {
      if (itk::Math::abs(partialDerivatives[i]) > itk::Math::abs(partialDerivatives[maxPartialDerivativeIndex]))
      {
        maxPartialDerivativeIndex = i;
      }
    }

    // Calculate the timestep required to effect a 1 pixel change
    potentialTimestep = 1.0 / itk::Math::abs(partialDerivatives[maxPartialDerivativeIndex]);

    // Check to make sure the timestep doesn't put the input past the next integral timestep
    //(since the derivatives can change)
    if (input + potentialTimestep > std::floor(input + 1.0))
    {
      input = std::floor(input + 1.0); // Set the input to the next integral time-step
    }
    else
    {
      timeStepSmallEnough = true;
    }
  }

  // Finalize the potential timestep into THE timestep
  const InputType timestep = potentialTimestep;

  // Get the index at the next timestep so we can calculate the offset
  const IndexType nextIndex = this->EvaluateToIndex(input + timestep);

  // For some reason, there's no way to convert OutputType to OffsetType
  OffsetType offset;
  for (unsigned int i = 0; i < VDimension; ++i)
  {
    offset[i] = nextIndex[i] - originalIndex[i];
  }

  // Update input timestep
  input += timestep;

  // Return the offset
  return offset;
}

template <unsigned int VDimension>
PolyLineParametricPath<VDimension>::PolyLineParametricPath()
{
  this->SetDefaultInputStepSize(0.3);
  this->m_VertexList = VertexListType::New();
}

template <unsigned int VDimension>
void
PolyLineParametricPath<VDimension>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Vertices:  " << m_VertexList << std::endl;
}
} // end namespace itk

#endif