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 PathResult? pathResult;
    public Dictionary<OsmNode, double>? gScore;
    private Dictionary<OsmNode, OsmNode>? _cameFromDict;
    private SpeedType _speedType;
    private double roadPriorityFactor, turnAngle;

    public Pathfinder(string workingDirectory, double roadPriorityFactor, double turnAngle)
    {
        if (!Path.Exists(workingDirectory))
            throw new DirectoryNotFoundException(workingDirectory);
        regionManager = new(workingDirectory);
        this.roadPriorityFactor = roadPriorityFactor;
        this.turnAngle = turnAngle;
    }

    public Pathfinder(RegionManager regionManager, double roadPriorityFactor, double turnAngle)
    {
        this.regionManager = regionManager;
        this.roadPriorityFactor = roadPriorityFactor;
        this.turnAngle = turnAngle;
    }
    
    public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle)
    {
        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;
        }

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

        bool found = false;
        bool stop = false;
        TimeSpan firstFound = TimeSpan.MaxValue;
        double maxGscore = double.MaxValue;

        while (openSetfScore.Count > 0 && !stop)
        {
            OsmNode currentNode = openSetfScore.Dequeue()!;
            if (currentNode.Equals(goalNode))
            {
                if (!found)
                {
                    firstFound = DateTime.Now - startCalc;
                    found = true;
                }
                maxGscore = gScore[goalNode];
                /*
                int removed = openSetfScore.Remove(gScore.Where(item => item.Value > maxGscore)
                    .ToDictionary(item => item.Key, item => item.Value).Keys);
                Console.WriteLine($"Removed {removed}");*/
            }

            if (found && DateTime.Now - startCalc > firstFound.Multiply(2))
                stop = true;

            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 ((!found || (found && tentativeGScore < maxGscore)) && tentativeGScore < gScore[neighbor])
                    {
                        if(!_cameFromDict.TryAdd(neighbor, currentNode))
                            _cameFromDict[neighbor] = currentNode;
                        gScore[neighbor] = tentativeGScore;
                        double h = Heuristic(currentNode, neighbor, goalNode, edge);
                        openSetfScore.Enqueue(neighbor, tentativeGScore + h);
                    }
                }
            }
        }
        
        TimeSpan calcTime = DateTime.Now - startCalc;
        if (!found)
        {
            pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0);
            Console.Write("No path found.");
            return this;
        }
        else
        {
            pathResult = GetPath(goalNode, calcTime);
        }
        Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
        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);
            if (nodeAngle < turnAngle)
                angle = 0;
            else
                angle = nodeAngle / 180;
        }
        double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode) * roadPriorityFactor;

        return distance / (speed * angle + prio + 1);
    }

    private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge)
    {
        if (neighborNode.Equals(goalNode)) return 0;
        double priority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode);
        if (priority == 0)
            return double.MaxValue;

        double distance = Utils.DistanceBetween(neighborNode, goalNode);
        
        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
        
        double roadPriority = priority * roadPriorityFactor;
        
        double angle = 0;
        if (_cameFromDict!.ContainsKey(currentNode))
        {
            OsmNode previousNode = _cameFromDict[currentNode];
            Vector v1 = new(currentNode, previousNode);
            Vector v2 = new(currentNode, neighborNode);
            double nodeAngle = v1.Angle(v2);
            if (nodeAngle < turnAngle)
                angle = 0;
            else
                angle = nodeAngle / 180;
        }

        return distance / (speed * angle + roadPriority + 1);
    }

    public void SaveResult(string path)
    {
        if(File.Exists(path))
            File.Delete(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));
            if (ang.Equals(double.NaN))
                return 0;
            double angle = Utils.RadiansToDegrees(ang);
            return angle;
        }
    }
}