OSMServer/Pathfinding/Pathfinder_Time.cs

using OSMDatastructure;
using OSMDatastructure.Graph;
using Utils = OSMDatastructure.Utils;

namespace Pathfinding;

public static partial class Pathfinder
{
    public static List<PathNode> AStarTime(string workingDir, Coordinates start,
        Coordinates goal, Tag.SpeedType vehicle)
    {
        RegionManager regionManager = new (workingDir);
        ValueTuple<OsmNode?, OsmNode?> startAndEndNode = SetupNodes(start, goal, regionManager);
        if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null)
            return new List<PathNode>();
        OsmNode goalNode = startAndEndNode.Item2!;

        PriorityQueue<OsmNode, double> toVisit = new();
        toVisit.Enqueue(startAndEndNode.Item1, 0);
        bool stop = false;

        while (toVisit.Count > 0)
        {
            OsmNode currentNode = toVisit.Dequeue();
            
            foreach (OsmEdge edge in currentNode.edges.Where(
                         edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle)))
            {
                OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                if (neighbor is not null)
                {
                    if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
                    
                    double newPotentialWeight = currentNode.currentPathWeight +
                                                EdgeWeight(currentNode, edge, vehicle, regionManager);
                    
                    if (newPotentialWeight < neighbor.currentPathWeight)
                    {
                        neighbor.previousPathNode = currentNode;
                        neighbor.currentPathWeight = newPotentialWeight;

                        if (neighbor.Equals(goalNode))
                        {
                            currentNode = neighbor;
                            currentNode.currentPathLength = 0;
                            while (currentNode != startAndEndNode.Item1 && currentNode.previousPathNode is not null)
                            {
                                currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength +
                                    Utils.DistanceBetween(currentNode, currentNode.previousPathNode);
                                currentNode = currentNode.previousPathNode;
                            }
                                
                            return GetRouteFromCalc(goalNode, regionManager);
                        }
                            //stop = true;
                        if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)))
                            toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager));
                    }
                }
            }
        }
        return GetRouteFromCalc(goalNode, regionManager);
    }
}