private static void Main()
        {
            char[][] matrix =
            {
                new[] { '-', 'S', '-', '-', 'X' },
                new[] { '-', 'X', 'X', '-', '-' },
                new[] { '-', '-', '-', 'X', '-' },
                new[] { 'X', '-', 'X', 'E', '-' },
                new[] { '-', '-', '-', '-', 'X' }
            };


            // looking for shortest path from 'S' at (0,1) to 'E' at (3,3)
            // obstacles marked by 'X'
            const int fromX = 0;
            const int fromY = 1;
            const int toX   = 3;
            const int toY   = 3;

            MatrixNode endNode = AStarSearch(matrix, fromX, fromY, toX, toY);

            // looping through the Parent nodes until we get to the start node
            Stack <MatrixNode> path = new Stack <MatrixNode>();

            while (endNode.X != fromX || endNode.Y != fromY)
            {
                path.Push(endNode);
                endNode = endNode.Parent;
            }

            path.Push(endNode);

            Console.WriteLine("The shortest path from  " +
                              "(" + fromX + "," + fromY + ")  to " +
                              "(" + toX + "," + toY + ")  is:  \n");

            while (path.Count > 0)
            {
                MatrixNode node = path.Pop();
                Console.WriteLine("(" + node.X + "," + node.Y + ")");
            }
        }
        public static MatrixNode AStarSearch(char[][] matrix, int fromX, int fromY, int toX, int toY)
        {
            // The set of nodes already evaluated
            Dictionary <string, MatrixNode> closedSet = new Dictionary <string, MatrixNode>();

            // The set of currently discovered nodes that are not evaluated yet.
            // Initially, only the start node is known.
            Dictionary <string, MatrixNode> openSet = new Dictionary <string, MatrixNode>();

            MatrixNode startNode = new MatrixNode
            {
                X = fromX,
                Y = fromY
            };

            string key = startNode.X + startNode.X.ToString();

            openSet.Add(key, startNode);

            // ReSharper disable once ConvertToLocalFunction
            Func <KeyValuePair <string, MatrixNode> > smallestInOpenSet = () =>
            {
                KeyValuePair <string, MatrixNode> smallest = openSet.ElementAt(0);

                foreach (var item in openSet)
                {
                    if (item.Value.Sum < smallest.Value.Sum)
                    {
                        smallest = item;
                    }
                    else if (item.Value.Sum == smallest.Value.Sum && item.Value.To < smallest.Value.To)
                    {
                        smallest = item;
                    }
                }

                return(smallest);
            };

            // add these values to current node's x and y values to get the left/up/right/bottom neighbors
            List <KeyValuePair <int, int> > fourNeighbors = new List <KeyValuePair <int, int> >
            {
                new KeyValuePair <int, int>(-1, 0),
                new KeyValuePair <int, int>(0, 1),
                new KeyValuePair <int, int>(1, 0),
                new KeyValuePair <int, int>(0, -1)
            };

            int maxX = matrix.GetLength(0);

            if (maxX == 0)
            {
                return(null);
            }

            int maxY = matrix[0].Length;

            // while openSet is not empty
            while (true)
            {
                if (openSet.Count == 0)
                {
                    return(null);
                }

                // current := the node in openSet having the lowest fScore[] value
                // if current = goal
                //  return reconstruct_path(cameFrom, current)
                KeyValuePair <string, MatrixNode> current = smallestInOpenSet();
                if (current.Value.X == toX && current.Value.Y == toY)
                {
                    return(current.Value);
                }

                openSet.Remove(current.Key);
                closedSet.Add(current.Key, current.Value);

                foreach (var plusXy in fourNeighbors)
                {
                    int    nbrX   = current.Value.X + plusXy.Key;
                    int    nbrY   = current.Value.Y + plusXy.Value;
                    string nbrKey = nbrX + nbrY.ToString();
                    if (nbrX < 0 || nbrY < 0 || nbrX >= maxX || nbrY >= maxY ||
                        matrix[nbrX][nbrY] == 'X' || //obstacles marked by 'X'
                        closedSet.ContainsKey(nbrKey))
                    {
                        continue;
                    }

                    if (openSet.ContainsKey(nbrKey))
                    {
                        MatrixNode curNbr = openSet[nbrKey];
                        int        from   = Math.Abs(nbrX - fromX) + Math.Abs(nbrY - fromY);
                        if (from < curNbr.Fr)
                        {
                            curNbr.Fr     = from;
                            curNbr.Sum    = curNbr.Fr + curNbr.To;
                            curNbr.Parent = current.Value;
                        }
                    }
                    else
                    {
                        MatrixNode curNbr = new MatrixNode
                        {
                            X  = nbrX,
                            Y  = nbrY,
                            Fr = Math.Abs(nbrX - fromX) + Math.Abs(nbrY - fromY),
                            To = Math.Abs(nbrX - toX) + Math.Abs(nbrY - toY)
                        };

                        curNbr.Sum    = curNbr.Fr + curNbr.To;
                        curNbr.Parent = current.Value;

                        openSet.Add(nbrKey, curNbr);
                    }
                }
            }
        }