OSMServer/Pathfinding/Pathfinder.cs

using System.Text.Json;
using OSMDatastructure;
using OSMDatastructure.Graph;
using static OSMDatastructure.Tag;
using WayType = OSMDatastructure.Tag.WayType;

namespace Pathfinding;

public class Pathfinder
{

    public RegionManager regionManager;
    public readonly string workingDir;
    public PathResult? pathResult;
    public Dictionary<OsmNode, double>? gScore;
    private Dictionary<OsmNode, OsmNode>? cameFromDict;
    private SpeedType _speedType;

    public Pathfinder(string workingDirectory)
    {
        if (!Path.Exists(workingDirectory))
            throw new DirectoryNotFoundException(workingDirectory);
        regionManager = new(workingDirectory);
        workingDir = workingDirectory;
    }
    
    public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates,
        SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
        double heuristicFewJunctionsPriority)
    {
        DateTime startCalc = DateTime.Now;
        regionManager = new RegionManager(workingDir);
        _speedType = vehicle;
        OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, _speedType);
        OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, _speedType);
        if (startNode is null || goalNode is null)
        {
            pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
            return this;
        }

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

        while (openSetfScore.Count > 0)
        {
            OsmNode currentNode = openSetfScore.Dequeue();
            if (currentNode.Equals(goalNode))
            {
                Console.WriteLine("Path found.");
                this.pathResult = GetPath(goalNode, DateTime.Now - startCalc);
                return this;
            }

            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);
                    gScore.TryAdd(neighbor, double.MaxValue);
                    if (tentativeGScore < gScore[neighbor])
                    {
                        if(!cameFromDict.TryAdd(neighbor, currentNode))
                            cameFromDict[neighbor] = currentNode;
                        gScore[neighbor] = tentativeGScore;
                        double h = Heuristic(currentNode, neighbor, goalNode, edge,
                            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);
                    }
                }
            }
        }

        pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
        return this;
    }

    public void SaveResult(string path)
    {
        FileStream fs = new (path, FileMode.CreateNew);
        JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default);
        fs.Dispose();
        Console.WriteLine($"Saved result to {path}");
    }

    private PathResult GetPath(OsmNode goalNode, TimeSpan calcFinished)
    {
        List<PathNode> path = new();
        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 new PathResult(calcFinished, path);
    }

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

    private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
    {
        double roadPriority = GetPriorityVehicleRoad(edge, 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;
        
        return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount);
    }
    
    private double GetPriorityVehicleRoad(OsmEdge edge, Region region)
    {
        if (_speedType == SpeedType.any)
            return 1;
        WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
        if(wayType is null)
            return 0;
        if (_speedType == 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 10;
                case WayType.secondary:
                case WayType.secondary_link:
                    return 7;
                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.0001;
            }
        }
        if (_speedType == 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;
    }
}