/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  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 itkHoughTransform2DCirclesImageFilter_hxx
#define itkHoughTransform2DCirclesImageFilter_hxx

#include "itkHoughTransform2DCirclesImageFilter.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkDiscreteGaussianImageFilter.h"
#include "itkGaussianDerivativeImageFunction.h"
#include "itkMinimumMaximumImageCalculator.h"
#include "itkMath.h"

namespace itk
{
template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::HoughTransform2DCirclesImageFilter()
{
  this->SetNumberOfRequiredInputs( 1 );
}

template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::SetRadius(double radius)
{
  this->SetMinimumRadius(radius);
  this->SetMaximumRadius(radius);
}

template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::EnlargeOutputRequestedRegion(DataObject *output)
{
  Superclass::EnlargeOutputRequestedRegion(output);
  output->SetRequestedRegionToLargestPossibleRegion();
}

template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::GenerateInputRequestedRegion()
{
  Superclass::GenerateInputRequestedRegion();
  if ( this->GetInput() )
    {
    InputImagePointer image =
      const_cast< InputImageType * >( this->GetInput() );
    image->SetRequestedRegionToLargestPossibleRegion();
    }
}


template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::VerifyPreconditions() ITKv5_CONST
{
  Superclass::VerifyPreconditions();

  if ( ! (m_GradientNormThreshold >= 0.0) )
  {
    itkExceptionMacro("Failed precondition: GradientNormThreshold >= 0.");
  }
}


template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::GenerateData()
{
  // Get the input and output pointers
  const InputImageConstPointer inputImage = this->GetInput(0);
  const OutputImagePointer     outputImage = this->GetOutput(0);

  // Allocate the output
  this->AllocateOutputs();
  outputImage->FillBuffer(0);

  using DoGFunctionType = GaussianDerivativeImageFunction< InputImageType >;
  const auto DoGFunction = DoGFunctionType::New();
  DoGFunction->SetSigma(m_SigmaGradient);
  DoGFunction->SetUseImageSpacing(m_UseImageSpacing);
  // Set input image _after_ setting the other GaussianDerivative properties,
  // to avoid multiple kernel recomputation within GaussianDerivativeImageFunction.
  DoGFunction->SetInputImage(inputImage);

  m_RadiusImage = RadiusImageType::New();

  m_RadiusImage->SetRegions( outputImage->GetLargestPossibleRegion() );
  m_RadiusImage->SetOrigin( inputImage->GetOrigin() );
  m_RadiusImage->SetSpacing( inputImage->GetSpacing() );
  m_RadiusImage->SetDirection( inputImage->GetDirection() );
  m_RadiusImage->Allocate( true ); // initialize buffer to zero

  ImageRegionConstIteratorWithIndex< InputImageType > image_it( inputImage,
    inputImage->GetRequestedRegion() );
  image_it.GoToBegin();

  const ImageRegion< 2 > & region = outputImage->GetRequestedRegion();

  while ( !image_it.IsAtEnd() )
    {
    if ( image_it.Get() > m_Threshold )
      {
      const Index< 2 > inputIndex = image_it.GetIndex();
      const typename DoGFunctionType::VectorType grad =
        DoGFunction->DoGFunctionType::EvaluateAtIndex( inputIndex );

      double Vx = grad[0];
      double Vy = grad[1];

      const double norm = std::sqrt(Vx * Vx + Vy * Vy);

      // if the gradient is not flat (using GradientNormThreshold to estimate flatness)
      if ( norm > m_GradientNormThreshold )
        {
        Vx /= norm;
        Vy /= norm;

        for ( double angle = -m_SweepAngle; angle <= m_SweepAngle; angle += 0.05 )
          {
          double i = m_MinimumRadius;
          double distance;

          do
            {
            const Index< 2 > outputIndex =
              {{
              Math::Round<IndexValueType>( inputIndex[0] - i * ( Vx * std::cos(angle) + Vy * std::sin(angle) ) ),
              Math::Round<IndexValueType>( inputIndex[1] - i * ( Vx * std::sin(angle) + Vy * std::cos(angle) ) )
              }};

            if ( region.IsInside(outputIndex) )
              {
              distance = std::sqrt(static_cast<double>((outputIndex[0] - inputIndex[0]) * (outputIndex[0] - inputIndex[0])
                                 + (outputIndex[1] - inputIndex[1]) * (outputIndex[1] - inputIndex[1])));

              ++outputImage->GetPixel(outputIndex);
              m_RadiusImage->GetPixel(outputIndex) += distance;
              }
            else
              {
              break;
              }
            ++i;
            }
          while ( distance < m_MaximumRadius );
          }
        }
      }
    ++image_it;
    }

  // Compute the average radius
  ImageRegionConstIterator< OutputImageType > output_it( outputImage,
    outputImage->GetLargestPossibleRegion() );
  ImageRegionIterator< RadiusImageType > radius_it( m_RadiusImage,
    m_RadiusImage->GetLargestPossibleRegion() );
  output_it.GoToBegin();
  radius_it.GoToBegin();
  while ( !output_it.IsAtEnd() )
    {
    if ( output_it.Get() > 1 )
      {
      radius_it.Value() /= output_it.Get();
      }
    ++output_it;
    ++radius_it;
    }
}

template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
typename HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >::CirclesListType &
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::GetCircles()
{
  // Make sure that all the required inputs exist and have a non-null value
  this->VerifyPreconditions();

  if ( this->GetMTime() == m_OldModifiedTime )
    {
    // If the filter has not been updated
    return m_CirclesList;
    }

  if( m_RadiusImage.IsNull() )
    {
    itkExceptionMacro(<<"Update() must be called before GetCircles().");
    }

  m_CirclesList.clear();

  if ( m_NumberOfCircles > 0 )
    {
    // Blur the accumulator in order to find the maximum
    using InternalImageType = Image< float, 2 >;

    // The variable "outputImage" is only used as input to gaussianFilter.
    // It should not be modified, because GetOutput(0) should not be changed.
    const auto outputImage = OutputImageType::New();
    outputImage->Graft(this->GetOutput(0));

    const auto gaussianFilter = DiscreteGaussianImageFilter< OutputImageType, InternalImageType >::New();

    gaussianFilter->SetInput(outputImage); // The output is the accumulator image
    gaussianFilter->SetVariance(m_Variance);
    gaussianFilter->SetUseImageSpacing(m_UseImageSpacing);

    gaussianFilter->Update();
    const InternalImageType::Pointer postProcessImage = gaussianFilter->GetOutput();

    const auto minMaxCalculator = MinimumMaximumImageCalculator< InternalImageType >::New();
    ImageRegionIterator< InternalImageType > it_input( postProcessImage,
      postProcessImage->GetLargestPossibleRegion() );

    CirclesListSizeType circles = 0;

    // Find maxima
    // Break out of "forever loop" as soon as the requested number of circles is found.
    for(;;)
      {
      minMaxCalculator->SetImage(postProcessImage);
      minMaxCalculator->ComputeMaximum();

      if ( minMaxCalculator->GetMaximum() <= 0 )
        {
        // When all pixel values in 'postProcessImage' are zero or less, no more circles
        // should be found. Note that a zero in 'postProcessImage' might correspond to a
        // zero in the accumulator image, or it might be that the pixel is within a
        // removed disc around a previously found circle center.
        break;
        }

      const InternalImageType::IndexType indexOfMaximum = minMaxCalculator->GetIndexOfMaximum();

      // Create a Circle Spatial Object
      const auto Circle = CircleType::New();
      Circle->SetId(static_cast<int>( circles ));
      Circle->SetRadiusInObjectSpace( m_RadiusImage->GetPixel( indexOfMaximum ) );

      CircleType::PointType center;
      center[0] = indexOfMaximum[0];
      center[1] = indexOfMaximum[1];
      Circle->SetCenterInObjectSpace(center);
      Circle->Update();

      m_CirclesList.push_back(Circle);

      circles++;
      if ( circles >= m_NumberOfCircles )
        {
        break;
        }

      // Remove a black disc from the Hough space domain
      for ( double angle = 0; angle <= 2 * itk::Math::pi; angle += itk::Math::pi / 1000 )
        {
        for ( double length = 0; length < m_DiscRadiusRatio *
          Circle->GetRadiusInObjectSpace()[0]; length += 1 )
          {
          const Index< 2 > index =
            {{
            Math::Round<IndexValueType>( indexOfMaximum[0] + length * std::cos(angle) ),
            Math::Round<IndexValueType>( indexOfMaximum[1] + length * std::sin(angle) )
            }};

          if ( postProcessImage->GetLargestPossibleRegion().IsInside(index) )
            {
            postProcessImage->SetPixel(index, 0);
            }
          }
        }
      }
    }

  m_OldModifiedTime = this->GetMTime();
  return m_CirclesList;
}

template< typename TInputPixelType, typename TOutputPixelType, typename TRadiusPixelType >
void
HoughTransform2DCirclesImageFilter< TInputPixelType, TOutputPixelType, TRadiusPixelType >
::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);

  os << indent << "Threshold: " << m_Threshold << std::endl;
  os << indent << "Gradient Norm Threshold: " << m_GradientNormThreshold << std::endl;
  os << indent << "Minimum Radius:  " << m_MinimumRadius << std::endl;
  os << indent << "Maximum Radius: " << m_MaximumRadius << std::endl;
  os << indent << "Derivative Scale : " << m_SigmaGradient << std::endl;
  os << indent << "Number Of Circles: " << m_NumberOfCircles << std::endl;
  os << indent << "Disc Radius Ratio: " << m_DiscRadiusRatio << std::endl;
  os << indent << "Accumulator blur variance: " << m_Variance << std::endl;
  os << indent << "Sweep angle : " << m_SweepAngle << std::endl;
  os << indent << "UseImageSpacing: " << m_UseImageSpacing << std::endl;

  itkPrintSelfObjectMacro( RadiusImage );

  os << indent << "CirclesList: " << std::endl;
  unsigned int i = 0;
  auto it = m_CirclesList.begin();
  while( it != m_CirclesList.end() )
    {
    os << indent << "[" << i << "]: " << *it << std::endl;
    ++it;
    ++i;
    }

  os << indent << "OldModifiedTime: "
    << NumericTraits< ModifiedTimeType >::PrintType( m_OldModifiedTime )
    << std::endl;
}
} // end namespace

#endif
