using UnityEngine;

namespace MathUtils
{
    public class LineSegment
    {
        public Vector3 point1;
        public Vector3 point2;

        public LineSegment()
        {
            point1 = new Vector3();
            point2 = new Vector3();
        }

        public LineSegment(Vector3 aPoint1, Vector3 aPoint2)
        {
            point1 = aPoint1;
            point2 = aPoint2;
        }

        public Vector3[] GetLinesAsArray()
        {
            return new Vector3[2] { point1, point2 };
        }

        public float SquareDistance()
        {
            return (point2.x - point1.x) * (point2.x - point1.x) +
                   (point2.y - point1.y) * (point2.y - point1.y) +
                   (point2.z - point1.z) * (point2.z - point1.z);
        }

        public float Distance()
        {
            return Mathf.Sqrt(SquareDistance());
        }

        public float Slope()
        {
            return (point2.y - point1.y) / (point2.x - point1.x);
        }

        public bool ContainsPoint(Vector3 aPoint)
        {
            return aPoint == point1 || aPoint == point2;
        }

        public bool ContainsSegment(Vector3 aPoint1, Vector3 aPoint2)
        {
            return (aPoint1 == point1 || aPoint1 == point2)
                   && (aPoint2 == point1 || aPoint2 == point2);
        }

        public bool ContainsSegment(ref LineSegment aLineSegment)
        {
            return (aLineSegment.point1 == point1 || aLineSegment.point1 == point2)
                   && (aLineSegment.point2 == point1 || aLineSegment.point2 == point2);
        }

        public Vector3 VectorForm()
        {
            if (point1.x < point2.x)
            {
                return (point2 - point1);
            }
            else
            {
                return (point1 - point2);
            }
        }

        public void LocalizePoints(Transform aTransform)
        {
            point1 = aTransform.InverseTransformPoint(point1);
            point2 = aTransform.InverseTransformPoint(point2);
        }

        public Vector3 Normal()
        {
            Vector3 lineVector = point2 - point1;
            return new Vector3(-lineVector.y, lineVector.x, lineVector.z);
        }

        public int WhichSideOfLineIsPointOn(Vector3 aPoint)
        {
            float slope = Slope();
            float yIntercept = point2.y - slope * point2.x;
            // sidecheck = mx - y + b
            float sideCheck = aPoint.x * slope - aPoint.y + yIntercept;
            sideCheck = MathUtil.RoundToDigits(sideCheck, ROUND_PLACE.HUNDREDS);

            if(sideCheck > 0)
            {
                return 1;
            }
            else if(sideCheck < 0)
            {
                return -1;
            }

            // Collinear
            return 0;
        }

        public float DistanceToPoint(Vector3 aPoint)
        {
            float distSquared = SquareDistance();
            if (distSquared == 0.0)
            {
                return Vector3.Distance(point1, aPoint);
            }
            // Consider the line extending the segment, parameterized as v + t (w - v).
            // We find projection of point p onto the line. 
            // It falls where t = [(p-v) . (w-v)] / |w-v|^2
            // We clamp t from [0,1] to handle points outside the segment vw.
            float t = Vector3.Dot(aPoint - point1, point2 - point1) / distSquared;
            t = Mathf.Clamp(t, 0, 1);
            Vector3 projection = point1 + t * (point2 - point1);  // Projection falls on the segment
            return Vector3.Distance(aPoint, projection);
        }

        /**
         * <summary> 
         * moves the points of the line segment such that the refernce point becomes on the line segment
         * </summary>
         * <param name="aRefPoint">Point that is to be on the new line segment transformation.</param>
         */
        public void ReallignToPoint(Vector3 aRefPoint)
        {
            float distanceToPoint = DistanceToPoint(aRefPoint);

            if (distanceToPoint == 0.0f)
            {
                // point is already on line segment
                return;
            }

            int sideCheck = WhichSideOfLineIsPointOn(aRefPoint);
            Vector3 vectorForm = VectorForm();
            Vector3 perpendicularVector;
            if (sideCheck > 0)
            {
                perpendicularVector = new Vector3(vectorForm.y, -vectorForm.x, vectorForm.z);
            }
            else
            {
                perpendicularVector = new Vector3(-vectorForm.y, vectorForm.x, vectorForm.z);
            }
            Vector3 deltaChange = perpendicularVector.normalized * distanceToPoint;
            point1 += deltaChange;
            point2 += deltaChange;
        }
    }

    public static class MathUtil
    {
        public static float CrossProduct2D(Vector2 point1, Vector2 point2)
        {
            return point1.x * point2.y - point1.y * point2.x;
        }

        public static float AngleBetweenRadians(Vector2 aVector1, Vector2 aVector2)
        {
            return Mathf.Acos(Vector2.Dot(aVector1, aVector2) / (aVector1.magnitude * aVector2.magnitude));
        }

        public static THREE_POINT_ORIENTATION GetPointsOrientation(Vector2 aPoint1, Vector2 aPoint2, Vector2 aPoint3)
        {
            float val = (aPoint2.y - aPoint1.y) * (aPoint3.x - aPoint2.x) -
                      (aPoint2.x - aPoint1.x) * (aPoint3.y - aPoint2.y);

            if (val == 0) return THREE_POINT_ORIENTATION.COLINEAR;
            return (val > 0) ? THREE_POINT_ORIENTATION.CLOCKWISE : THREE_POINT_ORIENTATION.COUNTER_CLOCKWISE;
        }

        // https://github.com/pgkelley4/line-segments-intersect/blob/master/js/line-segments-intersect.js
        public static bool ComputeLinesIntersect(Vector2 A, Vector2 B, Vector2 C, Vector2 D, out Vector2 aIntersectionPoint)
        {
            aIntersectionPoint = new Vector2();
            Vector2 CminusA = C - A;
            Vector2 r = B - A;
            Vector2 s = D - C;

            float uNumerator = CrossProduct2D(CminusA, r);
            float denominator = CrossProduct2D(r, s);

            if (uNumerator == 0 && denominator == 0)
            {
                // Lines are collinear
                return false;
            }

            if (denominator == 0f)
            {
                // Lines are parallel
                return false;
            }

            float CminusAxs = CrossProduct2D(CminusA, s);
            float rxsr = 1f / denominator;
            float t = CminusAxs * rxsr;
            float u = uNumerator * rxsr;

            if ((t >= 0f) && (t <= 1f) && (u >= 0f) && (u <= 1f))
            {
                aIntersectionPoint = new Vector2(A.x + t * r.x, A.y + t * r.y);
                return true;
            }

            return false;
        }

        public static float RoundToDigits(float aValue, ROUND_PLACE aRoundTo)
        {
            float place = (float)aRoundTo;
            return Mathf.Round(aValue * place) / place;
        }

        public static bool GetCircleFromPoints(Vector2 aPoint1, Vector2 aPoint2, Vector2 aPoint3, out Vector2 aCenter, out float aRadius)
        {
            Vector2 midpoint1 = new Vector2((aPoint1.x + aPoint2.x) / 2, (aPoint1.y + aPoint2.y) / 2);
            Vector2 midpoint2 = new Vector2((aPoint2.x + aPoint3.x) / 2, (aPoint2.y + aPoint3.y) / 2);
            float slope1 = (aPoint2.y - aPoint1.y) / (aPoint2.x - aPoint1.x);
            float slope2 = (aPoint3.y - aPoint2.y) / (aPoint3.x - aPoint2.x);
            float perpSlope1 = -1 / slope1;
            float perpSlope2 = -1 / slope2;
            float foundX = (-midpoint1.y + midpoint2.y - (perpSlope1 * -midpoint1.x - perpSlope2 * -midpoint2.x)) / (perpSlope1 - perpSlope2);
            float foundY = perpSlope1 * (foundX - midpoint1.x) + midpoint1.y;

            aCenter = new Vector2(foundX, foundY);
            aRadius = Vector2.Distance(aPoint1, aCenter);
            return true;
        }

        public static double InCircle(Vector4 pa, Vector4 pb, Vector4 pc, Vector4 pd)
        {
            Vector2 circleCenter;
            float radius;
            if (GetCircleFromPoints(pa, pb, pc, out circleCenter, out radius))
            {
                return radius - Vector2.Distance(circleCenter, pd);
            }

            return 0;
        }

        public static bool InCircleFast(Vector4 pa, Vector4 pb, Vector4 pc, Vector4 pd)
        {
            THREE_POINT_ORIENTATION orientation = GetPointsOrientation(pa, pb, pc);
            Vector4[] orientedPoints = new Vector4[3] { pa, pb, pc };
            if(orientation == THREE_POINT_ORIENTATION.CLOCKWISE)
            {
                orientedPoints = new Vector4[3] { pc, pb, pa };
            }

            float row0x = orientedPoints[0].x - pd.x;
            float row0y = orientedPoints[0].y - pd.y; 
            float row1x = orientedPoints[1].x - pd.x;
            float row1y = orientedPoints[1].y - pd.y; 
            float row2x = orientedPoints[2].x - pd.x;
            float row2y = orientedPoints[2].y - pd.y;
            Matrix4x4 testMatrix = new Matrix4x4();
            testMatrix.SetRow(0, new Vector4(orientedPoints[0].x, orientedPoints[0].y, orientedPoints[0].x* orientedPoints[0].x + orientedPoints[0].y* orientedPoints[0].y, 1));
            testMatrix.SetRow(1, new Vector4(orientedPoints[1].x, orientedPoints[1].y, orientedPoints[1].x* orientedPoints[1].x + orientedPoints[1].y* orientedPoints[1].y, 1));
            testMatrix.SetRow(2, new Vector4(orientedPoints[2].x, orientedPoints[2].y, orientedPoints[2].x* orientedPoints[2].x + orientedPoints[2].y* orientedPoints[2].y, 1));
            testMatrix.SetRow(3, new Vector4(pd.x, pd.y, pd.x* pd.x + pd.y* pd.y, 1));

            return testMatrix.determinant > 0;
        }
    }
}