using UnityEngine;
using System;
using System.Collections.Generic;

namespace TGS.PathFinding {

    public class PathFinderFastIrregular : IPathFinder {

        // Heap variables are initializated to default, but I like to do it anyway
        private Cell[] mGrid;
        private PriorityQueueB<int> mOpen;
        private List<PathFinderNode> mClose = new List<PathFinderNode>();
        private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
        private float mHEstimate = 1;
        private int mMaxSteps = 2000;
        private float mMaxSearchCost = 100000;
        private float mMaxCellCrossCost;
        private PathFinderNodeFast[] mCalcGrid;
        private byte mOpenNodeValue = 1;
        private byte mCloseNodeValue = 2;
        private OnCellCross mOnCellCross;
        private int mMinClearance;

        //Promoted local variables to member variables to avoid recreation between calls
        private float mH;
        private int mLocation;
        private int mNewLocation;
        private bool mFound;
        private int mEndLocation;
        private float mNewG;
        private int mCellGroupMask = -1;
        private bool mIgnoreCanCrossCheck;
        private bool mIgnoreCellCost;
        private bool mIncludeInvisibleCells;

        public PathFinderFastIrregular(Cell[] grid) {
            if (grid == null)
                throw new Exception("Grid cannot be null");

            mGrid = grid;

            if (mCalcGrid == null || mCalcGrid.Length != grid.Length)
                mCalcGrid = new PathFinderNodeFast[grid.Length];

            mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
        }

        public void SetCalcMatrix(Cell[] grid) {
            if (grid == null)
                throw new Exception("Grid cannot be null");
            if (grid.Length != mGrid.Length) // mGridX != (ushort) (mGrid.GetUpperBound(0) + 1) || mGridY != (ushort) (mGrid.GetUpperBound(1) + 1))
                throw new Exception("SetCalcMatrix called with matrix with different dimensions. Call constructor instead.");
            mGrid = grid;

            Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
            ComparePFNodeMatrix comparer = (ComparePFNodeMatrix)mOpen.comparer;
            comparer.SetMatrix(mCalcGrid);
        }

        public HeuristicFormula Formula {
            get { return mFormula; }
            set { mFormula = value; }
        }

        public float HeavyDiagonalsCost {
            get { return 0; }
            set { }
        }

        public CellType CellShape {
            get { return CellType.Irregular; }
            set { }
        }

        public float HeuristicEstimate {
            get { return mHEstimate; }
            set { mHEstimate = value; }
        }

        public float MaxSearchCost {
            get { return mMaxSearchCost; }
            set { mMaxSearchCost = value; }
        }

        public float MaxCellCrossCost {
            get { return mMaxCellCrossCost; }
            set { mMaxCellCrossCost = value; }
        }

        public int MaxSteps {
            get { return mMaxSteps; }
            set { mMaxSteps = value; }
        }


        public OnCellCross OnCellCross {
            get { return mOnCellCross; }
            set { mOnCellCross = value; }
        }

        public int CellGroupMask {
            get { return mCellGroupMask; }
            set { mCellGroupMask = value; }
        }

        public bool Diagonals {
            get { return false; }
            set { }
        }

        public bool IgnoreCanCrossCheck {
            get { return mIgnoreCanCrossCheck; }
            set { mIgnoreCanCrossCheck = value; }
        }

        public bool IgnoreCellCost {
            get { return mIgnoreCellCost; }
            set { mIgnoreCellCost = value; }
        }

        public bool IncludeInvisibleCells {
            get { return mIncludeInvisibleCells; }
            set { mIncludeInvisibleCells = value; }
        }

        public int MinClearance {
            get { return mMinClearance; }
            set { mMinClearance = value; }
        }

        public List<PathFinderNode> FindPath(TerrainGridSystem tgs, Cell startCell, Cell endCell, out float totalCost, bool evenLayout) {
            totalCost = 0;
            mFound = false;
            if (mOpenNodeValue > 250) {
                Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
                mOpenNodeValue = 1;
                mCloseNodeValue = 2;
            } else {
                mOpenNodeValue += 2;
                mCloseNodeValue += 2;
            }
            mOpen.Clear();
            mClose.Clear();
        
            mLocation = startCell.index;
            mEndLocation = endCell.index;
            mCalcGrid[mLocation].G = 0;
            mCalcGrid[mLocation].F = mHEstimate;
            mCalcGrid[mLocation].PX = (ushort)mLocation;
            mCalcGrid[mLocation].Status = mOpenNodeValue;
            mCalcGrid[mLocation].Steps = 0;
        
            mOpen.Push(mLocation);
            while (mOpen.Count > 0) {
                mLocation = mOpen.Pop();
        
                //Is it in closed list? means this node was already processed
                if (mCalcGrid[mLocation].Status == mCloseNodeValue)
                    continue;
        
                if (mLocation == mEndLocation) {
                    mCalcGrid[mLocation].Status = mCloseNodeValue;
                    mFound = true;
                    break;
                }
        
                // Lets calculate each successors
                List<Cell> neighbours = mGrid[mLocation].neighbours;
                int maxi = neighbours != null ? neighbours.Count : 0;
        
                bool hasSideCosts = false;
                float[] sideCosts = mGrid[mLocation].crossCost;
                if (!mIgnoreCellCost && sideCosts != null) {
                    hasSideCosts = true;
                }
        
                for (int i = 0; i < maxi; i++) {
        
                    Cell ncell = neighbours[i];
        
                    // Unbreakeable?
                    if (!ncell.canCross && !mIgnoreCanCrossCheck)
                        continue;
        
                    if (ncell.clearance < mMinClearance) {
                        continue;
                    }
        
                    if (!mIncludeInvisibleCells && !ncell.visible)
                        continue;
        
                    float gridValue = (ncell.group & mCellGroupMask) != 0 ? 1 : 0;
                    if (gridValue == 0)
                        continue;
        
                    if (hasSideCosts) {
                        gridValue = sideCosts[0]; // irregular topology cells do not have per-edge crossing costs
                        if (gridValue > mMaxCellCrossCost) continue;
                        if (gridValue <= 0)
                            gridValue = 1;
                    }
        
                    // Check custom validator
                    if (mOnCellCross != null) {
                        gridValue += mOnCellCross(tgs, ncell.index);
                    }
        
                    mNewG = mCalcGrid[mLocation].G + gridValue;
        
                    if (mNewG > mMaxSearchCost || mCalcGrid[mLocation].Steps >= mMaxSteps)
                        continue;
        
                    mNewLocation = ncell.index;
        
                    //Is it open or closed?
                    if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue) {
                        // The current node has less code than the previous? then skip this node
                        if (mCalcGrid[mNewLocation].G <= mNewG)
                            continue;
                    }
        
                    mCalcGrid[mNewLocation].PX = (ushort)mLocation;
                    mCalcGrid[mNewLocation].G = mNewG;
                    mCalcGrid[mNewLocation].Steps = mCalcGrid[mLocation].Steps + 1;
        
                    switch (mFormula) {
                        case HeuristicFormula.Euclidean:
                            mH = mHEstimate * Vector2.Distance(ncell.center, mGrid[mLocation].center);
                            break;
                        default:
                            mH = mHEstimate * Vector2.SqrMagnitude(ncell.center - mGrid[mLocation].center);
                            break;
                    }
                    mCalcGrid[mNewLocation].F = mNewG + mH;
        
                    mOpen.Push(mNewLocation);
                    mCalcGrid[mNewLocation].Status = mOpenNodeValue;
                }
        
                mCalcGrid[mLocation].Status = mCloseNodeValue;
            }
        
            if (mFound) {
                mClose.Clear();
                int posX = endCell.index;
                PathFinderNodeFast fNodeTmp = mCalcGrid[posX];
                totalCost = fNodeTmp.G;
                PathFinderNode fNode;
                fNode.F = fNodeTmp.F;
                fNode.G = fNodeTmp.G;
                fNode.H = 0;
                fNode.PX = fNodeTmp.PX;
                fNode.PY = fNodeTmp.PY;
                fNode.X = endCell.index;
                fNode.Y = 0;
        
                while (fNode.X != fNode.PX) {
                    mClose.Add(fNode);
                    posX = fNode.PX;
                    fNodeTmp = mCalcGrid[posX];
                    fNode.F = fNodeTmp.F;
                    fNode.G = fNodeTmp.G;
                    fNode.H = 0;
                    fNode.PX = fNodeTmp.PX;
                    fNode.PY = fNodeTmp.PY;
                    fNode.X = posX;
                }
                return mClose;
            }
            return null;
        }

        public List<PathFinderNode> FindPath(TerrainGridSystem tgs, Cell startCell, Cell endCell, int crossLevel, out float totalCost, bool evenLayout) {
            totalCost = 0;
            mFound = false;
            if (mOpenNodeValue > 250) {
                Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
                mOpenNodeValue = 1;
                mCloseNodeValue = 2;
            } else {
                mOpenNodeValue += 2;
                mCloseNodeValue += 2;
            }
            mOpen.Clear();
            mClose.Clear();

            mLocation = startCell.index;
            mEndLocation = endCell.index;
            mCalcGrid[mLocation].G = 0;
            mCalcGrid[mLocation].F = mHEstimate;
            mCalcGrid[mLocation].PX = (ushort)mLocation;
            mCalcGrid[mLocation].Status = mOpenNodeValue;
            mCalcGrid[mLocation].Steps = 0;

            mOpen.Push(mLocation);
            while (mOpen.Count > 0) {
                mLocation = mOpen.Pop();

                //Is it in closed list? means this node was already processed
                if (mCalcGrid[mLocation].Status == mCloseNodeValue)
                    continue;

                if (mLocation == mEndLocation) {
                    mCalcGrid[mLocation].Status = mCloseNodeValue;
                    mFound = true;
                    break;
                }

                // Lets calculate each successors
                List<Cell> neighbours = mGrid[mLocation].neighbours;
                int maxi = neighbours != null ? neighbours.Count : 0;

                bool hasSideCosts = false;
                float[] sideCosts = mGrid[mLocation].crossCost;
                if (!mIgnoreCellCost && sideCosts != null) {
                    hasSideCosts = true;
                }

                for (int i = 0; i < maxi; i++) {

                    Cell ncell = neighbours[i];

                    // Unbreakeable?
                    if (!ncell.CanCross(crossLevel) && !mIgnoreCanCrossCheck)
                        continue;

                    if (ncell.clearance < mMinClearance) {
                        continue;
                    }

                    if (!mIncludeInvisibleCells && !ncell.visible)
                        continue;

                    float gridValue = (ncell.group & mCellGroupMask) != 0 ? 1 : 0;
                    if (gridValue == 0)
                        continue;

                    if (hasSideCosts) {
                        gridValue = sideCosts[0]; // irregular topology cells do not have per-edge crossing costs
                        if (gridValue > mMaxCellCrossCost) continue;
                        if (gridValue <= 0)
                            gridValue = 1;
                    }

                    // Check custom validator
                    if (mOnCellCross != null) {
                        gridValue += mOnCellCross(tgs, ncell.index);
                    }

                    mNewG = mCalcGrid[mLocation].G + gridValue;

                    if (mNewG > mMaxSearchCost || mCalcGrid[mLocation].Steps >= mMaxSteps)
                        continue;

                    mNewLocation = ncell.index;

                    //Is it open or closed?
                    if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue) {
                        // The current node has less code than the previous? then skip this node
                        if (mCalcGrid[mNewLocation].G <= mNewG)
                            continue;
                    }

                    mCalcGrid[mNewLocation].PX = (ushort)mLocation;
                    mCalcGrid[mNewLocation].G = mNewG;
                    mCalcGrid[mNewLocation].Steps = mCalcGrid[mLocation].Steps + 1;

                    switch (mFormula) {
                        case HeuristicFormula.Euclidean:
                            mH = mHEstimate * Vector2.Distance(ncell.center, mGrid[mLocation].center);
                            break;
                        default:
                            mH = mHEstimate * Vector2.SqrMagnitude(ncell.center - mGrid[mLocation].center);
                            break;
                    }
                    mCalcGrid[mNewLocation].F = mNewG + mH;

                    mOpen.Push(mNewLocation);
                    mCalcGrid[mNewLocation].Status = mOpenNodeValue;
                }

                mCalcGrid[mLocation].Status = mCloseNodeValue;
            }

            if (mFound) {
                mClose.Clear();
                int posX = endCell.index;
                PathFinderNodeFast fNodeTmp = mCalcGrid[posX];
                totalCost = fNodeTmp.G;
                PathFinderNode fNode;
                fNode.F = fNodeTmp.F;
                fNode.G = fNodeTmp.G;
                fNode.H = 0;
                fNode.PX = fNodeTmp.PX;
                fNode.PY = fNodeTmp.PY;
                fNode.X = endCell.index;
                fNode.Y = 0;

                while (fNode.X != fNode.PX) {
                    mClose.Add(fNode);
                    posX = fNode.PX;
                    fNodeTmp = mCalcGrid[posX];
                    fNode.F = fNodeTmp.F;
                    fNode.G = fNodeTmp.G;
                    fNode.H = 0;
                    fNode.PX = fNodeTmp.PX;
                    fNode.PY = fNodeTmp.PY;
                    fNode.X = posX;
                }
                return mClose;
            }
            return null;
        }

        internal class ComparePFNodeMatrix : IComparer<int> {
            protected PathFinderNodeFast[] mMatrix;

            public ComparePFNodeMatrix(PathFinderNodeFast[] matrix) {
                mMatrix = matrix;
            }

            public int Compare(int a, int b) {
                if (mMatrix[a].F > mMatrix[b].F)
                    return 1;
                else if (mMatrix[a].F < mMatrix[b].F)
                    return -1;
                return 0;
            }

            public void SetMatrix(PathFinderNodeFast[] matrix) {
                mMatrix = matrix;
            }
        }
    }
}