【算法】万圣节前夕的迷宫挑战(二)

在十月底一个阳光明媚的周末，小悦开始她的徒步旅行，一头高高的马尾轻轻摇曳，充满了青春的活力。她的笑容如同春日的阳光，温暖而明亮，总是让人心情愉悦。那天的徒步旅行，她选择了一条山区路线，期望能欣赏到秋天那五彩斑斓的树叶和感受大自然的魅力。

旅途中，小悦遇到了一些意料之外的障碍。她发现自己的体力迅速流失，山路比她想象的要陡峭得多。每走一步，她都需要调整自己的步伐和呼吸，以更好地应对挑战。面对这些困难，她知道，除了身体的锻炼，还有心态的调整。为了继续前行，她需要保持积极乐观的态度。

小悦继续她的徒步旅行，欣赏着秋天的美景，同时也感受到了自己的成长和进步。回想起上周解决的迷宫障碍算法，她意识到这次的山区迷宫有一个重要的不同之处——体力的因素。山区迷宫不仅有曲折的小道和死胡同，还有陡峭的山坡和崎岖的山路。为了成功找到出路，小悦需要考虑自己的体力和耐力。她需要找到一条既能避开障碍物，又能最大限度地节省体力的路径。

于是，小悦开始仔细观察山区迷宫的地形和环境。她发现有些路径虽然看似直接，但是却需要爬过一个个陡峭的山坡，消耗大量体力。而有些路径虽然绕远了一些，但是却相对平缓，更加节省体力。为了成功找到出路并节省体力，小悦权衡利弊，选择了一条既能躲避障碍，又能最大限度地节省体力的路径。

她开始运用在徒步旅行中学到的技巧和知识，结合上星期解决迷宫的算法，寻找最佳路径。通过不断尝试和调整，她成功地避开了陡峭的山坡，选择相对平缓的山路。虽然这样的选择可能会绕远一些，但她相信这是最明智的决策。

在山区迷宫中艰难前行时，小悦感到体力不支，但她坚持下去。她知道只有克服体力的挑战，才能找到正确的出路。随着时间的推移，小悦的身体逐渐适应了徒步旅行的挑战。她的肌肉变得更加结实，体力也得到了提升。她的健美身材和流畅的步伐吸引了路人的目光。

最终，小悦成功地走出了山区迷宫。她感到一种前所未有的成就感和满足感。这次徒步旅行不仅是一次身体上的挑战，更是一次心理上和体力上的双重考验。通过这次旅行，小悦学会了在困难和挑战面前综合考虑各种因素做出明智的决策。

---

小悦在徒步时面临的问题是：她在二维数组NxN山区的起始位置[0,0]，只能在四个主要方向之一（即北、东、南、西）移动。需要移动到目标位置[N-1，N-1]，计算出最小的爬山量(体力)。相邻位置之间的爬山量(体力)定义为位置高度差（上升2或下降2）。
位置海拔高度定义为一个整数（0-9），代表上山和下山需要的体力。

图解：

      

---

 算法实现1：

    public static int PathFinder(string maze)
    {
        var M = maze.Replace("\n", "").Select(c => (int)c).ToArray(); // 将迷宫字符串转换为整数数组
        var N = (int)Math.Sqrt(M.Length); // 迷宫的维度
        var C = new int[M.Length]; // 每个节点的移动成本

        Array.Fill(C, int.MaxValue, 1, M.Length - 1);

        var Q = new List<int> { 0 }; // 定义队列

        while (Q[0] != M.Length - 1) // 当队列Q的第一个节点不是终点时，继续循环
        {
            var index = Q.First(); // 取出队列Q的第一个节点
            Q.RemoveAt(0); // 将队列Q的第一个节点移除

            foreach (var n in Neighbours(index, M, N)) // 遍历当前节点的邻居节点
            {
                int nc = C[index] + Math.Abs(M[n] - M[index]); // 计算从当前节点到邻居节点的新的移动成本
                if (C[n] > nc) // 如果邻居节点的当前移动成本大于新的移动成本
                {
                    C[n] = nc; // 更新邻居节点的移动成本
                    Insert(Q, C, n); // 将邻居节点插入到队列Q中
                }
            }
        }

        return C[Q[0]];
    }

    static void Insert(List<int> Q, int[] C, int index)
    {
        for (int i = 0; i < Q.Count; i++) // 遍历队列Q中的节点
        {
            if (C[Q[i]] > C[index]) // 如果当前节点的移动成本小于队列Q中的某个节点的移动成本
            {
                Q.Insert(i, index); // 将当前节点插入到队列Q中
                return;
            }
        }
        Q.Add(index); // 如果当前节点的移动成本大于队列Q中的所有节点的移动成本，将当前节点添加到队列Q的末尾
    }

    static List<int> Neighbours(int index, int[] M, int N)
    {
        var neighbours = new List<int>();

        if (index % N != 0) neighbours.Add(index - 1);      // 左边的邻居
        if (index % N != N - 1) neighbours.Add(index + 1);      // 右边的邻居
        if (index + N < M.Length) neighbours.Add(index + N);      // 下面的邻居
        if (index - N >= 0) neighbours.Add(index - N);      // 上面的邻居

        return neighbours;
    }

这个算法是一个基于Dijkstra算法的寻路算法，Dijkstra算法是由荷兰计算机科学家艾兹格·迪杰斯特拉在1956年提出的，因此又叫迪杰斯特拉算法。该算法是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。它的核心思想是利用优先队列来实现最短路径的搜索，同时通过引入启发式函数来加速搜索过程。在这个算法中，每个节点的权重是从起点到该节点的移动成本，而启发式函数则是从该节点到终点的估计成本。通过不断更新每个节点的移动成本和启发式函数，算法能够找到从起点到终点的最短路径。这个算法的优点是可以处理任意形状的迷宫，并且具有较好的时间复杂度。

通过测试用例解释这个基于Dijkstra算法的寻路算法：
var b = "010\n" +
"010\n" +
"010",

var c = "010\n" +
"101\n" +
"010",
Assert.AreEqual(2, Finder.PathFinder(b));
Assert.AreEqual(4, Finder.PathFinder(c));

基于Dijkstra算法的寻路算法是一种用于在加权图中找到节点之间最短路径的算法。它通过迭代计算每个节点的距离，从起点节点开始，直到到达终点节点。下面使用测试用例来解释这个算法的工作原理。

对于迷宫地图b，它的结构如下：

010

010

010

我们要找到从起点到终点的最短路径。根据Dijkstra算法的步骤，我们首先将起点节点标记为距离为0，然后计算与起点相邻的节点的距离。在这个例子中，起点节点是(0,0)，它相邻的节点是(1,0)和(0,1)。我们计算这些节点的距离，并选择距离最短的节点(1,0)作为下一个访问节点。然后，我们继续计算与(1,0)相邻的节点的距离，选择距离最短的节点作为下一个访问节点。以此类推，直到到达终点节点(2,2)。在这个过程中，我们记录下每个节点的距离，并选择最短路径。

对于迷宫地图b，最短路径为： (0,0) -> (1,0) -> (2,0) -> (2,1) -> (2,2)

这条路径的爬坡体力为2。

对于迷宫地图c，它的结构如下：

010

101

010

同样地，我们要找到从起点到终点的最短路径。根据Dijkstra算法的步骤，我们首先将起点节点标记为距离为0，然后计算与起点相邻的节点的距离。在这个例子中，起点节点是(0,0)，它相邻的节点是(1,0)和(0,1)。我们计算这些节点的距离，并选择距离最短的节点(1,0)作为下一个访问节点。然后，我们继续计算与(1,0)相邻的节点的距离，选择距离最短的节点作为下一个访问节点。以此类推，直到到达终点节点(2,2)。在这个过程中，我们记录下每个节点的距离，并选择最短路径。

对于迷宫地图c，最短路径为： (0,0) -> (1,0) -> (1,1) -> (2,1) -> (2,2)

这条路径的爬坡体力为4。

因此，基于Dijkstra算法的寻路算法能够找到从起点到终点的最短路径，并返回路径的爬坡体力。

注：算法1中并没有直接计算体力花费，而是根据题目给定的规则，将体力花费转化为了路径的代价。在这个算法中，代价的计算方式是通过累加每个节点之间的高度差来计算的。

具体来说，这个算法中的代价（C数组）表示从起点到每个节点的路径代价，也就是到达该节点需要的体力花费。在每次更新路径代价时，会根据当前节点和邻居节点的高度差来计算新的代价。这里使用了Math.Abs方法来计算高度差的绝对值，然后将其累加到路径代价中。

在算法的主循环中，通过遍历起点的邻居节点，计算新的路径代价，并将其与原来的路径代价进行比较。如果新的路径代价更小，则更新路径代价并将邻居节点插入到队列中。这样，算法会不断更新路径代价，直到找到终点或队列为空。

---

算法实现2：

    public static int PathFinder(string maze)
    {
        // 将迷宫字符串按行分割，并将每个字符转换为数字，存储在一个二维数组中
        var m = maze.Split('\n').Select(s => s.Select(c => int.Parse("" + c)).ToArray()).ToArray();
        var n = m.Length;
        var points = new Point[n, n]; // 创建一个二维数组，存储每个点的位置、启发值和移动距离
        int i, j;
        var neighbors = new List(); // 创建一个列表，存储当前点的相邻点

        // 初始化每个点的位置和启发值
        for (i = 0; i < n; ++i)
            for (j = 0; j < n; ++j)
                points[i, j] = new Point { I = i, J = j, Heuristic = (n - i) + (n - j) };

        var heap = new BinaryHeap(); // 创建一个二叉堆，用于存储和管理点对象
        points[0, 0].Moves = 0; // 设置起点的移动距离为0
        heap.Add(points[0, 0]); // 将起点加入二叉堆中

        while (heap.Count != 0)
        {
            var here = heap.Remove(); // 取出二叉堆中的最小值，即当前离起点最近的点
            i = here.I;
            j = here.J;

            if (i == n - 1 && j == n - 1) // 如果当前点为终点，返回从起点到该点的移动距离
                return here.Moves;

            neighbors.Clear(); // 清空相邻点列表
                               // 找出当前点的四个相邻点，并加入相邻点列表
            if (i + 1 < n) neighbors.Add(points[i + 1, j]);
            if (j + 1 < n) neighbors.Add(points[i, j + 1]);
            if (i - 1 >= 0) neighbors.Add(points[i - 1, j]);
            if (j - 1 >= 0) neighbors.Add(points[i, j - 1]);

            // 遍历相邻点列表
            foreach (var neighbor in neighbors)
            {
                // 计算从当前点到相邻点的移动距离
                var moves = here.Moves + Math.Abs(m[i][j] - m[neighbor.I][neighbor.J]);

                if (moves < neighbor.Moves) // 如果新的移动距离小于相邻点的移动距离
                {
                    neighbor.Moves = moves; // 更新相邻点的移动距离
                    neighbor.Score = moves + neighbor.Heuristic; // 更新相邻点的得分
                    heap.AddOrUpdate(neighbor); // 将相邻点加入或更新到二叉堆中
                }
            }
        }

        return -1; // 如果循环结束仍未找到终点，则返回-1
    }

    public class Point : IComparable
    {
        public int I { get; set; }
        public int J { get; set; }
        public int Score { get; set; } = int.MaxValue;
        public int Moves { get; set; } = int.MaxValue;
        public int Heuristic { get; set; }

        public int CompareTo(Point other)
        {
            return Score.CompareTo(other.Score);
        }
    }

    public class BinaryHeap where T : IComparable
    {
        private List _heap = new List(); // 存储堆中的元素
        private Dictionaryint> _items = new Dictionaryint>(); // 存储元素在堆中的索引

        public int Count => _heap.Count; // 堆中元素的个数

        public bool Contains(T item) => _items.ContainsKey(item); // 判断堆中是否包含指定元素

        public void Add(T item) // 向堆中添加元素
        {
            if (Contains(item)) throw new Exception("Item already added."); // 如果堆中已经包含该元素，则抛出异常
            _heap.Add(item); // 将元素添加到堆中
            UpdateIndices(Count - 1); // 更新元素在堆中的索引
            BubbleUp(_heap.Count - 1); // 将新添加的元素进行上浮操作
        }

        public T Peek() => _heap.FirstOrDefault(); // 获取堆顶元素

        public T Remove() // 移除并返回堆顶元素
        {
            if (_heap.Count == 0) return default(T); // 如果堆为空，则返回默认值
            var top = _heap.First(); // 获取堆顶元素
            var last = _heap.Last(); // 获取堆中最后一个元素
            _heap.RemoveAt(_heap.Count - 1); // 移除堆中最后一个元素
            _items.Remove(top); // 移除堆顶元素在索引字典中的记录
            if (_heap.Count == 0)
                return top;
            _heap[0] = last; // 将最后一个元素放到堆顶
            BubbleDown(0); // 将堆顶元素进行下沉操作
            return top;
        }

        public void AddOrUpdate(T item) // 添加或更新元素
        {
            if (_items.ContainsKey(item)) // 如果元素已经存在于堆中
            {
                BubbleUp(_items[item]); // 进行上浮操作
                BubbleDown(_items[item]); // 进行下沉操作
            }
            else Add(item); // 否则，将元素添加到堆中
        }

        private void BubbleUp(int index) // 上浮操作
        {
            var i = index;
            var p = (i - 1) / 2; // 计算父节点索引
            while (i > 0 && i < _heap.Count && _heap[i].CompareTo(_heap[p]) < 0) // 当前节点小于父节点时进行交换
            {
                Swap(i, p); // 交换当前节点和父节点
                i = p; // 更新当前节点索引为父节点索引
                p = (i - 1) / 2; // 更新父节点索引
            }
        }

        private void BubbleDown(int index) // 下沉操作
        {
            int i = index;
            int c = i * 2 + 1; // 计算左子节点索引
            bool swapped = true;
            while (c < _heap.Count && swapped) // 当左子节点存在且发生交换时继续下沉
            {
                if (c + 1 < _heap.Count && _heap[c + 1].CompareTo(_heap[c]) < 0) // 如果右子节点存在且小于左子节点
                    ++c; // 更新子节点索引为右子节点索引
                if (_heap[i].CompareTo(_heap[c]) > 0) // 当前节点大于子节点时进行交换
                {
                    Swap(i, c); // 交换当前节点和子节点
                    i = c; // 更新当前节点索引为子节点索引
                    c = i * 2 + 1; // 更新子节点索引
                }
                else swapped = false; // 如果当前节点小于等于子节点，则停止下沉
            }
        }

        private void Swap(int i, int j) // 交换元素位置
        {
            var t = _heap[i];
            _heap[i] = _heap[j];
            _heap[j] = t;
            UpdateIndices(i, j); // 更新交换后元素的索引
        }

        private void UpdateIndices(params int[] indices) // 更新元素在索引字典中的记录
        {
            foreach (var i in indices)
            {
                _items[_heap[i]] = i;
            }
        }
    }

算法2中的PathFinder方法使用了A*算法来解决这个问题。

A*算法是一种经典的启发式搜索算法，用于解决图搜索问题。它由Peter Hart、Nils Nilsson和Bertram Raphael于1968年提出，并在1972年由Peter Hart、Nils Nilsson和Bertram Raphael发表了一篇论文。

A*算法的产生原因是为了解决传统的图搜索算法在搜索空间较大时效率低下的问题。传统的图搜索算法，如深度优先搜索和广度优先搜索，会遍历所有可能的路径，直到找到目标节点。这种方法在搜索空间较大时，会浪费大量时间和计算资源。

A*算法通过引入启发式函数来优化搜索过程。启发式函数是一种估计从当前节点到目标节点的代价的函数。A*算法使用启发式函数来选择下一步移动的方向，以尽可能快地接近目标节点。

A*算法的核心思想是维护一个优先队列（通常使用二叉堆实现），按照启发式函数的值对节点进行排序。在每一步中，选择优先队列中具有最小启发式函数值的节点作为当前节点，然后将其周围的节点加入优先队列。通过不断选择具有最小启发式函数值的节点，A算法可以在较短的时间内找到从起点到目标节点的最短路径。

A**算法在解决各种图搜索问题中表现出色，并且已经被广泛应用于路径规划、游戏AI、人工智能等领域。它的优势在于能够在较短的时间内找到最优解，并且可以通过调整启发式函数来适应不同的问题和需求。

---

 对比算法1和算法2，Dijkstra算法和A*算法都是用于解决图搜索问题的算法，但它们在搜索策略和效率上有一些区别。

 

• 搜索策略：

 

1. • Dijkstra算法是一种无信息搜索算法，它会遍历所有可能的路径，直到找到目标节点。它将图中的节点按照距离起点的距离进行排序，并选择距离最小的节点作为当前节点进行扩展。这种搜索策略确保找到的路径是最短路径，但可能会遍历大量的无关节点。
   • A*算法是一种启发式搜索算法，它使用启发式函数来估计从当前节点到目标节点的代价。它根据启发式函数的值对节点进行排序，并选择具有最小启发式函数值的节点进行扩展。这种搜索策略可以帮助算法更快地接近目标节点，避免遍历大量无关节点。

 

• 效率：

 

1. • Dijkstra算法在最坏情况下的时间复杂度为O(|V|^2)，其中|V|是图中节点的数量。它需要遍历所有节点，并在每一步中选择距离起点最近的节点进行扩展。这使得Dijkstra算法在处理大规模图时效率较低。
   • A算法在最坏情况下的时间复杂度为O(|E|)，其中|E|是图中边的数量。它通过使用启发式函数来指导搜索，可以更快地找到最短路径。但是，A算法的效率高度依赖于启发式函数的质量。如果启发式函数不准确或不合理，A*算法可能会走弯路或陷入局部最优解

 

• 最短路径：

 

1. • Dijkstra算法保证找到的路径是最短路径，因为它遍历所有可能的路径并选择距离起点最近的节点进行扩展。这使得Dijkstra算法在寻找最短路径方面非常可靠。
   • A算法在启发式函数合理的情况下，也可以找到最短路径。启发式函数可以提供对目标节点的估计，帮助算法更快地接近目标节点。但是，如果启发式函数不准确或不合理，A算法可能会找到次优解或不完全最短路径。

 

• 走过路径的体力花费：

 

1. • Dijkstra算法没有考虑任何体力花费因素，它只关注路径的距离，如果需要计算体力花费，需要额外写路径代价的转化逻辑。因此，Dijkstra算法不能直接优化体力花费，而是只找到最短路径。
   • A算法可以通过合理选择启发式函数来考虑体力花费因素。启发式函数可以估计从当前节点到目标节点的体力花费，并在搜索过程中优先选择体力花费较低的节点。这使得A算法在考虑体力花费时更具优势。

综上所述，Dijkstra算法适用于需要找到最短路径的问题，并且对搜索空间的规模没有太高的要求。A*算法适用于大规模图搜索问题，并且可以通过合理选择启发式函数来提高搜索效率。在实际应用中，需要根据具体问题的特点和需求选择适合的算法。如果只关注最短路径，Dijkstra算法是一个可靠的选择。如果需要考虑体力花费，并且可以设计合理的启发式函数，A*算法可以更好地优化路径选择。然而，在实际应用中，需要根据具体问题的需求和约束来选择适合的算法。

---

 对比一周前的迷宫躲避障碍算法，在地图导航应用场景中，这四个算法（BFS、Dijkstra、A*和洪水递归算法）有以下区别：

1. 广度优先搜索（BFS）：BFS适用于无权图或迷宫中寻找最短路径。它能够找到最短路径，但没有考虑权重或启发式函数。BFS的时间复杂度为O(V+E)，其中V是顶点数，E是边数。

2. Dijkstra算法：Dijkstra算法适用于带有非负权重的图中寻找最短路径。它能够找到最短路径，并考虑了权重。Dijkstra算法的时间复杂度为O((V+E)logV)。Dijkstra算法适用于以下情况：

   • 地图中存在不同的道路或路径有不同的权重，比如某些道路有交通拥堵或者有收费（高速公路）。
   • 需要找到最短路径而不仅仅是一条路径。

3. A算法：A算法适用于带有启发式函数的图中寻找近似最短路径。它通过启发式函数来估计节点到终点的距离，并根据估计距离选择下一个要探索的节点。A算法的时间复杂度取决于启发式函数的准确性。A算法适用于以下情况：

   • 地图中存在不同的道路或路径有不同的权重，比如某些道路有交通拥堵或者有收费（高速公路）。
   • 地图中存在启发式函数可以提供对节点到终点距离的良好估计。
   • 需要找到近似最短路径，并且启发式函数能够提供较好的性能。

4. 洪水递归算法：洪水递归算法是一种简单的递归算法，它通过递归填充来更新每个位置的最小步数。洪水递归算法没有显式的启发式函数，只能找到从起点到终点的一条路径，但不一定是最短路径。洪水递归算法适用于以下情况：

   • 只需要找到一条路径，而不需要最短路径。
   • 地图中没有权重或者权重对结果影响不大。

除了地图导航，这四个算法（BFS、Dijkstra、A*和洪水递归算法）还有其他应用场景，例如：

1. BFS算法可以用于社交网络中的好友关系查找、迷宫游戏中的路径搜索、广告投放中的目标受众搜索等。

2. Dijkstra算法可以用于路由算法中的最短路径查找、电力系统中的电网最优化设计、通信网络中的数据传输优化等。

3. A*算法可以用于游戏AI中的路径规划、机器人导航中的路径规划、自动驾驶中的路径规划等。

4. 洪水递归算法可以用于图像处理中的区域填充、计算机视觉中的图像分割、自然语言处理中的词性标注等。

综上所述，这四个算法在许多领域都有广泛的应用，可以用于解决许多最短路径或路径规划问题。

---

测试用例：

using NUnit.Framework;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
public class SolutionTest
{
    [Test]
    public void SampleTests()
    {

        string a = "000\n" +
                   "000\n" +
                   "000",

               b = "010\n" +
                   "010\n" +
                   "010",

               c = "010\n" +
                   "101\n" +
                   "010",

               d = "0707\n" +
                   "7070\n" +
                   "0707\n" +
                   "7070",

               e = "700000\n" +
                   "077770\n" +
                   "077770\n" +
                   "077770\n" +
                   "077770\n" +
                   "000007",

               f = "777000\n" +
                   "007000\n" +
                   "007000\n" +
                   "007000\n" +
                   "007000\n" +
                   "007777",

               g = "000000\n" +
                   "000000\n" +
                   "000000\n" +
                   "000010\n" +
                   "000109\n" +
                   "001010";

        Assert.AreEqual(0, Finder.PathFinder(a));
        Assert.AreEqual(2, Finder.PathFinder(b));
        Assert.AreEqual(4, Finder.PathFinder(c));
        Assert.AreEqual(42, Finder.PathFinder(d));
        Assert.AreEqual(14, Finder.PathFinder(e));
        Assert.AreEqual(0, Finder.PathFinder(f));
        Assert.AreEqual(4, Finder.PathFinder(g));
    }

    [Test]
    public void RandomTests()
    {

        for (int nTimes = 0; nTimes < 6; nTimes++)
        {
            for (int n = 1; n < 101; n++)
            {
                var maze = GenerateMaze(n);
                Assert.AreEqual(RefFinder.PathFinder(maze), Finder.PathFinder(maze));
            }
        }
    }

    private Random rand = new Random();

    private string GenerateMaze(int n)
    {
        return string.Join("\n", Enumerable.Range(0, n).Select(x => string.Concat(Enumerable.Range(0, n).Select(v => $"{rand.Next(10)}"))));
    }

    private class RefFinder
    {
        private static List MOVES = new List { new Point(1, 0), new Point(0, 1), new Point(0, -1), new Point(-1, 0) };

        public static int PathFinder(string maze)
        {
            var aMazed = maze.Split("\n").Select(s => s.Select(i => i - 48).ToArray()).ToArray();
            int X = aMazed.Length - 1, Y = aMazed[0].Length - 1;
            Point pos = new Point(0, 0), end = new Point(X, Y);
            var q = new PriorityQueue();
            q.Push(new Tup(0, pos.Equals(end), pos));
            var seen = new Dictionaryint>() { [pos] = 0 };
            while (!end.Equals(q.Peek().Pos))
            {
                var curr = q.Pop();
                foreach (var move in MOVES)
                {
                    var x = curr.Pos.X + move.X;
                    var y = curr.Pos.Y + move.Y;
                    if (x >= 0 && x <= X && y >= 0 && y <= Y)
                    {
                        var p = new Point(x, y);
                        int nextCost = curr.Cost + Math.Abs(aMazed[curr.Pos.X][curr.Pos.Y] - aMazed[p.X][p.Y]);
                        var r = seen.TryGetValue(p, out var o);
                        if (!r)
                        {
                            seen.Add(p, nextCost);
                            q.Push(new Tup(nextCost, end.Equals(p), p));
                        }
                        else if (o > nextCost)
                        {
                            seen[p] = nextCost;
                            q.Push(new Tup(nextCost, end.Equals(p), p));
                        }
                    }
                }
            }
            return q.Peek().Cost;
        }

        /* ******************
              HELPER CLASS
           ******************/

        private class Tup : IComparable
        {
            public int Cost { get; }
            public bool IsEnd { get; }
            public Point Pos { get; }

            public Tup(int cost, bool isEnd, Point pos)
            {
                this.Pos = pos;
                this.Cost = cost;
                this.IsEnd = isEnd;
            }

            public override bool Equals(object obj)
            {
                if (obj == null || !(obj is Tup)) return false;
                var that = obj as Tup;
                return Cost == that.Cost && IsEnd == that.IsEnd && Pos == that.Pos;
            }

            public override string ToString() { return $"Tup({Cost}, {(IsEnd ? 1 : 0)}, ({Pos.X},{Pos.Y}))"; }

            public int CompareTo(Tup other)
            {
                return -(Cost != other.Cost ? Cost - other.Cost
         : IsEnd != other.IsEnd ? (IsEnd ? 1 : 0) - (other.IsEnd ? 1 : 0)
         : Pos.X != other.Pos.X ? Pos.X - other.Pos.X
         : Pos.Y - other.Pos.Y);
            }
        }
    }
}