/***************************************************************************
**
**  This file is part of DinverCore.
**
**  DinverCore 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.
**
**  DinverCore 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: 2009-05-06
**  Copyright: 2009-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

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

namespace DinverCore {

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

  Full description of class still missing
*/

/*!
  Description of constructor still missing
*/
VoronoiNavigator::VoronoiNavigator(const ScaledModels * m)
{
  TRACE;
  _cells=m;

  _currentAxis=-1;
  _currentCell=-1;

  _axisDistances=new double [ _cells->modelCount() ];
  _currentPoint=new double [ _cells->parameterCount() ];
}

/*!
  Description of destructor still missing
*/
VoronoiNavigator::~VoronoiNavigator()
{
  TRACE;
  delete [] _axisDistances;
  delete [] _currentPoint;
}

void VoronoiNavigator::printCurrentPoint()
{
  int nd=_cells->parameterCount();
  for(int id=0; id<nd; id++) {
    printf(" %lf",_currentPoint[id]/_cells->scale(id));
  }
  printf("\n");
}

/*!
  \fn void VoronoiNavigator::setLocation(int index)
  Set current point as one of the cell centers and set current cell to \a index.
*/

/*!
  \fn void VoronoiNavigator::setLocation(const int * coordinates)
  Set current point to any arbitrary \a coordinates, current cell is not set.
*/


/*!
  Set current axis and calculate the square distances to axis to init the iterative
  computation implemented in incrementAxis(). The current pos must be initialized by
  setCurrentPos().

  |Vk-Xj|=|Vj-Xj| defines the intersection of the cell boundary (between cells k and j)
  with the considered axis. k is the current cell.

  Along the axis i, looking at the cell boundary between cells k and j:

  dk^2 + (( Vk[i] - Xj[i] ) * s[i] )^2=dj^2 + (( Vj[i] - Xj[i] ) * s[i] )^2

  dk^2 + (Vk[i] * s[i] )^2 + (Xj[i] * s[i] )^2 - 2 * Vk[i] * Xj[i] * s[i]^2
                =dj^2 + (Vj[i] * s[i] )^2 + (Xj[i] * s[i] )^2 - 2 * Vj[i] * Xj[i] * s[i]^2

  dk^2 + (Vk[i] * s[i] )^2=dj^2 + (Vj[i] * s[i] )^2 - 2 * s[i]^2 * Xj[i] (Vj[i] - Vk[i] )

  where dk and dj are the distances from axis to centers of cells. Vk and Vj are the
  centers of cells k and j, respectively. Vk[i] is the ith coordinate of point Vk.
  s[i] is the scale along axis i.

  2 * Xj[i] * s[i]^2 * (Vk[i] - Vj[i] )=(Vk[i]^2 - Vj[i]^2) * s[i]^2 + (dk^2 - dj^2)

  Pre-calculates all dk and dj.
*/
void VoronoiNavigator::setCurrentAxis(int a)
{
  if(a==_cells->parameterCount()) a=0;
  _currentAxis=a;
  double d, dtot;
  double * dist=_axisDistances;
  int nm=_cells->modelCount();
  int nd=_cells->parameterCount();
  int id;
  for(int im=0;im<nm; im++ ) {
    dtot=0.0;
    for(id=0; id < _currentAxis; id++ ) {
      d=_currentPoint[ id ] - _cells->v(id)[im];
      dtot += d * d;
    }
    for(id++; id < nd;id++ ) {
      d=_currentPoint[ id ] - _cells->v(id)[im];
      dtot += d * d;
    }
    *(dist++ )=dtot;
  }
}

/*!
  Increment axis and update the distances to new axis
*/
void VoronoiNavigator::incrementAxis()
{
  TRACE;
  ASSERT(_currentAxis>-1);
  double d;
  int newAxis=_currentAxis + 1;
  if(newAxis==_cells->parameterCount()) newAxis=0;
  double xcur=_currentPoint[ _currentAxis ];
  double xnew=_currentPoint[ newAxis ];
  double * dist=_axisDistances;
  int nm=_cells->modelCount();
  for(int im=0;im<nm; im++ ) {
    d=xnew - _cells->v(newAxis)[im];
    double& distVal=*dist;
    distVal -= d * d;
    d=xcur - _cells->v(_currentAxis)[im];
    distVal += d * d;
    //fprintf(stdout,"Axis Dist=%8lg\n",sqrt(distVal));
    dist++;
  }
  _currentAxis=newAxis;
}

/*!
  If Xj[i] is right on the cell border (between Vj and Vk),

    2 * Xj[i] * s[i]^2 * (Vk[i] - Vj[i] )=(Vk[i]^2 - Vj[i]^2) * s[i]^2 + (dk^2 - dj^2)

  Or,

    2 * Xj[i] * s[i]=xLim/dx, dx includes the scaling

    if xLim/(2*dx)<Pk[i]*s[i] && xLim/dx>xMin, move xMin to xLim/dx
    if xLim/(2*dx)>Pk[i]*s[i] && xLim/dx<xMax, move xMax to xLim/dx

  Where Pk is the current point inside cell of Vk, hence,

    xLim < 2 * s[i] * Pk[i] * dx

    if dx>0, xLim/dx < 2 * s[i] * Pk[i]
    if dx<0, xLim/dx > 2 * s[i] * Pk[i]

  We would like to postpone the division by dx only when the limits is really moved. hence,
  we multiply xMin and xMax by dx (division is approximately 5 times slower than multiplication).
  xMin and xMax are multiplied by 2 upon startup.

  if dx>0, the conditions become:

    if xLim/dx<2*Pk[i]*s[i] && xLim>xMin*dx, move xMin to xLim/dx (a)
    if xLim/dx>2*Pk[i]*s[i] && xLim<xMax*dx, move xMax to xLim/dx (b)
    (first condition of (b) is always false, of (a) is always true)

  if dx<0, the conditions become:

    if xLim/dx<2*Pk[i]*s[i] && xLim<xMin*dx, move xMin to xLim/dx (a)
    if xLim/dx>2*Pk[i]*s[i] && xLim>xMax*dx, move xMax to xLim/dx (b)
    (first condition of (a) is always false, of (b) is always true)

  if dx==0, no intersection, hence skip

  if dx>0, the conditions become:

    if xLim>xMin*dx, move xMin to xLim/dx (a)

  if dx<0, the conditions become:

    if xLim>xMax*dx, move xMax to xLim/dx (b)

  _currentCell is k, _axisDistances[ _currentCell ] is dk
  _axisDistances[ j ] is dj

  Thus, we just test the sign of dx and we can directly compare to the current min and max of the cell.

  \a iMin and \a iMax are set the cell indexes of the two neighbors crossed by the current axis (-1 if
  there are no neighbors).
*/
void VoronoiNavigator::cellLimits(int& xMin, int& xMax, int& iMin, int& iMax)
{
  TRACE;
  ASSERT(_currentAxis>-1);
  ASSERT(_currentCell>-1);
  const double * vp=_cells->v(_currentAxis);
  const double * v2p=_cells->v2(_currentAxis);
  double vk=vp[_currentCell];
  double vk2dk2=v2p[_currentCell]+_axisDistances[_currentCell];
  double xLim, dx;
  double s=_cells->scale(_currentAxis);
  double s2=2.0*s;
  double dxMin=(double)xMin*s2;
  double dxMax=(double)xMax*s2;
  iMin=-1;
  iMax=-1;
  int n=_cells->modelCount();
  for(int j=0; j<n; j++) {
    dx=vk-vp[j];
    xLim=vk2dk2-(v2p[j]+_axisDistances[j]);
    if(dx>0.0) {
      if(xLim>dxMin*dx) {
        if(xLim<=0.0) {
          xLim=0.0;
        }
        dxMin=xLim/dx;
        iMin=j;
      }
    } else if(dx<0.0) {
      if(xLim>dxMax*dx) {
        if(xLim>=0.0) {
          xLim=0.0;
        }
        dxMax=xLim/dx;
        iMax=j;
      }
    }
  }
  /*
     Distances to axes are updated incrementaly, which is likely to introduce
     some errors in the determination of dxMin and dxMax. By experience, the
     maximum relative error is of the order of 1e-16. This is negligible except
     when rounding limits to integers. By default, we keep as conservative as
     possible, hence limits are rounded with ceil and floor for xMin and xMax,
     respectively. If the current point, which is a valid model (fit with all
     conditions) and which has proved to belong to the current cell, is very
     close to the edge of the cell, the conservative rounding might consider
     the current point as outside. Hence ceil() and floor() are replaced by round()
  */
  double invS=1.0/s;
  double invS2=0.5*invS;
  int pk=(int)round(_currentPoint[_currentAxis]*invS);
  if(iMin>-1) {
    dxMin*=invS2;
    xMin=(int) ceil(dxMin);
    if(pk==xMin-1) {
      xMin=pk;
      //App::log(tr("Adjust xMin limit to Pk(%1), dxMin=%2")
      //         .arg(pk).arg(dxMin, 0, 'g', 20) << endl;
    }
  }
  if(iMax>-1) {
    dxMax*=invS2;
    xMax=(int) floor(dxMax);
    if(pk==xMax+1) {
      xMax=pk;
      //App::log(tr("Adjust xMax limit to Pk(%1), dxMax=%2")
      //         .arg(pk).arg(dxMax, 0, 'g', 20) << endl;
    }
  }
}

/*!
  Debug purpose, get the cells containing model.
*/
QVector<int> VoronoiNavigator::cellAt(const int * p) const
{
  TRACE;
  QVector<int> belongs;
  double minDistance=std::numeric_limits<double>::infinity();
  int nm=_cells->modelCount();
  int nd=_cells->parameterCount();
  for(int im=0; im < nm; im++ ) {
    double distance=0.0, tmp;
    for(int id=0; id < nd; id++) {
      tmp=_cells->v(id)[im] - p[id] * _cells->scale(id);
      distance += tmp*tmp;
    }
    if(distance<minDistance) {
      belongs.clear();
      belongs.append(im);
      minDistance=distance;
    } else if(distance==minDistance) {
      belongs.append(im);
    }
  }
  return belongs;
}

/*!
  Debug purpose, get the cells containing scaled model.
*/
QVector<int> VoronoiNavigator::cellAt(const double * p) const
{
  TRACE;
  QVector<int> belongs;
  double minDistance=std::numeric_limits<double>::infinity();
  int nm=_cells->modelCount();
  int nd=_cells->parameterCount();
  for(int im=0; im < nm; im++ ) {
    double distance=0.0, tmp;
    for(int id=0; id < nd; id++) {
      tmp=_cells->v(id)[im] - p[id];
      distance += tmp*tmp;
    }
    if(distance<minDistance) {
      belongs.clear();
      belongs.append(im);
      minDistance=distance;
    } else if(distance==minDistance) {
      belongs.append(im);
    }
  }
  return belongs;
}

/*!
  Brute comparison of current distance, check for precison leakage
  This function is strictly for debug purposes
*/
void VoronoiNavigator::checkAxisDistances()
{
  TRACE;
  double d, dtot;
  double * dist=_axisDistances;
  int id;
  int nm=_cells->modelCount();
  int nd=_cells->parameterCount();
  for(int im=0;im<nm; im++ ) {
    dtot=0;
    for(id=0; id < _currentAxis; id++ ) {
      d=_currentPoint[ id ] - _cells->v(id)[im];
      dtot += d * d;
    }
    for(id++; id < nd; id++ ) {
      d=_currentPoint[ id ] - _cells->v(id)[im];
      dtot += d * d;
    }
    if(dtot > 0.0) {
      double relErr=( *dist - dtot)/dtot;
      if(fabs( relErr) > 1e-10) {
        App::log(tr("  Error in iterative square distance for model %1: %2\n"
                    "      iterative square distance=%3\n"
                    "      true square distance=%4\n")
                  .arg(im).arg(relErr).arg(*dist).arg(dtot));
      }
    } else {
      if(fabs(*dist)>1e-15) {
        App::log(tr("  Error in iterative square distance for model %1:\n"
                    "      iterative square distance=%2\n"
                    "      true square distance=%3\n")
                  .arg(im).arg(*dist).arg(dtot));
      }
    }
    dist++;
  }
}

/*!
  Check if current point belongs to current cell
*/
void VoronoiNavigator::checkCurrentPoint()
{
  QVector<int> iGenModel=cellAt(_currentPoint);
  if( ! iGenModel.contains(_currentCell) ) {
    printf("Model effectively NOT generated in cell %i but:\n", _currentCell);
    for(QVector<int>::iterator it=iGenModel.begin(); it!=iGenModel.end(); it++) {
      printf("%i ", *it);
    }
    printf("\n" );
    ASSERT(false);
  }
}

/*!
  The intersections are returned along current axis set by setAxis()
*/
PdfCurve VoronoiNavigator::intersections(double xMin, double xMax)
{
  ASSERT(_currentAxis>-1);
  ASSERT(_currentCell>-1);
  PdfCurve f;
  // f for the first cell, separate models into lower and higher (along axis)
  const double * vp=_cells->v(_currentAxis);
  const double * v2p=_cells->v2(_currentAxis);
  double vk=vp[_currentCell];
  double vk2dk2=v2p[ _currentCell ] + _axisDistances[ _currentCell ];
  double xLim, dx;
  double s=_cells->scale(_currentAxis);
  double s2=2.0*s;
  double dxMinLimit=xMin*s2;
  double dxMaxLimit=xMax*s2;
  double dxMin=dxMinLimit;
  double dxMax=dxMaxLimit;
  int n=_cells->modelCount();
  int * lowerCells=new int [n];
  int * higherCells=new int [n];
  int nLowerCells=0, nHigherCells=0, iMin=-1, iMax=-1;
  for(int j=0; j< n; j++ ) {
    dx=vk - vp[ j ];
    xLim=vk2dk2 - (v2p[ j ] + _axisDistances[ j ] );
    if(dx > 0.0) {
      if(xLim > dxMin * dx) {
        if(xLim<=0.0) {
          xLim=0.0;
        }
        dxMin=xLim/dx;
        iMin=j;
      }
      lowerCells[nLowerCells]=j;
      nLowerCells++;
    } else if(dx < 0.0) {
      if(xLim > dxMax * dx) {
        if(xLim>=0.0) {
          xLim=0.0;
        }
        dxMax=xLim/dx;
        iMax=j;
      }
      higherCells[nHigherCells]=j;
      nHigherCells++;
    }
  }
  /*if(dxMin>=dxMax) {
    printf("dxMin %lf dxMax %lf\n",dxMin,dxMax);
    checkCurrentPoint();
    ASSERT(false);
  }*/
  int originalCell=_currentCell;
  double invS2=0.5/s;
  if(iMin>-1) {
    _currentCell=iMin;
    lowerIntersections(f, dxMinLimit, lowerCells, nLowerCells, invS2);
  }
  //App::log("main append " << invS2*dxMin << " " << originalCell << endl;
  f.append(PdfPoint(invS2*dxMin, originalCell));
  //App::log("main append " << invS2*dxMax << " " << iMax << endl;
  f.append(PdfPoint(invS2*dxMax, iMax));
  //ASSERT(f.count()>1);
  if(iMax>-1) {
    _currentCell=iMax;
    higherIntersections(f, dxMaxLimit, higherCells, nHigherCells, invS2);
  }
  _currentCell=originalCell;
  delete [] lowerCells;
  delete [] higherCells;
  f.sort();
  //ASSERT(f.count()>1);
  return f;
}

/*!
  For intersections
*/
void VoronoiNavigator::lowerIntersections(PdfCurve& f, double dxMinLimit, int * cells, int nCells, double invS2)
{
  const double * vp=_cells->v(_currentAxis);
  const double * v2p=_cells->v2(_currentAxis);
  double vk=vp[_currentCell];
  double vk2dk2=v2p[ _currentCell ] + _axisDistances[ _currentCell ];
  double xLim, dx;
  double dxMin=dxMinLimit;
  int n=_cells->modelCount();
  int * subCells=new int [n];
  int nSubCells=0, iMin=-1;
  int j;
  for(int ji=0; ji<nCells; ji++) {
    j=cells[ji];
    dx=vk - vp[ j ];
    xLim=vk2dk2 - (v2p[ j ] + _axisDistances[ j ] );
    if(dx > 0.0) {
      if(xLim > dxMin * dx) {
        dxMin=xLim/dx;
        iMin=j;
      }
      subCells[nSubCells]=j;
      nSubCells++;
    }
  }
  int originalCell=_currentCell;
  if(iMin>-1) {
    _currentCell=iMin;
    lowerIntersections(f, dxMinLimit, subCells, nSubCells, invS2);
  }
  //App::log("lower append " << invS2*dxMin << " " << originalCell << endl;
  f.append(PdfPoint(invS2*dxMin, originalCell));
  delete [] subCells;
}

/*!
  For intersections
*/
void VoronoiNavigator::higherIntersections(PdfCurve& f, double dxMaxLimit, int * cells, int nCells, double invS2)
{
  const double * vp=_cells->v(_currentAxis);
  const double * v2p=_cells->v2(_currentAxis);
  double vk=vp[_currentCell];
  double vk2dk2=v2p[ _currentCell ] + _axisDistances[ _currentCell ];
  double xLim, dx;
  double dxMax=dxMaxLimit;
  int n=_cells->modelCount();
  int * subCells=new int [n];
  int nSubCells=0, iMax=-1;
  int j;
  for(int ji=0; ji<nCells; ji++) {
    j=cells[ji];
    dx=vk - vp[ j ];
    xLim=vk2dk2 - (v2p[ j ] + _axisDistances[ j ] );
    if(dx < 0.0) {
      if(xLim > dxMax * dx) {
        dxMax=xLim/dx;
        iMax=j;
      }
      subCells[nSubCells]=j;
      nSubCells++;
    }
  }
  if(iMax>-1) {
    //App::log("higher append " << invS2*dxMax << " " << iMax << endl;
    f.append(PdfPoint(invS2*dxMax, iMax));
    _currentCell=iMax;
    higherIntersections(f, dxMaxLimit, subCells, nSubCells, invS2);
  } else {
    //App::log("higher append " << invS2*dxMax << " -1"  << endl;
    f.append(PdfPoint(invS2*dxMax, -1));
  }
  delete [] subCells;
}

} // namespace DinverCore
