package unit;

import java.util.*;

/**
 * Unit test for open3d-0001: ValidatePoseGraphConnectivity O(V²×E) → O(V×E)
 *
 * Simulates Open3D GlobalOptimization.cpp connectivity BFS:
 *   Defective: std::find on component vector inside while-loop × edge scan
 *   Fixed:     unordered_set membership O(1) replaces the O(V) linear scan
 *
 * Compile: javac -d . PoseGraphConnectivityAlgorithm.java
 * Run:     java -ea unit.PoseGraphConnectivityAlgorithm
 */
public class PoseGraphConnectivityAlgorithm {

    static class PoseGraphEdge {
        final int source;
        final int target;
        final boolean uncertain;
        PoseGraphEdge(int src, int tgt, boolean uncertain) {
            source = src; target = tgt; this.uncertain = uncertain;
        }
    }

    static class PoseGraph {
        final int nNodes;
        final List<PoseGraphEdge> edges;
        PoseGraph(int nNodes, List<PoseGraphEdge> edges) {
            this.nNodes = nNodes; this.edges = edges;
        }
    }

    // ── defective implementation ──────────────────────────────────────────────

    static boolean defectiveValidateConnectivity(PoseGraph pg, boolean ignoreUncertain) {
        int nNodes = pg.nNodes;
        if (nNodes == 0) return true;

        List<Integer> toExplore = new ArrayList<>();
        List<Integer> component = new ArrayList<>();  // O(V) membership
        toExplore.add(0);
        component.add(0);

        while (!toExplore.isEmpty()) {
            int i = toExplore.remove(toExplore.size() - 1);
            for (PoseGraphEdge e : pg.edges) {
                if (ignoreUncertain && e.uncertain) continue;
                int adj = -1;
                if (e.source == i) adj = e.target;
                else if (e.target == i) adj = e.source;
                if (adj != -1) {
                    // O(V) linear scan inside while-loop × edge-scan
                    boolean found = false;
                    for (int c : component) if (c == adj) { found = true; break; }
                    if (!found) {
                        toExplore.add(adj);
                        component.add(adj);
                    }
                }
            }
        }
        return component.size() == nNodes;
    }

    // ── fixed implementation ──────────────────────────────────────────────────

    static boolean fixedValidateConnectivity(PoseGraph pg, boolean ignoreUncertain) {
        int nNodes = pg.nNodes;
        if (nNodes == 0) return true;

        List<Integer> toExplore = new ArrayList<>();
        Set<Integer> componentSet = new HashSet<>();  // O(1) membership
        toExplore.add(0);
        componentSet.add(0);

        while (!toExplore.isEmpty()) {
            int i = toExplore.remove(toExplore.size() - 1);
            for (PoseGraphEdge e : pg.edges) {
                if (ignoreUncertain && e.uncertain) continue;
                int adj = -1;
                if (e.source == i) adj = e.target;
                else if (e.target == i) adj = e.source;
                if (adj != -1) {
                    if (componentSet.add(adj)) {  // O(1) insert+dedup
                        toExplore.add(adj);
                    }
                }
            }
        }
        return componentSet.size() == nNodes;
    }

    // ── graph builders ────────────────────────────────────────────────────────

    /** Linear chain: 0-1-2-...-n-1 */
    static PoseGraph buildChain(int n) {
        List<PoseGraphEdge> edges = new ArrayList<>();
        for (int i = 0; i < n - 1; i++) edges.add(new PoseGraphEdge(i, i + 1, false));
        return new PoseGraph(n, edges);
    }

    /** Complete graph: all pairs connected */
    static PoseGraph buildComplete(int n) {
        List<PoseGraphEdge> edges = new ArrayList<>();
        for (int i = 0; i < n; i++)
            for (int j = i + 1; j < n; j++) edges.add(new PoseGraphEdge(i, j, false));
        return new PoseGraph(n, edges);
    }

    /** Disconnected: two isolated cliques */
    static PoseGraph buildDisconnected(int n) {
        List<PoseGraphEdge> edges = new ArrayList<>();
        int half = n / 2;
        for (int i = 0; i < half - 1; i++) edges.add(new PoseGraphEdge(i, i + 1, false));
        for (int i = half; i < n - 1; i++) edges.add(new PoseGraphEdge(i, i + 1, false));
        return new PoseGraph(n, edges);
    }

    /** Certain-edge-only graph: some edges marked uncertain */
    static PoseGraph buildMixed(int n, double uncertainFraction) {
        List<PoseGraphEdge> edges = new ArrayList<>();
        // Build a chain where every other edge is uncertain
        for (int i = 0; i < n - 1; i++) {
            boolean uncertain = (i % 2 == 1);
            edges.add(new PoseGraphEdge(i, i + 1, uncertain));
        }
        return new PoseGraph(n, edges);
    }

    // ── tests ─────────────────────────────────────────────────────────────────

    static int pass = 0, total = 0;

    static void assertTrue(String name, boolean cond) {
        total++;
        if (cond) { pass++; System.out.println("PASS " + name); }
        else       System.out.println("FAIL " + name);
    }

    public static void main(String[] args) {
        // Test 1: empty graph → connected (vacuously)
        {
            PoseGraph pg = new PoseGraph(0, Collections.emptyList());
            assertTrue("empty-defective", defectiveValidateConnectivity(pg, false));
            assertTrue("empty-fixed", fixedValidateConnectivity(pg, false));
        }

        // Test 2: single node → connected
        {
            PoseGraph pg = new PoseGraph(1, Collections.emptyList());
            assertTrue("single-node-defective", defectiveValidateConnectivity(pg, false));
            assertTrue("single-node-fixed", fixedValidateConnectivity(pg, false));
        }

        // Test 3: chain of 5 → connected
        {
            PoseGraph pg = buildChain(5);
            assertTrue("chain5-defective", defectiveValidateConnectivity(pg, false));
            assertTrue("chain5-fixed", fixedValidateConnectivity(pg, false));
        }

        // Test 4: complete graph of 6 → connected
        {
            PoseGraph pg = buildComplete(6);
            assertTrue("complete6-defective", defectiveValidateConnectivity(pg, false));
            assertTrue("complete6-fixed", fixedValidateConnectivity(pg, false));
        }

        // Test 5: disconnected → not connected
        {
            PoseGraph pg = buildDisconnected(6);
            assertTrue("disconnected6-defective", !defectiveValidateConnectivity(pg, false));
            assertTrue("disconnected6-fixed", !fixedValidateConnectivity(pg, false));
        }

        // Test 6: mixed uncertain edges — chain connected via uncertain edges only
        {
            // 4 nodes: 0--1(certain) 1--2(uncertain) 2--3(certain)
            List<PoseGraphEdge> edges = Arrays.asList(
                new PoseGraphEdge(0, 1, false),
                new PoseGraphEdge(1, 2, true),
                new PoseGraphEdge(2, 3, false));
            PoseGraph pg = new PoseGraph(4, edges);
            // Ignoring uncertain: 0-1 certain, 2-3 certain, but 1-2 uncertain → not connected
            assertTrue("uncertain-ignore-defective", !defectiveValidateConnectivity(pg, true));
            assertTrue("uncertain-ignore-fixed", !fixedValidateConnectivity(pg, true));
            // Not ignoring uncertain: all connected
            assertTrue("uncertain-include-defective", defectiveValidateConnectivity(pg, false));
            assertTrue("uncertain-include-fixed", fixedValidateConnectivity(pg, false));
        }

        // Test 7: both implementations agree on random graph
        {
            Random rng = new Random(42);
            int V = 30, extraEdges = 60;
            List<PoseGraphEdge> edges = new ArrayList<>();
            for (int i = 0; i < V - 1; i++) edges.add(new PoseGraphEdge(i, i + 1, false));
            for (int i = 0; i < extraEdges; i++) {
                int a = rng.nextInt(V), b = rng.nextInt(V);
                edges.add(new PoseGraphEdge(a, b, rng.nextBoolean()));
            }
            PoseGraph pg = new PoseGraph(V, edges);
            boolean d = defectiveValidateConnectivity(pg, false);
            boolean f = fixedValidateConnectivity(pg, false);
            assertTrue("random-graph-agree", d == f);
            assertTrue("random-graph-connected", f);  // chain ensures connectivity
        }

        // Test 8: performance — component membership check isolated at V=1000
        // Builds a large component list, then simulates the find-or-add operation
        {
            int V = 2000;
            // Simulate: component grows to V, each element checked V times = O(V²)
            long t0 = System.nanoTime();
            for (int r = 0; r < 10; r++) {
                List<Integer> component = new ArrayList<>(V);
                for (int i = 0; i < V; i++) {
                    // defective: linear scan to check if i already in component
                    boolean found = false;
                    for (int c : component) if (c == i) { found = true; break; }
                    if (!found) component.add(i);
                }
            }
            long tDef = System.nanoTime() - t0;

            t0 = System.nanoTime();
            for (int r = 0; r < 10; r++) {
                Set<Integer> componentSet = new HashSet<>(V * 2);
                List<Integer> componentList = new ArrayList<>(V);
                for (int i = 0; i < V; i++) {
                    // fixed: O(1) set insert
                    if (componentSet.add(i)) componentList.add(i);
                }
            }
            long tFix = System.nanoTime() - t0;

            double ratio = (double) tDef / tFix;
            System.out.printf("  Perf component-dedup V=%d: defective=%.1fms fixed=%.1fms ratio=%.1fx%n",
                V, tDef / 1e6, tFix / 1e6, ratio);
            assertTrue("perf-speedup", ratio > 5.0);
        }

        System.out.println(pass + "/" + total + " PASS");
        assert pass == total : pass + "/" + total + " passed";
    }
}