/***************************************************************************
**
**  This file is part of ArrayCore.
**
**  ArrayCore is free software: you can redistribute it and/or modify
**  it under the terms of the GNU General Public License as published by
**  the Free Software Foundation, either version 3 of the License, or
**  (at your option) any later version.
**
**  ArrayCore is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.
**
**  You should have received a copy of the GNU General Public License
**  along with Foobar.  If not, see <http://www.gnu.org/licenses/>
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2019-07-01
**  Copyright: 2019
**    Marc Wathelet (ISTerre, Grenoble, France)
**
***************************************************************************/

#include "FKPolarGradientSearch.h"
#if 0
namespace ArrayCore {

  /*!
    \class FKPolarGradientSearch FKPolarGradientSearch.h
    \brief Brief description of class still missing

    Full description of class still missing
  */

#define TRACE_GRADIENTSEARCH

  /*!
    Description of constructor still missing
  */
  FKPolarGradientSearch::FKPolarGradientSearch()
    : FKGridSearch()
  {
    TRACE;
    _searchGridIndexList=nullptr;
    // It does not hurt to init even if it is useless for three components
    _s1.fkp.initOneComponent();
    _s2.fkp.initOneComponent();
    _refinep.initOneComponent();
  }

  /*!
    Description of destructor still missing
  */
  FKPolarGradientSearch::~FKPolarGradientSearch()
  {
    TRACE;
    delete [] _searchGridIndexList;
  }

  void FKPolarGradientSearch::setGrid(const Point& gridSize)
  {
    //_cacheGridStep=cacheFactor;
    // Round grid size to integers in the scaled space
    _gridSize=gridSize;

    _climbStep=0.25;
    //_minStep=0.05*_climbStep;
    _minStep=0.005*_climbStep;
    _maxClimb=::sqrt(2.0);
    FKGridSearch::setGrid(-gridSize.x()*_realToScaled.x(), gridSize.x()*_realToScaled.x(), 1.0,
                          -gridSize.y()*_realToScaled.y(), gridSize.y()*_realToScaled.y(), 1.0,
                          -gridSize.z()*_realToScaled.z(), gridSize.z()*_realToScaled.z(), 1.0);
    // Function must be set to init grid
    _refinep.setSteering(function()->steering());
  }

  void FKPolarGradientSearch::localMax(int nMax, double absThres, double relThres)
  {
    clearMaxima();
#ifdef TRACE_GRADIENTSEARCH
    printf("Looking for local maxima\n");
#endif
    for(int i=0; i<_nxyz; i++) {
      freeClimbing(_coordinates[i], _climbStep, _coordinates[i]);
    }
    FunctionSearch::localMax(nMax, absThres, relThres);
    FunctionSearch::unique();
  }

  void FKPolarGradientSearch::freeClimbing(Point p, double step, const Point& p0)
  {
    const AbstractFKFunction& f=*function();
    if(!f.isInsideLimits(p*_scaledToReal)) {
      return;
    }
#ifdef TRACE_GRADIENTSEARCH
    printf("  free climbing from (%lf, %lf, %lf)\n", p.x()*_scaledToReal.x(),
                                                     p.y()*_scaledToReal.y(),
                                                     Angle::radiansToDegrees(p.z()*_scaledToReal.z()));
#endif
    State * s=&_s1;
    State * sOld=&_s2;

    int iSteps=0;

    s->fkp.setSteering(f.steering());

    sOld->p=p;
    sOld->fkp.setSteering(f.steering());
    f.setGradient(sOld->p*_scaledToReal, sOld->fkp);
    sOld->dp=sOld->fkp.gradient()*_realToScaled;
    sOld->dp*=step/sOld->dp.length();

    do {
      s->p=sOld->p+sOld->dp;
      f.setGradient(s->p*_scaledToReal, s->fkp);
      s->dp=s->fkp.gradient()*_realToScaled;
      s->dp*=step/s->dp.length();
      iSteps++;
#ifdef TRACE_GRADIENTSEARCH
      printf("      (%lf, %lf, %lf)\n",
             s->p.x()*_scaledToReal.x(), s->p.y()*_scaledToReal.y(), Angle::radiansToDegrees(s->p.z()*_scaledToReal.z()));
      FunctionSearchMaximum * max=f.createMaximum(s->fkp.value(), s->p*_scaledToReal);
      if(max) {
        _maxima.append(max);
      }
#endif
      // Value must increase and gradient cannot drastically change in direction
      // step is already set to a reasonable size (from peak width)
      // Second term of Wolfe condition is dropped (almost useless if small coef,
      // see textbook)
      bool validStep=s->fkp.value()>sOld->fkp.value() &&
                     sOld->dp.scalarProduct(s->dp)>0.0;
      if(!validStep || step<_climbStep) {
        f.setHessian(sOld->p*_scaledToReal, sOld->fkp);
        if(sOld->fkp.concavity(&_hessianScaledToReal)<0.0) {
#ifdef TRACE_GRADIENTSEARCH
          printf("    found negative concativity after %i steps\n", iSteps);
#endif
          Point dp=sOld->fkp.newtonStep()*_realToScaled;
          refineMax(sOld->p, dp);
          return;
        }
      }
      if(validStep) {
        qSwap(sOld, s);
        /*if(step<_climbStep) {
          step*=1.25;
        }*/
      } else {
        if(step>_minStep) {
          step*=0.5;
          sOld->dp*=step/sOld->dp.length();
#ifdef TRACE_GRADIENTSEARCH
          printf("    reducing step to %lf after %i steps\n", step, iSteps);
#endif
        } else {
#ifdef TRACE_GRADIENTSEARCH
          printf("      too small step %lf after %i steps\n",
                 step, iSteps);
#endif
          return;
        }
      }
    } while((sOld->p-p0).length()<_maxClimb);
#ifdef TRACE_GRADIENTSEARCH
    printf("    reaching max distance at (%lf, %lf, %lf) after %i steps\n",
           sOld->p.x()*_scaledToReal.x(),
           sOld->p.y()*_scaledToReal.y(),
           Angle::radiansToDegrees(sOld->p.z()*_scaledToReal.z()),
           iSteps);
#endif
  }

  /*!
    At wavenumber \a k the 2D concavity must be negative
    to unsure a proper convergence to a maximum

    Return false if no convergence.
  */
  void FKPolarGradientSearch::refineMax(Point p, Point dp)
  {
    double maxStep=_climbStep;
    const AbstractFKFunction& f=*function();
    int iNewtonSteps=0;
    Point oldp;
    double l=dp.length();
    if(l>maxStep) {
      dp*=maxStep/l;
    }
    while(dp.length()>p.length()*_precision) {
      oldp=p;
      p+=dp;
#ifdef TRACE_GRADIENTSEARCH
          printf("      (%lf, %lf, %lf)\n",
                 p.x()*_scaledToReal.x(),
                 p.y()*_scaledToReal.y(),
                 Angle::radiansToDegrees(p.z()*_scaledToReal.z()));
#endif
      iNewtonSteps++;
      f.setGradient(p*_scaledToReal, _refinep);
      f.setHessian(p*_scaledToReal, _refinep);
      if(_refinep.concavity(&_hessianScaledToReal)<0.0) {
        Point newdp=_refinep.newtonStep()*_realToScaled;
        l=newdp.length();
        if(l>maxStep) {
          newdp*=maxStep/l;
        }
        // Check that computed step is not the exact opposite of last step
        Point diff=dp;
        diff+=newdp;
        if(diff.length()<dp.length()*0.01) {
          maxStep*=0.5;
          dp=newdp*(maxStep/newdp.length());
#ifdef TRACE_GRADIENTSEARCH
          printf("      reducing Newton max step to %lf after %i steps\n",
                 maxStep, iNewtonSteps);
#endif
        } else {
          dp=newdp;
        }
      } else {
        if(maxStep>_minStep){
          maxStep*=0.5;
          p=oldp;
          dp*=maxStep/dp.length();
#ifdef TRACE_GRADIENTSEARCH
          printf("      reducing Newton max step to %lf after %i steps\n",
                 maxStep, iNewtonSteps);
#endif
        } else {
#ifdef TRACE_GRADIENTSEARCH
          printf("      too small Newton max step %lf after %i steps\n",
                 maxStep, iNewtonSteps);
#endif
          return;
        }
      }
    }
#ifdef TRACE_GRADIENTSEARCH
    printf("      peak found at (%lf, %lf, %lf) after %i Newton iterations\n",
           p.x()*_scaledToReal.x(),
           p.y()*_scaledToReal.y(),
           Angle::radiansToDegrees(p.z()*_scaledToReal.z()),
           iNewtonSteps);
#endif
    FunctionSearchMaximum * max=f.createMaximum(_refinep.value(), p*_scaledToReal);
    if(max) {
      _maxima.append(max);
    }
  }

} // namespace ArrayCore

#endif
