BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/OpticsClusteringAlgorithm.cs

namespace RobotNet10.RobotApp.Detection;

/// <summary>
/// OPTICS (Ordering Points To Identify the Clustering Structure) clustering algorithm
/// Implements density-based clustering with spatial indexing optimizations
/// </summary>
public class OpticsClusteringAlgorithm(double eps, int minPts)
{
    private readonly List<Point> _points = [];
    private readonly List<int> _orderedList = [];
    private readonly Dictionary<long, List<int>> _grid = [];
    private double _gridCellSize = 0;
    private KDTree? _kdTree;

    /// <summary>
    /// KD-tree implementation for efficient radius searches
    /// </summary>
    private class KDTree
    {
        private class Node
        {
            public int Idx { get; set; }
            public int Left { get; set; } = -1;
            public int Right { get; set; } = -1;
        }

        private readonly List<Point> _pts;
        private readonly List<Node> _nodes;
        private readonly int _root;

        public KDTree(List<Point> points)
        {
            _pts = points;
            _nodes = [];
            _root = -1;

            if (points.Count > 0)
            {
                var idxs = Enumerable.Range(0, points.Count).ToList();
                _root = BuildRec(idxs, 0, idxs.Count - 1, 0);
            }
        }

        private int BuildRec(List<int> idxs, int l, int r, int depth)
        {
            if (l > r) return -1;

            int axis = depth % 2;
            int m = (l + r) / 2;

            // Partition based on axis
            idxs.Sort(l, r - l + 1, Comparer<int>.Create((a, b) =>
            {
                if (axis == 0)
                    return _pts[a].X.CompareTo(_pts[b].X);
                return _pts[a].Y.CompareTo(_pts[b].Y);
            }));

            int nodeIdx = _nodes.Count;
            _nodes.Add(new Node { Idx = idxs[m] });
            _nodes[nodeIdx].Left = BuildRec(idxs, l, m - 1, depth + 1);
            _nodes[nodeIdx].Right = BuildRec(idxs, m + 1, r, depth + 1);
            return nodeIdx;
        }

        public List<int> RadiusSearch(Point q, double radius)
        {
            var result = new List<int>();
            double r2 = radius * radius;
            SearchRec(_root, q, r2, 0, result);
            return result;
        }

        private void SearchRec(int nodeIdx, Point q, double r2, int depth, List<int> output)
        {
            if (nodeIdx < 0) return;

            var node = _nodes[nodeIdx];
            var p = _pts[node.Idx];

            double dx = q.X - p.X;
            double dy = q.Y - p.Y;
            double dist2 = dx * dx + dy * dy;

            if (dist2 <= r2)
                output.Add(node.Idx);

            int axis = depth % 2;
            double diff = axis == 0 ? dx : dy;

            if (diff <= 0)
            {
                SearchRec(node.Left, q, r2, depth + 1, output);
                if (diff * diff <= r2)
                    SearchRec(node.Right, q, r2, depth + 1, output);
            }
            else
            {
                SearchRec(node.Right, q, r2, depth + 1, output);
                if (diff * diff <= r2)
                    SearchRec(node.Left, q, r2, depth + 1, output);
            }
        }
    }

    /// <summary>
    /// Add a single point to the dataset
    /// </summary>
    public void AddPoint(double x, double y, double range, double alpha)
    {
        _points.Add(new Point(x, y, range, alpha));

        // If grid is enabled, insert into the grid
        if (_gridCellSize > 0)
        {
            int ix = (int)Math.Floor(x / _gridCellSize);
            int iy = (int)Math.Floor(y / _gridCellSize);
            long key = CellKey(ix, iy);

            if (!_grid.ContainsKey(key))
                _grid[key] = [];

            _grid[key].Add(_points.Count - 1);
        }
    }

    /// <summary>
    /// Add multiple points to the dataset
    /// </summary>
    public void AddPoints(List<Point> newPoints)
    {
        _points.Clear();
        _points.AddRange(newPoints);

        // Build spatial index using eps as cell size
        _grid.Clear();
        _gridCellSize = eps;
        BuildSpatialIndex();
        BuildKDTree();
    }

    /// <summary>
    /// Clear all points and reset the algorithm state
    /// </summary>
    public void ClearPoints()
    {
        _points.Clear();
        _orderedList.Clear();
        _grid.Clear();
        _gridCellSize = 0;
        _kdTree = null;
    }

    /// <summary>
    /// Run the OPTICS clustering algorithm
    /// </summary>
    public void Run()
    {
        _orderedList.Clear();
        _orderedList.Capacity = _points.Count;

        for (int i = 0; i < _points.Count; i++)
        {
            if (!_points[i].Processed)
            {
                ExpandClusterOrder(i);
            }
        }
    }

    /// <summary>
    /// Get the ordered list of point indices after running OPTICS
    /// </summary>
    public IReadOnlyList<int> GetClusterOrder() => _orderedList.AsReadOnly();

    /// <summary>
    /// Extract clusters using a reachability distance threshold
    /// </summary>
    public List<List<int>> ExtractClusters(double clusterThreshold)
    {
        var clusters = new List<List<int>>();
        var currentCluster = new List<int>();
        double thrSq = clusterThreshold * clusterThreshold;

        for (int i = 0; i < _orderedList.Count; i++)
        {
            int pointIdx = _orderedList[i];

            if (_points[pointIdx].ReachabilityDistance > thrSq)
            {
                if (currentCluster.Count > 0)
                {
                    clusters.Add(currentCluster);
                    currentCluster = [];
                }
            }
            currentCluster.Add(pointIdx);
        }

        if (currentCluster.Count > 0)
        {
            clusters.Add(currentCluster);
        }

        return clusters;
    }

    /// <summary>
    /// Get clustered points as Point2D structures
    /// </summary>
    public List<List<Point2D>> GetClusters(double clusterThreshold)
    {
        var result = new List<List<Point2D>>();
        var clusters = ExtractClusters(clusterThreshold);

        foreach (var cluster in clusters)
        {
            var clusteredPoints = new List<Point2D>();
            foreach (int pointIdx in cluster)
            {
                var pt = _points[pointIdx];
                clusteredPoints.Add(new Point2D(pt.X, pt.Y));
            }
            result.Add(clusteredPoints);
        }

        return result;
    }

    private void ExpandClusterOrder(int pointIdx)
    {
        var neighbors = GetNeighbors(pointIdx);
        _points[pointIdx].Processed = true;
        _orderedList.Add(pointIdx);

        if (neighbors.Count >= minPts)
        {
            // Compute core distance for point_idx
            double coreDistPoint = double.MaxValue;
            if (neighbors.Count >= minPts)
            {
                var tmp = new List<(double, int)>(neighbors);
                tmp.Sort((a, b) => a.Item1.CompareTo(b.Item1));
                coreDistPoint = tmp[minPts - 1].Item1;
            }

            // Priority queue ordered by reachability distance
            var seeds = new SortedSet<(double, int)>(Comparer<(double, int)>.Create((a, b) =>
            {
                int cmp = a.Item1.CompareTo(b.Item1);
                return cmp != 0 ? cmp : a.Item2.CompareTo(b.Item2);
            }));

            foreach (var (dist, neighborIdx) in neighbors)
            {
                if (!_points[neighborIdx].Processed)
                {
                    double newReachDist = Math.Max(dist, coreDistPoint);
                    if (_points[neighborIdx].ReachabilityDistance == double.MaxValue)
                    {
                        _points[neighborIdx].ReachabilityDistance = newReachDist;
                        seeds.Add((newReachDist, neighborIdx));
                    }
                    else if (newReachDist < _points[neighborIdx].ReachabilityDistance)
                    {
                        _points[neighborIdx].ReachabilityDistance = newReachDist;
                        seeds.Add((newReachDist, neighborIdx));
                    }
                }
            }

            while (seeds.Count > 0)
            {
                var (_, current) = seeds.Min;
                seeds.Remove(seeds.Min);

                var currentNeighbors = GetNeighbors(current);
                _points[current].Processed = true;
                _orderedList.Add(current);

                if (currentNeighbors.Count >= minPts)
                {
                    // Compute core distance for current
                    double coreDistCurrent = double.MaxValue;
                    if (currentNeighbors.Count >= minPts)
                    {
                        var tmp2 = new List<(double, int)>(currentNeighbors);
                        tmp2.Sort((a, b) => a.Item1.CompareTo(b.Item1));
                        coreDistCurrent = tmp2[minPts - 1].Item1;
                    }

                    foreach (var (dist, neighborIdx) in currentNeighbors)
                    {
                        if (!_points[neighborIdx].Processed)
                        {
                            double newReachDist = Math.Max(dist, coreDistCurrent);
                            if (_points[neighborIdx].ReachabilityDistance == double.MaxValue)
                            {
                                _points[neighborIdx].ReachabilityDistance = newReachDist;
                                seeds.Add((newReachDist, neighborIdx));
                            }
                            else if (newReachDist < _points[neighborIdx].ReachabilityDistance)
                            {
                                _points[neighborIdx].ReachabilityDistance = newReachDist;
                                seeds.Add((newReachDist, neighborIdx));
                            }
                        }
                    }
                }
            }
        }
    }

    private List<(double, int)> GetNeighbors(int pointIdx)
    {
        var neighbors = new List<(double, int)>(32);

        // If we have a KD-tree, prefer it for radius queries
        if (_kdTree != null)
        {
            var ids = _kdTree.RadiusSearch(_points[pointIdx], eps);
            foreach (int idx in ids)
            {
                if (idx == pointIdx) continue;
                double distanceSq = EuclideanDistance(_points[pointIdx], _points[idx]);
                neighbors.Add((distanceSq, idx));
            }
            return neighbors;
        }

        if (_gridCellSize <= 0 || _grid.Count == 0)
        {
            // Fallback to brute-force
            for (int i = 0; i < _points.Count; i++)
            {
                if (i == pointIdx) continue;
                double distanceSq = EuclideanDistance(_points[pointIdx], _points[i]);
                if (distanceSq <= eps * eps)
                {
                    neighbors.Add((distanceSq, i));
                }
            }
            return neighbors;
        }

        var p = _points[pointIdx];
        int cx = (int)Math.Floor(p.X / _gridCellSize);
        int cy = (int)Math.Floor(p.Y / _gridCellSize);

        // Search neighbor cells around (cx, cy)
        for (int dx = -1; dx <= 1; dx++)
        {
            for (int dy = -1; dy <= 1; dy++)
            {
                long key = CellKey(cx + dx, cy + dy);
                if (_grid.TryGetValue(key, out var cellPoints))
                {
                    foreach (int idx in cellPoints)
                    {
                        if (idx == pointIdx) continue;
                        double distanceSq = EuclideanDistance(p, _points[idx]);
                        if (distanceSq <= eps * eps)
                        {
                            neighbors.Add((distanceSq, idx));
                        }
                    }
                }
            }
        }

        return neighbors;
    }

    private static double EuclideanDistance(Point p1, Point p2)
    {
        double dx = p1.X - p2.X;
        double dy = p1.Y - p2.Y;
        return dx * dx + dy * dy; // Returns squared distance
    }

    private void BuildSpatialIndex()
    {
        if (_gridCellSize <= 0) return;

        _grid.Clear();

        for (int i = 0; i < _points.Count; i++)
        {
            int ix = (int)Math.Floor(_points[i].X / _gridCellSize);
            int iy = (int)Math.Floor(_points[i].Y / _gridCellSize);
            long key = CellKey(ix, iy);

            if (!_grid.ContainsKey(key))
                _grid[key] = [];

            _grid[key].Add(i);
        }
    }

    private void BuildKDTree()
    {
        if (_points.Count == 0)
        {
            _kdTree = null;
            return;
        }

        _kdTree = new KDTree(_points);
    }

    private static long CellKey(int ix, int iy)
    {
        // Pack two 32-bit ints into one 64-bit key
        return ((long)ix << 32) ^ (uint)iy;
    }
}