package unit;

import java.util.*;

/**
 * pcl-0001: RegionGrowing::getSegmentFromPoint() clusters O(C×S) → point_labels[] O(1)
 *
 * In segmentation/include/pcl/segmentation/impl/region_growing.hpp::getSegmentFromPoint():
 *
 *   for (const auto& i_segment : clusters_)
 *   {
 *     const auto it = std::find(i_segment.indices.cbegin(), i_segment.indices.cend(), index);
 *     if (it != i_segment.indices.cend()) { ... break; }
 *   }
 *
 * std::find is a linear O(S) scan per cluster; the outer loop is O(C).
 * Total per-call cost: O(C × S) on average = O(N/2) worst case.
 *
 * The fix: point_labels_[index] is already set to the segment index by
 * applySmoothRegionGrowingAlgorithm() and used verbatim by assembleRegions():
 *
 *   clusters_[point_labels_[index]]  // O(1) direct array index
 *
 * Severity: HIGH
 * UNDF: assigned by generate_undf.py
 */
public class PclRegionGrowingGetSegmentTest {

    static long slowOps = 0;
    static long fastOps = 0;

    /**
     * Simulate the PCL clusters_ data structure:
     * a list of clusters, each holding a list of point indices.
     */
    static int[][] buildClusters(int numClusters, int pointsPerCluster) {
        int[][] clusters = new int[numClusters][pointsPerCluster];
        for (int c = 0; c < numClusters; c++) {
            for (int p = 0; p < pointsPerCluster; p++) {
                clusters[c][p] = c * pointsPerCluster + p;
            }
        }
        return clusters;
    }

    /**
     * SLOW: O(C × S) — scan all clusters, linear find in each.
     * Mirrors the defective PCL implementation.
     */
    static int[] getSegmentFromPointSlow(int[][] clusters, int queryIndex) {
        for (int[] cluster : clusters) {
            for (int idx : cluster) {
                slowOps++;
                if (idx == queryIndex) {
                    return cluster.clone();
                }
            }
        }
        return new int[0];
    }

    /**
     * FAST: O(1) — use point_labels[] direct array index.
     * Mirrors the patched PCL implementation.
     */
    static int[] getSegmentFromPointFast(int[][] clusters, int[] pointLabels, int queryIndex) {
        fastOps++;  // one array lookup
        int segmentIndex = pointLabels[queryIndex];
        if (segmentIndex < 0 || segmentIndex >= clusters.length) return new int[0];
        return clusters[segmentIndex].clone();
    }

    /**
     * Build point_labels[]: maps each point index to its cluster index.
     * This is what PCL's assembleRegions() establishes.
     */
    static int[] buildPointLabels(int[][] clusters) {
        int totalPoints = 0;
        for (int[] c : clusters) totalPoints += c.length;
        int[] labels = new int[totalPoints];
        Arrays.fill(labels, -1);
        for (int c = 0; c < clusters.length; c++) {
            for (int idx : clusters[c]) {
                labels[idx] = c;
            }
        }
        return labels;
    }

    public static void main(String[] args) {
        final int C = 500;   // clusters (typical LiDAR scan: hundreds to thousands)
        final int S = 200;   // points per cluster
        final int N = C * S; // total points = 100,000

        int[][] clusters = buildClusters(C, S);
        int[] pointLabels = buildPointLabels(clusters);

        // Query every point once — simulates a user loop over all points
        // (e.g. building a per-point cluster-membership map)
        slowOps = 0;
        fastOps = 0;

        int[] slowResult = null;
        int[] fastResult = null;

        for (int q = 0; q < N; q++) {
            slowResult = getSegmentFromPointSlow(clusters, q);
            fastResult = getSegmentFromPointFast(clusters, pointLabels, q);
        }

        // Verify correctness on last query point
        assert slowResult != null && fastResult != null;
        assert slowResult.length == fastResult.length
            : "result length mismatch: slow=" + slowResult.length + " fast=" + fastResult.length;
        Arrays.sort(slowResult);
        Arrays.sort(fastResult);
        assert Arrays.equals(slowResult, fastResult)
            : "result content mismatch";

        // Spot-check a query in the middle of the last cluster (worst case for slow)
        int worstQuery = N - 1;
        int[] sw = getSegmentFromPointSlow(clusters, worstQuery);
        int[] fw = getSegmentFromPointFast(clusters, pointLabels, worstQuery);
        Arrays.sort(sw); Arrays.sort(fw);
        assert Arrays.equals(sw, fw) : "worst-case query mismatch";

        long ratio = slowOps / Math.max(fastOps, 1);
        System.out.printf("N=%d  C=%d  S=%d%n", N, C, S);
        System.out.printf("slowOps (O(C×S) per query) : %,d%n", slowOps);
        System.out.printf("fastOps (O(1)  per query)  : %,d%n", fastOps);
        System.out.printf("ratio                      : %,d×%n", ratio);

        // Require at least 1000× improvement
        assert ratio >= 1000
            : "Expected >=1000x speedup, got " + ratio + "x";

        System.out.println("PASS");
    }
}