OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMImporter;

namespace Pathfinding;

public class Pathfinder
{

    private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
    {
        RegionManager regionManager = new RegionManager(workingDir);
        Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
        PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
        Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
        PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
        
        
    }
    
    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;
    }
}