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


namespace itk
{

template <typename TInternalComputationValueType>
ExhaustiveOptimizerv4<TInternalComputationValueType>::ExhaustiveOptimizerv4()
{
  this->m_NumberOfIterations = 0;
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::StartOptimization(bool /* doOnlyInitialization */)
{
  this->StartWalking();
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::StartWalking()
{
  itkDebugMacro("StartWalking");
  this->InvokeEvent(StartEvent());
  m_StopConditionDescription.str("");
  m_StopConditionDescription << this->GetNameOfClass() << ": Running";

  ParametersType initialPos = this->m_Metric->GetParameters();
  if (this->m_InitialPosition.GetSize() == initialPos.GetSize())
  {
    initialPos = this->m_InitialPosition; // correct size, so must have been set by the user
  }
  m_MinimumMetricValuePosition = initialPos;
  m_MaximumMetricValuePosition = initialPos;

  this->SetInitialPosition(initialPos); // store the initial position

  MeasureType initialValue = this->m_Metric->GetValue();
  m_MaximumMetricValue = initialValue;
  m_MinimumMetricValue = initialValue;

  this->m_CurrentIteration = 0;
  this->m_NumberOfIterations = 1;

  const unsigned int spaceDimension = this->m_Metric->GetParameters().GetSize();

  for (unsigned int i = 0; i < spaceDimension; ++i)
  {
    this->m_NumberOfIterations *= (2 * m_NumberOfSteps[i] + 1);
  }

  m_CurrentIndex.SetSize(spaceDimension);
  m_CurrentIndex.Fill(0);

  const ScalesType & scales = this->GetScales();
  // Make sure the scales have been set properly
  if (scales.size() != spaceDimension)
  {
    itkExceptionMacro("The size of Scales is " << scales.size() << ", but the NumberOfParameters is " << spaceDimension
                                               << '.');
  }

  // Setup first grid position.
  ParametersType position(spaceDimension);
  for (unsigned int i = 0; i < spaceDimension; ++i)
  {
    position[i] = this->GetCurrentPosition()[i] - m_NumberOfSteps[i] * m_StepLength * scales[i];
  }
  this->m_Metric->SetParameters(position);

  itkDebugMacro("Calling ResumeWalking");

  this->ResumeWalking();
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::ResumeWalking()
{
  itkDebugMacro("ResumeWalk");
  m_Stop = false;

  while (!m_Stop)
  {
    ParametersType currentPosition = this->GetCurrentPosition();

    if (m_Stop)
    {
      StopWalking();
      break;
    }

    m_CurrentValue = this->m_Metric->GetValue();

    if (m_CurrentValue > m_MaximumMetricValue)
    {
      m_MaximumMetricValue = m_CurrentValue;
      m_MaximumMetricValuePosition = currentPosition;
    }
    if (m_CurrentValue < m_MinimumMetricValue)
    {
      m_MinimumMetricValue = m_CurrentValue;
      m_MinimumMetricValuePosition = currentPosition;
    }

    if (m_Stop)
    {
      this->StopWalking();
      break;
    }

    m_StopConditionDescription.str("");
    m_StopConditionDescription << this->GetNameOfClass() << ": Running. "
                               << "@ index " << this->GetCurrentIndex() << " value is " << m_CurrentValue;

    this->InvokeEvent(IterationEvent());
    this->AdvanceOneStep();
    this->m_CurrentIteration++;
  }
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::StopWalking()
{
  itkDebugMacro("StopWalking");

  m_Stop = true;
  this->InvokeEvent(EndEvent());
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::AdvanceOneStep()
{
  itkDebugMacro("AdvanceOneStep");

  const unsigned int spaceDimension = this->m_Metric->GetParameters().GetSize();

  ParametersType newPosition(spaceDimension);
  this->IncrementIndex(newPosition);

  itkDebugMacro("new position = " << newPosition);

  this->m_Metric->SetParameters(newPosition);
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::IncrementIndex(ParametersType & newPosition)
{
  unsigned int       idx = 0;
  const unsigned int spaceDimension = this->m_Metric->GetParameters().GetSize();

  while (idx < spaceDimension)
  {
    m_CurrentIndex[idx]++;

    if (m_CurrentIndex[idx] > (2 * m_NumberOfSteps[idx]))
    {
      m_CurrentIndex[idx] = 0;
      ++idx;
    }
    else
    {
      break;
    }
  }

  if (idx == spaceDimension)
  {
    m_Stop = true;
    m_StopConditionDescription.str("");
    m_StopConditionDescription << this->GetNameOfClass() << ": "
                               << "Completed sampling of parametric space of size " << spaceDimension;
  }

  const ScalesType & scales = this->GetScales();
  for (unsigned int i = 0; i < spaceDimension; ++i)
  {
    newPosition[i] =
      (m_CurrentIndex[i] - m_NumberOfSteps[i]) * m_StepLength * scales[i] + this->GetInitialPosition()[i];
  }
}

template <typename TInternalComputationValueType>
const std::string
ExhaustiveOptimizerv4<TInternalComputationValueType>::GetStopConditionDescription() const
{
  return m_StopConditionDescription.str();
}

template <typename TInternalComputationValueType>
void
ExhaustiveOptimizerv4<TInternalComputationValueType>::SetInitialPosition(const ParametersType & param)
{
  m_InitialPosition = param;
  this->Modified();
}

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

  os << indent << "InitialPosition: " << m_InitialPosition << std::endl;
  os << indent << "CurrentValue: " << static_cast<typename NumericTraits<MeasureType>::PrintType>(m_CurrentValue)
     << std::endl;
  os << indent << "NumberOfSteps: " << static_cast<typename NumericTraits<StepsType>::PrintType>(m_NumberOfSteps)
     << std::endl;
  os << indent << "Stop: " << (m_Stop ? "On" : "Off") << std::endl;
  os << indent << "StepLength: " << m_StepLength << std::endl;
  os << indent << "CurrentIndex: " << m_CurrentIndex << std::endl;
  os << indent
     << "MaximumMetricValue: " << static_cast<typename NumericTraits<MeasureType>::PrintType>(m_MaximumMetricValue)
     << std::endl;
  os << indent
     << "MinimumMetricValue: " << static_cast<typename NumericTraits<MeasureType>::PrintType>(m_MinimumMetricValue)
     << std::endl;
  os << indent << "MinimumMetricValuePosition: " << m_MinimumMetricValuePosition << std::endl;
  os << indent << "MaximumMetricValuePosition: " << m_MaximumMetricValuePosition << std::endl;

  os << indent << "StopConditionDescription: " << m_StopConditionDescription.str() << std::endl;
}
} // end namespace itk

#endif