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

#include <math.h>

#include "ConcentricSearch.h"
#include "AbstractFunction.h"

namespace QGpCoreMath {

  /*!
    \class ConcentricSearch ConcentricSearch.h
    \brief Searches for maxima across a grid in concentric way

    The search is made along grid axes, not really along a perfect circle.
      \li x, y are the coordinates of the center (see setCenter());
      \li r0 is the starting radius (see setGrid());
      \li rMax is the maximum radius to cover (see setGrid());
      \li dr is the general step in radius and along the circle (see setGrid()).
    The refinements are performed in crosses like for searching for victims in an
    avalanche (ARVA).

    maxX(), maxY() and maxRadius() caracterize the exact position
    the last maximum. Refinement precision of set as 0.001 times the step dr.

    maximum() starts the search. It can be called several times until returning 0;
  */

/*!
  Initialize an invalid search (starting radius>maximum radius).
*/
ConcentricSearch::ConcentricSearch()
{
  TRACE;
  _segment=Top;
  _r0=std::numeric_limits<double>::infinity();
  _rMax=0.0;
  _dr=0.0;
}

ConcentricSearch::~ConcentricSearch()
{
  TRACE;
}

/*!
  \fn setCenter(double x, double y)
  \a x and \a y are the coordinates of the center.
*/

/*!
  \fn void setNoiseLevel(double nl)
  Noise level is the minimum value necessary to consider a refinement of a peak.
*/

/*!
  \a minR is the starting radius.
  \a dR is the general step (either in radius or along the circle).
  \a maxR is the maximum radius to cover.
*/
void ConcentricSearch::setGrid(double minR, double maxR, double dR)
{
  TRACE;
  _r0=minR;
  _rMax=maxR;
  _pos=_x-_r0;
  _dr=dR;
  ASSERT(_r0>2.0*_dr);
  _precision=_dr*0.001;
}

/*!
  Searches for the next maximum starting at current r0.
  After each turn, r0 is incremented by the step dr.
  It returns the value reached by the found maximum or 0.0,
  if no maximum has been found. It can be called several times
  while not returning 0.0.

  \a terminate is an optional flag to interrupt current search.
*/
double ConcentricSearch::maximum(QAtomicInt * terminate)
{
  TRACE;
  while(_r0<=_rMax) {
    if(terminate && terminate->testAndSetOrdered(true,true)) {
      return 0.0;
    }
    switch (_segment) {
    case Top:
      if(scanX(_x-_r0,_x+_r0,_y-_r0)) return _maxValue;
      _segment=Bottom;
      _pos=_x-_r0;
      break;
    case Bottom:
      if(scanX(_x-_r0,_x+_r0,_y+_r0)) return _maxValue;
      _segment=Left;
      _pos=_y-_r0;
      break;
    case Left:
      if(scanY(_y-_r0,_y+_r0,_x-_r0)) return _maxValue;
      _segment=Right;
      _pos=_y-_r0;
      break;
    case Right:
      if(scanY(_y-_r0,_y+_r0,_x+_r0)) return _maxValue;
      _segment=Top;
      _pos=_x-_r0;
      break;
    }
    _r0Mutex.MUTEX_LOCK;
    _r0+=_dr;
    _r0Mutex.unlock();
  }
  return 0.0;
}

double ConcentricSearch::r0() const
{
  TRACE;
  QMutexLocker ml(&_r0Mutex);
  return _r0;
}

/*!
  Return the maxium radius ever reached during refinements.
*/
double ConcentricSearch::maxRadius() const
{
  TRACE;
  double dx=_maxX-_x;
  double dy=_maxY-_y;
  return ::sqrt(dx*dx+dy*dy);
}

/*!
  \internal
  Scan for maxima in the X direction.
*/
bool ConcentricSearch::scanX(double min, double max, double y)
{
  TRACE;
  // Initialization: restart at last postion is supported
  // If not restarting (i.e. pos==min), compute V1
  if(fabs(_pos-min)<1e-15) {
    _v1=_function->value(Point2D(_pos, y));
    _pos+=_dr;
  }
  // If not restarting (i.e. pos==min+dr), compute V2
  if(fabs(_pos-(min+_dr))<1e-15) {
    _v2=_function->value(Point2D(_pos, y));
    _pos+=_dr;
    if(_v2<_v1 && _v1>_noiseLevel) {
      _maxValue=_v1;
      crossRefineX(_pos-2*_dr, y, _dr);
      return true;
    }
  }
  for(;_pos<max;_pos+=_dr) {
    _v3=_function->value(Point2D(_pos, y));
    if(_v2>_v1 && _v2>_v3 && _v2>_noiseLevel) {
      _maxValue=_v2;
      crossRefineY(_pos-_dr, y, 0.5*_dr);
      _v1=_v2;
      _v2=_v3;
      _pos+=_dr;
      return true;
    }
    _v1=_v2;
    _v2=_v3;
  }
  return false;
}

/*!
  \internal
  Scan for maxima in the Y direction.
*/
bool ConcentricSearch::scanY(double min, double max, double x)
{
  TRACE;
  if(fabs(_pos-min)<1e-15) {
    _v1=_function->value(Point2D(x, _pos));
    _pos+=_dr;
  }
  if(fabs(_pos-(min+_dr))<1e-15) {
    _v2=_function->value(Point2D(x,_pos));
    _pos+=_dr;
    if(_v2<_v1 && _v1>_noiseLevel) {
      _maxValue=_v1;
      crossRefineY(x, _pos-2*_dr, _dr);
      return true;
    }
  }
  for(;_pos<max;_pos+=_dr) {
    _v3=_function->value(Point2D(x,_pos));
    if(_v2>_v1 && _v2>_v3 && _v2>_noiseLevel) {
      _maxValue=_v2;
      crossRefineX(x,_pos-_dr, 0.5*_dr);
      _v1=_v2;
      _v2=_v3;
      _pos+=_dr;
      return true;
    }
    _v1=_v2;
    _v2=_v3;
  }
  return false;
}

/*!
  \internal
  Recursive refinement by changing X.
*/
void ConcentricSearch::crossRefineX(double x, double y, double dr)
{
  TRACE;
  if(dr<_precision) {
    _maxX=x;
    _maxY=y;
    return;
  }
  double v=_function->value(Point2D(x+dr, y));
  if(v>_maxValue) {
    x+=dr;
    while(v>_maxValue) {
      x+=dr;
      _maxValue=v;
      v=_function->value(Point2D(x, y));
    }
    crossRefineY(x-dr, y, 0.5*dr);
  } else {
    x-=dr;
    v=_function->value(Point2D(x, y));
    while(v>_maxValue) {
      x-=dr;
      _maxValue=v;
      v=_function->value(Point2D(x, y));
    }
    crossRefineY(x+dr, y, 0.5*dr);
  }
}

/*!
  \internal
  Recursive refinement by changing Y.
*/
void ConcentricSearch::crossRefineY(double x, double y, double dr)
{
  TRACE;
  if(dr<_precision) {
    _maxX=x;
    _maxY=y;
    return;
  }
  double v=_function->value(Point2D(x, y+dr));
  if(v>_maxValue) {
    y+=dr;
    while(v>_maxValue) {
      y+=dr;
      _maxValue=v;
      v=_function->value(Point2D(x, y));
    }
    crossRefineX(x, y-dr, 0.5*dr);
  } else {
    y-=dr;
    v=_function->value(Point2D(x, y));
    while(v>_maxValue) {
      y-=dr;
      _maxValue=v;
      v=_function->value(Point2D(x, y));
    }
    crossRefineX(x, y+dr, 0.5*dr);
  }
}

} // namespace QGpCoreMath
