using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PathFindingExample
{
    class PathFinding
    {
        public PathFinding()
        {

        }

        public List<Node> findPath(Node A, Node B)
        {
            var myList = new List<Node>();
            var searchedNodes = new List<Node>();
            var nodePath = new Stack<Node>();

            bool foundPath = false;

            //Add the first node onto our track
            nodePath.Push(A);
            searchedNodes.Add(A);
            while(!foundPath)
            {
                //No path
                if (nodePath.Count() == 0)
                {
                    Console.WriteLine("No path");
                    return nodePath.ToList();
                }
                //Find a node we haven't touched yet that's attached to the stack
                //If we have none, remove it from the stack
                //If there's no nodes left, there is no path.

                var possiblePaths = nodePath.Peek().neighbors.Where(a => !searchedNodes.Contains(a));
                if (possiblePaths.Count() > 0)
                {
                    //Start with the closest to the goal
                    var chosenPath = possiblePaths.OrderBy(x => Vector3.Distance(B.position, x.position)).First();
                    nodePath.Push(chosenPath);
                    searchedNodes.Add(chosenPath);
                }
                else
                {
                    nodePath.Pop();
                }
                if (nodePath.Count == 0)
                {
                    return nodePath.ToList();
                }

                //We found our goal
                if (nodePath.Peek() == B)
                {
                    return nodePath.ToList();
                }
            }


            return nodePath.ToList();
        }
    }

    class Node
    {
        public GameObject GO;
        public Vector3 position;
        public List<Node> neighbors;

        public static List<Node> allNodes;


        public Node(Vector3 pos)
        {
            this.position = pos;
            neighbors = new List<Node>();
            if (allNodes == null)
            {
                allNodes = new List<Node>();
            }
            allNodes.Add(this);
        }
        public void Connect(Node other)
        {
            if (other == this) { return; }
            if (neighbors.Contains(other) == false) { neighbors.Add(other); }
            if (other.neighbors.Contains(this) == false) { other.neighbors.Add(this); };
        }
    }
}