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

#include "itkMath.h"

namespace itk
{
/**
 *
 */
template <typename TLevelSet, typename TAuxValue, unsigned int VAuxDimension>
LevelSetVelocityNeighborhoodExtractor<TLevelSet, TAuxValue, VAuxDimension>::LevelSetVelocityNeighborhoodExtractor()
{
  m_AuxInsideValues = nullptr;
  m_AuxOutsideValues = nullptr;
  for (unsigned int i = 0; i < VAuxDimension; ++i)
  {
    m_AuxImage[i] = nullptr;
  }
}

/*
 *
 */
template <typename TLevelSet, typename TAuxValue, unsigned int VAuxDimension>
void
LevelSetVelocityNeighborhoodExtractor<TLevelSet, TAuxValue, VAuxDimension>::PrintSelf(std::ostream & os,
                                                                                      Indent         indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Input aux image: [";
  unsigned int j;
  for (j = 0; j + 1 < VAuxDimension; ++j)
  {
    os << m_AuxImage[j].GetPointer() << ", ";
  }
  os << m_AuxImage[j].GetPointer() << ']' << std::endl;
  os << indent << "AuxInsideValues: " << m_AuxInsideValues.GetPointer() << std::endl;
  os << indent << "AuxOutsideValues: " << m_AuxOutsideValues.GetPointer() << std::endl;
}

/*
 *
 */
template <typename TLevelSet, typename TAuxValue, unsigned int VAuxDimension>
void
LevelSetVelocityNeighborhoodExtractor<TLevelSet, TAuxValue, VAuxDimension>::Initialize()
{
  this->Superclass::Initialize();

  // create new empty auxiliary variable containers
  m_AuxInsideValues = AuxValueContainer::New();
  m_AuxOutsideValues = AuxValueContainer::New();
}

/*
 *
 */
template <typename TLevelSet, typename TAuxValue, unsigned int VAuxDimension>
double
LevelSetVelocityNeighborhoodExtractor<TLevelSet, TAuxValue, VAuxDimension>::CalculateDistance(Index & index)
{
  double distance = this->Superclass::CalculateDistance(index);

  if (distance >= this->GetLargeValue())
  {
    return distance;
  }

  double             centerValue[VAuxDimension];
  AuxValueType       auxPixel;
  AuxValueVectorType auxVector;

  for (unsigned int k = 0; k < VAuxDimension; ++k)
  {
    auxPixel = m_AuxImage[k]->GetPixel(index);
    centerValue[k] = static_cast<double>(auxPixel);
  }

  // if distance is zero, insert point in inside container
  if (this->GetLastPointIsInside())
  {
    for (unsigned int k = 0; k < VAuxDimension; ++k)
    {
      auxVector[k] = centerValue[k];
    }

    m_AuxInsideValues->InsertElement(m_AuxInsideValues->Size(), auxVector);

    return distance;
  }

  double                        denom = 0.0;
  double                        numer[VAuxDimension];
  typename Superclass::NodeType neighNode;

  for (unsigned int k = 0; k < VAuxDimension; ++k)
  {
    numer[k] = 0.0;
  }

  // The extend velocity value is a weighted value of
  // the speed values at point used in the computation
  // of the distance by the superclass.
  //
  // The weights is proportional to one over the square
  // of distance along the grid line to the zero set
  // crossing.

  for (unsigned int j = 0; j < SetDimension; ++j)
  {
    neighNode = this->GetNodeUsedInCalculation(j);
    if (neighNode.GetValue() >= this->GetLargeValue())
    {
      break;
    }

    denom += 1.0 / itk::Math::sqr(static_cast<double>(neighNode.GetValue()));
    for (unsigned int k = 0; k < VAuxDimension; ++k)
    {
      auxPixel = m_AuxImage[k]->GetPixel(neighNode.GetIndex());
      numer[k] += static_cast<double>(auxPixel) / itk::Math::sqr(static_cast<double>(neighNode.GetValue()));
    }
  }

  for (unsigned int k = 0; k < VAuxDimension; ++k)
  {
    numer[k] /= denom;
    auxVector[k] = numer[k];
  }

  if (this->GetLastPointIsInside())
  {
    m_AuxInsideValues->InsertElement(m_AuxInsideValues->Size(), auxVector);
  }
  else
  {
    m_AuxOutsideValues->InsertElement(m_AuxOutsideValues->Size(), auxVector);
  }

  return distance;
}
} // namespace itk

#endif