a*

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 interface WeightedGraph<L>
{
    double Cost(Vector3 a, Vector3 b);
    IEnumerable<Vector3> Neighbors(Vector3 id);
}





public class SquareGrid : WeightedGraph<Vector3>
{
    // 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 Vector3[] DIRS = new[]
        {
            new Vector3(1,0, 0),
            new Vector3(1,0, 1),
            new Vector3(1,0, -1),
            new Vector3(-1,0, 1),
            new Vector3(-1,0, -1),
            new Vector3(0,0, -1),
            new Vector3(-1,0, 0),
            new Vector3(0,0, 1)
        };

    public int width, height;
    public HashSet<Vector3> walls = new HashSet<Vector3>();
    public HashSet<Vector3> forests = new HashSet<Vector3>();

    public SquareGrid(int width, int height)
    {
        this.width = width;
        this.height = height;
    }

    public bool InBounds(Vector3 id)
    {
        return 0 <= id.x && id.x < width
            && 0 <= id.z && id.z < height;
    }

    public bool Passable(Vector3 id)
    {
        return !walls.Contains(id);
    }

    public double Cost(Vector3 a, Vector3 b)
    {
        return forests.Contains(b) ? 5 : 1;
    }

    public IEnumerable<Vector3> Neighbors(Vector3 id)
    {
        foreach (var dir in DIRS)
        {
            Vector3 next = new Vector3(id.x + dir.x, 0, id.z + dir.z);
            if (InBounds(next) && Passable(next))
            {
                yield return next;
            }
        }
    }
}


public class PriorityQueue<T>
{
    // 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 int Count
    {
        get { return elements.Count; }
    }

    public void Enqueue(Vector3 item, double priority)
    {
        NodeElement temp = new NodeElement();
        elements.Add(new NodeElement(item, priority));
    }
    public class NodeElement
    {
        public Vector3 item1;
        public double item2;
        public NodeElement(Vector3 _node, double _double)
        {
            item1 = _node;
            item2 = _double;
        }
        public NodeElement()
        {
            item1 = Vector3.zero;
            item2 = 0;
        }
    }
    public Vector3 Dequeue()
    {
        int bestIndex = 0;

        for (int i = 0; i < elements.Count; i++)
        {
            if (elements[i].item2 < elements[bestIndex].item2)
            {
                bestIndex = i;
            }
        }
        Vector3 bestItem = elements[bestIndex].item1;
        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<Vector3, Vector3> cameFrom
        = new Dictionary<Vector3, Vector3>();
    public Dictionary<Vector3, double> costSoFar
        = new Dictionary<Vector3, double>();

    // Note: a generic version of A* would abstract over Vector3 and
    // also Heuristic
    static public double Heuristic(Vector3 a, Vector3 b)
    {
        return Math.Abs(a.x - b.x) + Math.Abs(a.y - b.y);
    }
    Vector3 _start, _goal;
    public AStarSearch(SquareGrid graph, Vector3 start, Vector3 goal)
    {
        var frontier = new PriorityQueue<Vector3>();
        frontier.Enqueue(start, 0);
        _start = start;
        _goal = goal;
        cameFrom[start] = start;
        costSoFar[start] = 0;

        while (frontier.Count > 0)
        {
            Vector3 current = frontier.Dequeue();

            if (current.Equals(goal))
            {
                break;
            }

            foreach (var next in graph.Neighbors(current))
            {
                double newCost = costSoFar[current]
                    + graph.Cost(current, next);
                if (!costSoFar.ContainsKey(next)
                    || newCost < costSoFar[next])
                {
                    costSoFar[next] = newCost;
                    double priority = newCost + Heuristic(next, goal);
                    frontier.Enqueue(next, priority);
                    cameFrom[next] = current;
                }
            }
        }
    }

    public List<Vector3> FindShortestPath()
    {
        Vector3 next = _goal;
        List<Vector3> path = new List<Vector3>();
        path.Add(next);
        for (int i = 0; i < cameFrom.Count; ++i)
        {
            next = cameFrom[next];
            path.Add(next);
            if (next == _start)
            {
                path.Reverse();
                break;
            }
        }
        return path;
    }
}