#include "PlanimetryCalculus.h"

template <int k> qreal get(QPointF &p);
template <> qreal get<0>(QPointF &p) { return p.x(); }
template <> qreal get<1>(QPointF &p) { return p.y(); }

namespace PlanimetryCalculus {
namespace {
/*!
    \fn IntersectType intersects(const Circle &circle, const QLineF &line,
   PointList &intersectionPoints, bool calcInstersect) Returns the Intersection
   type of \a circle and \a line. Stores the intersection points in \a
   intersectionPoints if \a calcIntersect is \c true. Returns \c Error if either
   line or circe \c {isNull() == true}.

    \sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line,
                QPointFVector &intersectionPoints, IntersectType &type,
                bool calcInstersect) {
  if (!line.isNull()) {
    QPointF translationVector = line.p1();
    double alpha = angle(line); // angle between wold and line coordinate system

    QPointF originCircleL = circle.origin() - translationVector;
    rotateReference(originCircleL,
                    -alpha); // circle origin in line corrdinate system

    double y = originCircleL.y();
    double r = circle.radius();
    if (qAbs(y) > r) {
      type = NoIntersection;
      return false;
    } else if (qFuzzyCompare(qFabs(y), r)) { // tangent
      double x_ori = originCircleL.x();

      if (x_ori >= 0 && x_ori <= line.length()) {
        if (calcInstersect) {
          QPointF intersectionPt = QPointF(x_ori, 0);
          rotateReference(intersectionPt, alpha);
          intersectionPoints.append(intersectionPt + translationVector);
        }

        type = Tangent;
        return true;
      }

      type = NoIntersection;
      return false;
    } else { // secant
      double x_ori = originCircleL.x();
      double y_ori = originCircleL.y();
      double delta = qSqrt(qPow(r, 2) - qPow(y_ori, 2));
      double x1 =
          x_ori +
          delta; // x coordinate (line system) of fist intersection point
      double x2 =
          x_ori -
          delta; // x coordinate (line system) of second intersection point
      bool doesIntersect =
          false; // remember if actual intersection was on the line

      if (x1 >= 0 &&
          x1 <= line.length()) { // check if intersection point is on the line
        if (calcInstersect) {
          QPointF intersectionPt =
              QPointF(x1, 0); // first intersection point (line system)
          rotateReference(intersectionPt, alpha);
          intersectionPoints.append(
              intersectionPt +
              translationVector); // transform (to world system) and append
                                  // first intersection point
        }
        doesIntersect = true;
      }
      if (x2 >= 0 &&
          x2 <= line.length()) { // check if intersection point is on the line
        if (calcInstersect) {
          QPointF intersectionPt =
              QPointF(x2, 0); // second intersection point (line system)
          rotateReference(intersectionPt, alpha);
          intersectionPoints.append(
              intersectionPt +
              translationVector); // transform (to world system) and append
                                  // second intersection point
        }
        doesIntersect = true;
      }
      type = doesIntersect ? Secant : NoIntersection;
      return doesIntersect ? true : false;
    }
  }

  type = Error;
  return false;
}

/*!
    \fn  bool intersects(const Circle &circle1, const Circle &circle2, PointList
   &intersectionPoints, IntersectType type) Calculates the intersection points
   of two circles if present and stores the result in \a intersectionPoints.
    Returns the intersection type of the two cirles \a circle1 and \a circle2.

    The function assumes that the list \a intersectionPoints is empty.

    \note Returns Error if circle.isNull() returns true;

    \sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2,
                QPointFVector &intersectionPoints, IntersectType type,
                bool calcIntersection) {
  double r1 = circle1.radius();
  double r2 = circle2.radius();
  double d = distance(circle1.origin(), circle2.origin());
  double r = 0;
  double R = 0;
  if (r1 > r2) {
    R = r1; // large
    r = r2; // smallline1
  } else {
    // this branch is also choosen if r1 == r2
    R = r2;
    r = r1;
  }

  // determine intersection type
  if (r + d < R) {
    // this branch is also reached if d < rLarge && rSmall == 0
    type = InsideNoIntersection;
    return false;
  } else if (qFuzzyCompare(r + d, R)) {
    if (qFuzzyIsNull(d)) {
      type = CirclesEqual;
      return true;
    } else {
      type = InsideTouching;
    }
  } else if (d < R) {
    type = InsideIntersection;
  } else if (d - r < R) {
    type = OutsideIntersection;
  } else if (qFuzzyCompare(d - r, R)) {
    type = OutsideTouching;
  } else {
    type = OutsideNoIntersection;
    return false;
  }

  if (calcIntersection) {
    // calculate intersection

    // Coordinate system circle1: origin = circle1.origin(), x-axis towars
    // circle2.origin() y-axis such that the coordinate system is dextrorse
    // with z-axis outward faceing with respect to the drawing plane.
    double alpha =
        angle(circle1.origin(),
              circle2.origin()); // angle between world and circle1 system
    QPointF translationVector =
        circle1.origin(); // translation vector between world and circle1 system

    if (type == InsideTouching || type == OutsideTouching) {
      // Intersection point in coordinate system of circle 1.
      // Coordinate system circle1: origin = circle1.origin(), x-axis towars
      // circle2.origin() y-axis such that the coordinate system is dextrorse
      // with z-axis outward faceing with respect to the drawing plane.
      intersectionPoints.append(rotateReturn(QPointF(0, r1), alpha) +
                                translationVector);
    } else { // triggered if (   type == InsideIntersection
             //              || type == OutsideIntersection)
      // See fist branch for explanation

      // this equations are obtained by solving x^2+y^2=R^2 and (x -
      // d)^2+y^2=r^2
      double x = (qPow(d, 2) - qPow(r, 2) + qPow(R, 2)) / 2 / d;
      double y = 1 / 2 / d *
                 qSqrt(4 * qPow(d * R, 2) -
                       qPow(qPow(d, 2) - qPow(r, 2) + qPow(R, 2), 2));

      intersectionPoints.append(rotateReturn(QPointF(x, y), alpha) +
                                translationVector);
      intersectionPoints.append(rotateReturn(QPointF(x, -y), alpha) +
                                translationVector);
    }
    // Transform the coordinate to the world coordinate system. Alpha is the
    // angle between world and circle1 coordinate system.

    return true;
  }
  type = Error;
  return false;
}
} // end anonymous namespace

/*!
    \fn void rotatePoint(QPointF &point, double alpha)
    Rotates the \a point counter clockwisely by the angle \a alpha (in
   radiants).
*/
void rotateReference(QPointF &point, double alpha) {
  if (!point.isNull()) {
    double x = point.x();
    double y = point.y();

    point.setX(x * qCos(alpha) - y * qSin(alpha));
    point.setY(x * qSin(alpha) + y * qCos(alpha));
  }
}

void rotateReference(QPointFVector &points, double alpha) {
  for (int i = 0; i < points.size(); i++) {
    rotateReference(points[i], alpha);
  }
}

/*!
    \fn void rotatePointDegree(QPointF &point, double alpha)
    Rotates the \a point counter clockwisely by the angle \a alpha (in degrees).
*/
void rotateReferenceDegree(QPointF &point, double alpha) {
  rotateReference(point, alpha / 180 * M_PI);
}

void rotateReferenceDegree(QPointFVector &points, double alpha) {
  for (int i = 0; i < points.size(); i++) {
    rotateReferenceDegree(points[i], alpha);
  }
}

/*!
    \fn IntersectType intersects(const Circle &circle, const QLineF &line)
    Returns the Intersection type of \a circle and \a line.
    Returns \c Error if either line or circe \c {isNull() == true}.

    \sa QPointF, Circle
*/
bool intersects(const Circle &circle, const QLineF &line) {
  QPointFVector dummyList;
  IntersectType type = NoIntersection;
  return intersects(circle, line, dummyList, type,
                    false /* calculate intersection points*/);
}

bool intersects(const Circle &circle, const QLineF &line,
                QPointFVector &intersectionPoints) {
  IntersectType type = NoIntersection;
  return intersects(circle, line, intersectionPoints, type,
                    true /* calculate intersection points*/);
}

/*!
    \fn double distance(const QPointF &p1, const QPointF p2)
    Calculates the distance (2-norm) between \a p1 and \a p2.
    \sa QPointF
*/
double distance(const QPointF &p1, const QPointF p2) {
  double dx = p2.x() - p1.x();
  double dy = p2.y() - p1.y();

  return norm(dx, dy);
}

/*!
    \fn double distance(const QPointF &p1, const QPointF p2)
    Calculates the angle (in radiants) between the line defined by \a p1 and \a
   p2 and the x-axis according to the following rule. Angle = qAtan2(dy, dx),
   where dx = p2.x()-p1.x() and dy = p2.y()-p1.y().

    \note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will
   result in a angle of oposite sign. \sa QPointF
*/
double angle(const QPointF &p1, const QPointF p2) {
  double dx = p2.x() - p1.x();
  double dy = p2.y() - p1.y();

  return qAtan2(dy, dx);
}

/*!
    \fn double distance(const QPointF &p1, const QPointF p2)
    Calculates the angle (in degrees) between the line defined by \a p1 and \a
p2 and the x-axis according to the following rule. Angle = qAtan2(dy,
dx)*180/pi, where dx = p2.x()-p1.x() and dy = p2.y()-p1.y(). angle \note The
order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a
angle of oposite sign. \sa QPointF
*/
double angleDegree(const QPointF &p1, const QPointF p2) {
  return angle(p1, p2) * 180 / M_PI;
}

double truncateAngle(double angle) {
  while (angle < 0) {
    angle += 2 * M_PI;
  }
  while (angle > 2 * M_PI) {
    angle -= 2 * M_PI;
  }

  return angle;
}

double truncateAngleDegree(double angle) {
  return truncateAngle(angle / 180 * M_PI);
}

/*!
 * \fn IntersectType intersects(const QLineF &line1, const QLineF &line2,
 * QPointF &intersectionPt) Determines wheter \a line1 and \a line2 intersect
 * and of which type the intersection is. Stores the intersection point in \a
 * intersectionPt Returns the intersection type (\c IntersectType).
 *
 * Intersection Types:
 * \c NoIntersection
 * \c CornerCornerIntersection; A intersection is present such that two of the
 * lines cornes touch. \c EdgeCornerIntersection; A intersection is present such
 * that a corner and a edge touch. \c EdgeEdgeIntersection; A intersection is
 * present such two edges intersect. \c LinesParallel \c LinesEqual \c Error;
 * Returned if at least on line delivers isNULL() == true.
 *
 *
 * \sa QPointF
 */
bool intersects(const QLineF &line1, const QLineF &line2,
                QPointF &intersectionPt, IntersectType &type) {
  if (line1.isNull() || line2.isNull()) {
    type = Error;
    return false;
  }

  // line 1 coordinate system: origin line1.p1(), x-axis towards line1.p2()
  QPointF translationVector =
      line1.p1(); // translation vector between world and line1 system
  double alpha = angle(line1);
  double l1 = line1.length();

  QLineF line2L1 = line2;
  line2L1.translate(-translationVector);
  rotateReference(line2L1, -alpha);

  double x1 = line2L1.x1();
  double x2 = line2L1.x2();
  double y1 = line2L1.y1();
  double y2 = line2L1.y2();
  if (x1 > x2) {
    x1 = x2;
    x2 = line2L1.x1();
    y1 = y2;
    y2 = line2L1.y1();
  }

  double dx = (x2 - x1);
  double dy = (y2 - y1);
  double xNull = 0; // (xNull, 0) intersection point in line1 system

  if (!qFuzzyIsNull(dx)) {
    double k = dy / dx;

    if (qFuzzyIsNull(k)) {
      if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)) {
        type = LinesEqual;
        return true;
      } else {
        type = LinesParallel;
        return false;
      }
    }

    double d = (y1 * x2 - y2 * x1) / dx;
    xNull = -d / k;
  } else {
    // lines orthogonal
    if (signum(y1) != signum(y2)) {
      xNull = x1;
    } else {
      type = NoIntersection;
      return false;
    }
  }

  if (xNull >= x1 && xNull <= x2) {
    // determine intersection type#include "QVector3D"

    if (qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) {
      if (qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
        type = CornerCornerIntersection;
      else
        type = EdgeCornerIntersection;
    } else if (xNull > 0 && xNull < l1) {
      if (qFuzzyIsNull(y1) || qFuzzyIsNull(y2))
        type = EdgeCornerIntersection;
      else
        type = EdgeEdgeIntersection;
    } else {
      type = NoIntersection;
      return false;
    }
  } else {
    type = NoIntersection;
    return false;
  }

  intersectionPt = QPointF(xNull, 0); // intersection point in line1 system
  rotateReference(intersectionPt, alpha);
  intersectionPt += translationVector;

  return true;
}

/*!IntersectType type = NoIntersection;
 * \overload bool intersects(const QPolygonF &polygon, const QLineF &line,
 * PointList &intersectionList,  NeighbourList &neighbourList, IntersectList
 * &typeList) Checks if \a polygon intersect with \a line. Stores the
 * intersection points in \a intersectionList.
 *
 * Stores the indices of the closest two \a area vetices for each of
 * coorespoinding intersection points in \a neighbourList. * For example if an
 * intersection point is found between the first and the second vertex of the \a
 * area the intersection point will be stored in \a intersectionList and the
 * indices 1 and 2 will be stored in \a neighbourList. \a neighbourList has
 * entries of type \c {QPair<int, int>}, where \c{pair.first} would contain 1
 * and \c{pair.second} would contain 2, when relating to the above example.
 *
 * Returns the \c IntersectionType of each intersection point within a QVector.
 *
 * \sa QPair, QVector
 */
bool intersects(const QPolygonF &polygon, const QLineF &line,
                QPointFVector &intersectionList, NeighbourVector &neighbourList,
                IntersectVector &typeList) {

  if (polygon.size() >= 2) {
    IntersectVector intersectionTypeList;
    // Assemble a line form each tow consecutive polygon vertices and check
    // whether it intersects with line
    for (int i = 0; i < polygon.size(); i++) {

      QLineF interatorLine;
      QPointF currentVertex = polygon[i];
      QPointF nextVertex =
          polygon[PolygonCalculus::nextVertexIndex(polygon.size(), i)];
      interatorLine.setP1(currentVertex);
      interatorLine.setP2(nextVertex);

      QPointF intersectionPoint;
      IntersectType type;
      if (intersects(line, interatorLine, intersectionPoint, type)) {
        intersectionList.append(intersectionPoint);

        QPair<int, int> neighbours;
        neighbours.first = i;
        neighbours.second = PolygonCalculus::nextVertexIndex(polygon.size(), i);
        neighbourList.append(neighbours);

        typeList.append(type);
      }
    }

    if (typeList.size() > 0) {
      return true;
    } else {
      return false;
    }
  } else {
    return false;
  }
}

/*!
 * \overload IntersectType intersects(const QPolygonF &polygon, const QLineF
 * &line) Returns \c true if any intersection between \a polygon and \a line
 * exists, \c false else.
 *
 * \sa QPair, QVector
 */
bool intersects(const QPolygonF &polygon, const QLineF &line) {
  QPointFVector dummyGeo;
  QVector<QPair<int, int>> dummyNeighbour;
  intersects(polygon, line, dummyGeo, dummyNeighbour);

  if (dummyGeo.size() > 0)
    return true;

  return false;
}

void rotateReference(QLineF &line, double alpha) {
  line.setP1(rotateReturn(line.p1(), alpha));
  line.setP2(rotateReturn(line.p2(), alpha));
}

QPointF rotateReturn(QPointF point, double alpha) {
  rotateReference(point, alpha);
  return point;
}

QPointFVector rotateReturn(QPointFVector points, double alpha) {
  rotateReference(points, alpha);
  return points;
}

QLineF rotateReturn(QLineF line, double alpha) {
  rotateReference(line, alpha);
  return line;
}

bool intersects(const QLineF &line1, const QLineF &line2,
                QPointF &intersectionPt) {
  IntersectType dummyType;
  return intersects(line1, line2, intersectionPt, dummyType);
}

bool intersects(const QPolygonF &polygon, const QLineF &line,
                QPointFVector &intersectionList,
                NeighbourVector &neighbourList) {
  IntersectVector typeList;
  return intersects(polygon, line, intersectionList, neighbourList, typeList);
}

bool intersects(const QPolygonF &polygon, const QLineF &line,
                QPointFVector &intersectionList, IntersectVector &typeList) {
  NeighbourVector neighbourList;
  return intersects(polygon, line, intersectionList, neighbourList, typeList);
}

bool intersects(const QPolygonF &polygon, const QLineF &line,
                QPointFVector &intersectionList) {
  NeighbourVector neighbourList;
  IntersectVector typeList;
  return intersects(polygon, line, intersectionList, neighbourList, typeList);
}

/*!
    \fn IntersectType intersects(const Circle &circle1, const Circle &circle2)
    Returns the intersection type of the two cirles \a circle1 and \a circle2.

    \note Returns Error if circle.isNull() returns true;

    \sa Circle
*/
bool intersects(const Circle &circle1, const Circle &circle2) {
  IntersectType type = NoIntersection;
  QPointFVector intersectionPoints;
  return intersects(circle1, circle2, intersectionPoints, type,
                    false /*calculate intersection points*/);
}

bool intersects(const Circle &circle1, const Circle &circle2,
                IntersectType &type) {
  QPointFVector intersectionPoints;
  return intersects(circle1, circle2, intersectionPoints, type,
                    false /*calculate intersection points*/);
}

bool intersects(const Circle &circle1, const Circle &circle2,
                QPointFVector &intersectionPoints) {
  IntersectType type;
  return intersects(circle1, circle2, intersectionPoints, type);
}

bool intersects(const Circle &circle1, const Circle &circle2,
                QPointFVector &intersectionPoints, IntersectType &type) {
  return intersects(circle1, circle2, intersectionPoints, type,
                    true /*calculate intersection points*/);
}

;

double angle(const QLineF &line) { return angle(line.p1(), line.p2()); }

bool contains(const QLineF &line, const QPointF &point, IntersectType &type) {
  QPointF translationVector = line.p1();
  double alpha = angle(line);
  double l = line.length();

  QPointF pointL1 = rotateReturn(point - translationVector, -alpha);

  double x = pointL1.x();
  double y = pointL1.y();

  if (x >= 0 && x <= l) {
    if (qFuzzyIsNull(x) || qFuzzyCompare(x, l)) {
      if (qFuzzyIsNull(y)) {
        type = CornerCornerIntersection;
        return true;
      }
    } else {
      if (qFuzzyIsNull(y)) {
        type = EdgeCornerIntersection;
        return true;
      }
    }
  }
  type = NoIntersection;
  return false;
}

bool contains(const QLineF &line, const QPointF &point) {
  IntersectType dummyType;
  return contains(line, point, dummyType);
}

bool contains(const QPolygonF &polygon, const QPointF &point,
              IntersectType &type) {
  using namespace PolygonCalculus;
  if (polygon.containsPoint(point, Qt::FillRule::OddEvenFill)) {
    type = Interior;
    return true;
  }
  int size = polygon.size();
  for (int i = 0; i < size; i++) {
    QLineF line(polygon[i], polygon[nextVertexIndex(size, i)]);
    if (contains(line, point, type)) {
      return true;
    }
  }

  return false;
}

bool contains(const QPolygonF &polygon, const QPointF &point) {
  IntersectType dummyType;
  return contains(polygon, point, dummyType);
}

double norm(double x, double y) { return qSqrt(x * x + y * y); }

double norm(const QPointF &p) { return norm(p.x(), p.y()); }

double angle(const QPointF &p1) {
  return truncateAngle(qAtan2(p1.y(), p1.x()));
}

bool intersects(const Circle &circle, const QPolygonF &polygon,
                QVector<QPointFVector> &intersectionPoints,
                NeighbourVector &neighbourList, IntersectVector &typeList) {
  using namespace PolygonCalculus;
  for (int i = 0; i < polygon.size(); i++) {
    QPointF p1 = polygon[i];
    int j = nextVertexIndex(polygon.size(), i);
    QPointF p2 = polygon[j];
    QLineF line(p1, p2);

    QPointFVector lineIntersecPts;
    IntersectType type;
    if (intersects(circle, line, lineIntersecPts, type)) {
      QPair<int, int> neigbours;
      neigbours.first = i;
      neigbours.second = j;
      neighbourList.append(neigbours);
      typeList.append(type);
      intersectionPoints.append(lineIntersecPts);
    }
  }

  if (intersectionPoints.size() > 0)
    return true;
  else {
    return false;
  }
}

bool intersects(const Circle &circle, const QPolygonF &polygon,
                QVector<QPointFVector> &intersectionPoints,
                NeighbourVector &neighbourList) {
  QVector<IntersectType> types;
  return intersects(circle, polygon, intersectionPoints, neighbourList, types);
}

bool intersects(const Circle &circle, const QPolygonF &polygon,
                QVector<QPointFVector> &intersectionPoints,
                IntersectVector &typeList) {
  NeighbourVector neighbourList;
  return intersects(circle, polygon, intersectionPoints, neighbourList,
                    typeList);
}

bool intersects(const Circle &circle, const QPolygonF &polygon,
                QVector<QPointFVector> &intersectionPoints) {
  NeighbourVector neighbourList;
  return intersects(circle, polygon, intersectionPoints, neighbourList);
}

bool intersects(const Circle &circle, const QPolygonF &polygon) {
  QVector<QPointFVector> intersectionPoints;
  return intersects(circle, polygon, intersectionPoints);
}

bool intersects(const QLineF &line1, const QLineF &line2) {
  QPointF intersectionPoint;
  return intersects(line1, line2, intersectionPoint);
}

bool intersects(const Circle &circle, const QLineF &line,
                QPointFVector &intersectionPoints, IntersectType &type) {
  return intersects(circle, line, intersectionPoints, type,
                    true /* calculate intersection points*/);
}

bool intersectsFast(const QPolygonF &polygon, const QLineF &line) {
  for (int i = 0; i < polygon.size() - 1; ++i) {
    QLineF polygonSegment(polygon[i], polygon[i + 1]);

    if (QLineF::BoundedIntersection == line.intersect(polygonSegment, nullptr))
      return true;
  }

  return false;
}

//    bool intersectsFast(const QLineF &line1, const QLineF &line2, QPointF
//    &intersectionPt)
//    {
//        if (line1.isNull() || line2.isNull()){
//            return false;
//        }

//        // determine line equations y = kx+d
//        double dx1 = line1.dx();
//        double dy1 = line1.dy();
//        double k1;
//        double d1 = 0;
//        if (qFuzzyIsNull(dx1)) {
//            k1 = std::numeric_limits<double>::infinity();
//        } else {
//            k1 = dy1/dx1;
//            d1 = line1.y1()-k1*line1.x1();
//        }

//        double dx2 = line2.dx();
//        double dy2 = line2.dy();
//        double k2;
//        double d2 = 0;
//        if (qFuzzyIsNull(dx2)) {
//            k2 = std::numeric_limits<double>::infinity();
//        } else {
//            k2 = dy2/dx2;
//            d2 = line2.y1()-k2*line2.x1();
//        }

//        if (   !qFuzzyCompare(k1, std::numeric_limits<double>::infinity())
//               && !qFuzzyCompare(k1, std::numeric_limits<double>::infinity()))
//               {
//            double dk = k2-k1;
//            double dd = d2-d1;
//            if (qFuzzyIsNull(dk)) { // lines parallel
//                if (qFuzzyIsNull(dd)) { // lines colinear
//                    if (   ((line1.x1() >= line2.x1()) && ((line1.x1() <=
//                    line2.x2())))
//                        || ((line1.x2() >= line2.x1()) && ((line1.x2() <=
//                        line2.x2())))    ) // intersect? return true;

//                    return false;
//                }
//                return false;
//            }

//            double x_intersect = -dd/dk;

//            if (((x_intersect >= line2.x1()) && ((line1.x1() <= line2.x2())))

//        }

//        return true;
//    }

} // end namespace PlanimetryCalculus

/*!
    \class PlanimetryCalculus
    \inmodule Wima

    \brief The \c PlanimetryCalculus class provides routines handy for
   planimetrical (2D) calculations.
*/