#pragma once
/**
 * @file motion_simulation.h
 * @brief 装配体运动仿真
 *
 * 定义运动副（铰链、滑动、球面等），支持步进仿真和碰撞响应。
 *
 * @ingroup brep
 */

#include "vde/brep/assembly.h"
#include "vde/core/point.h"
#include <vector>
#include <string>
#include <functional>

namespace vde::brep {

// ═══════════════════════════════════════════════════════════
//  Joint Types
// ═══════════════════════════════════════════════════════════

/// 运动副类型
enum class JointType {
    Fixed,        ///< 固定连接（无相对运动）
    Revolute,     ///< 旋转副：绕轴旋转
    Prismatic,    ///< 滑动副：沿轴平移
    Cylindrical,  ///< 圆柱副：旋转 + 平移（同轴）
    Spherical,    ///< 球面副：绕点旋转（3 自由度）
    Planar,       ///< 平面副：平面内 2D 运动
};

/// 运动副定义：连接装配体树中两个节点
struct MotionJoint {
    std::string name;               ///< 运动副名称
    JointType type = JointType::Fixed;

    /// 连接的节点路径（装配体树中名称，用于查找）
    std::string parent_link;        ///< 父连杆名称
    std::string child_link;         ///< 子连杆名称

    /// 运动副几何定义
    core::Point3D  anchor;          ///< 锚点（世界坐标）
    core::Vector3D axis;            ///< 轴方向（单位向量，对 Revolute/Prismatic/Cylindrical）
    core::Vector3D plane_normal;    ///< 平面法线（对 Planar）

    /// 运动范围限制
    double limit_min = -std::numeric_limits<double>::max();  ///< 下限（弧度或长度）
    double limit_max =  std::numeric_limits<double>::max();  ///< 上限
};

// ═══════════════════════════════════════════════════════════
//  Simulation State
// ═══════════════════════════════════════════════════════════

/// 仿真状态（单帧）
struct SimulationState {
    double time = 0.0;                      ///< 仿真时间
    std::vector<double> joint_values;       ///< 各关节当前值（角度/位移）
    std::vector<std::string> collisions;    ///< 碰撞信息
};

/// 仿真配置
struct SimulationConfig {
    double time_step = 0.016;   ///< 时间步长（默认 60 FPS）
    double duration = 1.0;      ///< 总仿真时长
    bool detect_collision = true;  ///< 是否检测碰撞
    bool stop_on_collision = false; ///< 碰撞时是否停止
};

// ═══════════════════════════════════════════════════════════
//  Motion Simulation
// ═══════════════════════════════════════════════════════════

/**
 * @brief 运动仿真引擎
 *
 * 管理运动副，执行步进仿真，更新装配体节点变换。
 *
 * @code{.cpp}
 * MotionSimulation sim;
 * sim.add_joint({"hinge", JointType::Revolute,
 *                "base", "arm", Point3D(0,0,0), Vector3D(0,1,0)});
 *
 * sim.set_driver(0, [](double t) { return sin(t); }); // 正弦驱动
 *
 * auto states = sim.run(assembly, {.time_step=0.016, .duration=2.0});
 * for (auto& s : states) {
 *     std::cout << "t=" << s.time << " joint0=" << s.joint_values[0] << "\n";
 * }
 * @endcode
 *
 * @ingroup brep
 */
class MotionSimulation {
public:
    /// 添加运动副
    void add_joint(const MotionJoint& joint);

    /// 获取运动副数量
    [[nodiscard]] size_t joint_count() const { return joints_.size(); }

    /// 设置关节驱动函数（t → value）
    /// @param joint_index 关节索引
    /// @param driver 驱动函数 f(t) → joint value
    void set_driver(int joint_index, std::function<double(double)> driver);

    /// 设置恒定速度驱动
    /// @param joint_index 关节索引
    /// @param speed 恒定速度（rad/s 或 units/s）
    void set_constant_speed(int joint_index, double speed);

    /**
     * @brief 运行仿真
     *
     * 按时间步长推进，每步更新装配体节点变换。
     * 若 detect_collision 为 true，每步检测干涉。
     *
     * @param assembly 装配体（会被原地修改）
     * @param config 仿真配置
     * @return 各时间步的状态列表
     */
    [[nodiscard]] std::vector<SimulationState> run(
        Assembly& assembly, const SimulationConfig& config);

    /**
     * @brief 单步仿真
     *
     * 推进一个时间步长，更新装配体和关节值。
     *
     * @param assembly 装配体
     * @param dt 时间步长
     * @return 当前仿真状态
     */
    [[nodiscard]] SimulationState step(Assembly& assembly, double dt);

    /// 重置仿真时间
    void reset() { time_ = 0.0; joint_values_.assign(joints_.size(), 0.0); }

private:
    std::vector<MotionJoint> joints_;
    std::vector<std::function<double(double)>> drivers_;
    std::vector<double> joint_values_;
    double time_ = 0.0;

    /// Apply joint transform to the assembly
    void apply_joint_transform(Assembly& assembly, int joint_idx, double value);

    /// Find assembly node by name
    static AssemblyNode* find_node(AssemblyNode& root, const std::string& name);
};

} // namespace vde::brep