// 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/>.                                       *
 *                                                                          *
 ***************************************************************************/

#ifndef KCSOLVE_ONDSELADAPTER_H
#define KCSOLVE_ONDSELADAPTER_H

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include "IKCSolver.h"
#include "KCSolveGlobal.h"

namespace MbD
{
class ASMTAssembly;
class ASMTJoint;
class ASMTMarker;
class ASMTPart;
}  // namespace MbD

namespace KCSolve
{

/// IKCSolver implementation wrapping OndselSolver's Lagrangian MBD engine.
///
/// Translates KCSolve types (SolveContext, BaseJointKind, Transform) to
/// OndselSolver's ASMT hierarchy (ASMTAssembly, ASMTPart, ASMTJoint, etc.)
/// and extracts results back into SolveResult.
///
/// All OndselSolver #includes are confined to OndselAdapter.cpp.

class KCSolveExport OndselAdapter : public IKCSolver
{
public:
    OndselAdapter() = default;

    // ── IKCSolver pure virtuals ────────────────────────────────────

    std::string name() const override;
    std::vector<BaseJointKind> supported_joints() const override;
    SolveResult solve(const SolveContext& ctx) override;

    // ── IKCSolver overrides ────────────────────────────────────────

    SolveResult update(const SolveContext& ctx) override;

    SolveResult pre_drag(const SolveContext& ctx,
                         const std::vector<std::string>& drag_parts) override;
    SolveResult drag_step(
        const std::vector<SolveResult::PartResult>& drag_placements) override;
    void post_drag() override;

    SolveResult run_kinematic(const SolveContext& ctx) override;
    std::size_t num_frames() const override;
    SolveResult update_for_frame(std::size_t index) override;

    std::vector<ConstraintDiagnostic> diagnose(const SolveContext& ctx) override;

    bool is_deterministic() const override;
    bool supports_bundle_fixed() const override;
    void export_native(const std::string& path) override;

    /// Register OndselAdapter as "ondsel" in the SolverRegistry.
    /// Call once at module init time.
    static void register_solver();

private:
    // ── Assembly building ──────────────────────────────────────────

    void build_assembly(const SolveContext& ctx);
    std::shared_ptr<MbD::ASMTPart> make_part(const Part& part);
    std::shared_ptr<MbD::ASMTMarker> make_marker(const std::string& name,
                                                   const Transform& tf);
    std::shared_ptr<MbD::ASMTJoint> create_joint(const Constraint& c);
    void add_limits(const Constraint& c,
                    const std::string& marker_i,
                    const std::string& marker_j);
    void add_motions(const SolveContext& ctx,
                     const std::string& marker_i,
                     const std::string& marker_j,
                     const std::string& joint_id);
    void fix_grounded_parts(const SolveContext& ctx);
    void set_simulation_params(const SimulationParams& params);

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

    SolveResult extract_result() const;
    std::vector<ConstraintDiagnostic> extract_diagnostics() const;
    Transform extract_part_transform(
        const std::shared_ptr<MbD::ASMTPart>& part) const;

    // ── Quaternion ↔ rotation matrix conversion ────────────────────

    /// Convert unit quaternion (w,x,y,z) to 3×3 rotation matrix (row-major).
    static void quat_to_matrix(const std::array<double, 4>& q,
                                double (&mat)[3][3]);

    // ── Internal state ─────────────────────────────────────────────

    std::shared_ptr<MbD::ASMTAssembly> assembly_;
    std::unordered_map<std::string, std::shared_ptr<MbD::ASMTPart>> part_map_;
    std::vector<std::string> drag_part_ids_;
};

}  // namespace KCSolve

#endif  // KCSOLVE_ONDSELADAPTER_H