/***************************************************************************
**
**  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-04-09
**  Copyright: 2019
**    Marc Wathelet (ISTerre, Grenoble, France)
**
***************************************************************************/

#include "FKGradientSearch.h"
#include "FKPower.h"

namespace ArrayCore {

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

    Full description of class still missing
  */
#if 0
#define TRACE_GRADIENTSEARCH

  /*!
    Description of constructor still missing
  */
  FKGradientSearch::FKGradientSearch()
    : 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
  */
  FKGradientSearch::~FKGradientSearch()
  {
    TRACE;
    delete [] _searchGridIndexList;
  }

  /*!
    x_s=realToScaled*x_r

    x_r=scaledToReal*x_s
  */
  void FKGradientSearch::setScales(const Point& scaledToRealFactors)
  {
    _scaledToReal=scaledToRealFactors;
    _realToScaled.setX(1.0/_scaledToReal.x());
    _realToScaled.setY(1.0/_scaledToReal.y());
    _realToScaled.setZ(1.0/_scaledToReal.z());
    DoubleMatrix s(3, 1);
    s.at(0,0)=_scaledToReal.x();
    s.at(1,0)=_scaledToReal.y();
    s.at(2,0)=_scaledToReal.z();
    _hessianScaledToReal=s*s.transposed();
  }

  void FKGradientSearch::setGrid(const Point& gridSize)
  {
    //_cacheGridStep=cacheFactor;
    // Round grid size to integers in the scaled space
    _gridSize.setX(qFloor(gridSize.x()*_realToScaled.x())*_scaledToReal.x());
    _gridSize.setY(qFloor(gridSize.y()*_realToScaled.y())*_scaledToReal.y());
    _gridSize.setZ(qFloor(gridSize.z()*_realToScaled.z())*_scaledToReal.z());

    //_invCacheGridStep.setX(1.0/_cacheGridStep.x());
    //_invCacheGridStep.setY(1.0/_cacheGridStep.y());
    //_invCacheGridStep.setZ(1.0/_cacheGridStep.z());

    _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());

    /*int r[3];
    r[0]=qRound(gridStep.x()/_cacheGridStep.x());
    r[1]=qRound(gridStep.y()/_cacheGridStep.y());
    r[2]=qRound(gridStep.z()/_cacheGridStep.z());
    // Approximately center the sub grid around origin
    int n[3];
    n[0]=qFloor(static_cast<double>(_nx)/static_cast<double>(r[0]));
    n[1]=qFloor(static_cast<double>(_ny)/static_cast<double>(r[1]));
    n[2]=qFloor(static_cast<double>(_nz)/static_cast<double>(r[2]));
    int starti[3];
    starti[0]=(_nx-r[0]*n[0])/2;
    starti[1]=(_ny-r[1]*n[1])/2;
    starti[2]=(_nz-r[2]*n[2])/2;
    n[0]=qCeil(static_cast<double>(_nx)/static_cast<double>(r[0]));
    n[1]=qCeil(static_cast<double>(_ny)/static_cast<double>(r[1]));
    n[2]=qCeil(static_cast<double>(_nz)/static_cast<double>(r[2]));
    _searchGridCount=n[0]*n[1]*n[2];
    _searchGridIndexList=new int[_searchGridCount];
    int k=0;
    for(int iz=starti[2]; iz<_nz; iz+=r[2]) {
      for(int iy=starti[1]; iy<_ny; iy+=r[1]) {
        for(int ix=starti[0]; ix<_nx; ix+=r[0]) {
          _searchGridIndexList[k++]=iz*_nxy+iy*_nx+ix;
        }
      }
    }
    ASSERT(k==_searchGridCount);*/
  }

  void FKGradientSearch::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]);
      //gridClimbing(_searchGridIndexList[i]);
    }
    FunctionSearch::localMax(nMax, absThres, relThres);
    FunctionSearch::unique();
  }

  /*int FKGradientSearch::nextGridIndex(const State * s)
  {
    Point p=_coordinates[s->i];
    p+=s->dp;
#ifdef TRACE_GRADIENTSEARCH
    printf("    at (%lf, %lf, %lf)\n", p.x(), p.y(), p.z());
#endif
    int ix=qRound((p.x()+_gridSize.x())*_invCacheGridStep.x());
    int iy=qRound((p.y()+_gridSize.y())*_invCacheGridStep.y());
    int iz=qRound((p.z()+_gridSize.z())*_invCacheGridStep.z());
    if(ix<0 || ix>=_nx || iy<0 || iy>=_ny || iz<0 || iz>=_nz) {
      return -1;
    } else {
      return iz*_nxy+iy*_nx+ix;
    }
  }*/

  /*void FKGradientSearch::scaleStep(Point& step, const Point& refStep)
  {
    double scale=refStep.x()/fabs(step.x());
    double s=refStep.y()/fabs(step.y());
    if(s<scale) {
      scale=s;
    }
    if(step.z()!=0.0) {
      s=refStep.z()/fabs(step.z());
      if(s<scale) {
        scale=s;
      }
    }
    step*=scale;
  }*/

#if 0
  void FKGradientSearch::gridClimbing(int startGridIndex)
  {
    const AbstractFKFunction& f=*function();
    Point k0=_coordinates[startGridIndex];
    if(!f.isInsideLimits(k0)) {
      return;
    }
#ifdef TRACE_GRADIENTSEARCH
    printf("  grid climbing from (%lf, %lf, %lf)\n", k0.x(), k0.y(), k0.z());
#endif
    State * s=&_s1;
    State * sOld=&_s2;

    int iSteps=0;
    Point step=_climbStep;

    sOld->i=startGridIndex;
    sOld->p.setSteering(f.steering(sOld->i));
    sOld->dp=f.gradient(sOld->p);
    scaleStep(sOld->dp, step);

    Point dk;
    do {
      s->i=nextGridIndex(sOld);
      if(s->i<0) {
#ifdef TRACE_GRADIENTSEARCH
        printf("  Going outside limits\n");
#endif
        return;
      }
      s->p.setSteering(f.steering(s->i));
      s->dp=f.gradient(s->p);
      scaleStep(s->dp, step);
      iSteps++;
      // Value must increase and gradient cannot drastically change in direction
      // Wolfe condition with c1=0
      // step is already set to a reasonable size (from peak width)
      // The second part of Wolfe condition is useless here
      if(s->p.value()>sOld->p.value() && sOld->dp.scalarProduct(s->dp)>0.0) {
        qSwap(sOld, s);
      } else {
        if(f.concavity(sOld->p)<0.0) {
#ifdef TRACE_GRADIENTSEARCH
          printf("    found negative concativity after %i steps\n", iSteps);
#endif
          Point dk=sOld->p.newtonStep(step, Point());
          refineMax(_coordinates[sOld->i], step, dk);
          return;
        } else {
          step*=0.5;
          if(step.x()>_cacheGridStep.x() &&
             step.y()>_cacheGridStep.y() &&
             step.z()>_cacheGridStep.z()){
            scaleStep(sOld->dp, step);
#ifdef TRACE_GRADIENTSEARCH
            printf("    reducing step to (%lf, %lf, %lf) after %i steps\n",
                   step.x(), step.y(), step.z(), iSteps);
#endif
          } else {
#ifdef TRACE_GRADIENTSEARCH
            printf("    switching to free climbing after %i steps\n", iSteps);
#endif
            freeClimbing(_coordinates[sOld->i], step, k0);
            return;
          }
        }
      }
      dk=_coordinates[sOld->i];
      dk-=k0;
    } while(dk.x()<_maxClimb.x() &&
            dk.y()<_maxClimb.y() &&
            dk.z()<_maxClimb.z());
#ifdef TRACE_GRADIENTSEARCH
    const Point& k=_coordinates[sOld->i];
    printf("    reaching max distance at (%lf, %lf, %lf) after %i steps\n", k.x(), k.y(), k.z(), iSteps);
#endif
  }
#endif

  void FKGradientSearch::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 FKGradientSearch::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);
    }
  }
#endif
} // namespace ArrayCore

