/***************************************************************************
**
**  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: 2007-11-12
**  Copyright: 2007-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

#include "DirectionalSearch.h"

namespace QGpCoreMath {

/*!
  \class DirectionalSearch DirectionalSearch.h
  \brief Search a grid for maxima (global or local) along one direction only

  Full description of class still missing
*/

/*!
  Description of constructor still missing
*/
DirectionalSearch::DirectionalSearch()
{
  TRACE;
  _precision[0]=0.001;
  _nr=0;
  _r=0;
  _cx=1.0;
  _sy=0.0;
}

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

/*!
  Set current azimuth of search
*/
void DirectionalSearch::setAzimuth(double azimuth)
{
  TRACE;
  _cx=::cos(azimuth);
  _sy=::sin(azimuth);
}

/*!
  Init sampling in radius. setFunction() must be called before.
*/
void DirectionalSearch::setGrid(double minR, double maxR, double dR)
{
  TRACE;
  ASSERT(_function);

  int nr=(int)floor((maxR-minR)/dR+0.5)+1;
  if(nr!=_nr) {
    delete [] _r;
    _r=new double [nr];
    _nr=nr;
  }

  _function->initGrid(_nr);
  for(int i=0; i<_nr; i++) {
    _r[i]=minR+dR*(double)i;
    _function->initGrid(Point2D(_cx*_r[i], _sy*_r[i]), i);
  }
}

/*!
  Search current azimuth for its maximum and refine it
*/
void DirectionalSearch::globalMax(double absThres)
{
  TRACE;
  int maxi=0;
  double val,maxVal=-std::numeric_limits<double>::infinity();
  for(int i=0; i<_nr; i++) {
    val=_function->value(Point2D(_cx*_r[i], _sy*_r[i]), i);
    if(val>maxVal) {
      maxVal=val;
      maxi=i;
    }
  }
  // Refine search by recursion and re-regridding
  val=refineMax(lowLimit(maxi, _r),highLimit(maxi, _r, _nr), maxVal);
  clearMaxima();
  FunctionSearchMaximum * max=_function->createMaximum(val, _pos);
  if(max) {
    _maxima.append(max);
    FunctionSearch::globalMax(absThres);
  }
}

void DirectionalSearch::localMax(int nMax, double absThres, double relThres)
{
  TRACE;
  double * map=new double [_nr];
  for(int i=0; i<_nr; i++) {
    map[i]=_function->value(Point2D(_cx*_r[i], _sy*_r[i]), i);
  }
  clearMaxima();
  int nm=_nr-1;
  for(int i=1;i<nm;i++) {
    if(map[i-1]>0 && map[i]>map[i-1] &&
       map[i+1]>0 && map[i]>map[i+1]) {
      double val=refineMax(_r[i-1], _r[i+1], map[i]);
      FunctionSearchMaximum * max=_function->createMaximum(val, _pos);
      if(max) {
        _maxima.append(max);
      }
    }
  }
  delete [] map;
  FunctionSearch::localMax(nMax, absThres, relThres);
}

inline void DirectionalSearch::checkForMax(double r, double& maxVal, double& maxR)
{
  TRACE;
  double val=_function->value(Point2D(_cx*r, _sy*r));
  if(val>maxVal) {
    maxVal=val;
    maxR=r;
  }
}

/*!
  Refines the maximum of the grid in an area defined by radius limits.
  return value of the refined maximum
*/
double DirectionalSearch::refineMax(double minR, double maxR, double maxVal)
{
  TRACE;
  double r=(minR+maxR)*0.5;
  double dr=(maxR-minR)*0.5;

  double absPrecision;
  if(_relative[0]) {
    absPrecision=fabs(_precision[0]*r);
  } else {
    absPrecision=_precision[0];
  }

  while(dr>absPrecision) {
    dr*=0.5;

    double r1=r-dr;
    double r2=r+dr;

    checkForMax(r1, maxVal, r);
    checkForMax(r2, maxVal, r);
  }
  _pos.setX(_cx*r);
  _pos.setY(_sy*r);
  return maxVal;
}

} // namespace QGpCoreMath
