OSMServer/Pathfinding/Pathfinder.cs

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

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, double angleWeightFactor)
    {
        DateTime startCalc = DateTime.Now;
        _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>(),0 ,0);
            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))
            {
                TimeSpan calcTime = DateTime.Now - startCalc;
                pathResult = GetPath(goalNode, calcTime);
                Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
                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, angleWeightFactor);
                        //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>(),0 ,0);
        return this;
    }
    
    private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge)
    {
        double distance = Utils.DistanceBetween(currentNode, neighborNode);
        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
        
        double angle = 1;
        if (_cameFromDict!.ContainsKey(currentNode))
        {
            OsmNode previousNode = _cameFromDict[currentNode];
            Vector v1 = new(currentNode, previousNode);
            Vector v2 = new(currentNode, neighborNode);
            double nodeAngle = v1.Angle(v2);
            angle = (nodeAngle / 180);
        }
        double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode);
        return distance / (1 + speed * angle);
    }

    private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)
    {
        double roadPriority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode) * roadPriorityFactor;
        if (roadPriority == 0)
            return double.MaxValue;
        
        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType) * 0.0003;
        
        TagManager curTags = regionManager.GetRegion(currentNode.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;
        
        double angle = 0;
        if (_cameFromDict!.ContainsKey(currentNode))
        {
            OsmNode previousNode = _cameFromDict[currentNode];
            Vector v1 = new(previousNode, currentNode);
            Vector v2 = new(currentNode, neighborNode);
            double nodeAngle = v1.Angle(v2);
            angle = ((180 - nodeAngle) / 180) * nodeAngleFactor;
        }
        return Utils.DistanceBetween(neighborNode, goalNode) / (roadPriority + sameRoadName + junctionCount + angle);
    }

    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;
        double retDistance = 0;
        double weight = 0;
        while (_cameFromDict!.ContainsKey(_cameFromDict[currentNode]))
        {
            OsmEdge? currentEdge = _cameFromDict[currentNode].edges.FirstOrDefault(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);

            double distance = Utils.DistanceBetween(currentNode, _cameFromDict[currentNode]);
            retDistance += distance;
            weight += regionManager.GetSpeedForEdge(_cameFromDict[currentNode], currentEdge.wayId, _speedType);
            
            currentNode = _cameFromDict[currentNode];
        }

        path.Reverse();

        return new PathResult(calcFinished, path, retDistance, retDistance / (weight / path.Count));
    }

    private class Vector
    {
        public readonly float x, y;

        public Vector(float x, float y)
        {
            this.x = x;
            this.y = y;
        }

        public Vector(OsmNode n1, OsmNode n2)
        {
            this.x = n1.coordinates.longitude - n2.coordinates.longitude;
            this.y = n1.coordinates.latitude - n2.coordinates.latitude;
        }

        public double Angle(Vector v2)
        {
            return Angle(this, v2);
        }

        public static double Angle(Vector v1, Vector v2)
        {
            double dotProd = v1.x * v2.x + v1.y * v2.y;
            double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y);
            double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y);
            double ang = Math.Acos(dotProd / (v1L * v2L));
            double angle = Utils.RadiansToDegrees(ang);
            return angle;
        }
    }
}