Поиск Пути А* - C#

Формулировка задачи:

Есть проблема, я не могу понять как сделать так чтобы начиналась поиск пути с любой точки и доходил до любой точки которую я введу, что надо сделать? у меня получается только от 0,0 до 4,4 вот код

 int[,] CopMap = new int[5, 5];
           int [,]  map = new int[,]{
               {0,0,0,0,0},
               {-1,0,0,0,0},
               {0,0,0,0,0},
               {0,0,0,0,0},
               {0,0,0,0,0},
            };
            int b = 0;
            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < 5; j++)
                {
                    Console.Write(map[i,j] + " ");
 
                }
                Console.WriteLine();
            }
            Console.WriteLine();
            Console.WriteLine();
            //////////////////////////////// волна
            Console.WriteLine(" obrabotanij massive");
            int n = -1;
            int p = -1;
 
            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < 5; j++)
                {
                   
                        if (map[i, j] != -1 && map[i, j] != -3)
                        {
                            map[i, j] = n + 1;
                        }
                
                    n = n + 1;
                    Console.Write(map[i, j] + "\t");
                }
               
                n = p + 1;
                p = p + 1;
                Console.WriteLine();
            }
            Console.WriteLine("nashol " + b);
            int g = 0;
            int d=0;
            int max = 0;
            Console.WriteLine();
            Console.WriteLine();
            ////////////////////////////////////////////////////
            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < 5; j++)
                {
                    if(map[i,j]!=-1)
                    if (map[i, j] == 0)
                    {
                        max = map[i, j];
                    }
                    if (map[i,j]>max)
                    {
                        d = d + 1;
                        max++;
                        map[i, j] = 0;
                    }
                    Console.Write(map[i, j] + "\t");
                }
                d++;
                Console.WriteLine();
            }
            Console.WriteLine();

Решение задачи: «Поиск Пути А*»

textual
                            Листинг программы 
                        

                            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < 5; j++)
                {
                    if(map[i,j]!=-1)
                    if (map[i, j] == 2)
                    {
                            b = b + 1;
                            max = map[i,j];
                    }
                    if (b == 3)
                    {
                        if (map[i, j] > max)
                        {
                            d = d + 1;
                            max++;
                            map[i,j] = 0;
                        }
                       
                    }
                    
                    Console.Write(map[i, j] + "\t");
                }
            
                d++;
                Console.WriteLine();
            }
                        