using System;
using System.Collections.Generic;
using System.Linq;
using Sirenix.Utilities;
using UnityEngine;
using UnityEngine.AI;
using VOID.Generic.Dict;
using VOID.Generic.Navigation;
using VOID.Generic.Objects.Traversable;
using VOID.Generic.Objects.Unit;

namespace VOID.Generic.Util
{
    public static class NavMeshPathUtil
    {
        private const float CorrectingTargetRange = 5f;
        private const float CorrectingLengthRange = 0.5f;
        private const float CorrectingHeightRange = 0.75f;

        public static Vector3 SamplePosition(NavMeshAgent agent, Vector3 position)
        {
            NavMesh.SamplePosition(position, out var hit, CorrectingTargetRange, agent.areaMask);
            return hit.position;
        }

        /// <summary>
        /// Return NavMeshPath to given target destination from current position of agent.
        /// </summary>
        public static NavMeshPath GetPathTo(NavMeshAgent agent, Vector3 target)
        {
            var path = new NavMeshPath();
            NavMesh.SamplePosition(target, out var correctedTarget, CorrectingTargetRange, agent.areaMask);
            if (correctedTarget.hit) target = correctedTarget.position;
            NavMesh.CalculatePath(agent.transform.position, target, agent.areaMask, path);
            return path;
        }

        /// <summary>
        /// Alias to <see cref="GetPathShortenedByRange(List{Vector3}, Vector3, float)"/>
        /// </summary>
        /// <inheritdoc cref="GetPathShortenedByRange(List{Vector3}, Vector3, float)" />
        public static List<Vector3> GetPathShortenedByRange(NavMeshPath path, Vector3 destination, float untilInRange)
        {
            return GetPathShortenedByRange(path.corners.ToList(), destination, untilInRange);
        }

        /// <summary>
        /// Path shortened by given range.
        /// Result is List of Vector3 instead of NavMeshPath - because it is only internal :(
        /// </summary>
        public static List<Vector3> GetPathShortenedByRange(List<Vector3> path, Vector3 destination, float untilInRange)
        {
            // Invalid path with no corners?
            if (path.Count == 0) return new List<Vector3>();

            // First point of path is already in that range
            if (Vector3.Distance(path.First(), destination) < untilInRange) return new List<Vector3> { path[0] };

            for (var i = 0; i < path.Count - 1; i++)
            {
                var p1 = path[i];
                var p2 = path[i + 1];
                var intersects = LineIntersectSpherePoints(p1, p2, destination, untilInRange);

                // No intersects point found
                if (intersects.Length == 0) continue;

                var fullDistance = Vector3.Distance(p1, p2);
                var distance1 = Vector3.Distance(p1, intersects[0]) + Vector3.Distance(intersects[0], p2);
                var distance2 = Vector3.Distance(p1, intersects[1]) + Vector3.Distance(intersects[1], p2);
                var diff1 = Math.Abs(fullDistance - distance1);
                var diff2 = Math.Abs(fullDistance - distance2);

                if (diff1 < 0.05f) return path.Take(i + 1).Append(intersects[0]).ToList();
                if (diff2 < 0.05f) return path.Take(i + 1).Append(intersects[1]).ToList();
            }

            // Path cant reach destination at given range
            return path;
        }

        private static Vector3[] LineIntersectSpherePoints(Vector3 p1, Vector3 p2, Vector3 center, float radius)
        {
            var dp = new Vector3();
            Vector3[] sect;
            float a, b, c;
            float bb4ac;
            float mu1;
            float mu2;

            //  get the distance between X and Z on the segment
            dp.x = p2.x - p1.x;
            dp.z = p2.z - p1.z;
            //   I don't get the math here
            a = dp.x * dp.x + dp.z * dp.z;
            b = 2 * (dp.x * (p1.x - center.x) + dp.z * (p1.z - center.z));
            c = center.x * center.x + center.z * center.z;
            c += p1.x * p1.x + p1.z * p1.z;
            c -= 2 * (center.x * p1.x + center.z * p1.z);
            c -= radius * radius;
            bb4ac = b * b - 4 * a * c;
            if (Mathf.Abs(a) < float.Epsilon || bb4ac < 0)
                //  line does not intersect
                return Array.Empty<Vector3>();
            mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2 * a);
            mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2 * a);
            sect = new Vector3[2];
            sect[0] = new Vector3(p1.x + mu1 * (p2.x - p1.x), 0, p1.z + mu1 * (p2.z - p1.z));
            sect[1] = new Vector3(p1.x + mu2 * (p2.x - p1.x), 0, p1.z + mu2 * (p2.z - p1.z));

            return sect;
        }

        /// <summary>
        /// Correcting NavMeshPath with:
        /// - Make whole path sticking to ground
        /// - If path going through TraversableObject (NavMeshLink) then stick to it (with unit's offset)
        /// Result is PathCorrected instead of NavMeshPath - because it is only internal :(
        /// </summary>
        public static PathCorrected GetCorrectedPath(List<Vector3> path, UnitObject forUnit)
        {
            if (path.IsNullOrEmpty()) return null;
            
            // Starting point won't change
            var correctedPath = new List<Vector3>(path.GetRange(0, 1));
            var usedTraversables = new List<PathCorrectedLinkData>();
            var canTraverse = true;
            for (var i = 1; i < path.Count; i++)
            {
                if(canTraverse && NavigationManager.current.GetCachedTraversable(path[i - 1], out var traversable, out var isReverse))
                {
                    // Line between those two points is connected by NavMeshLink, thats why 
                    correctedPath.AddRange(GetCorrectedLinkPathBetweenPoints(path[i - 1], traversable, isReverse, forUnit));
                    usedTraversables.Add(new PathCorrectedLinkData() { traversable = traversable, isReverse = isReverse });
                    canTraverse = false;
                }
                else
                {
                    correctedPath.AddRange(GetCorrectedPathBetweenPoints(path[i - 1], path[i]));
                    canTraverse = true;
                }
            }
            return new PathCorrected() {path = correctedPath, traversables = usedTraversables};
        }

        private static List<Vector3> GetCorrectedLinkPathBetweenPoints(Vector3 start,
            TraversableObject traversable, bool isReverse, UnitObject forUnit)
        {
            var unitLadderOffset = forUnit.unitData.ladderPosition.localPosition;
            var correctedPath = traversable.traversableData.path.AsEnumerable();
            if (isReverse) correctedPath = correctedPath.Reverse();
            return correctedPath
                .Select(t => t.position + t.InverseTransformDirection(unitLadderOffset))
                .Prepend(start)
                .ToList();
        }

        private static List<Vector3> GetCorrectedPathBetweenPoints(Vector3 from, Vector3 to)
        {
            var correctedPath = new List<Vector3>();
            var stepsNeeded = Mathf.CeilToInt(Vector3.Distance(from, to) / CorrectingLengthRange);
            var stepProgress = Vector3.Scale(to - from, new Vector3(1, 0, 1)) / stepsNeeded;
            var point = from;
            for (var i = 1; i <= stepsNeeded; i++)
            {
                point += stepProgress;
                point = GetCorrectedPoint(point);
                correctedPath.Add(point);
            }

            return correctedPath;
        }

        private static Vector3 GetCorrectedPoint(Vector3 point)
        {
            var isHit = Physics.Raycast(
                new Vector3(point.x, point.y + CorrectingHeightRange, point.z),
                Vector3.down, out var hit,
                CorrectingHeightRange * 2f,
                LayerManager.Static);

            if (isHit) return hit.point;

            var pointString = $"({point.x}, {point.y}, {point.z})";
            Debug.LogWarning($"Can't find correct height of {pointString} in path. Using default point.");
            return point;
        }

        /// <summary>
        /// Alias to <see cref="GetPathLength(List{Vector3})"/>
        /// </summary>
        /// <inheritdoc cref="GetPathLength(List{Vector3})" />
        public static float GetPathLength(NavMeshPath navMeshPath)
        {
            return GetPathLength(navMeshPath.corners.ToList());
        }

        /// <summary>
        /// Returns length of given path
        /// </summary>
        public static float GetPathLength(List<Vector3> points)
        {
            if (points.Count <= 2) return 0f;

            var length = 0f;
            for (var i = 1; i < points.Count; i++) length += RangeUtil.GetDistance(points[i - 1], points[i]);

            return length;
        }

        /// <summary>
        /// Alias to <see cref="CalculatePathCost(List{Vector3})"/>
        /// </summary>
        /// <inheritdoc cref="CalculatePathCost(List{Vector3})" />
        public static float CalculatePathCost(NavMeshPath navMeshPath)
        {
            return CalculatePathCost(navMeshPath.corners.ToList());
        }

        /// <summary>
        /// Returns cost for given path
        /// </summary>
        public static float CalculatePathCost(List<Vector3> points)
        {
            float cost = 0;
            NavMeshHit hit;
            NavMesh.SamplePosition(points[0], out hit, 0.1f, NavMesh.AllAreas);
            var rayStart = points[0];
            var mask = hit.mask;
            var areaIndex = IndexFromMask(mask);

            for (var index = 1; index < points.Count; ++index)
            {
                var maxTries = 5;
                do
                {
                    var corner = points[index];
                    if (NavMesh.Raycast(rayStart, corner, out hit, mask))
                    {
                        cost += NavMesh.GetAreaCost(areaIndex) * hit.distance;
                        if (hit.mask != 0)
                            mask = hit.mask;
                        areaIndex = IndexFromMask(mask);
                        rayStart = hit.position;

                        if (hit.mask == 0)
                            rayStart += (corner - rayStart).normalized * 0.05f;
                    }
                    else
                    {
                        rayStart += (corner - rayStart).normalized * 0.05f;
                    }

                    maxTries--;
                } while (hit.hit && maxTries > 0);
            }

            return cost;
        }

        private static int IndexFromMask(int mask)
        {
            if (mask == 0) return -1;
            var i = 0;
            while (mask != 1)
            {
                mask >>= 1;
                ++i;
            }

            return i;
        }
        
        public static void SplitPath(Vector3[] path, float movePoints, out Vector3[] reachablePath, out Vector3[] unreachablePath)
        {
            var currentLength = 0f;
            for (var i = 1; i < path.Length; i++)
            {
                var distanceBetweenPoints = Vector3.Distance(path[i - 1], path[i]);
                currentLength += distanceBetweenPoints;
                
                // If still reachable then do nothing
                if (currentLength <= movePoints) continue;
                
                // Find exact point where unreachable path will start
                var interpolate = (currentLength - movePoints) / distanceBetweenPoints;
                var pointBetween = Vector3.Lerp(path[i], path[i - 1], interpolate);
                
                reachablePath = path.Where((_, index) => index < i).Append(pointBetween).ToArray();
                unreachablePath = path.Where((_, index) => index >= i).Prepend(pointBetween).ToArray();
                return;
            }
            
            // Destination is reachale, so there is no unreachable path
            reachablePath = path.ToArray();
            unreachablePath = null;
        }

        public static void SplitReachablePath(Vector3[] reachablePath, out List<PathType> pathTypes, out List<Vector3[]> paths)
        {
            // TODO: right now there is no "dangerous" and "natural" areas!
            // TODO: when those will be implemented then this place must be updated as well!
            pathTypes = new List<PathType>() { PathType.Reachable };
            paths = new List<Vector3[]>() { reachablePath };
        }
    }
}