/***************************************************************************
**
**  This file is part of QGpCompatibility.
**
**  QGpCompatibility 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.
**
**  QGpCompatibility 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: 2002-10-15
**  Copyright: 2002-2019
**    Marc Wathelet
**    Marc Wathelet (ULg, Liège, Belgium)
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

#include <QGpCoreTools.h>
#include "CompatFunction.h"

namespace QGpCompatibility {

/*!
  \class CompatFuntionPoint

  This is mainly used by CompatFunction::sort. It is not used by CompatFunction to store samples.
*/

/*!
  \class CompatFuntion

  This object handle three vectors that represent a function with error bars
  They must be allocated elsewhere to avoid duplication of data and useless
  memory space.
  This object can strongly modify the shape of those vectors but never deallocate
  them.
*/

/*!
  \fn x()
  Vector of X values
*/

/*!
  \fn y()
  Vector of Y values
*/

/*!
  \fn yErr()
  Vector of YErr values
*/

/*!
  \fn weight()
  Vector of Weight values
*/

/*!
  \fn y(double)
  y value at x
*/

/*!
  \fn size()
  Returns the size of vectors (number of points that defines the function)
*/

CompatFunction::CompatFunction()
{
  TRACE;
  _isSorted=false;
  _x=0;
  _y=0;
  _yErr=0;
  _weight=0;
  _n2=0;
}

/*!
  Don't forget to call deleteVectors(), x, y and dy are created here
*/
CompatFunction::CompatFunction(QVector<double> * x, QVector<double> * y, QVector<double> * dy)
{
  TRACE;
  _x=x;
  _y=y;
  _yErr=dy;
  _weight=0;
  _isSorted=false;
  _n2=0;
  sort();
}

CompatFunction::CompatFunction(QVector<Point>& f)
{
  TRACE;
  _n2=0;
  set(f);
}

CompatFunction::~CompatFunction()
{
  TRACE;}

/*! Construct a basic linear function between (x1,y1) and (x2,y2)
    You must call deleteVectors before deleting this object or call setX,Y or YErr
*/
void CompatFunction::line(double x1, double y1, double x2, double y2)
{
  TRACE;
  _x=new QVector<double>(2);
  _y=new QVector<double>(2);
  (*_x)[0]=x1;
  (*_y)[0]=y1;
  (*_x)[1]=x2;
  (*_y)[1]=y2;
  sort();
}

/*!
  Set vector of X, Y, and YErr values from a vector of Point, size must be >1

  Don't forget to call deleteVectors(), x, y and dy are allocated here
*/
void CompatFunction::set(QVector<Point>& f)
{
  TRACE;
  QVector<double> * x=new QVector<double>;
  QVector<double> * y=new QVector<double>;
  QVector<double> * dy=new QVector<double>;
  for(int i=0;i<f.count();i++) {
    Point& p=f[i];
    x->append(p.x());
    y->append(p.y());
    dy->append(p.z());
  }
  _x=0;
  _y=0;
  _yErr=0;
  _weight=0;
  setX(x);
  setY(y);
  setYErr(dy);
  sort();
}

/*!
  Set vector of X values, size must be >1
*/
void CompatFunction::setX(QVector<double> * x)
{
  TRACE;
  if(_y && (!x || _y->size()!=x->size())) {
    App::log(tr("CompatFunction::setX() : size for x and y vectors does not match,\n"
                 "please report to http://bug.geopsy.org.\n"));
    return;
  }
  _isSorted=false;
  _x=x;
}

/*!
  Set vector of Y values, size must equal to x size
*/
void CompatFunction::setY(QVector<double> * y)
{
  TRACE;
  if(_x &&(!y || _x->size()!=y->size())) {
    App::log(tr("CompatFunction::setY() : size for x and y vectors does not match,\n"
                "please report to http://bug.geopsy.org.\n"));
    return;
  }
  _isSorted=false;
  _y=y;
}

/*!
  Set vector of YErr values, size must equal to x size
*/
void CompatFunction::setYErr(QVector<double> * yErr)
{
  TRACE;
  if(_x) {
    if(!yErr || _x->size()!=yErr->size()) {
      App::log(tr("CompatFunction::setYErr() : size for x and yErr vectors does not match\n"));
      return;
    }
  } else { // _x==0
    if(!_y) {
      App::log(tr("CompatFunction::setYErr() : x or y must be defined before yErr\n"));
      return;
    }
    if(!yErr || _y->size()!=yErr->size()) {
      App::log(tr("CompatFunction::setYErr() : size for y and yErr vectors does not match\n"));
      return;
    }
  }
  _isSorted=false;
  _yErr=yErr;
}

/*!
  Set vector of ErrWeight values, size must equal to x size
*/
void CompatFunction::setWeight(QVector<double> * w)
{
  TRACE;
  if(_x) {
    if(!w || _x->size()!=w->size()) {
      App::log(tr("CompatFunction::setWeight() : size for x and w vectors does not match,\n"));
      return;
    }
  } else { // _x==0
    if(!_y) {
      App::log(tr("CompatFunction::setWeight() : x or y must be defined before w, \n"));
      return;
    }
    if(!w || _y->size()!=w->size()) {
      App::log(tr("CompatFunction::setWeight() : size for y and w vectors does not match, \n"));
      return;
    }
  }
  _isSorted=false;
  _weight=w;
}

/*!
  if atIndex is less than point count, the point atIndex is modified, else a new point is added
*/
void CompatFunction::addPoint(int atIndex, double x, double y, double z)
{
  TRACE;
  if(!_x || !_y) {
    deleteVectors();
    _x=new QVector<double>;
    _y=new QVector<double>;
    _yErr=new QVector<double>;
    _weight=new QVector<double>;
    _n2=1;
    _isSorted=true;
  }
  int n=_x->size();
  if(atIndex<n) {
    (*_x)[atIndex]=x;
    (*_y)[atIndex]=y;
    if(_yErr) (*_yErr)[atIndex]=z;
    if(_weight) (*_weight)[atIndex]=1.0;
  } else {
    _x->append(x);
    _y->append(y);
    if(_yErr) _yErr->append(z);
    if(_weight) _weight->append(1.0);
  }
  n++;
  if(_isSorted && ((atIndex>0 && (*_x)[atIndex-1]>x) ||
                    (atIndex+1<n && x>(*_x)[atIndex+1]))) sort();
  else if(n>1) {
    while((int)_n2<n) _n2=_n2 << 1; // multiply by 2
    _n2=_n2 >> 1;
  }
}

/*!
  Sort vectors by increasing x and enable all operations. If _x or _y is null
  this function does nothing, _yErr might be null, any error is then ignored
  if a value has more than one occurance in _x, only the first of accepted
*/
void CompatFunction::sort()
{
  TRACE;
  // Added test if empty function, do not sort it
  int n=_x->size();
  if(!_x || !_y || n<2) return;
  // Copy to a temporary structure:
  QList<CompatFunctionPoint> samples;
  if(_yErr) {
    if(_weight) {
      for(int i=0; i<n; i++)
        samples << CompatFunctionPoint((*_x)[i], (*_y)[i], (*_yErr)[i], (*_weight)[i]);
    } else {
      for(int i=0; i<n; i++)
        samples << CompatFunctionPoint((*_x)[i], (*_y)[i], (*_yErr)[i], 0);
    }
  } else {
    if(_weight) {
      App::log(tr("CompatFunction::sort() : weight cannot exist without yErr, \n"));
      return;
    }
    for(int i=0; i<n; i++)
      samples << CompatFunctionPoint((*_x)[i], (*_y)[i], 0, 0);
  }
  std::sort(samples.begin(), samples.end());
  unique(samples);
  if(_yErr) {
    if(_weight) {
      for(int i=0;i<samples.count();++i) {
        const CompatFunctionPoint& p=samples.at(i);
        (*_x)[i]=p.x();
        (*_y)[i]=p.mean();
        (*_yErr)[i]=p.stddev();
        (*_weight)[i]=p.weight();
      }
    } else {
      for(int i=0;i<samples.count();++i) {
        const CompatFunctionPoint& p=samples.at(i);
        (*_x)[i]=p.x();
        (*_y)[i]=p.mean();
        (*_yErr)[i]=p.stddev();
      }
    }
  } else {
    for(int i=0;i<samples.count();++i) {
      const CompatFunctionPoint& p=samples.at(i);
      (*_x)[i]=p.x();
      (*_y)[i]=p.mean();
    }
  }
  // Resize original vectors samples.count() is always less or equal to original size
  resize(samples.count());
  // allow all normal operation on this object
  _isSorted=true;
}

/*!
  Though this object is not the owner of the vectors, the real owner can call
  this function may be called to facilitate deletion of data
*/
void CompatFunction::deleteVectors()
{
  TRACE;
  delete _x;
  delete _y;
  delete _yErr;
  delete _weight;
  _x=0;
  _y=0;
  _yErr=0;
  _weight=0;
  _isSorted=false;
}

/*!
  Change size of vectors to n, if n &lt; vectors size, last element are
  removed if removeLast is true else the first elements are removed.
*/
void CompatFunction::resize(int n, bool removeLast)
{
  TRACE;
  if(_x->size()>n && !removeLast) {
    int removeCount=_x->size()-n;
    QVector<double>::iterator it=_x->begin();
    int i;
    for(i=0;i<removeCount;i++) it=_x->erase(it);
    it=_y->begin();
    for(i=0;i<removeCount;i++) it=_y->erase(it);
    if(_yErr) {
      it=_yErr->begin();
      for(int i=0;i<removeCount;i++) it=_yErr->erase(it);
    }
    if(_weight) {
      it=_weight->begin();
      for(int i=0;i<removeCount;i++) it=_weight->erase(it);
    }
  } else if(_x->size()!=n) {
    _x->resize(n);
    _y->resize(n);
    if(_yErr) _yErr->resize(n);
    if(_weight) _weight->resize(n);
  }
  _n2=1;
  while(_n2<n) _n2=_n2 << 1; // multiply by 2
  _n2=_n2 >> 1;
}

/*!
  Resample the curve with a constant adimensional distance between samples
  valX and valY are representative values use to adimensionalize
  Take identical values if X and Y have the same dimensions
  n samples are taken starting from min (x) to max (x), if min and max are
  outside of current range they are forced to match current range, no
  extrapolation will be performed
*/
void CompatFunction::resample_ds(int n, double min, double max,bool isLog,bool isInv,
                              double valX, double valY)
{
  TRACE;
  ASSERT(_isSorted);
  // Transforms abcsisse according options (log and inv)
  if(isInv) {
    inverseX();
    valX=1.0/valX;
  }
  if(isLog) {
    log10();
    valX=::log10(valX);
    min=::log10(min);
    max=::log10(max);
  }
  if(min<minX()) min=minX();
  if(max>maxX()) max=maxX();
  // Calculate the "distance" abscisse
  QVector<double> * tmp_dist=new QVector<double>(_x->size());
  (*tmp_dist)[0]=0;
  //cout << "********* original transformed values" << endl;
  int i;
  for(i=1;i<(int)_x->size();i++) {
    double deltaX,deltaY;
    deltaX=((*_x)[i]-(*_x)[i-1])/valX;
    deltaY=((*_y)[i]-(*_y)[i-1])/valY;
    (*tmp_dist)[i]=(*tmp_dist)[i-1]+sqrt(deltaX*deltaX+deltaY*deltaY);
    //cout << i << " " << (*_x)[i] << " " << (*_y)[i] << " " << (*tmp_dist)[i] << endl;
  }
  // Calculate the distance of min and max, and constant step
  double distMax=interpole(max,find(max,_x),_x,tmp_dist);
  double distMin=interpole(min,find(min,_x),_x,tmp_dist);
  double cstStep=(distMax-distMin)/(n-1);
  // Temporary storage for interpolated values
  double * tmp_x=new double[n];
  double * tmp_y=new double[n];
  double * tmp_yErr=0; if(_yErr) tmp_yErr=new double[n];
  double * tmp_weight=0; if(_weight) tmp_weight=new double[n];
  // Intepolate new values for _x, _y and _yErr
  double dist=distMin;
  //cout << "DistMinMaxStep: " <<  distMin << " " << distMax << " " << cstStep << endl;
  for(i=0;i<n;i++,dist+=cstStep) {
    int iDist=find(dist,tmp_dist);
    tmp_x[i]=interpole(dist,iDist,tmp_dist,_x);
    tmp_y[i]=interpole(dist,iDist,tmp_dist,_y);
    if(tmp_yErr) tmp_yErr[i]=interpole(dist,iDist,tmp_dist,_yErr);
    if(tmp_weight) tmp_weight[i]=interpole(dist,iDist,tmp_dist,_weight);
    //cout << i << " " << dist << " " << tmp_x[i] << " " << tmp_y[i] << endl;
  }
  delete tmp_dist;
  // resize original vectors
  resize(n);
  // copy interpolated values to orignal vectors
  for(i=0;i<n;i++) {
    (*_x)[i]=tmp_x[i];
    (*_y)[i]=tmp_y[i];
    if(tmp_yErr) (*_yErr)[i]=tmp_yErr[i];
    if(tmp_weight) (*_weight)[i]=tmp_weight[i];
  }
  // Clearing ...
  delete [] tmp_x;
  delete [] tmp_y;
  delete [] tmp_yErr;
  delete [] tmp_weight;
  // Re-Transforms abcsisse according options (log and inv)
  if(isLog) exp10();
  if(isInv) inverseX();
}

void CompatFunction::resample(int n, double min, double max,bool isLog,bool isInv,double invalid_value)
{
  TRACE;
  ASSERT(_isSorted);
  // Transforms abcsisse according options (log and inv)
  if(isInv) inverseX();
  if(isLog) {
    log10();
    min=::log10(min);
    max=::log10(max);
  }
  // Calculate the constant step
  double cstStep=(max-min)/(n-1);
  // Temporary storage for interpolated values
  double * tmp_x=new double[n];
  double * tmp_y=new double[n];
  double * tmp_yErr=0; if(_yErr) tmp_yErr=new double[n];
  double * tmp_weight=0; if(_weight) tmp_weight=new double[n];
  // Intepolate new values for _x, _y and _yErr
  double x=min;
  double txMin=minX()*(1-1e-15),txMax=maxX()*(1+1e-15);
  int i;
  for(i=0;i<n;i++,x+=cstStep) {
    tmp_x[i]=x;
    if(x>=txMin && x<=txMax) {
      int ix=find(x,_x);
      tmp_y[i]=interpole(x,ix,_x,_y);
      if(tmp_yErr) tmp_yErr[i]=interpole(x,ix,_x,_yErr);
      if(tmp_weight) tmp_weight[i]=interpole(x,ix,_x,_weight);
    } else {
      tmp_y[i]=invalid_value;
      if(tmp_yErr) tmp_yErr[i]=0;
      if(tmp_weight) tmp_weight[i]=1;
    }
  }
  // resize original vectors
  resize(n);
  // copy interpolated values to orignal vectors
  for(i=0;i<n;i++) {
    (*_x)[i]=tmp_x[i];
    (*_y)[i]=tmp_y[i];
    if(tmp_yErr) (*_yErr)[i]=tmp_yErr[i];
    if(tmp_weight) (*_weight)[i]=tmp_weight[i];
  }
  // Clearing ...
  delete [] tmp_x;
  delete [] tmp_y;
  delete [] tmp_yErr;
  delete [] tmp_weight;
  // Re-Transforms abcsisse according options (log and inv)
  if(isLog) exp10();
  if(isInv) inverseX();
}

/*!
  Resample the function at x defined by xModel (.x of xModel)
*/
void CompatFunction::resample(QVector<Point>& xModel,double invalid_value, bool doInterpole)
{
  TRACE;
  QVector<double> x(xModel.count());
  for(int i=0;i<xModel.count();i++) x[i]=xModel[i].x();
  resample(&x, invalid_value, doInterpole);
}

/*!
  Resample the function at x defined by xModel (e.g. from another function)
*/
void CompatFunction::resample(QVector<double> * xModel,double invalid_value, bool doInterpole)
{
  TRACE;
  ASSERT(_isSorted);
  // Temporary storage for interpolated values
  int n=xModel->size(); // number of points for resampled function
  double * tmp_y=new double[n];
  double * tmp_yErr=0; if(_yErr) tmp_yErr=new double[n];
  double * tmp_weight=0; if(_weight) tmp_weight=new double[n];
  // Intepolate new values for _x, _y and _yErr
  double x,txMin=minX()*(1-1e-15),txMax=maxX()*(1+1e-15);
  int i;
  for(i=0;i<n;i++) {
    x=xModel->at(i);
    if(x>=txMin && x<=txMax) {
      int ix=find(x,_x);
      int ix2=ix;
      if(fabs(x-(*_x)[ix2])<1e-15 || fabs(x-(*_x)[--ix2])<1e-15) {
        tmp_y[i]=(*_y)[ix2];
        if(tmp_yErr) tmp_yErr[i]=(*_yErr)[ix2];
        if(tmp_weight) tmp_weight[i]=(*_weight)[ix2];
      } else if(doInterpole) {
        if((*_y)[ix]==invalid_value || (*_y)[ix-1]==invalid_value) {
          tmp_y[i]=invalid_value;
          if(tmp_yErr) tmp_yErr[i]=0;
          if(tmp_weight) tmp_weight[i]=1;
        } else {
          tmp_y[i]=interpole(x,ix,_x,_y);
          if(tmp_yErr) tmp_yErr[i]=interpole(x,ix,_x,_yErr);
          if(tmp_weight) tmp_weight[i]=interpole(x,ix,_x,_weight);
        }
      } else {
        tmp_y[i]=invalid_value;
        if(tmp_yErr) tmp_yErr[i]=0;
        if(tmp_weight) tmp_weight[i]=1;
      }
    } else {
      tmp_y[i]=invalid_value;
      if(tmp_yErr) tmp_yErr[i]=0;
      if(tmp_weight) tmp_weight[i]=1;
    }
  }
  // resize original vectors
  resize(n);
  // copy interpolated values to orignal vectors
  for(i=0;i<n;i++) {
    (*_x)[i]=(*xModel)[i];
    (*_y)[i]=tmp_y[i];
    if(tmp_yErr) (*_yErr)[i]=tmp_yErr[i];
    if(tmp_weight) (*_weight)[i]=tmp_weight[i];
  }
  // Clearing ...
  delete [] tmp_y;
  delete [] tmp_yErr;
  delete [] tmp_weight;
}

/*!
  Cut function between two x values
*/
void CompatFunction::cut(double min, double max,bool isInv)
{
  TRACE;
  ASSERT(_isSorted);
  // Transforms abcsisse according options (inv)
  if(isInv) inverseX();
  if(min<minX()) min=minX();
  if(max>maxX()) max=maxX();
  // Find the first point
  int ix=find(min,_x);
  if((*_x)[ix]!=min) {
    (*_y)[ix-1]=interpole(min,ix,_x,_y);
    if(_yErr) (*_yErr)[ix-1]=interpole(min,ix,_x,_yErr);
    if(_weight) (*_weight)[ix-1]=interpole(min,ix,_x,_weight);
    ix--;
    (*_x)[ix]=min;
  }
  // Remove all samples with indexes less than ix
  resize(_x->count()-ix,false);
  // Find the last point
  ix=find(max,_x);
  if((*_x)[ix]!=max) {
    (*_y)[ix]=interpole(max,ix,_x,_y);
    if(_yErr) (*_yErr)[ix]=interpole(max,ix,_x,_yErr);
    if(_weight) (*_weight)[ix]=interpole(max,ix,_x,_weight);
    (*_x)[ix]=max;
  }
  // Remove all samples with indexes greater than ix
  resize(ix+1,true);
  if(isInv) inverseX();
}

/*!
  Remove points with value=invalidValue
*/
void CompatFunction::removeInvalidValues(double invalidValue)
{
  TRACE;
  ASSERT(_isSorted);
  QVector<double>::iterator itx=_x->begin();
  QVector<double>::iterator ity=_y->begin();
  QVector<double>::iterator ityErr=0;
  if(_yErr) ityErr=_yErr->begin();
  QVector<double>::iterator itweight=0;
  if(_weight) itweight=_weight->begin();
  for(int i=_x->count()-1;i>=0;i--) {
    if(*(ity+i)==invalidValue) {
      _x->erase(itx+i);
      _y->erase(ity+i);
      if(_yErr) _yErr->erase(ityErr+i);
      if(_weight) _weight->erase(itweight+i);
    }
  }
}

double CompatFunction::minX()
{
  TRACE;
  ASSERT(_isSorted);
  return (*_x)[0];
}

double CompatFunction::maxX()
{
  TRACE;
  ASSERT(_isSorted);
  return (*_x)[_x->size()-1];
}

double CompatFunction::minY()
{
  TRACE;
  ASSERT(_isSorted);
  double val=std::numeric_limits<double>::infinity();
  QVector<double>::iterator it;
  for(it=_y->begin();it!=_y->end();++it) if(*it<val) val=*it;
  return val;
}

int CompatFunction::minYAt()
{
  TRACE;
  ASSERT(_isSorted);
  double val=std::numeric_limits<double>::infinity();
  int imin=0;
  for(int i=_y->count()-1;i>=0;i--) {
    if(_y->at(i)<val) {
      val=_y->at(i);
      imin=i;
    }
  }
  return imin;
}

int CompatFunction::maxYAt()
{
  TRACE;
  ASSERT(_isSorted);
  double val=-std::numeric_limits<double>::infinity();
  int imax=0;
  int n=_y->count();
  for(int i=0;i<n;i++) {
    if(_y->at(i)>val) {
      val=_y->at(i);
      imax=i;
    }
  }
  return imax;
}

double CompatFunction::maxY()
{
  TRACE;
  ASSERT(_isSorted);
  double val=-std::numeric_limits<double>::infinity();
  QVector<double>::iterator it;
  for(it=_y->begin();it!=_y->end();++it) if(*it>val) val=*it;
  return val;
}

/*!
  Order not modified, just take _x=log10(_x)
*/
void CompatFunction::log10()
{
  TRACE;
  ASSERT(_isSorted);
  QVector<double>::iterator it;
  for(it=_x->begin();it!=_x->end();++it) *it=::log10(*it);
}

/*!
  Order not modified, just take _x=10^_x
*/
void CompatFunction::exp10()
{
  TRACE;
  ASSERT(_isSorted);
  QVector<double>::iterator it;
  for(it=_x->begin();it!=_x->end();++it) *it=pow(10,*it);
}

/*!
  Order of _x values must be changed, applied to _y and _yErr
*/
void CompatFunction::inverseX()
{
  TRACE;
  ASSERT(_isSorted);
  int n=_x->size();
  double * tmp=new double [n];
  n--;
  int i;
  for(i=0;i<=n;i++) tmp[n-i]=1.0/(*_x)[i];
  for(i=0;i<=n;i++) (*_x)[i]=tmp[i];
  for(i=0;i<=n;i++) tmp[n-i]=(*_y)[i];
  for(i=0;i<=n;i++) (*_y)[i]=tmp[i];
  if(_yErr) {
    for(i=0;i<=n;i++) tmp[n-i]=(*_yErr)[i];
    for(i=0;i<=n;i++) (*_yErr)[i]=tmp[i];
  }
  if(_weight) {
    for(i=0;i<=n;i++) tmp[n-i]=(*_weight)[i];
    for(i=0;i<=n;i++) (*_weight)[i]=tmp[i];
  }
  delete tmp;
}

void CompatFunction::inverseY(double invalidValue)
{
  TRACE;
  ASSERT(_isSorted);
  int n=_x->size();
  for(int i=0;i<n;i++) if((*_y)[i]!=0 && (*_y)[i]!=invalidValue) (*_y)[i]=1.0/(*_y)[i];
}

void CompatFunction::log10Y(double invalidValue)
{
  TRACE;
  ASSERT(_isSorted);
  int n=_x->size();
  for(int i=0;i<n;i++) {
    if((*_y)[i]>0 && (*_y)[i]!=invalidValue) (*_y)[i]=::log10((*_y)[i]);
    else (*_y)[i]=invalidValue;
  }
}

void CompatFunction::log10YErr()
{
  TRACE;
  ASSERT(_isSorted);
  if(!_yErr) return;
  int n=_x->size();
  for(int i=0;i<n;i++) {
    if((*_yErr)[i]!=0) (*_yErr)[i]=::log10((*_yErr)[i]);
  }
}

void CompatFunction::inverseYErr()
{
  TRACE;
  ASSERT(_isSorted);
  if(!_yErr) {
    fprintf(stderr,"inverseYErr needs the _yErr vector\n");
    return;
  }
  int n=_x->size();
  for(int i=0;i<n;i++) if((*_yErr)[i]!=0) (*_yErr)[i]=1.0/(*_yErr)[i];
}

double CompatFunction::fitFactor(double x,double val)
{
  TRACE;
  ASSERT(_isSorted);
  if(!_yErr) {
    fprintf(stderr,"inverseYErr needs the _yErr vector\n");
    return 0;
  }
  int index=find(x,_x);
  double mean=interpole(x,index,_x,_y);
  double stddev=interpole(x,index,_x,_yErr);
  double delta=(val-mean)/stddev;
  return (delta*delta);
}

double CompatFunction::fitFactor(int index,double val)
{
  TRACE;
  ASSERT(_isSorted);
  if(!_yErr) {
    fprintf(stderr,"inverseYErr needs the _yErr vector\n");
    return 0;
  }
  double mean=_y->at(index);
  double stddev=_yErr->at(index);
  double delta=(val-mean)/stddev;
  return (delta*delta);
}

/*!
  Interpole val in function defined by inVector versus outVector, indexInVector
  specify the position in vectors: interpole between [indexInVector-1] and
  [indexInVector]
*/
double CompatFunction::interpole(double val,int indexInVector,
                              QVector<double> * inVector,
                              QVector<double> * outVector)
{
  TRACE;
  return (*outVector)[indexInVector-1]+
         ((*outVector)[indexInVector]-(*outVector)[indexInVector-1]) /
         ((*inVector)[indexInVector]-(*inVector)[indexInVector-1]) *
         (val-(*inVector)[indexInVector-1]);
}

/*!
  Returns index of inVector that is just greater or equal to val, return value
  is always greater or equal to 1. Uses dichotomy, best for functions with
  more than 10 values. Never return a number less than 1
*/
int CompatFunction::find(double val,QVector<double> * inVector)
{
  TRACE;
  //cout << "******* Find " << val << endl;
  //for(uint i=0;i<inVector->size();i++)
  //  cout << i << " " << (*inVector)[i] << endl;
  if(!inVector) return 1;
  QVector<double>& vect=*inVector;
  if(val<=vect[0]) return 1;
  int lasti=(int)vect.size()-1;
  if(val>vect[lasti-1]) return lasti;
  int i=_n2;
  int step2=i >> 1;
  while(step2>0) {
    //cout << i << " " << lasti << " " << step2 << " " << vect[i] << endl;
    if(i>lasti) i-=step2;
    else if(val<=vect[i]) {
      if(val>vect[i-1]) break;
      i-=step2;
    } else i+=step2;
    step2=step2 >> 1;
  }
  //cout << "Returning " << i << endl;
  return i;
}

/*!
  Try to find modes by analysing first and second derivative
*/
void CompatFunction::getModes(QVector<double>& mean,
                           QVector<double>& stddev)
{
  TRACE;
  ASSERT(_isSorted);
  QVector<double>& xvect=*_x;
  QVector<double>& yvect=*_y;
  double d1,d2=(yvect[1]-yvect[0])/(xvect[1]-xvect[0]);
  double dd;
  dd=0;
  int lastMin=0, lastMax=-1;
  for(int i=2;i<xvect.size();i++) {
    d1=d2;
    d2=(yvect[i]-yvect[i-1])/(xvect[i]-xvect[i-1]);
    dd=2*(d2-d1)/((xvect[i-1]+xvect[i])-(xvect[i-2]+xvect[i-1]));
    // Check root of first derivative
    if((d1<0 && d2>0) || (d1>0 && d2<0) || (i==xvect.size()-1 && d2<0)) {
      if(dd<0) {
        // We have a maximum
        lastMax=i-1;
      } else {
        if(lastMax!=-1) {
          // Try to fit an exponential on function from lastMin to i-1
          double f0=yvect[lastMax];
          double x0=xvect[lastMax];
          double minSum=std::numeric_limits<double>::infinity();
          double minSigma=0;
          for(double sigma=1.0;sigma<200.0;sigma+=1.0) {
            double sigmaInv2=1.0/ (sigma*sigma);
            double sum=0;
            for(int j=lastMin;j<i;j++) {
              double deltaX=xvect[j]-x0;
              double deltaY=yvect[j]-f0*exp(-deltaX*deltaX*sigmaInv2);
              sum+=deltaY*deltaY;
            }
            if(sum<minSum) {
              minSum=sum;
              minSigma=sigma;
            }
          }
          mean.push_back(x0);
          stddev.push_back(minSigma);
        }
        lastMin=i-1;
      }
    }
  }
}

int CompatFunction::closestMax(int starti)
{
  TRACE;
  ASSERT(_isSorted);
  QVector<double>& xvect=*_x;
  QVector<double>& yvect=*_y;
  int n=xvect.size();
  int di;
  if(starti==0) {
    di=1;
    starti=1;
  }
  else if(starti>=n) {starti=n;di=-1;} else if(yvect[starti]>yvect[starti-1]) di=1;
  else if(yvect[starti]<yvect[starti-1]) di=-1;
  else return starti;
  while(starti>0 && starti<n) {
    if(di>0) {
      if(yvect[starti]<yvect[starti-1]) {
        starti--;
        break;
      }
    } else {
      if(yvect[starti]>yvect[starti-1]) break;
    }
    starti+=di;
  }
  return starti;
}

void CompatFunction::multiplyX(double value)
{
  TRACE;
  QVector<double>& xvect=*_x;
  for(int i=0;i<xvect.size();i++)
    xvect[i]*=value;
  if(value<=0) sort();
}

void CompatFunction::multiplyXbyY()
{
  TRACE;
  QVector<double>& xvect=*_x;
  QVector<double>& yvect=*_y;
  for(int i=0;i<xvect.size();i++)
    xvect[i]*=yvect[i];
  sort();
}

/*!
  Compute the sum of 0.5*(y[i]+y[i-1])*(x[i]-x[i-1])
  Which is the average value, if f is a normalized partition function
  with y being the cumulative probability and x the values of the random variable
*/
double CompatFunction::average()
{
  TRACE;
  QVector<double>& xvect=*_x;
  QVector<double>& yvect=*_y;
  double sum=0, lastx, curx, lasty, cury;
  lastx=xvect[0];
  lasty=yvect[0];
  for(int i=1;i<xvect.size();i++) {
    curx=xvect[i];
    cury=yvect[i];
    sum+=0.5*(curx+lastx)*(cury-lasty);
    lastx=curx;
    lasty=cury;
  }
  return sum;
}

/*!
  Compute the sum of (0.5*(y[i]+y[i-1])-average)^2*(x[i]-x[i-1])
  Which is the average value, if f is a normalized partition function
  with y being the cumulative probability and x the values of the random variable
*/
double CompatFunction::stddev(double average)
{
  TRACE;
  QVector<double>& xvect=*_x;
  QVector<double>& yvect=*_y;
  double sum=0, lastx, curx, lasty, cury;
  lastx=xvect[0];
  lasty=yvect[0];
  for(int i=1;i<xvect.size();i++) {
    curx=xvect[i];
    cury=yvect[i];
    double delta=0.5*(curx+lastx)-average;
    sum+=delta*delta*(cury-lasty);
    lastx=curx;
    lasty=cury;
  }
  return sqrt(sum);
}

} // namespace QGpCompatibility
