/*=========================================================================
 *
 *  Copyright UMC Utrecht and contributors
 *
 *  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.
 *
 *=========================================================================*/

#include "itkMoreThuenteLineSearchOptimizer.h"
#include <cmath> // For abs.
#include <limits>

namespace itk
{

/**
 * ****************** Constructor *****************************
 */

MoreThuenteLineSearchOptimizer::MoreThuenteLineSearchOptimizer()
{
  this->m_f = MeasureType{};
  this->m_dg = 0.0;
  this->m_InitialDerivativeProvided = false;
  this->m_InitialValueProvided = false;
  this->m_MaximumNumberOfIterations = 20;
  this->m_ValueTolerance = 1e-4;
  this->m_GradientTolerance = 0.9;
  this->m_IntervalTolerance = std::numeric_limits<double>::epsilon();
  this->SetMinimumStepLength(1e-20);
  this->SetMaximumStepLength(1e20);

  this->InitializeLineSearch();

} // end Constructor


/**
 * *************** SetInitialValue *****************************
 */

void
MoreThuenteLineSearchOptimizer::SetInitialValue(MeasureType value)
{
  this->m_InitialValueProvided = true;
  this->m_f = value;
  this->Modified();

} // end SetInitialValue()


/**
 * *************** SetInitialDerivative *****************************
 */

void
MoreThuenteLineSearchOptimizer::SetInitialDerivative(const DerivativeType & derivative)
{
  this->m_InitialDerivativeProvided = true;
  this->m_g = derivative;
  this->Modified();

} // end SetInitialDerivative()


/**
 * *************** GetCurrentValueAndDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer::GetCurrentValueAndDerivative(MeasureType & value, DerivativeType & derivative) const
{
  value = m_f;
  derivative = m_g;

} // end GetCurrentValueAndDerivative()


/**
 * ************************ GetCurrentDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer::GetCurrentDerivative(DerivativeType & derivative) const
{
  derivative = m_g;

} // end GetCurrentValueAndDerivative()


/**
 * ************************ GetCurrentValue *********************
 */

MoreThuenteLineSearchOptimizer::MeasureType
MoreThuenteLineSearchOptimizer::GetCurrentValue() const
{
  return m_f;

} // end GetCurrentValueAndDerivative()


/**
 * ************ GetCurrentDirectionalDerivative *********************
 */

double
MoreThuenteLineSearchOptimizer::GetCurrentDirectionalDerivative() const
{
  return m_dg;

} // end GetCurrentDirectionalDerivative()


/**
 * ************************** StartOptimization *********************
 */

void
MoreThuenteLineSearchOptimizer::StartOptimization()
{
  this->CheckSettings();

  this->SetCurrentPosition(this->GetInitialPosition());
  this->GetInitialValueAndDerivative();
  this->m_dg = this->DirectionalDerivative(this->m_g);

  this->InitializeLineSearch();

  this->InvokeEvent(StartEvent());

  if (this->m_dginit >= 0)
  {
    this->m_StopCondition = AscentSearchDirection;
    this->StopOptimization();
  }

  while (!this->m_Stop)
  {

    this->UpdateIntervalMinimumAndMaximum();
    this->BoundStep(this->m_step);
    this->PrepareForUnusualTermination();
    this->SetCurrentStepLength(this->m_step);
    this->ComputeCurrentValueAndDerivative();
    this->m_dg = this->DirectionalDerivative(this->m_g);
    this->TestConvergence(this->m_Stop);
    this->InvokeEvent(IterationEvent());
    if (this->m_Stop)
    {
      this->StopOptimization();
      break;
    }
    this->m_CurrentIteration++;
    this->ComputeNewStepAndInterval();
    this->ForceSufficientDecreaseInIntervalWidth();

  } // end while

} // end StartOptimization()


/**
 * ******************** StopOptimization******************************
 */

void
MoreThuenteLineSearchOptimizer::StopOptimization()
{
  this->m_Stop = true;
  this->InvokeEvent(EndEvent());

} // end StopOptimization()


/**
 * ******************** CheckSettings *********************************
 */

int
MoreThuenteLineSearchOptimizer::CheckSettings()
{
  if (this->GetCostFunction() == nullptr)
  {
    itkExceptionMacro("CostFunction has not been set!");
  }

  const unsigned int numberOfParameters = this->GetCostFunction()->GetNumberOfParameters();

  if (this->GetInitialPosition().GetSize() != numberOfParameters)
  {
    itkExceptionMacro("InitialPosition has incorrect dimension!");
  }

  if (this->GetLineSearchDirection().GetSize() != numberOfParameters)
  {
    itkExceptionMacro("LineSearchDirection has incorrect dimension!");
  }

  if (this->GetMinimumStepLength() <= 0.0)
  {
    itkExceptionMacro("MinimumStepLength must be higher than zero!");
  }

  if (this->GetMinimumStepLength() > this->GetMaximumStepLength())
  {
    itkExceptionMacro("MinimumStepLength must be smaller than MaximumStepLength!");
  }

  if (this->GetGradientTolerance() < this->GetValueTolerance())
  {
    itkExceptionMacro("GradientTolerance must be greater than ValueTolerance!");
  }
  return 0;

} // end CheckSettings()


/**
 * *************** GetInitialValueAndDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer::GetInitialValueAndDerivative()
{

  if (!(this->m_InitialValueProvided && this->m_InitialDerivativeProvided))
  {

    try
    {
      if (!this->m_InitialValueProvided && !this->m_InitialDerivativeProvided)
      {
        this->GetCostFunction()->GetValueAndDerivative(this->GetInitialPosition(), this->m_f, this->m_g);
      }
      else if (!this->m_InitialValueProvided)
      {
        this->m_f = this->GetCostFunction()->GetValue(this->GetInitialPosition());
      }
      else if (!this->m_InitialDerivativeProvided)
      {
        this->GetCostFunction()->GetDerivative(this->GetInitialPosition(), this->m_g);
      }
    }
    catch (const ExceptionObject &)
    {
      this->m_StopCondition = MetricError;
      // this->StopOptimization(); //not here since no start event has been generated yet

      /** Any user provided initial values/derivatives may not be
       * valid anymore */
      this->m_InitialDerivativeProvided = false;
      this->m_InitialValueProvided = false;

      throw;
    }
  } // end if no initial value or derivative provided.

  /** Any user provided initial values/derivatives are not
   * valid anymore */

  this->m_InitialDerivativeProvided = false;
  this->m_InitialValueProvided = false;

} // end GetInitialValueAndDerivative()


/**
 * ***************** InitializeLineSearch ******************************
 *
 * Set some member variables to their initial values.
 * Assumes m_f and m_dg have been set already.
 */

void
MoreThuenteLineSearchOptimizer::InitializeLineSearch()
{
  this->m_Stop = false;
  this->m_StopCondition = Unknown;
  this->m_CurrentIteration = 0;
  this->m_SufficientDecreaseConditionSatisfied = false;
  this->m_CurvatureConditionSatisfied = false;
  this->m_CurrentStepLength = 0.0;

  this->m_finit = this->m_f;
  this->m_fx = this->m_finit;
  this->m_fy = this->m_finit;
  this->m_step = this->GetInitialStepLengthEstimate();
  this->m_stepx = 0.0;
  this->m_stepy = 0.0;
  this->m_stepmin = 0.0;
  this->m_stepmax = 0.0;
  this->m_dginit = this->m_dg;
  this->m_dgx = this->m_dginit;
  this->m_dgy = this->m_dginit;
  this->m_dgtest = this->GetValueTolerance() * this->m_dginit;
  this->m_width = this->GetMaximumStepLength() - this->GetMinimumStepLength();
  this->m_width1 = this->m_width / 0.5;
  this->m_brackt = false;
  this->m_stage1 = true;
  this->m_SafeGuardedStepFailed = false;

} // end InitializeLineSearch()


/**
 * *************** UpdateIntervalMinimumAndMaximum ********************
 *
 * Set the minimum and maximum steps to correspond to the present
 * interval of uncertainty
 */

void
MoreThuenteLineSearchOptimizer::UpdateIntervalMinimumAndMaximum()
{
  const double xtrapf = 4.0;

  if (this->m_brackt)
  {
    this->m_stepmin = std::min(this->m_stepx, this->m_stepy);
    this->m_stepmax = std::max(this->m_stepx, this->m_stepy);
  }
  else
  {
    this->m_stepmin = this->m_stepx;
    this->m_stepmax = this->m_step + xtrapf * (this->m_step - this->m_stepx);
  }

} // end UpdateIntervalMinimumAndMaximum()


/**
 * ************************* BoundStep ********************************
 *
 * Force a step to be within the bounds MinimumStepLength and
 * MaximumStepLength
 */

void
MoreThuenteLineSearchOptimizer::BoundStep(double & step) const
{
  step = std::max(step, this->GetMinimumStepLength());
  step = std::min(step, this->GetMaximumStepLength());

} // end BoundStep()


/**
 * ******************** PrepareForUnusualTermination ********************
 *
 * If an unusual termination is to occur then let m_step be the lowest
 * point obtained so far
 */

void
MoreThuenteLineSearchOptimizer::PrepareForUnusualTermination()
{
  if ((this->m_brackt && (this->m_step <= this->m_stepmin || this->m_step >= this->m_stepmax)) ||
      this->m_CurrentIteration >= this->GetMaximumNumberOfIterations() - 1 || this->m_SafeGuardedStepFailed ||
      (this->m_brackt && this->m_stepmax - this->m_stepmin <= this->GetIntervalTolerance() * this->m_stepmax))
  {
    this->m_step = this->m_stepx;
  }

} // end PrepareForUnusualTermination()


/**
 * ***************** ComputeCurrentValueAndDerivative ********************
 *
 * Ask the cost function to compute m_f and m_g at the current position.
 */

void
MoreThuenteLineSearchOptimizer::ComputeCurrentValueAndDerivative()
{
  try
  {
    this->GetCostFunction()->GetValueAndDerivative(this->GetCurrentPosition(), this->m_f, this->m_g);
  }
  catch (const ExceptionObject &)
  {
    this->m_StopCondition = MetricError;
    this->StopOptimization();
    throw;
  }

} // end ComputeCurrentValueAndDerivative()


/**
 * ************************** TestConvergence ****************************
 *
 * Test for convergence.
 */

void
MoreThuenteLineSearchOptimizer::TestConvergence(bool & stop)
{
  stop = false;
  const double & step = this->m_step;

  MeasureType ftest1 = this->m_finit + step * this->m_dgtest;

  this->m_SufficientDecreaseConditionSatisfied = (this->m_f <= ftest1);
  this->m_CurvatureConditionSatisfied = (std::abs(this->m_dg) <= this->GetGradientTolerance() * (-this->m_dginit));

  if ((this->m_brackt && (step <= this->m_stepmin || step >= this->m_stepmax)) || this->m_SafeGuardedStepFailed)
  {
    this->m_StopCondition = RoundingError;
    stop = true;
  }

  if (step == this->GetMaximumStepLength() && this->m_SufficientDecreaseConditionSatisfied &&
      this->m_dg <= this->m_dgtest)
  {
    this->m_StopCondition = StepTooLarge;
    stop = true;
  }

  if (step == this->GetMinimumStepLength() &&
      (!(this->m_SufficientDecreaseConditionSatisfied) || this->m_dg >= this->m_dgtest))
  {
    this->m_StopCondition = StepTooSmall;
    stop = true;
  }

  if (this->m_CurrentIteration >= this->GetMaximumNumberOfIterations() - 1)
  {
    this->m_StopCondition = MaximumNumberOfIterations;
    stop = true;
  }

  if (this->m_brackt && this->m_stepmax - this->m_stepmin <= this->GetIntervalTolerance() * this->m_stepmax)
  {
    this->m_StopCondition = IntervalTooSmall;
    stop = true;
  }

  if (this->m_SufficientDecreaseConditionSatisfied && this->m_CurvatureConditionSatisfied)
  {
    this->m_StopCondition = StrongWolfeConditionsSatisfied;
    stop = true;
  }

} // end TestConvergence()


/**
 * ****************** ComputeNewStepAndInterval ************************
 *
 * Update the interval of uncertainty and compute the new step
 */

void
MoreThuenteLineSearchOptimizer::ComputeNewStepAndInterval()
{
  int returncode = 0;

  /** In the first stage we seek a step for which the modified
   * function has a nonpositive value and nonnegative derivative. */

  /** Stage1? */
  if (this->m_stage1 && this->m_SufficientDecreaseConditionSatisfied &&
      this->m_dg >= this->m_dginit * std::min(this->GetValueTolerance(), this->GetGradientTolerance()))
  {
    this->m_stage1 = false;
  }

  /* A modified function is used to predict the step only if
   * we have not obtained a step for which the modified
   * function has a nonpositive function value and nonnegative
   * derivative, and if a lower function value has been
   * obtained but the decrease is not sufficient. */

  if (this->m_stage1 && this->m_f <= this->m_fx && !(this->m_SufficientDecreaseConditionSatisfied))
  {
    /* Define the modified function and derivative values. */
    const double & dgtest = this->m_dgtest;

    double fm = this->m_f - this->m_step * dgtest;
    double fxm = this->m_fx - this->m_stepx * dgtest;
    double fym = this->m_fy - this->m_stepy * dgtest;
    double dgm = this->m_dg - dgtest;
    double dgxm = this->m_dgx - dgtest;
    double dgym = this->m_dgy - dgtest;

    /* Call SafeGuardedStep to update the interval of uncertainty */
    /* and to compute the new step. */
    returncode = this->SafeGuardedStep(this->m_stepx,
                                       fxm,
                                       dgxm,
                                       this->m_stepy,
                                       fym,
                                       dgym,
                                       this->m_step,
                                       fm,
                                       dgm,
                                       this->m_brackt,
                                       this->m_stepmin,
                                       this->m_stepmax);

    /* Reset the function and gradient values. */
    this->m_fx = fxm + this->m_stepx * dgtest;
    this->m_fy = fym + this->m_stepy * dgtest;
    this->m_dgx = dgxm + dgtest;
    this->m_dgy = dgym + dgtest;
  }
  else
  {
    /* Call SafeGuardedStep to update the interval of uncertainty */
    /* and to compute the new step. */
    returncode = this->SafeGuardedStep(this->m_stepx,
                                       this->m_fx,
                                       this->m_dgx,
                                       this->m_stepy,
                                       this->m_fy,
                                       this->m_dgy,
                                       this->m_step,
                                       this->m_f,
                                       this->m_dg,
                                       this->m_brackt,
                                       this->m_stepmin,
                                       this->m_stepmax);
  }

  if (returncode == 0)
  {
    this->m_SafeGuardedStepFailed = true;
  }

} // end ComputeNewStepAndInterval()


/**
 * ************** ForceSufficientDecreaseInIntervalWidth ******************
 *
 * Force a sufficient decrease in the size of the interval of uncertainty
 */

void
MoreThuenteLineSearchOptimizer::ForceSufficientDecreaseInIntervalWidth()
{
  if (this->m_brackt)
  {
    const double & stx = this->m_stepx;
    const double & sty = this->m_stepy;

    if (std::abs(sty - stx) >= .66 * this->m_width1)
    {
      this->m_step = stx + .5 * (sty - stx);
    }
    this->m_width1 = this->m_width;
    this->m_width = std::abs(sty - stx);
  }

} // end ForceSufficientDecreaseInIntervalWidth()


/**
 * ************************** SafeGuardedStep ****************************
 *
 * Advance a step along the line search direction and update
 * the interval of uncertainty. Returns 0 if an error has occurred.
 */

int
MoreThuenteLineSearchOptimizer::SafeGuardedStep(double &     stx,
                                                double &     fx,
                                                double &     dx,
                                                double &     sty,
                                                double &     fy,
                                                double &     dy,
                                                double &     stp,
                                                const double fp,
                                                const double dp,
                                                bool &       brackt,
                                                const double stpmin,
                                                const double stpmax) const
{
  /** This function is largely just a copy of the following function
   * taken from netlib/lbfgs.c */

  /** Original documentation: */

  /**    void mcstep_(stx, fx, dx, sty, fy, dy, stp, fp, dp,
   *             brackt, stpmin, stpmax, info)
   *      doublereal *stx, *fx, *dx, *sty, *fy, *dy, *stp, *fp, *dp;
   *      logical *brackt;
   *      doublereal *stpmin, *stpmax;
   *      integer *info;
   */

  /*     SUBROUTINE MCSTEP */

  /*     THE PURPOSE OF MCSTEP IS TO COMPUTE A SAFEGUARDED STEP FOR */
  /*     A LINESEARCH AND TO UPDATE AN INTERVAL OF UNCERTAINTY FOR */
  /*     A MINIMIZER OF THE FUNCTION. */

  /*     THE PARAMETER STX CONTAINS THE STEP WITH THE LEAST FUNCTION */
  /*     VALUE. THE PARAMETER STP CONTAINS THE CURRENT STEP. IT IS */
  /*     ASSUMED THAT THE DERIVATIVE AT STX IS NEGATIVE IN THE */
  /*     DIRECTION OF THE STEP. IF BRACKT IS SET TRUE THEN A */
  /*     MINIMIZER HAS BEEN BRACKETED IN AN INTERVAL OF UNCERTAINTY */
  /*     WITH ENDPOINTS STX AND STY. */

  /*     THE SUBROUTINE STATEMENT IS */

  /*       SUBROUTINE MCSTEP(STX,FX,DX,STY,FY,DY,STP,FP,DP,BRACKT, */
  /*                        STPMIN,STPMAX,INFO) */

  /*     WHERE */

  /*       STX, FX, AND DX ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE BEST STEP OBTAINED */
  /*         SO FAR. THE DERIVATIVE MUST BE NEGATIVE IN THE DIRECTION */
  /*         OF THE STEP, THAT IS, DX AND STP-STX MUST HAVE OPPOSITE */
  /*         SIGNS. ON OUTPUT THESE PARAMETERS ARE UPDATED APPROPRIATELY. */

  /*       STY, FY, AND DY ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE OTHER ENDPOINT OF */
  /*         THE INTERVAL OF UNCERTAINTY. ON OUTPUT THESE PARAMETERS ARE */
  /*         UPDATED APPROPRIATELY. */

  /*       STP, FP, AND DP ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE CURRENT STEP. */
  /*         IF BRACKT IS SET TRUE THEN ON INPUT STP MUST BE */
  /*         BETWEEN STX AND STY. ON OUTPUT STP IS SET TO THE NEW STEP. */

  /*       BRACKT IS A LOGICAL VARIABLE WHICH SPECIFIES IF A MINIMIZER */
  /*         HAS BEEN BRACKETED. IF THE MINIMIZER HAS NOT BEEN BRACKETED */
  /*         THEN ON INPUT BRACKT MUST BE SET FALSE. IF THE MINIMIZER */
  /*         IS BRACKETED THEN ON OUTPUT BRACKT IS SET TRUE. */

  /*       STPMIN AND STPMAX ARE INPUT VARIABLES WHICH SPECIFY LOWER */
  /*         AND UPPER BOUNDS FOR THE STEP. */

  /*       INFO IS AN INTEGER OUTPUT VARIABLE SET AS FOLLOWS: */
  /*         IF INFO = 1,2,3,4,5, THEN THE STEP HAS BEEN COMPUTED */
  /*         ACCORDING TO ONE OF THE FIVE CASES BELOW. OTHERWISE */
  /*         INFO = 0, AND THIS INDICATES IMPROPER INPUT PARAMETERS. */

  /*     SUBPROGRAMS CALLED */

  /*       FORTRAN-SUPPLIED ... ABS,MAX,MIN,SQRT */

  /*     ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. JUNE 1983 */
  /*     JORGE J. MORE', DAVID J. THUENTE */

  /** The info variable is now replaced by the returncode. */
  /** The variables are no longer passed as pointers, but by reference. */

  /* System generated locals */
  double d__1;

  /* Local variables */
  double sgnd, stpc, stpf, stpq, p, q, gamma, r, s, theta;
  bool   bound;

  int returncode = 0;

  /* CHECK THE INPUT PARAMETERS FOR ERRORS. */

  if ((brackt && (stp <= std::min(stx, sty) || stp >= std::max(stx, sty))) || dx * (stp - stx) >= 0. || stpmax < stpmin)
  {
    return returncode;
  }

  /* DETERMINE IF THE DERIVATIVES HAVE OPPOSITE SIGN. */

  sgnd = dp * (dx / std::abs(dx));

  /*     FIRST CASE. A HIGHER FUNCTION VALUE. */
  /*     THE MINIMUM IS BRACKETED. IF THE CUBIC STEP IS CLOSER */
  /*     TO STX THAN THE QUADRATIC STEP, THE CUBIC STEP IS TAKEN, */
  /*     ELSE THE AVERAGE OF THE CUBIC AND QUADRATIC STEPS IS TAKEN. */

  if (fp > fx)
  {
    returncode = 1;
    bound = true;
    theta = (fx - fp) * 3 / (stp - stx) + dx + dp;
    s = std::max(std::max(std::abs(theta), std::abs(dx)), std::abs(dp));
    d__1 = theta / s;
    gamma = s * std::sqrt(d__1 * d__1 - dx / s * (dp / s));
    if (stp < stx)
    {
      gamma = -gamma;
    }
    p = gamma - dx + theta;
    q = gamma - dx + gamma + dp;
    r = p / q;
    stpc = stx + r * (stp - stx);
    stpq = stx + dx / ((fx - fp) / (stp - stx) + dx) / 2 * (stp - stx);
    if (std::abs(stpc - stx) < std::abs(stpq - stx))
    {
      stpf = stpc;
    }
    else
    {
      stpf = stpc + (stpq - stpc) / 2;
    }
    brackt = true;

    /*     SECOND CASE. A LOWER FUNCTION VALUE AND DERIVATIVES OF */
    /*     OPPOSITE SIGN. THE MINIMUM IS BRACKETED. IF THE CUBIC */
    /*     STEP IS CLOSER TO STX THAN THE QUADRATIC (SECANT) STEP, */
    /*     THE CUBIC STEP IS TAKEN, ELSE THE QUADRATIC STEP IS TAKEN. */
  }
  else if (sgnd < 0.)
  {
    returncode = 2;
    bound = false;
    theta = (fx - fp) * 3 / (stp - stx) + dx + dp;
    s = std::max(std::max(std::abs(theta), std::abs(dx)), std::abs(dp));
    d__1 = theta / s;
    gamma = s * std::sqrt(d__1 * d__1 - dx / s * (dp / s));
    if (stp > stx)
    {
      gamma = -gamma;
    }
    p = gamma - dp + theta;
    q = gamma - dp + gamma + dx;
    r = p / q;
    stpc = stp + r * (stx - stp);
    stpq = stp + dp / (dp - dx) * (stx - stp);
    if (std::abs(stpc - stp) > std::abs(stpq - stp))
    {
      stpf = stpc;
    }
    else
    {
      stpf = stpq;
    }
    brackt = true;

    /*     THIRD CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
    /*     SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DECREASES. */
    /*     THE CUBIC STEP IS ONLY USED IF THE CUBIC TENDS TO INFINITY */
    /*     IN THE DIRECTION OF THE STEP OR IF THE MINIMUM OF THE CUBIC */
    /*     IS BEYOND STP. OTHERWISE THE CUBIC STEP IS DEFINED TO BE */
    /*     EITHER STPMIN OR STPMAX. THE QUADRATIC (SECANT) STEP IS ALSO */
    /*     COMPUTED AND IF THE MINIMUM IS BRACKETED THEN THE THE STEP */
    /*     CLOSEST TO STX IS TAKEN, ELSE THE STEP FARTHEST AWAY IS TAKEN. */
  }
  else if (std::abs(dp) < std::abs(dx))
  {
    returncode = 3;
    bound = true;
    theta = (fx - fp) * 3 / (stp - stx) + dx + dp;
    s = std::max(std::max(std::abs(theta), std::abs(dx)), std::abs(dp));

    /* THE CASE GAMMA = 0 ONLY ARISES IF THE CUBIC DOES NOT TEND */
    /* TO INFINITY IN THE DIRECTION OF THE STEP. */

    d__1 = theta / s;
    d__1 = d__1 * d__1 - dx / s * (dp / s);
    gamma = s * std::sqrt(std::max(0., d__1));
    if (stp > stx)
    {
      gamma = -gamma;
    }
    p = gamma - dp + theta;
    q = gamma + (dx - dp) + gamma;
    r = p / q;
    if (r < 0. && gamma != 0.)
    {
      stpc = stp + r * (stx - stp);
    }
    else if (stp > stx)
    {
      stpc = stpmax;
    }
    else
    {
      stpc = stpmin;
    }
    stpq = stp + dp / (dp - dx) * (stx - stp);
    if (brackt)
    {
      if (std::abs(stp - stpc) < std::abs(stp - stpq))
      {
        stpf = stpc;
      }
      else
      {
        stpf = stpq;
      }
    }
    else
    {
      if (std::abs(stp - stpc) > std::abs(stp - stpq))
      {
        stpf = stpc;
      }
      else
      {
        stpf = stpq;
      }
    }

    /*     FOURTH CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
    /*     SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DOES */
    /*     NOT DECREASE. IF THE MINIMUM IS NOT BRACKETED, THE STEP */
    /*     IS EITHER STPMIN OR STPMAX, ELSE THE CUBIC STEP IS TAKEN. */
  }
  else
  {
    returncode = 4;
    bound = false;
    if (brackt)
    {
      theta = (fp - fy) * 3 / (sty - stp) + dy + dp;
      s = std::max(std::max(std::abs(theta), std::abs(dy)), std::abs(dp));
      d__1 = theta / s;
      gamma = s * std::sqrt(d__1 * d__1 - dy / s * (dp / s));
      if (stp > sty)
      {
        gamma = -gamma;
      }
      p = gamma - dp + theta;
      q = gamma - dp + gamma + dy;
      r = p / q;
      stpc = stp + r * (sty - stp);
      stpf = stpc;
    }
    else if (stp > stx)
    {
      stpf = stpmax;
    }
    else
    {
      stpf = stpmin;
    }
  }

  /*     UPDATE THE INTERVAL OF UNCERTAINTY. THIS UPDATE DOES NOT */
  /*     DEPEND ON THE NEW STEP OR THE CASE ANALYSIS ABOVE. */

  if (fp > fx)
  {
    sty = stp;
    fy = fp;
    dy = dp;
  }
  else
  {
    if (sgnd < 0.)
    {
      sty = stx;
      fy = fx;
      dy = dx;
    }
    stx = stp;
    fx = fp;
    dx = dp;
  }

  /*     COMPUTE THE NEW STEP AND SAFEGUARD IT. */

  stpf = std::clamp(stpf, stpmin, stpmax);
  stp = stpf;
  if (brackt && bound)
  {
    if (sty > stx)
    {
      d__1 = stx + (sty - stx) * .66f;
      stp = std::min(d__1, stp);
    }
    else
    {
      d__1 = stx + (sty - stx) * .66f;
      stp = std::max(d__1, stp);
    }
  }

  return returncode;

} // end SafeGuardedStep()


/**
 * ****************** PrintSelf *****************************
 */

void
MoreThuenteLineSearchOptimizer::PrintSelf(std::ostream & os, Indent indent) const
{
  /** Call the superclass' PrintSelf. */
  Superclass::PrintSelf(os, indent);

  os << indent << "m_CurrentIteration: " << this->m_CurrentIteration << std::endl;
  os << indent << "m_InitialDerivativeProvided: " << (this->m_InitialDerivativeProvided ? "true" : "false")
     << std::endl;
  os << indent << "m_InitialValueProvided: " << (this->m_InitialValueProvided ? "true" : "false") << std::endl;
  os << indent << "m_StopCondition: " << this->m_StopCondition << std::endl;
  os << indent << "m_Stop: " << (this->m_Stop ? "true" : "false") << std::endl;
  os << indent
     << "m_SufficientDecreaseConditionSatisfied: " << (this->m_SufficientDecreaseConditionSatisfied ? "true" : "false")
     << std::endl;
  os << indent << "m_CurvatureConditionSatisfied: " << (this->m_CurvatureConditionSatisfied ? "true" : "false")
     << std::endl;

  os << indent << "m_step: " << this->m_step << std::endl;
  os << indent << "m_stepx: " << this->m_stepx << std::endl;
  os << indent << "m_stepy: " << this->m_stepy << std::endl;
  os << indent << "m_stepmin: " << this->m_stepmin << std::endl;
  os << indent << "m_stepmax: " << this->m_stepmax << std::endl;

  os << indent << "m_f: " << this->m_f << std::endl;
  os << indent << "m_fx: " << this->m_fx << std::endl;
  os << indent << "m_fy: " << this->m_fy << std::endl;
  os << indent << "m_finit: " << this->m_finit << std::endl;

  os << indent << "m_g: " << this->m_g << std::endl;
  os << indent << "m_dg: " << this->m_dg << std::endl;
  os << indent << "m_dginit: " << this->m_dginit << std::endl;
  os << indent << "m_dgx: " << this->m_dgx << std::endl;
  os << indent << "m_dgy: " << this->m_dgy << std::endl;
  os << indent << "m_dgtest: " << this->m_dgtest << std::endl;

  os << indent << "m_width: " << this->m_width << std::endl;
  os << indent << "m_width1: " << this->m_width1 << std::endl;
  os << indent << "m_brackt: " << (this->m_brackt ? "true" : "false") << std::endl;
  os << indent << "m_stage1: " << (this->m_stage1 ? "true" : "false") << std::endl;
  os << indent << "m_SafeGuardedStepFailed: " << (this->m_SafeGuardedStepFailed ? "true" : "false") << std::endl;

  os << indent << "m_MaximumNumberOfIterations: " << this->m_MaximumNumberOfIterations << std::endl;
  os << indent << "m_ValueTolerance: " << this->m_ValueTolerance << std::endl;
  os << indent << "m_GradientTolerance: " << this->m_GradientTolerance << std::endl;
  os << indent << "m_IntervalTolerance: " << this->m_IntervalTolerance << std::endl;

} // end PrintSelf()


} // end namespace itk
