// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
 *                                                                          *
 *   Copyright (c) 2025 Kindred Systems <development@kindred-systems.com>   *
 *                                                                          *
 *   This file is part of FreeCAD.                                          *
 *                                                                          *
 *   FreeCAD is free software: you can redistribute it and/or modify it     *
 *   under the terms of the GNU Lesser General Public License as            *
 *   published by the Free Software Foundation, either version 2.1 of the   *
 *   License, or (at your option) any later version.                        *
 *                                                                          *
 *   FreeCAD is distributed in the hope that it will be useful, but         *
 *   WITHOUT ANY WARRANTY; without even the implied warranty of             *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU       *
 *   Lesser General Public License for more details.                        *
 *                                                                          *
 *   You should have received a copy of the GNU Lesser General Public       *
 *   License along with FreeCAD. If not, see                                *
 *   <https://www.gnu.org/licenses/>.                                       *
 *                                                                          *
 ***************************************************************************/

#include "OndselAdapter.h"
#include "SolverRegistry.h"

#include <Base/Console.h>

#include <OndselSolver/CREATE.h>
#include <OndselSolver/ASMTAssembly.h>
#include <OndselSolver/ASMTAngleJoint.h>
#include <OndselSolver/ASMTConstantGravity.h>
#include <OndselSolver/ASMTCylSphJoint.h>
#include <OndselSolver/ASMTCylindricalJoint.h>
#include <OndselSolver/ASMTFixedJoint.h>
#include <OndselSolver/ASMTGearJoint.h>
#include <OndselSolver/ASMTGeneralMotion.h>
#include <OndselSolver/ASMTLineInPlaneJoint.h>
#include <OndselSolver/ASMTMarker.h>
#include <OndselSolver/ASMTParallelAxesJoint.h>
#include <OndselSolver/ASMTPart.h>
#include <OndselSolver/ASMTPerpendicularJoint.h>
#include <OndselSolver/ASMTPlanarJoint.h>
#include <OndselSolver/ASMTPointInPlaneJoint.h>
#include <OndselSolver/ASMTRackPinionJoint.h>
#include <OndselSolver/ASMTRevCylJoint.h>
#include <OndselSolver/ASMTRevoluteJoint.h>
#include <OndselSolver/ASMTRotationLimit.h>
#include <OndselSolver/ASMTRotationalMotion.h>
#include <OndselSolver/ASMTScrewJoint.h>
#include <OndselSolver/ASMTSimulationParameters.h>
#include <OndselSolver/ASMTSphSphJoint.h>
#include <OndselSolver/ASMTSphericalJoint.h>
#include <OndselSolver/ASMTTranslationLimit.h>
#include <OndselSolver/ASMTTranslationalJoint.h>
#include <OndselSolver/ASMTTranslationalMotion.h>
#include <OndselSolver/ExternalSystem.h>

// For System::jointsMotionsDo and diagnostic iteration
#include <OndselSolver/Constraint.h>
#include <OndselSolver/Joint.h>
#include <OndselSolver/System.h>

using namespace MbD;

namespace KCSolve
{

// ── Static registration ────────────────────────────────────────────

void OndselAdapter::register_solver()
{
    SolverRegistry::instance().register_solver(
        "ondsel",
        []() { return std::make_unique<OndselAdapter>(); });
}


// ── IKCSolver identity ─────────────────────────────────────────────

std::string OndselAdapter::name() const
{
    return "OndselSolver (Lagrangian)";
}

bool OndselAdapter::is_deterministic() const
{
    return true;
}

bool OndselAdapter::supports_bundle_fixed() const
{
    return false;
}

std::vector<BaseJointKind> OndselAdapter::supported_joints() const
{
    return {
        BaseJointKind::Coincident,
        BaseJointKind::PointOnLine,
        BaseJointKind::PointInPlane,
        BaseJointKind::Concentric,
        BaseJointKind::Tangent,
        BaseJointKind::Planar,
        BaseJointKind::LineInPlane,
        BaseJointKind::Parallel,
        BaseJointKind::Perpendicular,
        BaseJointKind::Angle,
        BaseJointKind::Fixed,
        BaseJointKind::Revolute,
        BaseJointKind::Cylindrical,
        BaseJointKind::Slider,
        BaseJointKind::Ball,
        BaseJointKind::Screw,
        BaseJointKind::Gear,
        BaseJointKind::RackPinion,
        BaseJointKind::DistancePointPoint,
        BaseJointKind::DistanceCylSph,
    };
}


// ── Quaternion → rotation matrix ───────────────────────────────────

void OndselAdapter::quat_to_matrix(const std::array<double, 4>& q,
                                    double (&mat)[3][3])
{
    double w = q[0], x = q[1], y = q[2], z = q[3];
    double xx = x * x, yy = y * y, zz = z * z;
    double xy = x * y, xz = x * z, yz = y * z;
    double wx = w * x, wy = w * y, wz = w * z;

    mat[0][0] = 1.0 - 2.0 * (yy + zz);
    mat[0][1] = 2.0 * (xy - wz);
    mat[0][2] = 2.0 * (xz + wy);
    mat[1][0] = 2.0 * (xy + wz);
    mat[1][1] = 1.0 - 2.0 * (xx + zz);
    mat[1][2] = 2.0 * (yz - wx);
    mat[2][0] = 2.0 * (xz - wy);
    mat[2][1] = 2.0 * (yz + wx);
    mat[2][2] = 1.0 - 2.0 * (xx + yy);
}


// ── Assembly building ──────────────────────────────────────────────

std::shared_ptr<ASMTPart> OndselAdapter::make_part(const Part& part)
{
    auto mbdPart = CREATE<ASMTPart>::With();
    mbdPart->setName(part.id);

    auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
    massMarker->setMass(part.mass);
    massMarker->setDensity(1.0);
    massMarker->setMomentOfInertias(1.0, 1.0, 1.0);
    mbdPart->setPrincipalMassMarker(massMarker);

    const auto& pos = part.placement.position;
    mbdPart->setPosition3D(pos[0], pos[1], pos[2]);

    double mat[3][3];
    quat_to_matrix(part.placement.quaternion, mat);
    mbdPart->setRotationMatrix(
        mat[0][0], mat[0][1], mat[0][2],
        mat[1][0], mat[1][1], mat[1][2],
        mat[2][0], mat[2][1], mat[2][2]);

    return mbdPart;
}

std::shared_ptr<ASMTMarker> OndselAdapter::make_marker(const std::string& markerName,
                                                        const Transform& tf)
{
    auto mbdMarker = CREATE<ASMTMarker>::With();
    mbdMarker->setName(markerName);

    const auto& pos = tf.position;
    mbdMarker->setPosition3D(pos[0], pos[1], pos[2]);

    double mat[3][3];
    quat_to_matrix(tf.quaternion, mat);
    mbdMarker->setRotationMatrix(
        mat[0][0], mat[0][1], mat[0][2],
        mat[1][0], mat[1][1], mat[1][2],
        mat[2][0], mat[2][1], mat[2][2]);

    return mbdMarker;
}

std::shared_ptr<ASMTJoint> OndselAdapter::create_joint(const Constraint& c)
{
    auto param = [&](std::size_t i, double fallback = 0.0) -> double {
        return i < c.params.size() ? c.params[i] : fallback;
    };

    switch (c.type) {
        case BaseJointKind::Coincident:
            return CREATE<ASMTSphericalJoint>::With();

        case BaseJointKind::PointOnLine: {
            auto j = CREATE<ASMTCylSphJoint>::With();
            j->distanceIJ = param(0);
            return j;
        }

        case BaseJointKind::PointInPlane: {
            auto j = CREATE<ASMTPointInPlaneJoint>::With();
            j->offset = param(0);
            return j;
        }

        case BaseJointKind::Concentric: {
            auto j = CREATE<ASMTRevCylJoint>::With();
            j->distanceIJ = param(0);
            return j;
        }

        case BaseJointKind::Tangent: {
            auto j = CREATE<ASMTPlanarJoint>::With();
            j->offset = param(0);
            return j;
        }

        case BaseJointKind::Planar: {
            auto j = CREATE<ASMTPlanarJoint>::With();
            j->offset = param(0);
            return j;
        }

        case BaseJointKind::LineInPlane: {
            auto j = CREATE<ASMTLineInPlaneJoint>::With();
            j->offset = param(0);
            return j;
        }

        case BaseJointKind::Parallel:
            return CREATE<ASMTParallelAxesJoint>::With();

        case BaseJointKind::Perpendicular:
            return CREATE<ASMTPerpendicularJoint>::With();

        case BaseJointKind::Angle: {
            auto j = CREATE<ASMTAngleJoint>::With();
            j->theIzJz = param(0);
            return j;
        }

        case BaseJointKind::Fixed:
            return CREATE<ASMTFixedJoint>::With();

        case BaseJointKind::Revolute:
            return CREATE<ASMTRevoluteJoint>::With();

        case BaseJointKind::Cylindrical:
            return CREATE<ASMTCylindricalJoint>::With();

        case BaseJointKind::Slider:
            return CREATE<ASMTTranslationalJoint>::With();

        case BaseJointKind::Ball:
            return CREATE<ASMTSphericalJoint>::With();

        case BaseJointKind::Screw: {
            auto j = CREATE<ASMTScrewJoint>::With();
            j->pitch = param(0);
            return j;
        }

        case BaseJointKind::Gear: {
            auto j = CREATE<ASMTGearJoint>::With();
            j->radiusI = param(0);
            j->radiusJ = param(1);
            return j;
        }

        case BaseJointKind::RackPinion: {
            auto j = CREATE<ASMTRackPinionJoint>::With();
            j->pitchRadius = param(0);
            return j;
        }

        case BaseJointKind::DistancePointPoint: {
            auto j = CREATE<ASMTSphSphJoint>::With();
            j->distanceIJ = param(0);
            return j;
        }

        case BaseJointKind::DistanceCylSph: {
            auto j = CREATE<ASMTCylSphJoint>::With();
            j->distanceIJ = param(0);
            return j;
        }

        // Unsupported types
        case BaseJointKind::Universal:
        case BaseJointKind::Cam:
        case BaseJointKind::Slot:
        case BaseJointKind::Custom:
            Base::Console().warning(
                "KCSolve: OndselAdapter does not support joint kind %d for constraint '%s'\n",
                static_cast<int>(c.type), c.id.c_str());
            return nullptr;
    }

    return nullptr;  // unreachable, but silences compiler warnings
}

void OndselAdapter::add_limits(const Constraint& c,
                                const std::string& marker_i,
                                const std::string& marker_j)
{
    for (const auto& lim : c.limits) {
        switch (lim.kind) {
            case Constraint::Limit::Kind::TranslationMin: {
                auto limit = CREATE<ASMTTranslationLimit>::With();
                limit->setName(c.id + "-LimitLenMin");
                limit->setMarkerI(marker_i);
                limit->setMarkerJ(marker_j);
                limit->settype("=>");
                limit->setlimit(std::to_string(lim.value));
                limit->settol(std::to_string(lim.tolerance));
                assembly_->addLimit(limit);
                break;
            }
            case Constraint::Limit::Kind::TranslationMax: {
                auto limit = CREATE<ASMTTranslationLimit>::With();
                limit->setName(c.id + "-LimitLenMax");
                limit->setMarkerI(marker_i);
                limit->setMarkerJ(marker_j);
                limit->settype("=<");
                limit->setlimit(std::to_string(lim.value));
                limit->settol(std::to_string(lim.tolerance));
                assembly_->addLimit(limit);
                break;
            }
            case Constraint::Limit::Kind::RotationMin: {
                auto limit = CREATE<ASMTRotationLimit>::With();
                limit->setName(c.id + "-LimitRotMin");
                limit->setMarkerI(marker_i);
                limit->setMarkerJ(marker_j);
                limit->settype("=>");
                limit->setlimit(std::to_string(lim.value) + "*pi/180.0");
                limit->settol(std::to_string(lim.tolerance));
                assembly_->addLimit(limit);
                break;
            }
            case Constraint::Limit::Kind::RotationMax: {
                auto limit = CREATE<ASMTRotationLimit>::With();
                limit->setName(c.id + "-LimitRotMax");
                limit->setMarkerI(marker_i);
                limit->setMarkerJ(marker_j);
                limit->settype("=<");
                limit->setlimit(std::to_string(lim.value) + "*pi/180.0");
                limit->settol(std::to_string(lim.tolerance));
                assembly_->addLimit(limit);
                break;
            }
        }
    }
}

void OndselAdapter::add_motions(const SolveContext& ctx,
                                 const std::string& marker_i,
                                 const std::string& marker_j,
                                 const std::string& joint_id)
{
    // Collect motions that target this joint.
    std::vector<const MotionDef*> joint_motions;
    for (const auto& m : ctx.motions) {
        if (m.joint_id == joint_id) {
            joint_motions.push_back(&m);
        }
    }

    if (joint_motions.empty()) {
        return;
    }

    // If there are two motions of different kinds on the same joint,
    // combine them into a GeneralMotion (cylindrical joint case).
    if (joint_motions.size() == 2
        && joint_motions[0]->kind != joint_motions[1]->kind) {
        auto motion = CREATE<ASMTGeneralMotion>::With();
        motion->setName(joint_id + "-GeneralMotion");
        motion->setMarkerI(marker_i);
        motion->setMarkerJ(marker_j);

        for (const auto* m : joint_motions) {
            if (m->kind == MotionDef::Kind::Rotational) {
                motion->angIJJ->atiput(2, m->rotation_expr);
            }
            else {
                motion->rIJI->atiput(2, m->translation_expr);
            }
        }
        assembly_->addMotion(motion);
        return;
    }

    // Single motion or multiple of the same kind.
    for (const auto* m : joint_motions) {
        switch (m->kind) {
            case MotionDef::Kind::Rotational: {
                auto motion = CREATE<ASMTRotationalMotion>::With();
                motion->setName(joint_id + "-AngularMotion");
                motion->setMarkerI(marker_i);
                motion->setMarkerJ(marker_j);
                motion->setRotationZ(m->rotation_expr);
                assembly_->addMotion(motion);
                break;
            }
            case MotionDef::Kind::Translational: {
                auto motion = CREATE<ASMTTranslationalMotion>::With();
                motion->setName(joint_id + "-LinearMotion");
                motion->setMarkerI(marker_i);
                motion->setMarkerJ(marker_j);
                motion->setTranslationZ(m->translation_expr);
                assembly_->addMotion(motion);
                break;
            }
            case MotionDef::Kind::General: {
                auto motion = CREATE<ASMTGeneralMotion>::With();
                motion->setName(joint_id + "-GeneralMotion");
                motion->setMarkerI(marker_i);
                motion->setMarkerJ(marker_j);
                if (!m->rotation_expr.empty()) {
                    motion->angIJJ->atiput(2, m->rotation_expr);
                }
                if (!m->translation_expr.empty()) {
                    motion->rIJI->atiput(2, m->translation_expr);
                }
                assembly_->addMotion(motion);
                break;
            }
        }
    }
}

void OndselAdapter::fix_grounded_parts(const SolveContext& ctx)
{
    for (const auto& part : ctx.parts) {
        if (!part.grounded) {
            continue;
        }

        auto it = part_map_.find(part.id);
        if (it == part_map_.end()) {
            continue;
        }

        // Assembly-level marker at the part's placement.
        std::string asmMarkerName = "ground-" + part.id;
        auto asmMarker = make_marker(asmMarkerName, part.placement);
        assembly_->addMarker(asmMarker);

        // Part-level marker at identity.
        std::string partMarkerName = "FixingMarker";
        auto partMarker = make_marker(partMarkerName, Transform::identity());
        it->second->addMarker(partMarker);

        // Fixed joint connecting them.
        auto fixedJoint = CREATE<ASMTFixedJoint>::With();
        fixedJoint->setName("ground-fix-" + part.id);
        fixedJoint->setMarkerI("/OndselAssembly/" + asmMarkerName);
        fixedJoint->setMarkerJ("/OndselAssembly/" + part.id + "/" + partMarkerName);
        assembly_->addJoint(fixedJoint);
    }
}

void OndselAdapter::set_simulation_params(const SimulationParams& params)
{
    auto mbdSim = assembly_->simulationParameters;
    mbdSim->settstart(params.t_start);
    mbdSim->settend(params.t_end);
    mbdSim->sethout(params.h_out);
    mbdSim->sethmin(params.h_min);
    mbdSim->sethmax(params.h_max);
    mbdSim->seterrorTol(params.error_tol);
}

void OndselAdapter::build_assembly(const SolveContext& ctx)
{
    assembly_ = CREATE<ASMTAssembly>::With();
    assembly_->setName("OndselAssembly");
    part_map_.clear();

    // Do NOT set externalSystem->freecadAssemblyObject — breaking the coupling.

    // Add parts.
    for (const auto& part : ctx.parts) {
        auto mbdPart = make_part(part);
        assembly_->addPart(mbdPart);
        part_map_[part.id] = mbdPart;
    }

    // Fix grounded parts.
    fix_grounded_parts(ctx);

    // Add constraints (joints + limits + motions).
    for (const auto& c : ctx.constraints) {
        if (!c.activated) {
            continue;
        }

        auto mbdJoint = create_joint(c);
        if (!mbdJoint) {
            continue;
        }

        // Create markers on the respective parts.
        auto it_i = part_map_.find(c.part_i);
        auto it_j = part_map_.find(c.part_j);
        if (it_i == part_map_.end() || it_j == part_map_.end()) {
            Base::Console().warning(
                "KCSolve: constraint '%s' references unknown part(s)\n",
                c.id.c_str());
            continue;
        }

        std::string markerNameI = c.id + "-mkrI";
        std::string markerNameJ = c.id + "-mkrJ";

        auto mkrI = make_marker(markerNameI, c.marker_i);
        it_i->second->addMarker(mkrI);

        auto mkrJ = make_marker(markerNameJ, c.marker_j);
        it_j->second->addMarker(mkrJ);

        std::string fullMarkerI = "/OndselAssembly/" + c.part_i + "/" + markerNameI;
        std::string fullMarkerJ = "/OndselAssembly/" + c.part_j + "/" + markerNameJ;

        mbdJoint->setName(c.id);
        mbdJoint->setMarkerI(fullMarkerI);
        mbdJoint->setMarkerJ(fullMarkerJ);
        assembly_->addJoint(mbdJoint);

        // Add limits (only when not in simulation mode).
        if (!ctx.simulation.has_value() && !c.limits.empty()) {
            add_limits(c, fullMarkerI, fullMarkerJ);
        }

        // Add motions.
        if (!ctx.motions.empty()) {
            add_motions(ctx, fullMarkerI, fullMarkerJ, c.id);
        }
    }

    // Set simulation parameters if present.
    if (ctx.simulation.has_value()) {
        set_simulation_params(*ctx.simulation);
    }
}


// ── Result extraction ──────────────────────────────────────────────

Transform OndselAdapter::extract_part_transform(
    const std::shared_ptr<ASMTPart>& part) const
{
    Transform tf;
    double x, y, z;
    part->getPosition3D(x, y, z);
    tf.position = {x, y, z};

    double q0, q1, q2, q3;
    part->getQuarternions(q0, q1, q2, q3);
    // OndselSolver returns (w, x, y, z) — matches our convention.
    tf.quaternion = {q0, q1, q2, q3};

    return tf;
}

SolveResult OndselAdapter::extract_result() const
{
    SolveResult result;
    result.status = SolveStatus::Success;

    for (const auto& [id, mbdPart] : part_map_) {
        SolveResult::PartResult pr;
        pr.id = id;
        pr.placement = extract_part_transform(mbdPart);
        result.placements.push_back(std::move(pr));
    }

    return result;
}

std::vector<ConstraintDiagnostic> OndselAdapter::extract_diagnostics() const
{
    std::vector<ConstraintDiagnostic> diags;

    if (!assembly_ || !assembly_->mbdSystem) {
        return diags;
    }

    assembly_->mbdSystem->jointsMotionsDo([&](std::shared_ptr<Joint> jm) {
        if (!jm) {
            return;
        }

        bool isRedundant = false;
        jm->constraintsDo([&](std::shared_ptr<MbD::Constraint> con) {
            if (!con) {
                return;
            }
            std::string spec = con->constraintSpec();
            if (spec.rfind("Redundant", 0) == 0) {
                isRedundant = true;
            }
        });

        if (isRedundant) {
            // Extract the constraint name from the solver's joint name.
            // Format: "/OndselAssembly/ground_moves#Joint001" → "Joint001"
            std::string fullName = jm->name;
            std::size_t hashPos = fullName.find_last_of('#');
            std::string cleanName = (hashPos != std::string::npos)
                ? fullName.substr(hashPos + 1)
                : fullName;

            ConstraintDiagnostic diag;
            diag.constraint_id = cleanName;
            diag.kind = ConstraintDiagnostic::Kind::Redundant;
            diag.detail = "Constraint is redundant";
            diags.push_back(std::move(diag));
        }
    });

    return diags;
}


// ── Solve operations ───────────────────────────────────────────────

SolveResult OndselAdapter::solve(const SolveContext& ctx)
{
    try {
        build_assembly(ctx);
        assembly_->runPreDrag();
        return extract_result();
    }
    catch (const std::exception& e) {
        Base::Console().warning("KCSolve: OndselAdapter solve failed: %s\n", e.what());
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
    catch (...) {
        Base::Console().warning("KCSolve: OndselAdapter solve failed: unknown exception\n");
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
}

SolveResult OndselAdapter::update(const SolveContext& ctx)
{
    return solve(ctx);
}


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

SolveResult OndselAdapter::pre_drag(const SolveContext& ctx,
                                     const std::vector<std::string>& drag_parts)
{
    drag_part_ids_ = drag_parts;

    try {
        build_assembly(ctx);
        assembly_->runPreDrag();
        return extract_result();
    }
    catch (const std::exception& e) {
        Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: %s\n", e.what());
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
    catch (...) {
        Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: unknown exception\n");
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
}

SolveResult OndselAdapter::drag_step(
    const std::vector<SolveResult::PartResult>& drag_placements)
{
    if (!assembly_) {
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }

    try {
        auto dragParts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();

        for (const auto& dp : drag_placements) {
            auto it = part_map_.find(dp.id);
            if (it == part_map_.end()) {
                continue;
            }
            auto& mbdPart = it->second;

            // Update position.
            const auto& pos = dp.placement.position;
            mbdPart->updateMbDFromPosition3D(pos[0], pos[1], pos[2]);

            // Update rotation.
            double mat[3][3];
            quat_to_matrix(dp.placement.quaternion, mat);
            mbdPart->updateMbDFromRotationMatrix(
                mat[0][0], mat[0][1], mat[0][2],
                mat[1][0], mat[1][1], mat[1][2],
                mat[2][0], mat[2][1], mat[2][2]);

            dragParts->push_back(mbdPart);
        }

        assembly_->runDragStep(dragParts);
        return extract_result();
    }
    catch (...) {
        // Drag step failures are non-fatal — caller will skip this frame.
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
}

void OndselAdapter::post_drag()
{
    if (assembly_) {
        assembly_->runPostDrag();
    }
    drag_part_ids_.clear();
}


// ── Kinematic simulation ───────────────────────────────────────────

SolveResult OndselAdapter::run_kinematic(const SolveContext& ctx)
{
    try {
        build_assembly(ctx);
        assembly_->runKINEMATIC();

        auto result = extract_result();
        result.num_frames = assembly_->numberOfFrames();
        return result;
    }
    catch (const std::exception& e) {
        Base::Console().warning("KCSolve: OndselAdapter run_kinematic failed: %s\n", e.what());
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
    catch (...) {
        Base::Console().warning(
            "KCSolve: OndselAdapter run_kinematic failed: unknown exception\n");
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }
}

std::size_t OndselAdapter::num_frames() const
{
    if (!assembly_) {
        return 0;
    }
    return assembly_->numberOfFrames();
}

SolveResult OndselAdapter::update_for_frame(std::size_t index)
{
    if (!assembly_) {
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }

    if (index >= assembly_->numberOfFrames()) {
        return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
    }

    assembly_->updateForFrame(index);
    return extract_result();
}


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

std::vector<ConstraintDiagnostic> OndselAdapter::diagnose(const SolveContext& ctx)
{
    // Ensure we have a solved assembly to inspect.
    if (!assembly_ || !assembly_->mbdSystem) {
        solve(ctx);
    }
    return extract_diagnostics();
}

// ── Native export ──────────────────────────────────────────────────

void OndselAdapter::export_native(const std::string& path)
{
    if (assembly_) {
        assembly_->outputFile(path);
    }
}

}  // namespace KCSolve