/***************************************************************************
**
**  This file is part of TapePositioningSystem.
**
**  TapePositioningSystem 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.
**
**  TapePositioningSystem 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: 2012-11-15
**  Copyright: 2012-2019
**    Marc Wathelet (ISTerre, Grenoble, France)
**
***************************************************************************/

#include <QGpCoreTools.h>

#include "Triangulator.h"
#include "Cluster.h"
#include "Node.h"
#include "Anchorage.h"

namespace TapePositioningSystem {

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

    Full description of class still missing
  */

  /*!
    Description of constructor still missing
  */
  Triangulator::Triangulator()
  {
    TRACE;
    _maxStddev=0.05;
    _progress=0;
  }

  /*!
    Description of destructor still missing
  */
  Triangulator::~Triangulator()
  {
    TRACE;
    qDeleteAll(_clusters);
    for(QMap<QString, Node *>::const_iterator it=_nodes.begin(); it!=_nodes.end(); it++) {
      delete it.value();
    }
  }

  Node * Triangulator::addNode(const QString& name)
  {
    TRACE;
    Node * n=new Node;
    n->setName(name);
    _nodes.insert(name, n);
    return n;
  }

  Node * Triangulator::node(const QString& name) const
  {
    TRACE;
    QMap<QString, Node *>::const_iterator it=_nodes.find(name);
    if(it!=_nodes.end()) {
      return it.value();
    } else {
      return 0;
    }
  }

  void Triangulator::addDistance(const QString& node1, const QString& node2, double value)
  {
    TRACE;
    Node * n1=node(node1);
    if(!n1) {
      n1=addNode(node1);
    }
    Node * n2=node(node2);
    if(!n2) {
      n2=addNode(node2);
    }
    n1->addDistance(n2, value);
    n2->addDistance(n1, value);
  }

  /*!
    \a stddev is the estimated precision for distance measurements.
  */
  void Triangulator::aggregate(int clusterCount, double stddev)
  {
    TRACE;

    // Builds all possible clusters made of robust quads, centered around each node
    // Reference: Moore et al. 2004, SenSys'04, November 3-5, 2004, Baltimore, Maryland, USA
    QList<Cluster *> quads;
    for(QMap<QString, Node *>::const_iterator it=_nodes.begin(); it!=_nodes.end(); it++) {
      quads.append(it.value()->robustQuads(stddev));
    }
    App::log(1, tr("Found %1 robust quads\n").arg(quads.count()));
    // Eliminate double entries
    std::sort(quads.begin(), quads.end(), Cluster::lessThan);
    uniqueDeleteAll(quads, Cluster::equal);
    App::log(1, tr("Found %1 robust and unique quads\n").arg(quads.count()));
    if(App::verbosity()>=2) {
      for(QList<Cluster *>::const_iterator it=quads.begin(); it!=quads.end(); it++) {
        App::log(2, "  "+(*it)->name()+"\n");
      }
    }

    // Aggregates as much quads as possible for all clusters
    _clusters.clear();

    if(_progress) {
      _progress->setCaption(tr("Aggregating clusters"));
    }

    //orderedAggregate(quads);
    //exhaustiveAggregate(quads);
    randomAggregate(clusterCount, quads);

    qDeleteAll(quads);
  }

  QVector<int> Triangulator::randomIndex(Random& generator, int n)
  {
    TRACE;
    QVector<int> index(n);
    for(int i=0; i<n; i++) {
      index[i]=i;
    }
    // shuffle
    if(n>0) {
      for(int i=2*n; i>=0; i--) {
        int i1=generator.uniform(0, n-1);
        int i2=generator.uniform(0, n-1);
        qSwap(index[i1], index[i2]);
      }
    }
    return index;
  }

  void Triangulator::randomAggregate(int clusterCount, QList<Cluster *> quads)
  {
    Random generator;
    int nq=quads.count();
    if(nq<=0) return;
    int trialCount=clusterCount*10;
    while(_clusters.count()<clusterCount && trialCount>0) {
      QVector<int> quadIndex=randomIndex(generator, nq);
      Cluster * parent=new Cluster(*quads.at(quadIndex.first()));
      App::log(4, tr("------- NEW CLUSTER starting with %1 -------\n").arg(parent->name()));
      bool somethingAdded;
      do {
        somethingAdded=false;
        for(int iq=0; iq<nq; iq++) {
          Cluster * quad=quads.at(quadIndex.at(iq));
          Anchorage * a=parent->anchorage(*quad);
          if(a) {
            App::log(4, tr("%1 nodes, adding %2\n").arg(parent->count()).arg(a->n()->name()));
            if(parent->add(*quad, a, _maxStddev)) {
              somethingAdded=true;
              delete a;
            } else {
              somethingAdded=false;
              delete a;
              delete parent;
              parent=0;
              break;
            }
          }
        }
      } while (somethingAdded);
      trialCount--;
      if(parent && parent->checkDistances(_maxStddev)) {
        _clusters.append(parent);
        if(_progress) {
          _progress->setValue(_clusters.count());
        }
      } else {
        delete parent;
      }
    }
  }

  bool Triangulator::exhaustiveAggregate(Cluster * parent, QList<Cluster *> quads)
  {
    bool somethingAdded=false;
    int nq=quads.count();
    for(int iq=0; iq<nq; iq++) {
      Cluster * quad=quads.at(iq);
      Anchorage * a=parent->anchorage(*quad);
      if(a) {
        Cluster * c=new Cluster(*parent);
        if(c->add(*quad, a, _maxStddev)) {
          // Build a copy of quad list with this quad just added
          QList<Cluster *> quadsToAdd;
#if(QT_VERSION >= QT_VERSION_CHECK(4, 7, 0))
          quadsToAdd.reserve(nq-1);
#endif
          int ic;
          for(ic=0; ic<iq; ic++) {
            quadsToAdd.append(quads.at(ic));
          }
          for(ic++; ic<nq; ic++) {
            quadsToAdd.append(quads.at(ic));
          }
          exhaustiveAggregate(c, quadsToAdd);
          somethingAdded=true;
          delete a;
        } else {
          delete a;
          delete parent;
          return false;
        }
      }
    }
    if(somethingAdded) {
      delete parent;
    } else {
      _clusters.append(parent);
      if(_progress) {
        _progress->setValue(_clusters.count());
      }
    }
    return true;
  }

  void Triangulator::orderedAggregate(QList<Cluster *> quads)
  {
    int nq=quads.count();
    for(int iqp=0; iqp<nq; iqp++) {
      Cluster * parent=new Cluster(*quads.at(iqp));
      bool somethingAdded;
      do {
        somethingAdded=false;
        for(int iq=0; iq<nq; iq++) {
          Cluster * quad=quads.at(iq);
          Anchorage * a=parent->anchorage(*quad);
          if(a) {
            if(parent->add(*quad, a, _maxStddev)) {
              somethingAdded=true;
              delete a;
            } else {
              delete a;
              delete parent;
              parent=0;
              break;
            }
          }
        }
      } while (somethingAdded);
      if(parent) {
        _clusters.append(parent);
        if(_progress) {
          _progress->setValue(_clusters.count());
        }
      }
    }
  }

  /*!
    Sets a consistance coordinate system for all clusters.
    \a origin defines the translation of the relative coordinates,
    \a north defines the rotation and \eastward solved the flip ambiguity.
  */
  void Triangulator::setCoordinates(Node * origin, Node * north, Node * eastward)
  {
    TRACE;
    for(int i=_clusters.count()-1; i>=0; i--) {
      Cluster * c=_clusters.at(i);
      if(!c->setCoordinateSystem(origin, north, eastward)) {
        delete c;
        _clusters.removeAt(i);
      }
    }
  }

  void Triangulator::printClusters()
  {
    TRACE;
    QTextStream s(stdout);
    for(QList<Cluster *>::iterator itc=_clusters.begin(); itc!=_clusters.end(); itc++) {
      Cluster * c=*itc;
      s << "# " << c->name() << "\n";
      QList<Node *> nodes=c->nodes();
      for(QList<Node *>::iterator it=nodes.begin(); it!=nodes.end(); it++) {
        s << c->coordinates(*it) << " 0 " << (*it)->name() << "\n";
      }
    }
    s.flush();
  }

  void Triangulator::setPosteriorPrecision(double precision)
  {
    TRACE;
    _maxStddev=precision;
  }

  /*!
    Removes all clusters that do not match with prior information.
  */
  void Triangulator::setPriorCoordinates(const QMap<QString, Point>& prior, double precision)
  {
    TRACE;
    for(QMap<QString, Node *>::const_iterator itn=_nodes.begin(); itn!=_nodes.end(); itn++) {
      Node * n=itn.value();
      QMap<QString, Point>::const_iterator itprior=prior.find(n->name());
      if(itprior!=prior.end()) {
        const Point2D& p=itprior.value();
        for(int i=_clusters.count()-1; i>=0; i--) {
          Cluster * c=_clusters.at(i);
          QMap<Node *, Point2D>::const_iterator itcn=c->find(n);
          if(itcn!=c->end()) {
            if(p.distanceTo(itcn.value())>precision) {
              delete c;
              _clusters.removeAt(i);
            }
          }
        }
      }
    }
  }

  QSet<Point2D> Triangulator::solutions(const QString& nodeName) const
  {
    TRACE;
    Node * n=node(nodeName);
    // Gathers all coordinates in QSet to remove dupplicate entries
    QSet<Point2D> coordinates;
    for(QList<Cluster *>::const_iterator itc=_clusters.begin(); itc!=_clusters.end(); itc++) {
      Cluster * c=*itc;
      QMap<Node *, Point2D>::const_iterator itcn=c->find(n);
      if(itcn!=c->end()) {
        // Do not consider varitions below 1 mm
        Point2D p= itcn.value();
        p.setX(floor(p.x()*1e3)*1e-3);
        p.setY(floor(p.y()*1e3)*1e-3);
        coordinates.insert(p);
      }
    }
    return coordinates;
  }

  Covariance Triangulator::covariance(const QString& nodeName) const
  {
    TRACE;
    QSet<Point2D> coordinates=solutions(nodeName);
    double p[2];
    Statistics statx, staty;
    Covariance cov(2);
    for(QSet<Point2D>::iterator its=coordinates.begin(); its!=coordinates.end(); its++) {
      const Point2D& coord=*its;
      p[0]=coord.x();
      p[1]=coord.y();
      statx.add(p[0]);
      staty.add(p[1]);
      cov.add(p);
    }
    return cov;
  }

} // namespace TapePositioningSystem
