using GarmentButton.FlatenArray;
using Unity.Burst;
using Unity.Collections;
using Unity.Entities;
using Unity.Mathematics;
using Unity.Transforms;


namespace GarmentButton.PathFinding
{
    [BurstCompile]
    [UpdateAfter(typeof(DebugMouseHitSystem))]
    public partial struct CalculatePathSystem : ISystem
    {


        [BurstCompile]
        public void OnCreate(ref SystemState state)
        {
            state.RequireForUpdate<EndSimulationEntityCommandBufferSystem.Singleton>();
            state.RequireForUpdate<GridAdditionalData>();
            var queryGrid = SystemAPI.QueryBuilder().WithAll<GridData, Grid>().WithNone<UpdateCheck, UpdatePoints, CastTag>().Build();

            var query = SystemAPI.QueryBuilder().WithAll<PathElement, EndPointPath, LocalTransform>().Build();
            state.RequireForUpdate(query);
            state.RequireForUpdate(queryGrid);
        }




        [BurstCompile]
        public void OnUpdate(ref SystemState state)
        {
            var gridData = SystemAPI.GetSingleton<GridData>();
            var gridAdditional = SystemAPI.GetSingleton<GridAdditionalData>();
            BlobAssetReference<PointPathBlobAsset> grid = SystemAPI.GetSingleton<Grid>().Value;
            var converter = new FlatenFloatArray2D(gridData.GridSize, gridData.CellSize, gridData.Center);
            var lenghtGrid = grid.Value.Array.Length;
            var ecb = SystemAPI.GetSingleton<EndSimulationEntityCommandBufferSystem.Singleton>().CreateCommandBuffer(state.WorldUnmanaged);

            state.Dependency = new PathCalculationJob
            {
                Converter = converter,
                ECB = ecb.AsParallelWriter(),
                GroundTypes = gridAdditional.Types.AsReadOnly(),
                Grid = grid,
                LenghtGrid = lenghtGrid,
            }.ScheduleParallel(state.Dependency);
        }

    }
    [WithAll(typeof(EndPointPath))]
    [BurstCompile]
    public partial struct PathCalculationJob : IJobEntity
    {

        private const int MOVE_DISTANCE_STRIGHT = 10;
        private const int MOVE_DISTANCE_DAIGNOLE = 14;
        private const int MOVE_DISTANCE_HEIGHT = 10;
        private const int LIMIT_DISTANCE_HEIGHT = 35;
        private const int HOW_FAR_CAN_JUMP = 10;
        private const int START_SEARCH_DISTANCE = 10;
        private const ushort INVALID_END_POINT_INDEX = ushort.MaxValue;


        [ReadOnly] public BlobAssetReference<PointPathBlobAsset> Grid;
        [ReadOnly] public NativeArray<GroundType>.ReadOnly GroundTypes;
        [ReadOnly] public int LenghtGrid;
        [ReadOnly] public FlatenFloatArray2D Converter;
        public EntityCommandBuffer.ParallelWriter ECB;
        public void Execute(Entity entity, [ChunkIndexInQuery] int sortKey, ref DynamicBuffer<PathElement> buffer, ref EndPointIndexPath endPointIndex, in EndPointPath target, in LocalTransform starPosition)
        {

            buffer.Clear();
            var gridPath = new NativeArray<PathNode>(LenghtGrid, Allocator.Temp);
            for (int i = 0; i < LenghtGrid; i++)
            {
                var point = Grid.Value.Array[i];
                var data = new PathNode
                {
                    Position = new int2(point.X, point.Z),
                    Height = point.Y,
                    CurrentIndex = i,
                    GCost = int.MaxValue,
                    CameFromIndex = -1
                };
                data.CalculateFCost();

                gridPath[i] = data;
            }

            var destination = target.Position;
            var starIndex = Converter.GetCellAtPosition(starPosition.Position.xz);
            var destinationIndex = Converter.GetCellAtPosition(destination.xz);
            endPointIndex.Value = INVALID_END_POINT_INDEX;
            starIndex = GetNeighbourNodesForStart(starIndex, Converter, ((int)(starPosition.Position.y * 10)) + 1);
            var starPoint = gridPath[starIndex];
            var endPoint = gridPath[destinationIndex];
            starPoint.GCost = 0;
            starPoint.HCost = CalculateDistance(starPoint, endPoint);
            starPoint.CalculateFCost();
            gridPath[starIndex] = starPoint;

            //Wanted To search
            var initialSearchCapacity = math.min(LenghtGrid, 128);
            var requestToSearch = new NativeList<PathNode>(initialSearchCapacity, Allocator.Temp);

            //Already Searched
            var searched = new NativeList<PathNode>(initialSearchCapacity, Allocator.Temp);
            var closed = new NativeArray<byte>(LenghtGrid, Allocator.Temp, NativeArrayOptions.ClearMemory);
            var openIndexes = new NativeArray<int>(LenghtGrid, Allocator.Temp, NativeArrayOptions.ClearMemory);
            AddToOpen(ref requestToSearch, openIndexes, starPoint);
            var gridWidth = Converter.GridSize.x;
            var gridHeight = Converter.GridSize.y;
            var foundUsablePath = false;

            while (requestToSearch.Length > 0)
            {
                var currentNode = PopOpen(ref requestToSearch, openIndexes);
                if (closed[currentNode.CurrentIndex] != 0)
                    continue;

                if (currentNode.CurrentIndex == endPoint.CurrentIndex)
                {
                    CalculatePath(buffer, gridPath, currentNode, entity, sortKey);
                    foundUsablePath = true;
                    break;
                }

                AddToClosed(currentNode.CurrentIndex, closed, ref searched, currentNode);

                var currentGroundType = GroundTypes[currentNode.CurrentIndex];
                var currentX = currentNode.CurrentIndex % gridWidth;
                var currentY = currentNode.CurrentIndex / gridWidth;
                for (var yOffset = -1; yOffset <= 1; yOffset++)
                {
                    for (var xOffset = -1; xOffset <= 1; xOffset++)
                    {
                        if ((xOffset | yOffset) == 0)
                            continue;

                        var neighbourX = currentX + xOffset;
                        var neighbourY = currentY + yOffset;
                        if ((uint)neighbourX >= (uint)gridWidth || (uint)neighbourY >= (uint)gridHeight)
                            continue;

                        var index = neighbourY * gridWidth + neighbourX;
                        var neighbourNode = gridPath[index];
                        var neighbourGroundType = GroundTypes[index];

                        if ((int)neighbourGroundType >= (int)GroundType.Blocked)
                        {
                            AddToClosed(index, closed, ref searched, neighbourNode);
                            continue;
                        }
                        if (closed[index] != 0)
                            continue;

                        int layerBase = 0;
                        switch (neighbourGroundType)
                        {
                            case GroundType.Air:
                                if (currentGroundType == GroundType.Air && currentNode.CameFromIndex != -1)
                                    if (!IsSameDirection(gridPath[currentNode.CameFromIndex], currentNode, neighbourNode))
                                        layerBase = 20;
                                break;
                            case GroundType.MoveAble:
                            case GroundType.NearGround:
                                layerBase = 15;
                                break;

                        }

                        if (!CanJumpIt(currentNode, neighbourNode))
                        {
                            continue;
                        }

                        int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode, neighbourNode) + layerBase;
                        if (tentativeGCost < neighbourNode.GCost)
                        {


                            neighbourNode.CameFromIndex = currentNode.CurrentIndex;



                            neighbourNode.GCost = tentativeGCost;
                            neighbourNode.HCost = CalculateDistance(neighbourNode, endPoint);
                            neighbourNode.CalculateFCost();
                            gridPath[index] = neighbourNode;

                            if (!CheckCanJumpThrough(index, gridPath))
                            {
                                RemoveNodeFromOpen(ref requestToSearch, openIndexes, index);
                                AddToClosed(index, closed, ref searched, neighbourNode);
                                continue;
                            }


                            var indexOpenList = openIndexes[index] - 1;
                            if (indexOpenList >= 0)
                            {
                                UpdateOpen(ref requestToSearch, openIndexes, indexOpenList, neighbourNode);
                                continue;
                            }
                            AddToOpen(ref requestToSearch, openIndexes, neighbourNode);
                        }

                    }
                }
            }

            if (buffer.Length == 0)
            {
                foreach (var node in searched)
                {
                    var closesAirPoint = GroundTypes[node.CurrentIndex] == GroundType.Air && node.CameFromIndex != -1;
                    if (!closesAirPoint)
                        continue;

                    CalculatePath(buffer, gridPath, node, entity, sortKey);
                    foundUsablePath = buffer.Length > 0;
                    break;
                }
            }

            if (foundUsablePath)
                endPointIndex.Value = (ushort)destinationIndex;

            ECB.SetComponentEnabled<EndPointPath>(sortKey, entity, false);
            gridPath.Dispose();
            requestToSearch.Dispose();
            searched.Dispose();
            closed.Dispose();
            openIndexes.Dispose();
        }



        private bool IsSameDirection(PathNode back, PathNode currentNode, PathNode neighbourNode)
        {
            var backDirection = back.Position - currentNode.Position;
            var frontDirection = currentNode.Position - neighbourNode.Position;
            return backDirection.x == frontDirection.x && backDirection.y == frontDirection.y;
        }
        private void AddToClosed(int index, NativeArray<byte> closed, ref NativeList<PathNode> searched, PathNode node)
        {
            if (closed[index] != 0)
                return;

            closed[index] = 1;
            searched.Add(node);
        }

        private void RemoveNodeFromOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int nodeIndex)
        {
            var openListIndex = openIndexes[nodeIndex] - 1;
            if (openListIndex < 0)
                return;

            RemoveFromOpen(ref openList, openIndexes, openListIndex);
        }

        private PathNode PopOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes)
        {
            var node = openList[0];
            RemoveFromOpen(ref openList, openIndexes, 0);
            return node;
        }

        private void AddToOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, PathNode node)
        {
            openList.Add(node);
            var openListIndex = openList.Length - 1;
            openIndexes[node.CurrentIndex] = openListIndex + 1;
            SiftOpenUp(ref openList, openIndexes, openListIndex);
        }

        private void UpdateOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int openListIndex, PathNode node)
        {
            openList[openListIndex] = node;
            openIndexes[node.CurrentIndex] = openListIndex + 1;
            SiftOpenUp(ref openList, openIndexes, openListIndex);
        }

        private void RemoveFromOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int openListIndex)
        {
            var removedNodeIndex = openList[openListIndex].CurrentIndex;
            var lastOpenIndex = openList.Length - 1;
            var movedNodeIndex = openList[lastOpenIndex].CurrentIndex;

            openIndexes[removedNodeIndex] = 0;
            openList[openListIndex] = openList[lastOpenIndex];
            openList.RemoveAtSwapBack(lastOpenIndex);

            if (openListIndex < openList.Length)
            {
                openIndexes[movedNodeIndex] = openListIndex + 1;
                SiftOpenDown(ref openList, openIndexes, openListIndex);
                SiftOpenUp(ref openList, openIndexes, openListIndex);
            }
        }

        private void SiftOpenUp(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int openListIndex)
        {
            while (openListIndex > 0)
            {
                var parentIndex = (openListIndex - 1) >> 1;
                if (HasHigherPriority(openList[parentIndex], openList[openListIndex]))
                    break;

                SwapOpen(ref openList, openIndexes, parentIndex, openListIndex);
                openListIndex = parentIndex;
            }
        }

        private void SiftOpenDown(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int openListIndex)
        {
            while (true)
            {
                var leftIndex = (openListIndex << 1) + 1;
                if (leftIndex >= openList.Length)
                    break;

                var rightIndex = leftIndex + 1;
                var bestIndex = leftIndex;
                if (rightIndex < openList.Length && HasHigherPriority(openList[rightIndex], openList[leftIndex]))
                    bestIndex = rightIndex;

                if (HasHigherPriority(openList[openListIndex], openList[bestIndex]))
                    break;

                SwapOpen(ref openList, openIndexes, openListIndex, bestIndex);
                openListIndex = bestIndex;
            }
        }

        private void SwapOpen(ref NativeList<PathNode> openList, NativeArray<int> openIndexes, int leftIndex, int rightIndex)
        {
            var temp = openList[leftIndex];
            openList[leftIndex] = openList[rightIndex];
            openList[rightIndex] = temp;
            openIndexes[openList[leftIndex].CurrentIndex] = leftIndex + 1;
            openIndexes[openList[rightIndex].CurrentIndex] = rightIndex + 1;
        }

        private bool HasHigherPriority(PathNode left, PathNode right)
        {
            if (left.FCost != right.FCost)
                return left.FCost < right.FCost;

            return left.HCost < right.HCost;
        }

        public void CalculatePath(DynamicBuffer<PathElement> buffer, NativeArray<PathNode> pathNodes, PathNode endNodes, Entity entity, int index)
        {
            var reversePath = new NativeList<int>(40, Allocator.Temp);
            var currentNode = endNodes;
            var lastGroundType = GroundType.Ground;
            var currentDirection = int2.zero;
            var lastHeightDirection = 0f;
            while (currentNode.CameFromIndex != -1)
            {
                var direction = pathNodes[currentNode.CameFromIndex].Position - pathNodes[currentNode.CurrentIndex].Position;
                var heightDirection = pathNodes[currentNode.CameFromIndex].Height - pathNodes[currentNode.CurrentIndex].Height;
                var currentGround = GroundTypes[currentNode.CurrentIndex];
                var nextGround = GroundTypes[currentNode.CameFromIndex];

                var sameDirection = (currentDirection.x == direction.x && currentDirection.y == direction.y);
                var sameType = lastGroundType == currentGround;
                var isDifferentTypeNext = currentGround != nextGround;
                var sameHeightDirection = lastHeightDirection == heightDirection;

                if (sameDirection && sameType && !isDifferentTypeNext && sameHeightDirection)
                {

                    // reversePath[reversePath.Length - 1] = currentNode.CurrentIndex;
                }
                else
                {
                    reversePath.Add(currentNode.CurrentIndex);
                }
                lastHeightDirection = heightDirection;
                lastGroundType = currentGround;
                currentDirection = direction;
                currentNode = pathNodes[currentNode.CameFromIndex];
            }
            for (int i = reversePath.Length - 1; i >= 0; i--)
            {
                buffer.Add(new PathElement
                {
                    IndexGrid = (ushort)reversePath[i]
                });

            }
            reversePath.Dispose();

#if UNITY_EDITOR && PATHFINDING_DEBUG
            var path = ECB.AddBuffer<PointsData>(index, entity);

            for (int i = 0; i < pathNodes.Length; i++)
                path.Add(new PointsData { Cost = pathNodes[i].GCost });
            
            ECB.AddComponent<DebugAll>(index, entity);
#endif
        }
        private bool CheckCanJumpThrough(int index, NativeArray<PathNode> pathPoint)
        {
            var currentNode = pathPoint[index];
            int count = 0;
            while (currentNode.CameFromIndex != -1 && GroundTypes[currentNode.CurrentIndex] == GroundType.Air)
            {
                if (count > HOW_FAR_CAN_JUMP)
                {
                    return false;
                }

                currentNode = pathPoint[currentNode.CameFromIndex];
                count++;
            }
            return true;
        }
        private bool IsJumpingToAir(PathNode current, PathNode next) => (GroundTypes[current.CurrentIndex] != GroundType.Air && GroundTypes[next.CurrentIndex] == GroundType.Air);
        private bool IsJumpingToGround(PathNode current, PathNode next) => (GroundTypes[current.CurrentIndex] == GroundType.Ground && GroundTypes[next.CurrentIndex] == GroundType.Ground);
        private bool CanJumpIt(PathNode current, PathNode next)
        {
            var isNeededJump = next.Height > current.Height;
            if (!isNeededJump)
                return true;

            var differenceHeight = next.Height - current.Height;
            var canReachHeight = differenceHeight < LIMIT_DISTANCE_HEIGHT;
            if (!canReachHeight)
                return false;

            var isHeightMoreThanOneBox = differenceHeight > 15f;
            if (!isHeightMoreThanOneBox)
                return true;

            return IsJumpingToGround(current, next);
        }

        private int GetNeighbourNodesForStart(int currentNodeIndex, FlatenFloatArray2D converter, int height)
        {
            if (Grid.Value.Array[currentNodeIndex].Y < height + 2)
                return currentNodeIndex;

            var gridWidth = converter.GridSize.x;
            var gridHeight = converter.GridSize.y;
            var originX = currentNodeIndex % gridWidth;
            var originY = currentNodeIndex / gridWidth;
            for (var distance = 1; distance <= START_SEARCH_DISTANCE; distance++)
            {
                for (var direction = 0; direction < 8; direction++)
                {
                    var neighbourX = originX;
                    var neighbourY = originY;
                    switch ((Direction)direction)
                    {
                        case Direction.Up_Left:
                            neighbourX -= distance;
                            neighbourY += distance;
                            break;
                        case Direction.Up:
                            neighbourY += distance;
                            break;
                        case Direction.Up_Right:
                            neighbourX += distance;
                            neighbourY += distance;
                            break;
                        case Direction.Down_Left:
                            neighbourX -= distance;
                            neighbourY -= distance;
                            break;
                        case Direction.Down:
                            neighbourY -= distance;
                            break;
                        case Direction.Down_Right:
                            neighbourX += distance;
                            neighbourY -= distance;
                            break;
                        case Direction.Left:
                            neighbourX -= distance;
                            break;
                        case Direction.Right:
                            neighbourX += distance;
                            break;
                    }

                    if ((uint)neighbourX >= (uint)gridWidth || (uint)neighbourY >= (uint)gridHeight)
                        continue;

                    var index = neighbourY * gridWidth + neighbourX;
                    if (Grid.Value.Array[index].Y > height)
                        continue;

                    return index;
                }
            }
            return currentNodeIndex;
        }
        private int CalculateDistance(PathNode a, PathNode b)
        {
            int xDistance = math.abs(a.Position.x - b.Position.x);
            int yDistance = math.abs(a.Position.y - b.Position.y);
            int heightDistance = a.Height - b.Height;
            int remaining = math.abs(xDistance - yDistance);
            return MOVE_DISTANCE_DAIGNOLE * math.min(xDistance, yDistance) + MOVE_DISTANCE_STRIGHT * remaining + MOVE_DISTANCE_HEIGHT + heightDistance;
        }
    }
}