/***************************************************************************
**
**  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-16
**  Copyright: 2012-2019
**    Marc Wathelet (ISTerre, Grenoble, France)
**
***************************************************************************/

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

namespace TapePositioningSystem {

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

    Full description of class still missing
  */

  Cluster::Cluster()
  {
    TRACE;
    _type=None;
  }

  Cluster::Cluster(const Cluster &o)
  {
    TRACE;
    _type=o._type;
    _origin=o._origin;
    _nodes=o._nodes;
  }

  QString Cluster::name() const
  {
    TRACE;
    QMap<Node *, Point2D>::const_iterator it=_nodes.begin();
    QString n;
    if(it!=_nodes.end()) {
      n=it.key()->name();
    } else {
      return "empty";
    }
    for(it++; it!=_nodes.end(); it++) {
      n+=","+it.key()->name();
    }
    return n;
  }

  bool Cluster::operator==(const Cluster& o) const
  {
    TRACE;
    if(count()!=o.count()) {
      return false;
    }
    QMap<Node *, Point2D>::const_iterator it1=_nodes.begin();
    QMap<Node *, Point2D>::const_iterator it2=o._nodes.begin();
    while(it1!=_nodes.end() &&
          it1.key()==it2.key()){
      it1++;
      it2++;
    }
    return it1==_nodes.end();
  }

  bool Cluster::operator<(const Cluster& o) const
  {
    TRACE;
    if(count()==o.count()) {
      QMap<Node *, Point2D>::const_iterator it1=_nodes.begin();
      QMap<Node *, Point2D>::const_iterator it2=o._nodes.begin();
      while(it1!=_nodes.end() &&
            it1.key()==it2.key()){
        it1++;
        it2++;
      }
      if(it1!=_nodes.end()) {
        return it1.key()<it2.key();
      } else {
        return false;
      }
    } else {
      return count()<o.count();
    }
  }

  void Cluster::setOrigin(Node * node)
  {
    TRACE;
    _nodes.clear();
    _nodes.insert(node, Point2D());
    _origin=node;
  }

  /*!
    \a maxStddev is used only for strong quads. It is the required precision to validate
    the relative positionning inside the quad. For weak quads, there is no way to cross check
    distances between nodes.
  */
  bool Cluster::setQuad(QList<Node *> nodes, double maxStddev)
  {
    TRACE;

    if(nodes.count()!=3) {
      App::log(tr("A quad is made of 4 nodes\n") );
      return false;
    }
    _type=Quad;
    QList<Node *> fullLinkNodes;
    if(nodes.at(0)->isConnected(nodes.at(1)) &&
       nodes.at(0)->isConnected(nodes.at(2))) {
       fullLinkNodes.append(nodes.at(0));
    }
    if(nodes.at(1)->isConnected(nodes.at(0)) &&
       nodes.at(1)->isConnected(nodes.at(2))) {
      fullLinkNodes.append(nodes.at(1));
    }
    if(nodes.at(2)->isConnected(nodes.at(0)) &&
       nodes.at(2)->isConnected(nodes.at(1))) {
      fullLinkNodes.append(nodes.at(2));
    }
    switch(fullLinkNodes.count()) {
    case 3:
      _type=StrongQuad;
      if(!calculateStrongQuad(nodes, maxStddev)) {
        return false;
      }
      break;
    case 1: {
        Node * a=fullLinkNodes.first();
        nodes.removeAll(a);
        _type=WeakQuad;
        if(!calculateWeakQuad(a, nodes)) {
          return false;
        }
      }
      break;
    default:
      return false;
    }
    return true;
  }

  /*!
    For quads only (node count is 4). Returns true if b*sin(theta)^2>3*stddev
    where b is the length of the shortest side and theta the smallest angle.

    Cluster must fist be calculated with calculateQuad()
  */
  bool Cluster::isRobustQuad(double stddev) const
  {
    TRACE;
    if(_nodes.count()!=4) {
      return false;
    }
    //App::log(5, tr("Testing robustness of quad %1\n").arg(name());
    QList<Point2D> points=_nodes.values();
    return isRobustTriangle(points.at(0), points.at(1), points.at(2), stddev) &&
           isRobustTriangle(points.at(1), points.at(2), points.at(3), stddev) &&
           isRobustTriangle(points.at(0), points.at(2), points.at(3), stddev) &&
           isRobustTriangle(points.at(0), points.at(1), points.at(3), stddev);
  }

  bool Cluster::isRobustTriangle(const Point2D& a, const Point2D& b, const Point2D& c, double stddev)
  {
    TRACE;
    double ab=a.distanceTo(b);
    double bc=b.distanceTo(c);
    double ca=c.distanceTo(a);
    double minLength=ab;
    if(bc<minLength) {
      minLength=bc;
    }
    if(ca<minLength) {
      minLength=ca;
    }
    double azimuth_ab=a.azimuthTo(b);
    double azimuth_bc=b.azimuthTo(c);
    double azimuth_ca=c.azimuthTo(a);
    double angle_a=fabs(azimuth_ab-(azimuth_ca+M_PI));
    while(angle_a>2*M_PI) angle_a-=2.0*M_PI;
    if(angle_a>M_PI) {
      angle_a=2.0*M_PI-angle_a;
    }
    double angle_b=fabs(azimuth_bc-(azimuth_ab+M_PI));
    while(angle_b>2*M_PI) angle_b-=2.0*M_PI;
    if(angle_b>M_PI) {
      angle_b=2.0*M_PI-angle_b;
    }
    double angle_c=fabs(azimuth_ca-(azimuth_bc+M_PI));
    while(angle_c>2*M_PI) angle_c-=2.0*M_PI;
    if(angle_c>M_PI) {
      angle_c=2.0*M_PI-angle_c;
    }
    double minAngle=angle_a;
    if(angle_b<minAngle) {
      minAngle=angle_b;
    }
    if(angle_c<minAngle) {
      minAngle=angle_c;
    }
    //App::log(5, tr("Lengths: %1, %2, %3, angles: %4, %5, %6\n").arg(ab).arg(bc).arg(ca).arg(angle_a*180/M_PI).arg(angle_b*180/M_PI).arg(angle_c*180/M_PI);
    double sinMinAngle=sin(minAngle);
    return minLength*sinMinAngle*sinMinAngle>3.0*stddev;
  }

  /*!
    Calculates the coordinates of a strong quad.
  */
  bool Cluster::calculateStrongQuad(QList<Node *> nodes, double maxStddev)
  {
    TRACE;
    Node * a=nodes.at(0);
    Node * b=nodes.at(1);
    Node * c=nodes.at(2);
    App::log(2, tr("Strong quad %1 %2 %3 %4\n").arg(_origin->name()).arg(a->name()).arg(b->name()).arg(c->name()));

    double oa=_origin->distance(a);
    _nodes.insert(a, Point2D(oa, 0.0));
    Circle c1(0.0, 0.0, _origin->distance(b));
    Circle c2(oa, 0.0, a->distance(b));
    QVector<Point2D> solutions=c1.intersect(c2);
    switch(solutions.count()) {
    case 1:
      _nodes.insert(b, solutions.first());
      break;
    case 2:
      if(solutions.first().y()>=0.0) {
        _nodes.insert(b, solutions.first());
      } else {
        _nodes.insert(b, solutions.last());
      }
      break;
    default:
      App::log(1, tr("No solution when localizing node %1 (unconsistent distances)\n").arg(b->name()));
      return false;
    }

    Circle c3(_nodes[_origin], _origin->distance(c));
    Circle c4(_nodes[a], a->distance(c));
    Circle c5(_nodes[b], b->distance(c));

    bool ok;
    int n=0;
    Point2D p, m, m2, stddev;
    p=c3.intersect(c4, ok, c5);
    if(ok) {m+=p; m2+=p*p; n++;}
    p=c4.intersect(c5, ok, c3);
    if(ok) {m+=p; m2+=p*p; n++;}
    p=c5.intersect(c3, ok, c4);
    if(ok) {m+=p; m2+=p*p; n++;}
    if(n!=0) {
      m/=n;
      stddev=m2/n-m*m;
      stddev.setX(::sqrt(stddev.x()));
      stddev.setY(::sqrt(stddev.y()));
      if(maxStddev>0.0 && stddev.distanceTo(Point2D())>maxStddev) {
        App::log(1, tr("Too large stddev when localizing node %1: %2\n").arg(c->name()).arg(stddev.toString()));
        return false;
      }
    } else {
      App::log(1, tr("No solution when localizing node %1 (unconsistent distances)\n").arg(c->name()));
      return false;
    }
    _nodes.insert(c, m);
    return true;
  }

  /*!
    Calculates the coordinates of a weak quad.

    A weak quad has one missing distance, the line between the origin and the full link node (\a fullLinkNode)
    is assumed to be a diagonal, not an external side.
  */
  bool Cluster::calculateWeakQuad(Node * fullLinkNode, QList<Node *> nodes)
  {
    TRACE;
    App::log(1, tr("Weak quad %1 %2 %3 %4\n").arg(_origin->name()).arg(fullLinkNode->name()).arg(nodes.at(0)->name()).arg(nodes.at(1)->name()));
    double oa=_origin->distance(fullLinkNode);
    _nodes.insert(fullLinkNode, Point2D(oa, 0.0));
    for(int i=0; i<2; i++) {
      Node * b=nodes.at(i);
      Circle c1(0.0, 0.0, _origin->distance(b));
      Circle c2(oa, 0.0, fullLinkNode->distance(b));
      QVector<Point2D> solutions=c1.intersect(c2);
      switch(solutions.count()) {
      case 1:
        _nodes.insert(b, solutions.first());
        break;
      case 2:
        if(solutions.first().y()>=0.0) {
          _nodes.insert(b, solutions.at(i));
        } else {
          _nodes.insert(b, solutions.at(1-i));
        }
        break;
      default:
        App::log(1, tr("No solution when localizing node %1 (unconsistent distances)\n").arg(b->name()));
        return false;
      }
    }
    return true;
  }

  /*!
    Check for anchors between this cluster and \a quad.
  */
  Anchorage * Cluster::anchorage(const Cluster& quad) const
  {
    TRACE;

    // Preliminary checks
    if(quad.count()!=4) {
      App::log(tr("A quad must have exactly 4 nodes\n") );
      return 0;
    }
    // Identification of the new node
    Anchorage * res=new Anchorage;
    for(QMap<Node *, Point2D>::const_iterator it=quad._nodes.begin(); it!=quad._nodes.end(); it++) {
      Node * quadNode=it.key();
      if(_nodes.contains(quadNode)) {
        res->addAnchor(quadNode);
      } else if(res->n()) {
        // More than 1 node does not belong to this cluster hence less than 3 in common
        delete res;
        return 0;
      } else {
        res->setNewNode(quadNode);
      }
    }

    if(!res->isValidAnchor()) {
      delete res;
      return 0;
    } else {
      return res;
    }
  }

  /*!
    Adds a new quad to this cluster with a valid \a anchoring.
  */
  bool Cluster::add(const Cluster& quad, const Anchorage * anchorage, double maxStddev)
  {
    TRACE;

    // Localization
    Circle c1(_nodes[anchorage->a()], quad.distance(anchorage->n(), anchorage->a()));
    Circle c2(_nodes[anchorage->b()], quad.distance(anchorage->n(), anchorage->b()));
    Circle c3(_nodes[anchorage->c()], quad.distance(anchorage->n(), anchorage->c()));

    bool ok;
    int n=0;
    Point2D p, m, m2, stddev;
    p=c1.intersect(c2, ok, c3);
    if(ok) {m+=p; m2+=p*p; n++;}
    p=c2.intersect(c3, ok, c1);
    if(ok) {m+=p; m2+=p*p; n++;}
    p=c3.intersect(c1, ok, c2);
    if(ok) {m+=p; m2+=p*p; n++;}
    if(n!=0) {
      m/=n;
      stddev=m2/n-m*m;
      if(stddev.x()<0.0) stddev.setX(0.0);
      if(stddev.y()<0.0) stddev.setY(0.0);
      stddev.setX(::sqrt(stddev.x()));
      stddev.setY(::sqrt(stddev.y()));
      double d=stddev.distanceTo(Point2D());
      if(maxStddev>0.0 && d>maxStddev) {
        App::log(1, tr("Too large stddev when localizing node %1: %2\n").arg(anchorage->n()->name()).arg(d));
        return false;
      } else {
        App::log(4, tr("stddev when localizing node %1: %2\n").arg(anchorage->n()->name()).arg(d));
      }
    } else {
      App::log(1, tr("No solution when localizing node %1 (unconsistent distances)\n").arg(anchorage->n()->name()));
      return false;
    }
    _nodes.insert(anchorage->n(), m);
    return true;
  }

  double Cluster::distance(Node * node1, Node * node2) const
  {
    TRACE;

    return _nodes[node1].distanceTo(_nodes[node2]);
  }

  bool Cluster::checkDistances(double maxStddev) const
  {
    TRACE;
    QList<Node *> l=_nodes.keys();
    for(QList<Node *>::iterator itn=l.begin(); itn!=l.end(); itn++) {
      Node * parent=*itn;
      const QMap<Node *, double>& ds=parent->distances();
      for(QMap<Node *, double>::const_iterator it=ds.begin(); it!=ds.end(); it++) {
        double err=fabs(it.value()-distance(parent, it.key()));
        if(err>maxStddev) {
          App::log(1, tr("Checking distances: distance between %1 and %2 does not match (err=%3 m)\n").arg(parent->name()).arg(it.key()->name()).arg(err));
          return false;
        }
      }
    }
    return true;
  }

  bool Cluster::setCoordinateSystem(Node *origin, Node *north, Node *eastward)
  {
    TRACE;
    if(!origin || !north || !eastward) {
      return false;
    }
    QMap<Node *, Point2D>::iterator itlu;
    // Translation
    itlu=_nodes.find(origin);
    if(itlu==_nodes.end()) {
      App::log(tr("Setting coordinates: origin node '%1' does not belong to this cluster\n").arg(origin->name()) );
      return false;
    }
    Point2D co=Point2D()-itlu.value();
    for(QMap<Node *, Point2D>::iterator it=_nodes.begin(); it!=_nodes.end(); it++) {
      it.value().translate(co);
    }

    // Rotation
    itlu=_nodes.find(north);
    if(itlu==_nodes.end()) {
      App::log(tr("Setting coordinates: north node '%1' does not belong to this cluster\n").arg(north->name()) );
      return false;
    }
    Matrix2x2 rotMatrix;
    Angle angle;
    angle.setRadianAzimuth(itlu.value().azimuth());
    rotMatrix.rotation(angle);
    for(QMap<Node *, Point2D>::iterator it=_nodes.begin(); it!=_nodes.end(); it++) {
      it.value()=rotMatrix*it.value();
    }

    // Flip
    itlu=_nodes.find(eastward);
    if(itlu==_nodes.end()) {
      App::log(tr("Setting coordinates: eastward node '%1' does not belong to this cluster\n").arg(eastward->name()) );
      return false;
    }
    if(itlu.value().x()<0.0) {
      for(QMap<Node *, Point2D>::iterator it=_nodes.begin(); it!=_nodes.end(); it++) {
        it.value().setX(-it.value().x());
      }
    }

    return true;
  }

} // namespace TapePositioningSystem
