"""
Phase 5 in-client console tests.

Paste into the FreeCAD Python console (or run via: exec(open(...).read())).
Tests the full Assembly -> KindredSolver pipeline without the unittest harness.

Expected output: all lines print PASS. Any FAIL indicates a regression.
"""

import FreeCAD as App
import JointObject
import kcsolve

_pref = App.ParamGet("User parameter:BaseApp/Preferences/Mod/Assembly")
_orig_solver = _pref.GetString("Solver", "")

_results = []


def _report(name, passed, detail=""):
    status = "PASS" if passed else "FAIL"
    msg = f"  [{status}] {name}"
    if detail:
        msg += f" — {detail}"
    print(msg)
    _results.append((name, passed))


def _new_doc(name="Phase5Test"):
    if App.ActiveDocument and App.ActiveDocument.Name == name:
        App.closeDocument(name)
    App.newDocument(name)
    App.setActiveDocument(name)
    return App.ActiveDocument


def _cleanup(doc):
    App.closeDocument(doc.Name)


def _make_assembly(doc):
    asm = doc.addObject("Assembly::AssemblyObject", "Assembly")
    asm.resetSolver()
    jg = asm.newObject("Assembly::JointGroup", "Joints")
    return asm, jg


def _make_box(asm, x=0, y=0, z=0, size=10):
    box = asm.newObject("Part::Box", "Box")
    box.Length = size
    box.Width = size
    box.Height = size
    box.Placement = App.Placement(App.Vector(x, y, z), App.Rotation())
    return box


def _ground(jg, obj):
    gnd = jg.newObject("App::FeaturePython", "GroundedJoint")
    JointObject.GroundedJoint(gnd, obj)
    return gnd


def _make_joint(jg, joint_type, ref1, ref2):
    joint = jg.newObject("App::FeaturePython", "Joint")
    JointObject.Joint(joint, joint_type)
    refs = [[ref1[0], ref1[1]], [ref2[0], ref2[1]]]
    joint.Proxy.setJointConnectors(joint, refs)
    return joint


# ── Test 1: Registry ────────────────────────────────────────────────


def test_solver_registry():
    """Verify kindred solver is registered and available."""
    names = kcsolve.available()
    _report(
        "registry: kindred in available()", "kindred" in names, f"available={names}"
    )

    solver = kcsolve.load("kindred")
    _report("registry: load('kindred') succeeds", solver is not None)
    _report(
        "registry: solver name",
        solver.name() == "Kindred (Newton-Raphson)",
        f"got '{solver.name()}'",
    )

    joints = solver.supported_joints()
    _report(
        "registry: supported_joints non-empty",
        len(joints) > 0,
        f"{len(joints)} joint types",
    )


# ── Test 2: Preference switching ────────────────────────────────────


def test_preference_switching():
    """Verify solver preference controls which backend is used."""
    doc = _new_doc("PrefTest")
    try:
        # Set to kindred
        _pref.SetString("Solver", "kindred")
        asm, jg = _make_assembly(doc)

        box1 = _make_box(asm, 0, 0, 0)
        box2 = _make_box(asm, 50, 0, 0)
        _ground(box1)
        _make_joint(jg, 0, [box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]])

        result = asm.solve()
        _report(
            "pref: kindred solve succeeds", result == 0, f"solve() returned {result}"
        )

        # Switch back to ondsel
        _pref.SetString("Solver", "ondsel")
        asm.resetSolver()
        result2 = asm.solve()
        _report(
            "pref: ondsel solve succeeds after switch",
            result2 == 0,
            f"solve() returned {result2}",
        )
    finally:
        _cleanup(doc)


# ── Test 3: Fixed joint ─────────────────────────────────────────────


def test_fixed_joint():
    """Two boxes + ground + fixed joint -> placements match."""
    _pref.SetString("Solver", "kindred")
    doc = _new_doc("FixedTest")
    try:
        asm, jg = _make_assembly(doc)
        box1 = _make_box(asm, 10, 20, 30)
        box2 = _make_box(asm, 40, 50, 60)
        _ground(box2)
        _make_joint(jg, 0, [box2, ["Face6", "Vertex7"]], [box1, ["Face6", "Vertex7"]])

        same = box1.Placement.isSame(box2.Placement, 1e-6)
        _report(
            "fixed: box1 matches box2 placement",
            same,
            f"box1={box1.Placement.Base}, box2={box2.Placement.Base}",
        )
    finally:
        _cleanup(doc)


# ── Test 4: Revolute joint + DOF ─────────────────────────────────────


def test_revolute_dof():
    """Revolute joint -> solve succeeds, DOF = 1."""
    _pref.SetString("Solver", "kindred")
    doc = _new_doc("RevoluteTest")
    try:
        asm, jg = _make_assembly(doc)
        box1 = _make_box(asm, 0, 0, 0)
        box2 = _make_box(asm, 100, 0, 0)
        _ground(box1)
        _make_joint(jg, 1, [box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]])

        result = asm.solve()
        _report("revolute: solve succeeds", result == 0, f"solve() returned {result}")

        dof = asm.getLastDoF()
        _report("revolute: DOF = 1", dof == 1, f"DOF = {dof}")
    finally:
        _cleanup(doc)


# ── Test 5: No ground ───────────────────────────────────────────────


def test_no_ground():
    """No grounded parts -> returns -6."""
    _pref.SetString("Solver", "kindred")
    doc = _new_doc("NoGroundTest")
    try:
        asm, jg = _make_assembly(doc)
        box1 = _make_box(asm, 0, 0, 0)
        box2 = _make_box(asm, 50, 0, 0)

        joint = jg.newObject("App::FeaturePython", "Joint")
        JointObject.Joint(joint, 0)
        refs = [[box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]]]
        joint.Proxy.setJointConnectors(joint, refs)

        result = asm.solve()
        _report("no-ground: returns -6", result == -6, f"solve() returned {result}")
    finally:
        _cleanup(doc)


# ── Test 6: Solve stability ─────────────────────────────────────────


def test_stability():
    """Solving twice gives identical placements."""
    _pref.SetString("Solver", "kindred")
    doc = _new_doc("StabilityTest")
    try:
        asm, jg = _make_assembly(doc)
        box1 = _make_box(asm, 10, 20, 30)
        box2 = _make_box(asm, 40, 50, 60)
        _ground(box2)
        _make_joint(jg, 0, [box2, ["Face6", "Vertex7"]], [box1, ["Face6", "Vertex7"]])

        asm.solve()
        plc1 = App.Placement(box1.Placement)
        asm.solve()
        plc2 = box1.Placement

        same = plc1.isSame(plc2, 1e-6)
        _report("stability: two solves identical", same)
    finally:
        _cleanup(doc)


# ── Test 7: Standalone solver API ────────────────────────────────────


def test_standalone_api():
    """Use kcsolve types directly without FreeCAD Assembly objects."""
    solver = kcsolve.load("kindred")

    # Two parts: one grounded, one free
    p1 = kcsolve.Part()
    p1.id = "base"
    p1.placement = kcsolve.Transform.identity()
    p1.grounded = True

    p2 = kcsolve.Part()
    p2.id = "arm"
    p2.placement = kcsolve.Transform()
    p2.placement.position = [100.0, 0.0, 0.0]
    p2.placement.quaternion = [1.0, 0.0, 0.0, 0.0]
    p2.grounded = False

    # Fixed joint
    c = kcsolve.Constraint()
    c.id = "fix1"
    c.part_i = "base"
    c.marker_i = kcsolve.Transform.identity()
    c.part_j = "arm"
    c.marker_j = kcsolve.Transform.identity()
    c.type = kcsolve.BaseJointKind.Fixed

    ctx = kcsolve.SolveContext()
    ctx.parts = [p1, p2]
    ctx.constraints = [c]

    result = solver.solve(ctx)
    _report(
        "standalone: solve status",
        result.status == kcsolve.SolveStatus.Success,
        f"status={result.status}",
    )
    _report("standalone: DOF = 0", result.dof == 0, f"dof={result.dof}")

    # Check that arm moved to origin
    for pr in result.placements:
        if pr.id == "arm":
            dist = sum(x**2 for x in pr.placement.position) ** 0.5
            _report("standalone: arm at origin", dist < 1e-6, f"distance={dist:.2e}")
            break


# ── Test 8: Diagnose API ────────────────────────────────────────────


def test_diagnose():
    """Diagnose overconstrained system via standalone API."""
    solver = kcsolve.load("kindred")

    p1 = kcsolve.Part()
    p1.id = "base"
    p1.placement = kcsolve.Transform.identity()
    p1.grounded = True

    p2 = kcsolve.Part()
    p2.id = "arm"
    p2.placement = kcsolve.Transform()
    p2.placement.position = [50.0, 0.0, 0.0]
    p2.placement.quaternion = [1.0, 0.0, 0.0, 0.0]
    p2.grounded = False

    # Two fixed joints = overconstrained
    c1 = kcsolve.Constraint()
    c1.id = "fix1"
    c1.part_i = "base"
    c1.marker_i = kcsolve.Transform.identity()
    c1.part_j = "arm"
    c1.marker_j = kcsolve.Transform.identity()
    c1.type = kcsolve.BaseJointKind.Fixed

    c2 = kcsolve.Constraint()
    c2.id = "fix2"
    c2.part_i = "base"
    c2.marker_i = kcsolve.Transform.identity()
    c2.part_j = "arm"
    c2.marker_j = kcsolve.Transform.identity()
    c2.type = kcsolve.BaseJointKind.Fixed

    ctx = kcsolve.SolveContext()
    ctx.parts = [p1, p2]
    ctx.constraints = [c1, c2]

    diags = solver.diagnose(ctx)
    _report(
        "diagnose: returns diagnostics", len(diags) > 0, f"{len(diags)} diagnostic(s)"
    )
    if diags:
        kinds = [d.kind for d in diags]
        _report(
            "diagnose: found redundant",
            kcsolve.DiagnosticKind.Redundant in kinds,
            f"kinds={[str(k) for k in kinds]}",
        )


# ── Run all ──────────────────────────────────────────────────────────


def run_all():
    print("\n=== Phase 5 Console Tests ===\n")

    test_solver_registry()
    test_preference_switching()
    test_fixed_joint()
    test_revolute_dof()
    test_no_ground()
    test_stability()
    test_standalone_api()
    test_diagnose()

    # Restore original preference
    _pref.SetString("Solver", _orig_solver)

    # Summary
    passed = sum(1 for _, p in _results if p)
    total = len(_results)
    print(f"\n=== {passed}/{total} passed ===\n")
    if passed < total:
        failed = [name for name, p in _results if not p]
        print(f"FAILED: {', '.join(failed)}")


run_all()