OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMDatastructure.Graph;
using RenderPath;
using static OSMDatastructure.Tag;
using WayType = OSMDatastructure.Tag.WayType;

namespace Pathfinding;

public static class Pathfinder
{
    
    public static List<PathNode> AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates,
        SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
        double heuristicFewJunctionsPriority)
    {
        RegionManager regionManager = new RegionManager(workingDir);
        OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
        OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
        if (startNode is null || goalNode is null)
            return new List<PathNode>();

        PriorityQueue<OsmNode, double> openSetfScore = new();
        openSetfScore.Enqueue(startNode, 0);
        Dictionary<OsmNode, OsmNode> cameFromDict = new();
        Dictionary<OsmNode, double> gScore = new();
        gScore.Add(startNode, 0);

        while (openSetfScore.Count > 0)
        {
            OsmNode currentNode = openSetfScore.Dequeue();
            if (currentNode.Equals(goalNode))
            {
                Console.WriteLine("Path found.");
                List<PathNode> path = GetPath(cameFromDict, goalNode, regionManager);
                Console.WriteLine("Rendering.");
                List<Coordinates> coords = new();
                foreach(PathNode pn in path)
                    coords.Add(pn.coordinates);
                Renderer.DrawLoadedNodes(gScore, coords, workingDir);
                Console.WriteLine("Done.");
                return path;
            }

            foreach (OsmEdge edge in currentNode.edges)
            {
                OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                if (neighbor is not null)
                {
                    double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager);
                    gScore.TryAdd(neighbor, double.MaxValue);
                    if (tentativeGScore < gScore[neighbor])
                    {
                        if (cameFromDict.ContainsKey(neighbor))
                            cameFromDict[neighbor] = currentNode;
                        else
                            cameFromDict.Add(neighbor, currentNode);
                        if (gScore.ContainsKey(neighbor))
                            gScore[neighbor] = tentativeGScore;
                        else
                            gScore.Add(neighbor, tentativeGScore);
                        double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager,
                            heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
                        //Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}");
                        openSetfScore.Enqueue(neighbor, tentativeGScore + h);
                        
                    }
                }
            }
        }

        return new List<PathNode>();
    }

    private static List<PathNode> GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, RegionManager regionManager)
    {
        List<PathNode> path = new List<PathNode>();
        OsmNode currentNode = goalNode;
        while (cameFromDict.ContainsKey(cameFromDict[currentNode]))
        {
            OsmEdge? currentEdge = cameFromDict[currentNode].edges.First(edge => edge.neighborId == currentNode.nodeId);
            HashSet<Tag>? tags =
                regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge.wayId);
            PathNode? newNode = PathNode.FromOsmNode(currentNode, tags);
            if(newNode is not null)
                path.Add(newNode);
            currentNode = cameFromDict[currentNode];
        }

        path.Reverse();

        return path;
    }

    private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
    {
        double distance = Utils.DistanceBetween(fromNode, neighborNode);
        double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle);
        double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
        return distance / (speed + prio * 50);
    }

    private static double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
    {
        double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
        
        TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager;
        TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;

        bool sameName = false;
        string? curName = (string?)curTags.GetTag(edge.wayId, TagType.name);
        bool sameRef = false;
        string? curRef = (string?)curTags.GetTag(edge.wayId, TagType.tagref);
        if(curName is not null)
            foreach (OsmEdge pEdge in neighborNode.edges)
            {
                if ((string?)nextTags.GetTag(pEdge.wayId, TagType.name) == curName)
                    sameName = true;
                if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
                    sameRef = true;
            }
        double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;

        double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
        
        //Console.WriteLine($"{roadPriority:000.00} {sameRoadName:000.00} {junctionCount:000.00} {distanceImprovement:+000.00;-000.00;0000.00}");

        return Utils.DistanceBetween(neighborNode, goalNode) - (roadPriority + sameRoadName + junctionCount) * 20;
    }
    
    private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
    {
        if (vehicle == SpeedType.any)
            return 1;
        WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
        if(wayType is null)
            return 0;
        if (vehicle == SpeedType.car)
        {
            switch (wayType)
            {
                case WayType.motorway:
                case WayType.motorway_link:
                case WayType.motorroad:
                case WayType.trunk:
                case WayType.trunk_link:
                case WayType.primary:
                case WayType.primary_link:
                    return 8;
                case WayType.secondary:
                case WayType.secondary_link:
                    return 6;
                case WayType.tertiary:
                case WayType.tertiary_link:
                    return 5;
                case WayType.unclassified:
                case WayType.residential:
                case WayType.road:
                case WayType.living_street:
                    return 2;
                case WayType.service:
                case WayType.track:
                    return 0.01;
            }
        }
        if (vehicle == SpeedType.pedestrian)
        {
            switch (wayType)
            {
                case WayType.pedestrian:
                case WayType.corridor:
                case WayType.footway:
                case WayType.path:
                case WayType.steps:
                case WayType.residential: 
                case WayType.living_street:
                    return 10;
                case WayType.service:
                case WayType.cycleway:
                case WayType.bridleway:
                case WayType.road:
                case WayType.track:
                case WayType.unclassified:
                    return 5;
                case WayType.tertiary:
                case WayType.tertiary_link:
                case WayType.escape:
                    return 2;
            }
        }

        return 0.01;
    }
}