using System;
using System.Collections.Generic;

namespace MonoTest
{
    public class AStar
    {
        public static int resultLength=0;

        class Node
        {
            public Node(int x, int y)
            {
                posX = x;
                posY = y;
                neighbors = new List<Node>();

            }

            public List<Node> neighbors;
            public int posX;
            public int posY;

            public float g = float.PositiveInfinity;
            public float f = float.PositiveInfinity;
            public Node previous = null;
        }

        public static void Main()
        {
            for(int i=0; i<1000; ++i)
            {
                performAStar();
            }
        }

        public static void performAStar()
        {
            string map = "S   XXX               X\n"
                        +"XX      XXXXXXXXXXXXX X\n"
                        +"       XX             X\n"
                        +"X     XXX        XXXXXX\n"
                        +"        XXXX   XXXXXXXX\n"
                        +"XXXXXXX    X          X\n"
                        +"XX         X   XXXXX  X\n"
                        +"          XX     XX   X\n"
                        +"XXXXXXXXXXX           X\n"
                        +"XX          XXXXXXXXX X\n"
                        +"       XXXXXX         X\n"
                        +"X     XXX        XXXXXX\n"
                        +"        XXXX   XXXXXXXX\n"
                        +"XXXXXXX    X          X\n"
                        +"XX         X   XXXXX  X\n"
                        +"          XX     XX   X\n"
                        +"XXXXX XXXXX           X\n"
                        +"XX          XXXX XXXX X\n"
                        +"       XXXXXX         X\n"
                        +"X     XXX        XXXXXX\n"
                        +"        XXXX   XXXXXXXX\n"
                        +"XXXXX XX   X          X\n"
                        +"XX         X   XXXXX  X\n"
                        +"          XX     XX   X\n"
                        +"XXXXX XXXXXXXXXXXX    X\n"
                        +"              G XX    X\n"
                        +"XXXXXXXXXXXXXXXXXXXXXXX";
            string[] lines = map.Split ('\n');

            Node start = null;
            Node goal = null;

            int width = lines [0].Length;
            int height = lines.Length;

            Node[,] nodes = new Node[width,height];
            //initialize the node grid with zeros
            for (int x = 0; x < width; x++)
                for (int y = 0; y < height; y++)
                    nodes [x,y] = null;

            //create the accessible nodes on the grid
            for (int y = 0; y < height; y++)
            {
                string line = lines [y];
                for (int x = 0; x < width; x++)
                {
                    char c = line [x];

                    //accessible node?
                    if (c != 'X')
                    {
                        Node node = new Node (x, y);

                        if (c == 'S')
                            start = node;
                        if (c == 'G')
                            goal = node;

                        nodes [x,y] = node;
                    }
                }
            }

            //connect the nodes
            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    if(nodes [x,y] == null)
                        continue;

                    //up
                    if (y > 0 && nodes [x,y - 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x,y - 1]);
                    
                    //up right
                    if (y > 0 && x < width - 1 && nodes [x + 1,y - 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x + 1,y - 1]);
                    
                    //right
                    if (x < width - 1 && nodes [x + 1,y] != null)
                        nodes [x,y].neighbors.Add (nodes [x + 1,y]);

                    //right down
                    if (x < width - 1 && y < height && nodes [x + 1,y + 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x + 1,y + 1]);

                    //down
                    if (y < height && nodes [x,y + 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x,y + 1]);
                    
                    //down left
                    if (y < height && x > 0 && nodes [x - 1,y + 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x - 1,y + 1]);
                    
                    //left
                    if (x > 0 && nodes [x - 1,y] != null)
                        nodes [x,y].neighbors.Add (nodes [x - 1,y]);
                    
                    //left up
                    if (x > 0 && y > 0 && nodes [x - 1,y - 1] != null)
                        nodes [x,y].neighbors.Add (nodes [x - 1,y - 1]);
                }
            }

            //perform the actal pathfinding
            if (!aStar(start, goal))
            {
                Console.Error.WriteLine("Unable to find a path.");
            }

            //reconstruct the path
            List<Node> path = new List<Node> ();
            Node waypoint = goal;
            path.Add (goal);
            while (waypoint.previous != null)
            {
                path.Add (waypoint.previous);
                waypoint = waypoint.previous;
            }

            string result = "";

            for (int i = path.Count - 1; i >= 0; i--)
            {
                Node p = path[i];
                result += "("+p.posX.ToString()+","+p.posY.ToString()+")";
            }

            //make sure the compiler can't optimize away any important computations by saving the result:
            resultLength = result.Length;
            //Console.WriteLine(result);

        }

        private static bool aStar(Node start, Node goal)
        {
            List<Node> closedSet = new List<Node> ();
            List<Node> openSet = new List<Node> ();

            openSet.Add (start);
            start.g = 0.0f;
            start.f = h (start, goal);

            while (openSet.Count > 0)
            {
                Node current = findBest(openSet);

                if(current == goal)
                    return true;

                openSet.Remove(current);
                closedSet.Add(current);

                foreach(Node neighbor in current.neighbors)
                {
                    if(closedSet.Contains(neighbor))
                       continue;

                    float newG = current.g + h(current, neighbor);
                    if(newG < neighbor.g)
                    {
                        neighbor.g = newG;
                        neighbor.f = newG + h(neighbor, goal);
                        neighbor.previous = current;
                    }

                    if(!openSet.Contains(neighbor))
                    {
                        openSet.Add(neighbor);
                    }
                }
            }

            return false;
        }

        private static float h(Node current, Node goal)
        {
            double dx = current.posX - goal.posX;
            double dy = current.posY - goal.posY;
            return (float)Math.Sqrt(dx*dx + dy*dy);
        }

        private static Node findBest(List<Node> list)
        {
            Node best = null;
            float bestF = float.PositiveInfinity;

            foreach (Node node in list) 
            {
                if(node.f < bestF)
                {
                    best = node;
                    bestF = node.f;
                }
            }

            return best;
        }
    }
}