using RobotNet.MapShares.Dtos;

namespace RobotNet.RobotManager.Services.Planner.Space;

public class KDTree(List<NodeDto> nodes)
{
    private readonly KDTreeNode? Root = BuildTree(nodes, 0);

    private static KDTreeNode? BuildTree(List<NodeDto> nodes, int depth)
    {
        if (nodes.Count == 0) return null;
        int axis = depth % 2;
        nodes.Sort((a, b) => axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y));
        int median = nodes.Count / 2;

        return new KDTreeNode
        {
            Node = nodes[median],
            Axis = axis,
            Left = BuildTree(nodes.GetRange(0, median), depth + 1),
            Right = BuildTree(nodes.GetRange(median + 1, nodes.Count - median - 1), depth + 1)
        };
    }

    public NodeDto? FindNearest(double x, double y, double limitDistance)
    {
        if (Root is null) return null;
        var (node, dist) = Nearest(Root, x, y, null, double.MaxValue);
        return dist <= limitDistance ? node : null;
    }

    private static (NodeDto?, double) Nearest(KDTreeNode? node, double x, double y, NodeDto? best, double bestDist)
    {
        if (node == null) return (best, bestDist);

        double d = Math.Sqrt(Math.Pow(node.Node.X - x, 2) + Math.Pow(node.Node.Y - y, 2));
        if (d < bestDist)
        {
            best = node.Node;
            bestDist = d;
        }

        double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
        KDTreeNode? nearSide = delta < 0 ? node.Left : node.Right;
        KDTreeNode? farSide = delta < 0 ? node.Right : node.Left;

        (best, bestDist) = Nearest(nearSide, x, y, best, bestDist);
        if (Math.Abs(delta) < bestDist)
            (best, bestDist) = Nearest(farSide, x, y, best, bestDist);

        return (best, bestDist);
    }
}

public class KDTreeNode
{
    public NodeDto Node { get; set; } = new();
    public KDTreeNode? Left { get; set; }
    public KDTreeNode? Right { get; set; }
    public int Axis { get; set; } // 0 for X, 1 for Y
}
