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

// A* needs only a WeightedGraph and a Vector3 type L, and does *not*
// have to be a grid. However, in the example code I am using a grid.





public class Direction
{
    public int x, y;
    public Direction(int _x, int _y)
    {
        x = _x;
        y = _y;
    }
    public Direction()
    {
        x = 0;
        y = 0;
    }
}
public class SquareGrid
{
    // Implementation notes: I made the fields public for convenience,
    // but in a real project you'll probably want to follow standard
    // style and make them private.
    public static readonly Direction[] DIRS = new[]
        {
            new Direction(1, 0),
            new Direction(1, 1),
            new Direction(1, -1),
            new Direction(-1, 1),
            new Direction(-1, -1),
            new Direction(0, -1),
            new Direction(-1, 0),
            new Direction(0, 1)
        };

    public int width, height;
    public NodeElement[,] nodes;
    public SquareGrid(int width, int height)
    {
        nodes = new NodeElement[width, height];
        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
            {
                nodes[x, y] = new NodeElement(new Vector3(x, 0, y), 1, true);
            }
        }
        this.width = width;
        this.height = height;
    }
    public NodeElement FindNode(int x, int y)
    {
        if (InBounds(x, y))
            return nodes[x, y];
        else
            return null;
    }
    public bool InBounds(NodeElement id)
    {
        return 0 <= id.x && id.x < width
            && 0 <= id.y && id.y < height;
    }
    public bool InBounds(int x, int y)
    {
        return 0 <= x && x < width
            && 0 <= y && y < height;
    }

    public double Cost(NodeElement a, NodeElement b)
    {
        double c = b.item2;
        if (a.x != b.x && a.y != b.y)
            c += 0.1f;
        return c;
    }

    public IEnumerable<NodeElement> Neighbors(NodeElement id)
    {
        foreach (var dir in DIRS)
        {
            int x = (id.x + dir.x);
            int y = (id.y + dir.y);
            if (InBounds(x, y))
            {
                NodeElement next = FindNode(x, y);
                if (next.Passable())
                {
                    //Debug.DrawRay(next.pos(), Vector3.up, Color.blue, 1);
                    yield return next;
                }
            }
        }
    }
}
[System.Serializable]
public class NodeElement
{
    public int x, y;
    public double item2;
    public bool passable;
    public Seeker occupied;
    public NodeElement cameFrom;
    public double costSofar;
    public bool passed;
    public double priority;
    public MeshRenderer mesh;
    public NodeElement(Vector3 _node, double _double, bool _passable)
    {
        x = (int)_node.x;
        y = (int)_node.z;
        item2 = _double;
        passable = _passable;
        occupied = null;
        costSofar = 0;
        cameFrom = null;
        passed = false;
        priority = 0;
        mesh = null;
    }
    public NodeElement()
    {
        x = 0;
        y = 0;
        item2 = 0;
        passable = false;
        occupied = null;
        costSofar = 0;
        cameFrom = null;
        passed = false;
        priority = 0;
        mesh = null;
    }
    public bool Passable()
    {
        return passable && occupied == null;
    }
    public bool Passable(Seeker seeker)
    {
        return passable && (occupied == null || occupied == seeker);
    }
    public Vector3 pos()
    {
        return new Vector3(x, 0, y);
    }/*
    public bool Surrounded()
    {
        SquareGrid grid = PathfindingMap.Grid();
        foreach (var dir in SquareGrid.DIRS)
        {
            int _x = (x + dir.x);
            int _y = (y + dir.y);
            if (grid.InBounds(_x, _y))
            {
                NodeElement next = grid.FindNode(_x, _y);
                if (!next.Passable())
                {
                    return false;
                }
            }
        }
        return true;
    }*/
}
public class PriorityQueue
{
    // I'm using an unsorted array for this example, but ideally this
    // would be a binary heap. Find a binary heap class:
    // * https://bitbucket.org/BlueRaja/high-speed-priority-queue-for-c/wiki/Home
    // * http://visualstudiomagazine.com/articles/2012/11/01/priority-queues-with-c.aspx
    // * http://xfleury.github.io/graphsearch.html
    // * http://stackoverflow.com/questions/102398/priority-queue-in-net
    private List<NodeElement> elements = new List<NodeElement>();
    public List<NodeElement> elementstoremove = new List<NodeElement>();

    public int Count
    {
        get { return elements.Count; }
    }

    public void Enqueue(NodeElement next, double priority)
    {
        next.passed = true;
        next.priority = priority;
        elements.Add(next);
        elementstoremove.Add(next);
    }
    public NodeElement Dequeue()
    {
        int bestIndex = 0;
        for (int i = 0; i < elements.Count; i++)
        {
            if (elements[i].priority < elements[bestIndex].priority)
            {
                bestIndex = i;
            }
        }
        NodeElement bestItem = elements[bestIndex];
        elements.RemoveAt(bestIndex);
        return bestItem;
    }

}


/* NOTE about types: in the main article, in the Python code I just
 * use numbers for costs, heuristics, and priorities. In the C++ code
 * I use a typedef for this, because you might want int or double or
 * another type. In this C# code I use double for costs, heuristics,
 * and priorities. You can use an int if you know your values are
 * always integers, and you can use a smaller size number if you know
 * the values are always small. */

public class AStarSearch
{
    /*public Dictionary<NodeElement, NodeElement> cameFrom
        = new Dictionary<NodeElement, NodeElement>();
    public Dictionary<NodeElement, double> costSoFar
        = new Dictionary<NodeElement, double>();
*/
    // Note: a generic version of A* would abstract over Vector3 and
    // also Heuristic
    static public double Heuristic(int xa, int ya, int xb, int yb)
    {
        return Math.Abs(xa - xb) + Math.Abs(ya - yb);
    }
    NodeElement _start, _goal, closestPossible;
    PriorityQueue frontier;
    public AStarSearch(SquareGrid graph, NodeElement goal, NodeElement start)
    {
        int count = 0;
        frontier = new PriorityQueue();
        frontier.Enqueue(start, 0);
        _start = start;
        _goal = goal;
        closestPossible = _start;
        while (frontier.Count > 0)
        {
            NodeElement current = frontier.Dequeue();

            if (current.Equals(goal)||count>500)
            {
                break;
            }

            foreach (var next in graph.Neighbors(current))
            {
                double newCost = current.costSofar + graph.Cost(current, next);
                if (!next.passed || (newCost < next.costSofar && next.costSofar > 0))
                {
                    count++;
                    next.costSofar = newCost;
                    double priority = newCost + Heuristic(next.x, next.y, goal.x, goal.y);
                    double h = Mathf.Sqrt(next.x - goal.x + next.y - goal.y);
                    double h2 = Mathf.Sqrt(closestPossible.x - goal.x + closestPossible.y - goal.y);
                    frontier.Enqueue(next, priority);
                    next.cameFrom = current;
                    if (h2 > h)
                    {
                        closestPossible = next;
                    }
                }
            }
        }
    }

    public List<NodeElement> FindShortestPath()
    {
        NodeElement next = _goal,start;
        List<NodeElement> path = new List<NodeElement>();
        if (next.cameFrom == null)
        {
            next = closestPossible;
            start = closestPossible;
        }
        else
        {
            start = _goal;
        }
        path.Add(next);
        for (int i = 0; i < start.costSofar; ++i)
        {
            next = next.cameFrom;
            path.Add(next);
            if (next == _start)
            {
                path.Reverse();
                break;
            }
        }
        for (int i = 0; i < frontier.elementstoremove.Count; i++)
        {

            NodeElement node = frontier.elementstoremove[i];
            node.passed = false;
            node.cameFrom = null;
            node.costSofar = 0;
        }
        return path;
    }
}