/***************************************************************************
**
**  This file is part of QGpCoreMath.
**
**  This library is free software; you can redistribute it and/or
**  modify it under the terms of the GNU Lesser General Public
**  License as published by the Free Software Foundation; either
**  version 2.1 of the License, or (at your option) any later version.
**
**  This file 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 Lesser General Public
**  License for more details.
**
**  You should have received a copy of the GNU Lesser General Public
**  License along with this library; if not, write to the Free Software
**  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2004-08-23
**  Copyright: 2004-2019
**    Marc Wathelet
**    Marc Wathelet (ULg, Liège, Belgium)
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

#include "GridSearch.h"

//#define GRIDSEARCH_TIMING

namespace QGpCoreMath {

  /*!
    \class GridSearch GridSearch.h
    \brief Search a grid for maxima (global or local)
  */

  /*!
  */
  GridSearch::GridSearch()
  {
    TRACE;
    _nx=0;
    _ny=0;
    _nz=0;
    _nxy=0;
    _nxyz=0;
    _diag=0;
    _last=0;
    _secondToLast=0;
    _coordinates=nullptr;
  }

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

  void GridSearch::setGrid(double minX, double maxX, double dX,
                           double minY, double maxY, double dY,
                           double minZ, double maxZ, double dZ)
  {
    TRACE;
    ASSERT(_function);
    if(maxX<minX) {
      qSwap(minX, maxX);
    }
    if(maxY<minY) {
      qSwap(minY, maxY);
    }
    if(maxZ<minZ) {
      qSwap(minZ, maxZ);
    }

    int nx=qRound((maxX-minX)/dX)+1;
    int ny=qRound((maxY-minY)/dY)+1;
    int nz=qRound((maxZ-minZ)/dZ)+1;
    if(ny==1 && nz>1) {
      App::log(tr("nz cannot be >1 while ny==1, skipping setGrid\n"));
      return;
    }

    _nx=nx;
    _ny=ny;
    _nz=nz;
    _nxy=_nx*_ny;
    _nxyz=_nxy*_nz;
    switch(_nz) {
    case 1:
      switch(_ny) {
      case 1:
      case 2: // Y component is not optimized
        _diag=1;
        break;
      default:
        _diag=_nx+1;
        break;
      }
      break;
    case 2: // Z component is not optimized
      if(_ny==1) {
        _diag=1;
      } else {
        _diag=_nx+1;
      }
      break;
    default:
      _diag=_nxy+_nx+1;
      break;
    }
    _last=_nxyz-1;
    _secondToLast=_last-_diag;

    delete [] _coordinates;
    _coordinates=new Point[_nxyz];

    // For 2D and 1D grids, Y and Z solution is always fixed
    if(_nz==1) {
      _pos.setZ(minZ);
      if(_ny==1) {
        _pos.setY(minY);
      }
    }

    // Round min values to have a grid centered around origin
    minX=qCeil(minX/dX)*dX;
    minY=qCeil(minY/dY)*dY;
    minZ=qCeil(minZ/dZ)*dZ;
    int k=0;
    Point p;
    for(int iz=0; iz<_nz; iz++) {
      p.setZ(minZ+dZ*static_cast<double>(iz));
      for(int iy=0; iy<_ny; iy++) {
        p.setY(minY+dY*static_cast<double>(iy));
        for(int ix=0; ix<_nx; ix++) {
          p.setX(minX+dX*static_cast<double>(ix));
          _coordinates[k++]=p;
        }
      }
    }

    if(_function->initGrid(_nxyz)) {
      App::log(2, tr("Initializing a grid with %1 x %2 x %3 values\n")
                        .arg(_nx).arg(_ny).arg(_nz));
      for(int k=0; k<_nxyz; k++) {
        _function->initGrid(_coordinates[k], k);
      }
      App::log(2, tr("Grid initialized\n"));
    }
  }

  inline void GridSearch::refineMax(int k, double maxVal)
  {
    double val=refineMax(_coordinates[k-_diag], _coordinates[k+_diag], maxVal);
    FunctionSearchMaximum * max=_function->createMaximum(val, _pos);
    if(max) {
      _maxima.append(max);
    }
  }

  /*!
    Search all grid for its maximum and refine it
  */
  void GridSearch::globalMax(double absThres)
  {
    TRACE;
    int maxk=0;
    double val, maxVal=-std::numeric_limits<double>::infinity();
    clearMaxima();
#ifdef GRIDSEARCH_TIMING
    QTime chrono;
    chrono.start();
    int t, nValues=0;
#endif
    for(int k=0; k<=_last; k++) {
      val=value(k);
#ifdef GRIDSEARCH_TIMING
      if(val>=0) {
        nValues++;
      }
#endif
      if(val>maxVal) {
        maxVal=val;
        maxk=k;
      }
    }
#ifdef GRIDSEARCH_TIMING
    t=chrono.elapsed();
    App::log(9, tr("Global grid search took %1 ms (%2 values, %3 ms/value)\n")
                      .arg(t).arg(nValues).arg((double)t/(double)nValues));
    chrono.start();
#endif
    /*if(maxk<_diag) {
      maxk=_diag;
    } else if(maxk>_secondToLast) {
      maxk=_secondToLast;
    }*/
    if(!isOnEdge(maxk)) {
      refineMax(maxk, maxVal);
      FunctionSearch::globalMax(absThres);
    }
#ifdef GRIDSEARCH_TIMING
    t=chrono.elapsed();
    App::log(9, tr("Refined grid search took %1 ms\n").arg(t));
#endif
  }

  inline void GridSearch::setNeighbors1(int k)
  {
    _neighbors[0]=k-1;
    _neighbors[1]=k+1;
  }

  inline void GridSearch::setNeighbors2(int k)
  {
    int dky;
    setNeighbors1(k);

    dky=k-_nx;
    _neighbors[2]=dky-1;
    _neighbors[3]=dky;
    _neighbors[4]=dky+1;

    dky=k+_nx;
    _neighbors[5]=dky-1;
    _neighbors[6]=dky;
    _neighbors[7]=dky+1;
  }

  inline void GridSearch::setNeighbors3(int k)
  {
    int dkz, dky;
    setNeighbors2(k);

    dkz=k-_nxy;

    dky=dkz-_nx;
    _neighbors[8]=dky-1;
    _neighbors[9]=dky;
    _neighbors[10]=dky+1;

    _neighbors[11]=dkz-1;
    _neighbors[12]=dkz;
    _neighbors[13]=dkz+1;

    dky=dkz+_nx;
    _neighbors[14]=dky-1;
    _neighbors[15]=dky;
    _neighbors[16]=dky+1;

    dkz=k+_nxy;

    dky=dkz-_nx;
    _neighbors[17]=dky-1;
    _neighbors[18]=dky;
    _neighbors[19]=dky+1;

    _neighbors[20]=dkz-1;
    _neighbors[21]=dkz;
    _neighbors[22]=dkz+1;

    dky=dkz+_nx;
    _neighbors[23]=dky-1;
    _neighbors[24]=dky;
    _neighbors[25]=dky+1;
  }

  void GridSearch::localMax(int nMax, double absThres, double relThres)
  {
    TRACE;
    double * map=new double [_nxyz];
    // Fill in the map with the standard cached grid
    for(int k=0; k<_nxyz; k++) {
      map[k]=value(k);
    }
    clearMaxima();
    // Do not test if values are null to allow the detection
    // of peaks close to the limit (outside which values are null).
    // All valid values are assumed to be positive.
    // Peaks that cut by the limits will generate false peaks right on the limit
    // Proper filtering of theses peaks must be implemented (see FKLoop)
    switch(_nz) {
    case 1:
      if(_ny==1) {
        for(int k=_diag; k<=_secondToLast; k++) {
          setNeighbors1(k);
          if(map[k]>map[_neighbors[0]] &&
             map[k]>map[_neighbors[1]]) {
            refineMax(k, map[k]);
          }
        }
      } else {
        for(int k=_diag; k<=_secondToLast; k++) {
          setNeighbors2(k);
          if(map[k]>map[_neighbors[0]] &&
             map[k]>map[_neighbors[1]] &&
             map[k]>map[_neighbors[2]] &&
             map[k]>map[_neighbors[3]] &&
             map[k]>map[_neighbors[4]] &&
             map[k]>map[_neighbors[5]] &&
             map[k]>map[_neighbors[6]] &&
             map[k]>map[_neighbors[7]]) {
            refineMax(k, map[k]);
          }
        }
      }
      break;
    case 2: { // Used for binary ellipticity (see HRFKRayleigh)
              // Z component is not optimized, ny>1
              // Not merged with the classical loop for 2D above
              // because we do not want to find a maximum at the limit
              // between the two planes with distinct Z
        int n=_nxy-_diag;
        for(int k=_diag; k<=n; k++) {
          setNeighbors2(k);
          if(map[k]>map[_neighbors[0]] &&
             map[k]>map[_neighbors[1]] &&
             map[k]>map[_neighbors[2]] &&
             map[k]>map[_neighbors[3]] &&
             map[k]>map[_neighbors[4]] &&
             map[k]>map[_neighbors[5]] &&
             map[k]>map[_neighbors[6]] &&
             map[k]>map[_neighbors[7]]) {
            refineMax(k, map[k]);
          }
        }
        for(int k=_nxy+_diag; k<=_secondToLast; k++) {
          setNeighbors2(k);
          if(map[k]>map[_neighbors[0]] &&
             map[k]>map[_neighbors[1]] &&
             map[k]>map[_neighbors[2]] &&
             map[k]>map[_neighbors[3]] &&
             map[k]>map[_neighbors[4]] &&
             map[k]>map[_neighbors[5]] &&
             map[k]>map[_neighbors[6]] &&
             map[k]>map[_neighbors[7]]) {
            refineMax(k, map[k]);
          }
        }
      }
      break;
    default:
      for(int k=_diag; k<=_secondToLast; k++) {
        setNeighbors3(k);
        if(map[k]>map[_neighbors[0]] &&
           map[k]>map[_neighbors[1]] &&
           map[k]>map[_neighbors[2]] &&
           map[k]>map[_neighbors[3]] &&
           map[k]>map[_neighbors[4]] &&
           map[k]>map[_neighbors[5]] &&
           map[k]>map[_neighbors[6]] &&
           map[k]>map[_neighbors[7]] &&
           map[k]>map[_neighbors[8]] &&
           map[k]>map[_neighbors[9]] &&
           map[k]>map[_neighbors[10]] &&
           map[k]>map[_neighbors[11]] &&
           map[k]>map[_neighbors[12]] &&
           map[k]>map[_neighbors[13]] &&
           map[k]>map[_neighbors[14]] &&
           map[k]>map[_neighbors[15]] &&
           map[k]>map[_neighbors[16]] &&
           map[k]>map[_neighbors[17]] &&
           map[k]>map[_neighbors[18]] &&
           map[k]>map[_neighbors[19]] &&
           map[k]>map[_neighbors[20]] &&
           map[k]>map[_neighbors[21]] &&
           map[k]>map[_neighbors[22]] &&
           map[k]>map[_neighbors[23]] &&
           map[k]>map[_neighbors[24]] &&
           map[k]>map[_neighbors[25]]) {
          refineMax(k, map[k]);
        }
      }
      break;
    }
    delete [] map;
    FunctionSearch::localMax(nMax, absThres, relThres);
  }

  /*!
    For given a X and Y, only one maximum along Z is taken.
    If Z is not available: for a given X, only one maximum along Y is taken.
  */
  void GridSearch::localMaxBest(int nMax, double absThres, double relThres)
  {
    TRACE;
    double * map;
    int * index3D;
    switch(_nz) {
    case 1: {
        map=new double [_nx];
        index3D=new int[_nx];
        double val, maxVal;
        int maxIndex=0;
        // Fill in the map with the standard cached grid
        for(int k=0; k<_nx; k++) {
          maxVal=-std::numeric_limits<double>::infinity();
          for(int iy=0; iy<_ny; iy++) {
            val=value(iy*_nx+k);
            if(val>maxVal) {
              maxVal=val;
              maxIndex=iy;
            }
          }
          map[k]=maxVal;
          index3D[k]=maxIndex*_nx+k;
        }
        clearMaxima();
        int n=_nx-2;
        switch(_ny) {
        case 2:
          for(int k=1; k<=n; k++) {
            setNeighbors1(k);
            if(map[k]>map[_neighbors[0]] &&
               map[k]>map[_neighbors[1]]) {
              int i=index3D[k];
              if(i<1) {
                i++;
              } else if(i>_secondToLast) {
                i--;
              }
              refineMax(i, map[k]);
            }
          }
          break;
        default:
          for(int k=1; k<=n; k++) {
            setNeighbors1(k);
            if(map[k]>map[_neighbors[0]] &&
               map[k]>map[_neighbors[1]]) {
              // Check 3D index is not on the edges, only Z axis has issues
              // Shift only by one
              int i=index3D[k];
              if(i<_diag) {
                i+=_nx;
              } else if(i>_secondToLast) {
                i-=_nx;
              }
              refineMax(i, map[k]);
            }
          }
          break;
        }
      }
      break;
    default: {
        map=new double [_nxy];
        index3D=new int[_nxy];
        double val, maxVal;
        int maxIndex=0;
        // Fill in the map with the standard cached grid
        for(int k=0; k<_nxy; k++) {
          maxVal=-std::numeric_limits<double>::infinity();
          for(int iz=0; iz<_nz; iz++) {
            val=value(iz*_nxy+k);
            if(val>maxVal) {
              maxVal=val;
              maxIndex=iz;
            }
          }
          map[k]=maxVal;
          index3D[k]=maxIndex*_nxy+k;
        }
        clearMaxima();
        // Do not test if values are null to allow the detection
        // of peaks close to the limit (outside which values are null).
        // All valid values are assumed to be positive.
        // Peaks that cut by the limits will generate false peaks right on the limit
        // Proper filtering of theses peaks must be implemented (see FKWorker)
        int n=_nxy-_nx-2;
        for(int k=_nx+1; k<=n; k++) {
          setNeighbors2(k);
          if(map[k]>map[_neighbors[0]] &&
             map[k]>map[_neighbors[1]] &&
             map[k]>map[_neighbors[2]] &&
             map[k]>map[_neighbors[3]] &&
             map[k]>map[_neighbors[4]] &&
             map[k]>map[_neighbors[5]] &&
             map[k]>map[_neighbors[6]] &&
             map[k]>map[_neighbors[7]]) {
            // Check 3D index is not on the edges, only Z axis has issues
            // Shift only by one
            int i=index3D[k];
            if(i<_diag) {
              i+=_nxy;
            } else if(i>_secondToLast) {
              i-=_nxy;
            }
            refineMax(i, map[k]);
          }
        }
      }
      break;
    }
    delete [] index3D;
    delete [] map;
    FunctionSearch::localMax(nMax, absThres, relThres);
  }

  inline void GridSearch::checkForMax(const Point& p, double& maxVal)
  {
    TRACE;
    double val=_function->value(p);
    if(val>maxVal) {
      maxVal=val;
      _pos=p;
    }
  }

  /*!
    Refines the maximum of the grid in an area defined by x and y limits.
    return value of the refined maximum
  */
  double GridSearch::refineMax(const Point& min, const Point& max, double maxVal)
  {
    TRACE;
    //App::log(tr("Refine max from %1 to %2, max=%3\n").arg(min.toString()).arg(max.toString()).arg(maxVal) );
    _pos=min;
    _pos+=max;
    _pos*=0.5;
    Point delta=max;
    delta-=min;
    delta*=0.5;

    Point absPrecision;
    if(_relative[0]) {
      absPrecision.setX(fabs(_pos.x()*_precision[0]));
      if(absPrecision.x()==0.0) {
        absPrecision.setX(fabs(_precision[0]*max.x()));
      }
    } else {
      absPrecision.setX(_precision[0]);
    }
    if(_relative[1]) {
      absPrecision.setY(fabs(_pos.y()*_precision[1]));
      if(absPrecision.y()==0.0) {
        absPrecision.setY(fabs(_precision[1]*max.y()));
      }
    } else {
      absPrecision.setY(_precision[1]);
    }
    if(_relative[2]) {
      absPrecision.setZ(fabs(_pos.z()*_precision[2]));
      if(absPrecision.z()==0.0) {
        absPrecision.setZ(fabs(_precision[2]*max.z()));
      }
    } else {
      absPrecision.setZ(_precision[2]);
    }

    // For 1D and 2D keep non optimized component with the correct value if any
    Point p1=_pos;
    Point p2=_pos;
    if(delta.z()==0.0) {
      if(delta.y()==0.0) {
        while(delta.x()>absPrecision.x()) {
          delta.setX(0.5*delta.x());
          p1.setX(_pos.x()-delta.x());
          p2.setX(_pos.x()+delta.x());
          checkForMax(p1, maxVal);
          checkForMax(p2, maxVal);
        }
      } else {
        while(delta.x()>absPrecision.x() ||
              delta.y()>absPrecision.y()) {
          delta.setX(0.5*delta.x());
          delta.setY(0.5*delta.y());
          p1.setX(_pos.x()-delta.x());
          p2.setX(_pos.x()+delta.x());
          p1.setY(_pos.y()-delta.y());
          p2.setY(_pos.y()+delta.y());
          checkForMax(p1, maxVal);
          checkForMax(Point(p1.x(), p2.y(), p1.z()), maxVal);
          checkForMax(Point(p2.x(), p1.y(), p1.z()), maxVal);
          checkForMax(p2, maxVal);
        }
      }
    } else {
      while(delta.x()>absPrecision.x() ||
            delta.y()>absPrecision.y() ||
            delta.z()>absPrecision.z()) {
        delta*=0.5;
        p1=_pos;
        p1-=delta;
        p2=_pos;
        p2+=delta;

        // p1.z()
        checkForMax(p1, maxVal);
        checkForMax(Point(p1.x(), _pos.y(), p1.z()), maxVal);
        checkForMax(Point(p1.x(), p2.y(), p1.z()), maxVal);

        checkForMax(Point(_pos.x(), p1.y(), p1.z()), maxVal);
        checkForMax(Point(_pos.x(), _pos.y(), p1.z()), maxVal);
        checkForMax(Point(_pos.x(), p2.y(), p1.z()), maxVal);

        checkForMax(Point(p2.x(), p1.y(), p1.z()), maxVal);
        checkForMax(Point(p2.x(), _pos.y(), p1.z()), maxVal);
        checkForMax(Point(p2.x(), p2.y(), p1.z()), maxVal);
        // _pos.z()
        checkForMax(Point(p1.x(), p1.y(), _pos.z()), maxVal);
        checkForMax(Point(p1.x(), _pos.y(), _pos.z()), maxVal);
        checkForMax(Point(p1.x(), p2.y(), _pos.z()), maxVal);

        checkForMax(Point(_pos.x(), p1.y(), _pos.z()), maxVal);
        checkForMax(Point(_pos.x(), p2.y(), _pos.z()), maxVal);

        checkForMax(Point(p2.x(), p1.y(), _pos.z()), maxVal);
        checkForMax(Point(p2.x(), _pos.y(), _pos.z()), maxVal);
        checkForMax(Point(p2.x(), p2.y(), _pos.z()), maxVal);
        // p2.z()
        checkForMax(Point(p1.x(), p1.y(), p2.z()), maxVal);
        checkForMax(Point(p1.x(), _pos.y(), p2.z()), maxVal);
        checkForMax(Point(p1.x(), p2.y(), p2.z()), maxVal);

        checkForMax(Point(_pos.x(), p1.y(), p2.z()), maxVal);
        checkForMax(Point(_pos.x(), _pos.y(), p2.z()), maxVal);
        checkForMax(Point(_pos.x(), p2.y(), p2.z()), maxVal);

        checkForMax(Point(p2.x(), p1.y(), p2.z()), maxVal);
        checkForMax(Point(p2.x(), _pos.y(), p2.z()), maxVal);
        checkForMax(p2, maxVal);
      }
    }
    return maxVal;
  }

  bool GridSearch::isOnEdge(int k)
  {
    int z=k/_nxy;
    k-=z*_nxy;
    int y=k/_nx;
    k-=y*_nx;
    int x=k;
    if(_nz>2) {
      return z==0 || z==_nz-1;
    }
    if(_ny>2) {
      return y==0 || y==_ny-1;
    }
    return x==0 || x==_nx-1;
  }

} // namespace QGpCoreMath
