OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMImporter;

namespace Pathfinding;

public class Pathfinder
{

    public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
    {
        RegionManager regionManager = new RegionManager(workingDir);
        Region startRegion, goalRegion;
        PathNode startNode, goalNode;
        try
        {
            startRegion = regionManager.GetRegion(start);
            startNode = PathNode.FromNode(ClosestNodeToCoordinates(start, startRegion)!); //TODO null handling
        }
        catch (FileNotFoundException e)
        {
            throw new Exception(string.Format("No region at coordinates {0}", start), e);
        }
        try
        {
            goalRegion = regionManager.GetRegion(goal);
            goalNode = PathNode.FromNode(ClosestNodeToCoordinates(goal, goalRegion)!); //TODO null handling
        }
        catch (FileNotFoundException e)
        {
            throw new Exception(string.Format("No region at coordinates {0}", start), e);
        }
        
        List<PathNode> toVisit = new() { startNode };
        startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
        bool stop = false;

        while (toVisit.Count > 0 && !stop)
        {
            PathNode closestNodeToGoal = toVisit.First();
            foreach (PathNode node in toVisit)
            {
                if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
                {
                    closestNodeToGoal = node;
                }
            }

            foreach (Connection connection in closestNodeToGoal.GetConnections())
            {
                PathNode? neighbor = (PathNode)regionManager.GetNode(connection.endNodeCoordinates);
                if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
                {
                    neighbor.previousPathNode = closestNodeToGoal;
                    neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
                                                 Utils.DistanceBetween(closestNodeToGoal, neighbor);
                    
                    if (neighbor.Equals(goalNode))
                        stop = true;
                    else
                        toVisit.Add(neighbor);
                }
            }
        }

        List<PathNode> path = new();
        PathNode currentNode = goalNode;
        while (!currentNode.Equals(startNode))
        {
            path.Add(currentNode);
            currentNode = currentNode.previousPathNode!;
        }
        path.Add(startNode);

        return path;
    }
    
    private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
    {
        ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region);
        return closestId != null ? region.GetNode((ulong)closestId) : null;
    }
    
    private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region)
    {
        ulong? closestId = null;
        double closestDistance = double.MaxValue, distance;

        foreach (KeyValuePair<ulong, Node> kv in region.GetNodes())
        {
            distance = Utils.DistanceBetween(kv.Value, coordinates);
            if (distance < closestDistance)
            {
                closestDistance = distance;
                closestId = kv.Key;
            }
        }
        return closestId;
    }
}