public static long AStarWithMinPathLengthCheck(Matrix matrix, IProfiler profiler = null) { if (profiler == null) { profiler = IProfiler.Default; } var minPathLengths = new long[matrix.NumCols, matrix.NumRows]; for (int col = 0; col < matrix.NumCols; ++col) { for (int row = 0; row < matrix.NumRows; ++row) { minPathLengths[col, row] = long.MaxValue; } } var diagonalMins = Euler81.Program.DiagonalMins(matrix); var diagonalMinSums = diagonalMins.Reverse().PartialSums().Reverse().Append(0).ToArray(); Func <SearchState, long> heuristicFunc = s => s.pathSum + diagonalMinSums[s.currentLocation.col + s.currentLocation.row + 1]; //Func<SearchState, long> heuristicFunc = s => s.pathSum; // assume all values are minimum value Func <SearchState, IEnumerable <SearchState> > neighborFunc = s => NeighborFunc(s, matrix.NumCols, matrix.NumRows, heuristicFunc, minPathLengths); Func <SearchState, bool> goalFunc = s => s.currentLocation.col == matrix.NumCols - 1 && s.currentLocation.row == matrix.NumRows - 1; Euler.MinHeap <SearchState> searchFrontier = new Euler.MinHeap <SearchState>(); var startState = new SearchState { matrix = matrix, currentLocation = (col : 0, row : 0), locationsInPath = new HashSet <(int col, int row)>((col : 0, row : 0).Yield()), pathSum = matrix[0, 0] }; startState.heuristicValue = heuristicFunc(startState); searchFrontier.Add(startState); int numInspected = 0; while (searchFrontier.Count > 0) { SearchState next; using (profiler.Time("Pop search state")) { next = searchFrontier.Pop(); } ++numInspected; if (numInspected % 1 == 0) { // TODO: progress //Console.WriteLine($"{numInspected}/{numInspected+searchFrontier.Count}: {next.currentLocation}, {next.locationsInPath.Count}, {next.pathSum}, {next.heuristicValue}"); } if (goalFunc(next)) { // TODO: progress //Console.WriteLine($"Goal reached. Path sum: {next.pathSum}"); return(next.pathSum); } else { using (profiler.Time("Get neighbors and add to heap")) searchFrontier.AddRange(neighborFunc(next)); } } throw new Exception("Goal state not reached!"); }