// Copyright (C) Stichting Deltares 2019. All rights reserved.
//
// This file is part of the Dam Engine.
//
// The Dam Engine is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// This program 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 Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
// All names, logos, and references to "Deltares" are registered trademarks of
// Stichting Deltares and remain full property of Stichting Deltares at all times.
// All rights reserved.

using System;
using System.Collections.Generic;

namespace Deltares.DamEngine.Data.Geometry
{
    /// <summary>
    /// Type of intersection
    /// </summary>
    public enum LineIntersection
    {
        /// <summary>
        /// The no intersection
        /// </summary>
        NoIntersection = 0,
        /// <summary>
        /// The intersects
        /// </summary>
        Intersects = 1,
        /// <summary>
        /// The parallel
        /// </summary>
        Parallel = 2
    }

    /// <summary>
    /// Position of point towards polygon
    /// </summary>
    public enum PointInPolygon
    {
        /// <summary>
        /// The inside polygon
        /// </summary>
        InsidePolygon = 0,
        /// <summary>
        /// The on polygon edge
        /// </summary>
        OnPolygonEdge = 1,
        /// <summary>
        /// The outside polygon
        /// </summary>
        OutsidePolygon = 2
    }

    /// <summary>
    /// Static class Routines2D
    /// </summary>
    public static class Routines2D
    {
        public struct LineConstant
        {
            public double X;
            public double Y;
            public double Z;
        }

        private const double cEpsilon = 1.0e-4;

        /// <summary>
        /// Checks if the 2D Lines strictly intersect. Strictly means that the intersection point must be part of both lines so
        /// extrapolated points do not count.
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <param name="point3X">The point3 x.</param>
        /// <param name="point3Z">The point3 z.</param>
        /// <param name="point4X">The point4 x.</param>
        /// <param name="point4Z">The point4 z.</param>
        /// <param name="intersectionPoint">The intersection point.</param>
        /// <returns>
        /// Intersection status as well as the intersection point (when found, else null)
        /// </returns>
        /// <remarks>
        /// For connected paralllel lines, the connection point will be returned as valid intersection point.
        /// </remarks>
        public static LineIntersection DetermineIf2DLinesIntersectStrickly(double point1X, double point1Z, double point2X, double point2Z,
            double point3X, double point3Z, double point4X, double point4Z, out Point2D intersectionPoint)
        {
            return DetermineIf2DLinesIntersectStrickly(point1X, point1Z, point2X, point2Z, point3X, point3Z, point4X, point4Z, 
                out intersectionPoint, cEpsilon);
        }


        /// <summary>
        /// Doeses the point exist in line.
        /// </summary>
        /// <param name="linePoint1X">The line point1 x.</param>
        /// <param name="linePoint1Z">The line point1 z.</param>
        /// <param name="linePoint2X">The line point2 x.</param>
        /// <param name="linePoint2Z">The line point2 z.</param>
        /// <param name="pointX">The point x.</param>
        /// <param name="pointZ">The point z.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns></returns>
        public static bool DoesPointExistInLine(double linePoint1X, double linePoint1Z, double linePoint2X, double linePoint2Z, 
            double pointX, double pointZ, double tolerance)
        {
            // Function to find if lies on or close to a line (within a given tolerance)
            // Input - a line defined by lTwo points, a third point to test and tolerance aValue
            // Output - TRUE or FALSE

            // first check whether the points are identical
            if (Math.Abs(linePoint1X - linePoint2X) < tolerance && Math.Abs(linePoint1Z - linePoint2Z) < tolerance)
            {
                return Compute2DDistance(pointX, pointZ, linePoint1X, linePoint1Z) < tolerance;
            }

            // then check if point is within the bounding rectangle formed by 2 line points
            var maxX = Math.Max(linePoint1X, linePoint2X);
            var minX = Math.Min(linePoint1X, linePoint2X);
            var maxY = Math.Max(linePoint1Z, linePoint2Z);
            var minY = Math.Min(linePoint1Z, linePoint2Z);

            minX -= tolerance;
            minY -= tolerance;
            maxX += tolerance;
            maxY += tolerance;

            var x3 = pointX;
            var y3 = pointZ;

            if (!(x3 > minX && x3 < maxX && y3 > minY && y3 < maxY))
            {
                return false;
            }

            // check if perpendicular distance between point and line is within tolerance
            var a = linePoint1Z - linePoint2Z;
            var b = linePoint2X - linePoint1X;
            var c = -((a * linePoint1X) + (b * linePoint1Z));
            var distance = ((a * x3) + (b * y3) + c) / Compute2DDistance(linePoint1X, linePoint1Z, linePoint2X, linePoint2Z);

            return Math.Abs(distance) < tolerance;
        }

        private static void UndoAddIfNeeded(GeometryLoop polygon, bool needed)
        {
            if (needed)
            {
                polygon.CalcPoints.Remove(polygon.CalcPoints[polygon.CalcPoints.Count - 1]);
            }
        }

        /// <summary>
        /// Find if points Ax,ay are between a boundary polygon.
        /// </summary>
        /// <param name="polygon">The polygon.</param>
        /// <param name="x">The x.</param>
        /// <param name="z">The z.</param>
        /// <returns></returns>
        public static PointInPolygon CheckIfPointIsInPolygon(GeometryLoop polygon, double x, double z)
        {
            // This is done by calculating and adding the angles from the point to all points
            // of the polygon (using the ArcTan2 function). If the sum of these angles is
            // aproximate zero the point lies outside the polygon.
            // If the sum in aproximate + or - 2Pi the point lies in the polygon;
            // Performance tests have proven that this algorithm performs better than the 
            // polynomial winding number algorithm described at:
            // http://geomalgorithms.com/a03-_inclusion.html

            const double epsilon = 1e-10;
            var result = PointInPolygon.OutsidePolygon;

            // add the last point at the end, cos the polygon must be closed, so the last node equals the first
            var pointAdded = false;
            if (polygon.CalcPoints.Count > 0)
            {
                if (!polygon.CalcPoints[0].LocationEquals(polygon.CalcPoints[polygon.CalcPoints.Count - 1]))
                {
                    polygon.CalcPoints.Add(polygon.CalcPoints[0]);
                    pointAdded = true;
                }
            }

            var polygonPoints = polygon.CalcPoints.Count;
            if (polygonPoints > 2)
            {
                var deltaX = polygon[0].X - x;
                var deltaZ = polygon[0].Z - z;
                var absX = Math.Abs(deltaX);
                var absZ = Math.Abs(deltaZ);
                if ((absX < epsilon) && (absZ < epsilon))
                {
                    UndoAddIfNeeded(polygon, pointAdded);
                    return PointInPolygon.OnPolygonEdge;
                }

                var al1 = Math.Atan2(deltaZ, deltaX);
                double som = 0;
                var index = 1;

                while (index < polygonPoints)
                {
                    deltaX = polygon[index].X - x;
                    deltaZ = polygon[index].Z - z;
                    absX = Math.Abs(deltaX);
                    absZ = Math.Abs(deltaZ);
                    if ((absX < epsilon) && (absZ < epsilon))
                    {
                        UndoAddIfNeeded(polygon, pointAdded);
                        return PointInPolygon.OnPolygonEdge;
                    }
                    var al2 = Math.Atan2(deltaZ, deltaX);
                    var al3 = al2 - al1;

                    if (al3 < -Math.PI)
                    {
                        al3 = al3 + (2.0*Math.PI);
                    }
                    if (al3 > Math.PI)
                    {
                        al3 = al3 - (2.0*Math.PI);
                    }
                    if (((Math.PI - al3) < epsilon) || ((Math.PI + al3) < epsilon))
                    {
                        UndoAddIfNeeded(polygon, pointAdded);
                        return PointInPolygon.OnPolygonEdge;
                    }
                    som = som + al3;
                    al1 = al2;
                    index++;
                }

                if ((som > (1.9*Math.PI)) || (som < (-1.9*Math.PI)))
                {
                    result = PointInPolygon.InsidePolygon;
                }
                else
                {
                    result = PointInPolygon.OutsidePolygon;
                }                
            }
            UndoAddIfNeeded(polygon, pointAdded);
            return result;
        }

        /// <summary>
        /// Intersection of Circle and line
        /// </summary>
        /// <param name="aX"></param>
        /// <param name="aY"></param>
        /// <param name="aR"></param>
        /// <param name="aX1"></param>
        /// <param name="aX2"></param>
        /// <param name="aY1"></param>
        /// <param name="aY2"></param>
        public static List<Point2D> IntersectCircleline(double aX, double aY, double aR, double aX1, double aX2, double aY1, double aY2)
        {
            // Solve by filling in parametric equation of line :
            // X = x1 + u*(x2 - x1)
            // Y = y1 + u*(y2 - y1)
            // Compare it to that of a cricle:
            // (X - xm)^2 + (Y - ym)^2 = r^2
            // And check solve for parameter u in a quadratic equation.
            // - no solutions => no intersections
            // - one or two solutions => intersection when -eps <= u <= 1+eps.
            // If (x1,y1) close to circle edge, return (x1,y1).

            const double lAbcEps = 1E-8;
            var lX1 = aX2 - aX1;
            var lX2 = aX1 - aX;
            var lY1 = aY2 - aY1;
            var lY2 = aY1 - aY;
            var result = new List<Point2D>();
            if ((Math.Abs(lX1) > lAbcEps) || (Math.Abs(lY1) > lAbcEps))
            {
                var lA = lX1*lX1 + lY1*lY1;
                var lB = 2*(lX1*lX2 + lY1*lY2);
                var lC = lX2*lX2 + lY2*lY2 - aR*aR;
                var lD = lB*lB - 4*lA*lC;

                if (lD > lAbcEps)
                {
                    var lU = (-lB + Math.Sqrt(lD))/(2*lA);
                    if ((lU >= -lAbcEps) && (lU <= (1.0 + lAbcEps)))
                    {
                        result.Add(new Point2D{ X= aX1 + lU*lX1, Z = aY1 + lU*lY1});
                    }
                    lU = (-lB - Math.Sqrt(lD))/(2*lA);

                    if ((lU >= -lAbcEps) && (lU <= (1.0 + lAbcEps)))
                    {
                        result.Add(new Point2D{ X = aX1 + lU*lX1, Z = aY1 + lU*lY1});
                    }
                }
                else if (Math.Abs(lD) <= lAbcEps)
                {
                    var lU = (-lB)/(2*lA);
                    if ((lU >= -lAbcEps) && (lU <= 1.0 + lAbcEps))
                    {
                        result.Add(new Point2D { X = aX1 + lU*lX1, Z = aY1 + lU*lY1});
                    }
                }
            }
            else if (Math.Abs(Math.Pow(lX2, 2) + Math.Pow(lY2, 2) - Math.Pow(aR, 2)) < lAbcEps)
            {
                result.Add(new Point2D() {X = aX1, Z = aY1});
            }
            return result;
        }

        
        /// <summary>
        /// Checks if values are equal
        /// </summary>
        /// <param name="x1"></param>
        /// <param name="x2"></param>
        /// <param name="tolerance"></param>
        /// <returns></returns>
        public static bool AreEqual(double x1, double x2, double tolerance)
        {
            return (Math.Abs(x1 - x2) < tolerance);
        }

        /// <summary>
        /// Takes the cross product of two vectors
        /// </summary>
        /// <param name="pointAx">X-coordinate of the first point</param>
        /// <param name="pointAy">Y-coordinate of the first point</param>
        /// <param name="pointBx">X-coordinate of the second point</param>
        /// <param name="pointBy">Y-coordinate of the second point</param>
        /// <returns></returns>
        private static double CrossProduct(double pointAx, double pointAy, double pointBx, double pointBy)
        {
            return pointAx*pointBy - pointBx*pointAy;
        }

        /// <summary>
        /// Description: Finds the Normal. FPoint1 and FPoint2 are the line coordinates
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <returns></returns>
        private static LineConstant CalculateNormalLineConstants(double point1X, double point1Z, double point2X, double point2Z)
        {
            var lLineConstant = new LineConstant
            {
                X = point2Z - point1Z,
                Y = -(point2X - point1X),
                Z = (point2Z - point1Z) * point1X - (point2X - point1X) * point1Z
            };

            return lLineConstant;
        }

        /// <summary>
        /// Checks if lTwo lines are parallel. The Normal Line Constants are required.
        /// </summary>
        /// <param name="aLine1ConstantX">a line1 constant x.</param>
        /// <param name="aLine1ConstantY">a line1 constant y.</param>
        /// <param name="aLine2ConstantX">a line2 constant x.</param>
        /// <param name="aLine2ConstantY">a line2 constant y.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns></returns>
        private static bool AreLinesParallel(double aLine1ConstantX, double aLine1ConstantY, double aLine2ConstantX, double aLine2ConstantY, double tolerance)
        {
            return Math.Abs(CrossProduct(aLine1ConstantX, aLine2ConstantX, aLine1ConstantY, aLine2ConstantY)) < tolerance;
        }


        public static LineIntersection DetermineIf2DLinesIntersectWithExtrapolation(Point2D aPoint1, Point2D aPoint2, Point2D aPoint3, Point2D aPoint4, ref Point2D aIntersectionPoint)
        {
            var lLineConstant1 = CalculateNormalLineConstants(aPoint1.X, aPoint1.Z, aPoint2.X, aPoint2.Z);
            var lLineConstant2 = CalculateNormalLineConstants(aPoint3.X, aPoint3.Z, aPoint4.X, aPoint4.Z);

            var lResult = DetermineIf2DLinesIntersectWithExtrapolation(lLineConstant1, lLineConstant2, out aIntersectionPoint);

            return lResult;
        }

        /// <summary>
        /// Finds the Intersection Points for two given 2D Lines.
        /// The intersection point does not need to be on either given line, extrapolation is used to find it beyond the length of the given lines.
        /// Note that parallel lines never return an intersection point, not even when there two parallel lines are neatly connected.
        /// </summary>
        /// <param name="aLine1Constant"></param>
        /// <param name="aLine2Constant"></param>
        /// <param name="aIntersectionPoint"></param>
        /// <returns></returns>
        public static LineIntersection DetermineIf2DLinesIntersectWithExtrapolation(LineConstant aLine1Constant, LineConstant aLine2Constant, out Point2D aIntersectionPoint)
        {
            aIntersectionPoint = new Point2D(0.0, 0.0);

            var lA1 = aLine1Constant.X;
            var lB1 = aLine1Constant.Y;
            var lC1 = aLine1Constant.Z;
            var lA2 = aLine2Constant.X;
            var lB2 = aLine2Constant.Y;
            var lC2 = aLine2Constant.Z;

            if (AreLinesParallel(lA1, lA2, lB1, lB2, cEpsilon))
            {
                aIntersectionPoint = null;
                return LineIntersection.Parallel;
            }

            var lX = (lB2*lC1 - lB1*lC2)/(lA1*lB2 - lA2*lB1);
            var lY = (lC1*lA2 - lC2*lA1)/(lA2*lB1 - lA1*lB2);

            aIntersectionPoint.X = lX;
            aIntersectionPoint.Z = lY;

            return LineIntersection.Intersects;
        }

        /// <summary>
        /// Calculates the Euclidean distance between two points on a 2-dimensional plane
        /// </summary>
        /// <param name="aX1">X-coordinate of the first point</param>
        /// <param name="aY1">Y-coordinate of the first point</param>
        /// <param name="aX2">X-coordinate of the second point</param>
        /// <param name="aY2">Y-coordinate of the second point</param>
        /// <returns>The distance between the given points.</returns>
        private static double Compute2DDistance(double aX1, double aY1, double aX2, double aY2)
        {
            var lX = aX1 - aX2;
            var lY = aY1 - aY2;

            return Math.Sqrt(lX * lX + lY * lY);
        }

        /// <summary>
        /// Checks if the 2D Lines strictly intersect. Strictly means that the intersection point must be part of both lines so
        /// extrapolated points do not count.
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <param name="point3X">The point3 x.</param>
        /// <param name="point3Z">The point3 zx.</param>
        /// <param name="point4X">The point4 x.</param>
        /// <param name="point4Z">The point4 z.</param>
        /// <param name="intersectionPoint">The intersection point.</param>
        /// <param name="tolerance">Non-inclusive maximum allowed distance between two possibly intersecting endpoints of the lines</param>
        /// <returns>
        /// Intersection status as well as the intersection point (when found, else null)
        /// </returns>
        /// <remarks>
        /// For connected paralllel lines, the connection point will be returned as valid intersection point.
        /// </remarks>
        private static LineIntersection DetermineIf2DLinesIntersectStrickly(double point1X, double point1Z, double point2X, double point2Z,
            double point3X, double point3Z, double point4X, double point4Z, out Point2D intersectionPoint, double tolerance)
        {
            var lineConstant1 = CalculateNormalLineConstants(point1X, point1Z, point2X, point2Z);
            var lineConstant2 = CalculateNormalLineConstants(point3X, point3Z, point4X, point4Z);

            var result = DetermineIf2DLinesIntersectWithExtrapolation(lineConstant1, lineConstant2, out intersectionPoint);

            if (result == LineIntersection.Intersects)
            {
                //check if lies on the line and is not somewhere outside !
                if ((DoesPointExistInLine(point1X, point1Z, point2X, point2Z, intersectionPoint.X, intersectionPoint.Z, tolerance) &&
                     DoesPointExistInLine(point3X, point3Z, point4X, point4Z, intersectionPoint.X, intersectionPoint.Z, tolerance)) == false)
                {
                    intersectionPoint = null;
                    result = LineIntersection.NoIntersection;
                }
            }
            else
            {
                if (result == LineIntersection.Parallel && !DoLinesAtLeastPartialyOverlap(point1X, point1Z, point2X, point2Z, point3X, point3Z, 
                    point4X, point4Z, tolerance))
                {
                    if (DetermineIfPointsCoincide(point1X, point1Z, point3X, point3Z, tolerance) || 
                        DetermineIfPointsCoincide(point1X, point1Z, point4X, point4Z, tolerance))
                    {
                        intersectionPoint = new Point2D(point1X, point1Z);
                        result = LineIntersection.Intersects;
                    }
                    if (DetermineIfPointsCoincide(point2X, point2Z, point3X, point3Z, tolerance) || 
                        DetermineIfPointsCoincide(point2X, point2Z, point4X, point4Z, tolerance))
                    {
                        intersectionPoint = new Point2D(point2X, point2Z);
                        result = LineIntersection.Intersects;
                    }
                }
            }
            return result;
        }

        /// <summary>
        /// Determines whether the given lines at least partialy overlap.
        /// Note that purely connected parallel lines are NOT considered to be overlapping.
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <param name="point3X">The point3 x.</param>
        /// <param name="point3Z">The point3 z.</param>
        /// <param name="point4X">The point4 x.</param>
        /// <param name="point4Z">The point4 z.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns></returns>
        private static bool DoLinesAtLeastPartialyOverlap(double point1X, double point1Z, double point2X, double point2Z,
            double point3X, double point3Z, double point4X, double point4Z, double tolerance)
        {
            var result = AreLinesParallel(point1X, point1Z, point2X, point2Z, point3X, point3Z, point4X, point4Z, tolerance);

            // lines not parallel so can not overlap
            if (!result)
            {
                return false;
            }

            if (Math.Abs(point1X - point2X) < double.Epsilon)
            {
                // lines are vertical so check Y values
                var max12 = Math.Max(point1Z, point2Z);
                var min12 = Math.Min(point1Z, point2Z);
                var max34 = Math.Max(point3Z, point4Z);
                var min34 = Math.Min(point3Z, point4Z);
                // When max of point1/point2 does not exceed min of point3/point4 lines do not overlap
                // When max of point3/point4 does not exceed min of point1/point2 lines do not overlap
                if (max12 <= min34 || min12 >= max34)
                {
                    result = false;
                }
            }
            else
            {
                // lines are not vertical so check X values
                var max12 = Math.Max(point1X, point2X);
                var min12 = Math.Min(point1X, point2X);
                var max34 = Math.Max(point3X, point4X);
                var min34 = Math.Min(point3X, point4X);
                if (max12 <= min34 || min12 >= max34)
                {
                    result = false;
                }
            }
            return result;
        }

        /// <summary>
        /// Description: Checks if two lines are parallel within the given tolerance
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <param name="point3X">The point3 x.</param>
        /// <param name="point3Z">The point3 z.</param>
        /// <param name="point4X">The point4 x.</param>
        /// <param name="point4Z">The point4 z.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns>
        /// True when parallel
        /// </returns>
        private static bool AreLinesParallel(double point1X, double point1Z, double point2X, double point2Z,
            double point3X, double point3Z, double point4X, double point4Z, double tolerance)
        {

            double aX = point2X - point1X;
            double aY = point2Z - point1Z;
        
            double bX = point4X - point3X;
            double bY = point4Z - point3Z;
            
            Normalize(ref aX, ref aY);
            Normalize(ref bX, ref bY);
            return AreLinesParallel(aX, aY, bX, bY, tolerance);
        }

        /// <summary>
        /// Normalizes this instance.
        /// </summary>
        private static void Normalize(ref double pointX, ref double pointY)
        {
            double q = Math.Sqrt(pointX * pointX + pointY * pointY);

            if (q != 0)
            {
                pointX = pointX / q;
                pointY = pointY / q;                
            }
        }

        /// <summary>
        /// Checks if the points coincide
        /// </summary>
        /// <param name="point1X">The point1 x.</param>
        /// <param name="point1Z">The point1 z.</param>
        /// <param name="point2X">The point2 x.</param>
        /// <param name="point2Z">The point2 z.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns></returns>
        private static bool DetermineIfPointsCoincide(double point1X, double point1Z, double point2X, double point2Z, double tolerance)
        {
            if ((Math.Abs(point1X - point2X)) < tolerance && Math.Abs(point1Z - point2Z) < tolerance)
            {
                return true;
            }

            return false;
        }
    }
}