// SPDX-License-Identifier: LGPL-2.1-or-later

#include <gtest/gtest.h>

#include <FCConfig.h>

#include <App/Application.h>
#include <Mod/Assembly/Solver/IKCSolver.h>
#include <Mod/Assembly/Solver/OndselAdapter.h>
#include <Mod/Assembly/Solver/Types.h>
#include <src/App/InitApplication.h>

#include <algorithm>
#include <cmath>

using namespace KCSolve;

// ── Fixture ────────────────────────────────────────────────────────

class OndselAdapterTest : public ::testing::Test
{
protected:
    static void SetUpTestSuite()
    {
        tests::initApplication();
    }

    void SetUp() override
    {
        adapter_ = std::make_unique<OndselAdapter>();
    }

    /// Build a minimal two-part context with a single constraint.
    static SolveContext twoPartContext(BaseJointKind jointKind,
                                      bool groundFirst = true)
    {
        SolveContext ctx;

        Part p1;
        p1.id = "Part1";
        p1.placement = Transform::identity();
        p1.grounded = groundFirst;
        ctx.parts.push_back(p1);

        Part p2;
        p2.id = "Part2";
        p2.placement = Transform::identity();
        p2.placement.position = {100.0, 0.0, 0.0};
        p2.grounded = false;
        ctx.parts.push_back(p2);

        Constraint c;
        c.id = "Joint1";
        c.part_i = "Part1";
        c.marker_i = Transform::identity();
        c.part_j = "Part2";
        c.marker_j = Transform::identity();
        c.type = jointKind;
        ctx.constraints.push_back(c);

        return ctx;
    }

    std::unique_ptr<OndselAdapter> adapter_;
};


// ── Identity / capability tests ────────────────────────────────────

TEST_F(OndselAdapterTest, Name)  // NOLINT
{
    auto n = adapter_->name();
    EXPECT_FALSE(n.empty());
    EXPECT_NE(n.find("Ondsel"), std::string::npos);
}

TEST_F(OndselAdapterTest, SupportedJoints)  // NOLINT
{
    auto joints = adapter_->supported_joints();
    EXPECT_FALSE(joints.empty());

    // Must include core kinematic joints.
    EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Fixed), joints.end());
    EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Revolute), joints.end());
    EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Cylindrical), joints.end());
    EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Ball), joints.end());

    // Must exclude unsupported types.
    EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Universal), joints.end());
    EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Cam), joints.end());
    EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Slot), joints.end());
}

TEST_F(OndselAdapterTest, IsDeterministic)  // NOLINT
{
    EXPECT_TRUE(adapter_->is_deterministic());
}

TEST_F(OndselAdapterTest, SupportsBundleFixed)  // NOLINT
{
    EXPECT_FALSE(adapter_->supports_bundle_fixed());
}


// ── Solve round-trips ──────────────────────────────────────────────

TEST_F(OndselAdapterTest, SolveFixedJoint)  // NOLINT
{
    auto ctx = twoPartContext(BaseJointKind::Fixed);
    auto result = adapter_->solve(ctx);

    EXPECT_EQ(result.status, SolveStatus::Success);
    EXPECT_FALSE(result.placements.empty());

    // Both parts should end up at the same position (fixed joint).
    const auto* pr1 = &result.placements[0];
    const auto* pr2 = &result.placements[1];
    if (pr1->id == "Part2") {
        std::swap(pr1, pr2);
    }

    // Part1 is grounded — should remain at origin.
    EXPECT_NEAR(pr1->placement.position[0], 0.0, 1e-3);
    EXPECT_NEAR(pr1->placement.position[1], 0.0, 1e-3);
    EXPECT_NEAR(pr1->placement.position[2], 0.0, 1e-3);

    // Part2 should be pulled to Part1's position by the fixed joint
    // (markers are both identity, so the parts are welded at the same point).
    EXPECT_NEAR(pr2->placement.position[0], 0.0, 1e-3);
    EXPECT_NEAR(pr2->placement.position[1], 0.0, 1e-3);
    EXPECT_NEAR(pr2->placement.position[2], 0.0, 1e-3);
}

TEST_F(OndselAdapterTest, SolveRevoluteJoint)  // NOLINT
{
    auto ctx = twoPartContext(BaseJointKind::Revolute);
    auto result = adapter_->solve(ctx);

    EXPECT_EQ(result.status, SolveStatus::Success);
    EXPECT_FALSE(result.placements.empty());
}

TEST_F(OndselAdapterTest, SolveNoGroundedParts)  // NOLINT
{
    // OndselAdapter itself doesn't require grounded parts — that check
    // lives in AssemblyObject. The solver should still attempt to solve.
    auto ctx = twoPartContext(BaseJointKind::Fixed, /*groundFirst=*/false);
    auto result = adapter_->solve(ctx);

    // May succeed or fail depending on OndselSolver's behavior, but must not crash.
    EXPECT_TRUE(result.status == SolveStatus::Success
                || result.status == SolveStatus::Failed);
}

TEST_F(OndselAdapterTest, SolveCatchesException)  // NOLINT
{
    // Malformed context: constraint references non-existent parts.
    SolveContext ctx;

    Part p;
    p.id = "LonePart";
    p.placement = Transform::identity();
    p.grounded = true;
    ctx.parts.push_back(p);

    Constraint c;
    c.id = "BadJoint";
    c.part_i = "DoesNotExist";
    c.marker_i = Transform::identity();
    c.part_j = "AlsoDoesNotExist";
    c.marker_j = Transform::identity();
    c.type = BaseJointKind::Fixed;
    ctx.constraints.push_back(c);

    // Should not crash — returns Failed or succeeds with warnings.
    auto result = adapter_->solve(ctx);
    SUCCEED();  // If we get here without crashing, the test passes.
}


// ── Drag protocol ──────────────────────────────────────────────────

TEST_F(OndselAdapterTest, DragProtocol)  // NOLINT
{
    auto ctx = twoPartContext(BaseJointKind::Revolute);

    auto preResult = adapter_->pre_drag(ctx, {"Part2"});
    EXPECT_EQ(preResult.status, SolveStatus::Success);

    // Move Part2 slightly.
    SolveResult::PartResult dragPlc;
    dragPlc.id = "Part2";
    dragPlc.placement = Transform::identity();
    dragPlc.placement.position = {10.0, 5.0, 0.0};

    auto stepResult = adapter_->drag_step({dragPlc});
    // drag_step may fail if the solver can't converge — that's OK.
    EXPECT_TRUE(stepResult.status == SolveStatus::Success
                || stepResult.status == SolveStatus::Failed);

    // post_drag must not crash.
    adapter_->post_drag();
    SUCCEED();
}


// ── Diagnostics ────────────────────────────────────────────────────

TEST_F(OndselAdapterTest, DiagnoseRedundant)  // NOLINT
{
    // Over-constrained: two fixed joints between the same two parts.
    SolveContext ctx;

    Part p1;
    p1.id = "PartA";
    p1.placement = Transform::identity();
    p1.grounded = true;
    ctx.parts.push_back(p1);

    Part p2;
    p2.id = "PartB";
    p2.placement = Transform::identity();
    p2.placement.position = {50.0, 0.0, 0.0};
    p2.grounded = false;
    ctx.parts.push_back(p2);

    Constraint c1;
    c1.id = "FixedJoint1";
    c1.part_i = "PartA";
    c1.marker_i = Transform::identity();
    c1.part_j = "PartB";
    c1.marker_j = Transform::identity();
    c1.type = BaseJointKind::Fixed;
    ctx.constraints.push_back(c1);

    Constraint c2;
    c2.id = "FixedJoint2";
    c2.part_i = "PartA";
    c2.marker_i = Transform::identity();
    c2.part_j = "PartB";
    c2.marker_j = Transform::identity();
    c2.type = BaseJointKind::Fixed;
    ctx.constraints.push_back(c2);

    auto diags = adapter_->diagnose(ctx);
    // With two identical fixed joints, one must be redundant.
    bool hasRedundant = std::any_of(diags.begin(), diags.end(), [](const auto& d) {
        return d.kind == ConstraintDiagnostic::Kind::Redundant;
    });
    EXPECT_TRUE(hasRedundant);
}