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

#include "Segment.h"

namespace QGpCoreMath {

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

    Full description of class still missing
  */

  Segment::Segment(const Segment& o)
  {
    _point[0]=o._point[0];
    _point[1]=o._point[1];
    _limits=o._limits;
  }

  void Segment::set(const Point& p1, const Point& p2)
  {
    _point[0]=p1;
    _point[1]=p2;
    _limits.setLimits(_point[0], _point[1]);
  }

  void Segment::set(double x1, double y1, double z1,
                    double x2, double y2, double z2)
  {
    _point[0]=Point(x1, y1, z1);
    _point[1]=Point(x2, y2, z2);
    _limits.setLimits(_point[0], _point[1]);
  }

  /*!
    Sets the length of segment to \a l, keeping unchanged the direction
    and the first point.
  */
  void Segment::setLength(double l)
  {
    _point[1]=_point[0]+directionVector()*l;
    _limits.setLimits(_point[0], _point[1]);
  }

  /*!
    Reverses segment direction
  */
  void Segment::swap()
  {
    qSwap(_point[0], _point[1]);
  }

  /*!
    Returns true if this segment intersects segment \a seg. The intersection point is returned in \a p.
  */
  bool Segment::intersects(const Segment& seg, Point& p) const
  {
    TRACE;
    Point u1=_point[1];
    u1-=_point[0];
    Point u2=seg._point[1];
    u2-=seg._point[0];
    double d, k;
    d=u2.x()*u1.y()-u2.y()*u1.x();
    if(d!=0.0) {
      k=(u1.x()*(seg._point[0].y()-_point[0].y())-u1.y()*(seg._point[0].x()-_point[0].x()))/d;
      p=u2*k+seg._point[0];
    } else {
      d=u2.x()*u1.z()-u2.z()*u1.x();
      if(d!=0.0) {
        k=(u1.x()*(seg._point[0].z()-_point[0].z())-u1.z()*(seg._point[0].x()-_point[0].x()))/d;
        p=u2*k+seg._point[0];
      } else {
        d=u2.y()*u1.z()-u2.z()*u1.y();
        if(d!=0.0) {
          k=(u1.y()*(seg._point[0].z()-_point[0].z())-u1.z()*(seg._point[0].y()-_point[0].y()))/d;
          p=u2*k+seg._point[0];
        } else {
          return false;
        }
      }
    }
    if(_limits.includes(p) && seg._limits.includes(p)) return true;
    else return false;
  }

  /*!
    Returns true if this segment intersects plane \a pl. The intersection point is returned in \a p.
  */
  bool Segment::intersects(const Plane& pl, Point& p) const
  {
    TRACE;
    Point v=_point[1];
    v-=_point[0];
    double denom=pl.normalVector().scalarProduct(v);
    if(fabs(denom)<1e-10) {
      return false;
    }
    double alpha=(pl.d()-pl.normalVector().scalarProduct(_point[0]))/denom;
    if(alpha<0.0 || alpha>1.0) { // intersection outside the segment
      return false;
    } else {
      p=_point[0]+v*alpha;
      return true;
    }
  }

  /*!
    Returns true if \a p is located on the segment (1e-10 precision)
  */
  bool Segment::includes(const Point& p, double precision) const
  {
    TRACE;
    if(!_limits.includes(p)) return false;
    Point u=_point[1];
    u-=_point[0];
    double k;
    if(u.x()!=0.0) {
      k=(p.x()-_point[0].x())/u.x();
      if(fabs(p.y()-(k*u.y()+_point[0].y()))<precision &&
         fabs(p.z()-(k*u.z()+_point[0].z()))<precision) {
        return true;
      }
    }
    if(u.y()!=0.0) {
      k=(p.y()-_point[0].y())/u.y();
      if(fabs(p.x()-(k*u.x()+_point[0].x()))<precision &&
         fabs(p.z()-(k*u.z()+_point[0].z()))<precision) {
        return true;
      }
    }
    if(u.z()!=0.0) {
      k=(p.z()-_point[0].z())/u.z();
      if(fabs(p.x()-(k*u.x()+_point[0].x()))<precision &&
         fabs(p.y()-(k*u.y()+_point[0].y()))<precision) {
        return true;
      }
    }
    return false;
  }

  Point Segment::directionVector() const
  {
    TRACE;
    Point p=_point[1];
    p-=_point[0];
    p/=p.length();
    return p;
  }

  bool Segment::parallelTo(const Segment& o) const
  {
    Point u=directionVector();
    Point v=o.directionVector();
    return u.isSimilar(v, 1e-5) || u.isSimilar(v*-1, 1e-5);
  }

  bool Segment::parallelTo(const Plane& o) const
  {
    Point u=directionVector();
    Point v=o.normalVector();
    return fabs(u.scalarProduct(v))<1e-5;
  }

  /*!
    u is the direction vector of segment \a seg1.
    v is the direction vector of segment \a seg2.
    sc and tc are coefficients on these segments
  */
  inline Segment Segment::linkSegment(double sc, double tc,
                                      const Segment& seg1, const Point& u,
                                      const Segment& seg2, const Point& v)
  {
    Segment d;
    if(sc<0.0) {
      d=seg2.distanceTo(seg1._point[0]);
    } else if(sc>1.0) {
      d=seg2.distanceTo(seg1._point[1]);
    } else {
      if(tc<0.0) {
        d=seg1.distanceTo(seg2._point[0]);
        d.swap();
      } else if(tc>1.0) {
        d=seg1.distanceTo(seg2._point[1]);
        d.swap();
      } else {
        d=Segment(seg1._point[0]+u*sc, seg2._point[0]+v*tc);
      }
    }
    return d;
  }

  /*!
     Based on http://geomalgorithms.com/a07-_distance.html

     The length of the returned segment is the distance between the
     two segments. The end points are the closest points on the two segments.

     \a relPos is set according relative position of segments:

     \li Skew           Not crossing each other and not parallel
     \li Crossing       Crossing segments
     \li Parallel       Parallel segments, supported by distinct lines
     \li Aligned        Segments supported by the same line, not continuous, nor overlapping segments
     \li Continuous     Segments are aligned and touch each other by one of their extreme points
     \li SmallOverlap   Overlapping segments of less than 20% of their length
     \li StrongOverlap  Overlapping segments of more than 20% of their length

     Two segments are returned in the case of Parallel. They limit the overlapping range.
     In all other cases, a single segment is returned.

     One segment is returned for skew and crossing segments.
     In case of parallel segments, from 1 to 4 segments can be returned.
     In case of nearby segments, the number of segment is likely 1 or 2.
     In case distant segments, imprecisions may lead to several segments.
  */
  QVector<Segment> Segment::distanceTo(const Segment& seg, RelativePosition& relPos) const
  {
    TRACE;
    QVector<Segment> res;
    Point u=_point[1];
    u-=_point[0];
    Point v=seg._point[1];
    v-=seg._point[0];
    Point w0=_point[0];
    w0-=seg._point[0];
    double a=u.scalarProduct(u);
    double b=u.scalarProduct(v);
    double c=v.scalarProduct(v);
    double d=u.scalarProduct(w0);
    double e=v.scalarProduct(w0);
    double denom=a*c-b*b;
    if(denom/(a*c)<1e-10) { // Parallel segments, negative value are theoretically impossible
      Segment s[4];
      s[0]=linkSegment(0.0, d/b, *this, u, seg, v);
      s[1]=linkSegment(1.0, (a+d)/b, *this, u, seg, v);
      s[2]=linkSegment(-d/a, 0.0, *this, u, seg, v);
      s[3]=linkSegment((b-d)/a, 1.0, *this, u, seg, v);
      double minLength=std::numeric_limits<double>::infinity();
      for(int i=0; i<4; i++) {
        double l=s[i].length();
        if(fabs(l-minLength)<1e-4) {
          res.append(s[i]);
        } else if(l<minLength) {
          minLength=l;
          res.clear();
          res.append(s[i]);
        }
      }
      if(minLength<1e-10) { // Adjacent or Overlap
        if(_point[0].isSimilar(seg._point[0], 1e-2) ||
           _point[0].isSimilar(seg._point[1], 1e-2) ||
           _point[1].isSimilar(seg._point[0], 1e-2) ||
           _point[1].isSimilar(seg._point[1], 1e-2)) {
          relPos=Continuous;
        } else {
          if(minLength<0.2*length() && minLength<0.2*seg.length()) {
            relPos=SmallOverlap;
          } else {
            relPos=StrongOverlap;
          }
        }
      } else { // Parallel or Aligned
        if(parallelTo(res.first())) { // Linking segments itself parallel to segments
          relPos=Aligned;
        } else {
          relPos=Parallel;
          std::sort(res.begin(), res.end());
          for(int i=res.count()-1; i>0; i--) {
            if(res.at(i).isSimilar(res.at(i-1), 1e-2)) {
              res.remove(i);
            }
          }
          if(res.count()==1) {
            relPos=Continuous; // Not really if considering only segments
                               // Just means that there is no overlap
          } // Else 2 segments, sometimes 3 or 4 in case for distant segments
        }
      }
    } else {
      denom=1.0/denom;
      Segment s=linkSegment((b*e-c*d)*denom, (a*e-b*d)*denom, *this, u, seg, v);
      if(s.length()<1e-10) {
        relPos=Crossing;
      } else {
        relPos=Skew;
      }
      res.append(s);
    }
    return res;
  }

  bool Segment::operator<(const Segment& o) const
  {
    if(_point[0]<o._point[0]) {
      return true;
    } else if(_point[0]==o._point[0]) {
      return _point[1]<o._point[1];
    }
    return false;
  }

  bool Segment::operator==(const Segment& o) const
  {
    if((_point[0]==o._point[0] && _point[1]==o._point[1]) ||
       (_point[0]==o._point[1] && _point[1]==o._point[0])) {
      return true;
    } else  {
      return false;
    }
  }

  bool Segment::isSimilar(const Segment& o, double precision) const
  {
    return (_point[0].isSimilar(o._point[0], precision) &&
            _point[1].isSimilar(o._point[1], precision)) ||
           (_point[0].isSimilar(o._point[1], precision) &&
            _point[1].isSimilar(o._point[0], precision));
  }

  /*!
    Calculates the distance to a point.
  */
  Segment Segment::distanceTo(const Point &p) const
  {
    TRACE;
    Point u=p;
    u-=_point[0];
    Point v=_point[1];
    v-=_point[0];
    double l=length();
    double a=v.scalarProduct(u)/(l*l);
    if(a<=0.0) {
      return Segment(p, _point[0]);
    } else if(a>=1.0) {
      return Segment(p, _point[1]);
    } else {
      return Segment(p, _point[0]+v*a);
    }
  }

} // namespace QGpCoreMath
