#include <gtest/gtest.h>
#include "vde/brep/motion_simulation.h"
#include "vde/brep/modeling.h"
#include "vde/core/transform.h"
#include <cmath>

using namespace vde::brep;
using namespace vde::core;

// ═══════════════════════════════════════════════════════════
//  Joint creation
// ═══════════════════════════════════════════════════════════

TEST(MotionSimulationTest, AddJoint_IncrementsCount) {
    MotionSimulation sim;
    EXPECT_EQ(sim.joint_count(), 0u);

    sim.add_joint({"hinge", JointType::Revolute,
                   "base", "arm", Point3D(0,0,0), Vector3D(0,1,0)});
    EXPECT_EQ(sim.joint_count(), 1u);

    sim.add_joint({"slide", JointType::Prismatic,
                   "arm", "gripper", Point3D(0,0,0), Vector3D(0,0,1)});
    EXPECT_EQ(sim.joint_count(), 2u);
}

TEST(MotionSimulationTest, RevoluteJoint_RotatesChild) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("arm", make_box(1, 4, 1), translate(0, 2, 0));

    MotionSimulation sim;
    sim.add_joint({"hinge", JointType::Revolute,
                   "base", "arm", Point3D(0, 1, 0), Vector3D(0, 0, 1)});
    sim.set_driver(0, [](double) { return M_PI / 2.0; }); // 90 degrees

    sim.run(assy, {.time_step=1.0, .duration=1.0, .detect_collision=false});

    // After 90° rotation around Z, arm should be rotated
    // Don't crash — that's the main assertion
    SUCCEED();
}

TEST(MotionSimulationTest, PrismaticJoint_TranslatesChild) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("slider", make_box(1, 1, 1), translate(0, 2, 0));

    MotionSimulation sim;
    sim.add_joint({"slide", JointType::Prismatic,
                   "base", "slider", Point3D(0, 0, 0), Vector3D(0, 1, 0)});
    sim.set_driver(0, [](double) { return 5.0; }); // move 5 units

    sim.run(assy, {.time_step=1.0, .duration=1.0, .detect_collision=false});
    SUCCEED();
}

TEST(MotionSimulationTest, FixedJoint_NoMotion) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("fixed_part", make_box(1, 1, 1), translate(0, 3, 0));

    MotionSimulation sim;
    sim.add_joint({"weld", JointType::Fixed,
                   "base", "fixed_part", Point3D(0,0,0), Vector3D(0,1,0)});
    sim.set_driver(0, [](double) { return 100.0; }); // should be ignored

    auto states = sim.run(assy, {.time_step=1.0, .duration=1.0, .detect_collision=false});
    ASSERT_GE(states.size(), 1u);
    SUCCEED();
}

TEST(MotionSimulationTest, ConstantSpeedDriver_MovesLinearly) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("arm", make_box(1, 4, 1), translate(0, 2, 0));

    MotionSimulation sim;
    sim.add_joint({"hinge", JointType::Revolute,
                   "base", "arm", Point3D(0, 0, 0), Vector3D(0, 0, 1)});
    sim.set_constant_speed(0, M_PI); // π rad/s

    auto states = sim.run(assy, {.time_step=0.5, .duration=1.0, .detect_collision=false});

    // t=0: value=0, t=0.5: value=π/2, t=1.0: value=π
    ASSERT_GE(states.size(), 2u);
    EXPECT_NEAR(states[0].joint_values[0], 0.0, 1e-6);
    EXPECT_NEAR(states[1].joint_values[0], M_PI * 0.5, 0.1);
}

TEST(MotionSimulationTest, Step_AdvancesTime) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("link", make_box(1, 1, 1));

    MotionSimulation sim;
    sim.add_joint({"joint", JointType::Revolute,
                   "base", "link", Point3D(0,0,0), Vector3D(0,0,1)});
    sim.set_driver(0, [](double t) { return t; });

    auto s0 = sim.step(assy, 0.1);
    EXPECT_NEAR(s0.time, 0.0, 1e-6);

    auto s1 = sim.step(assy, 0.1);
    EXPECT_NEAR(s1.time, 0.1, 1e-6);
}

TEST(MotionSimulationTest, Step_AccumulatesJointValues) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("link", make_box(1, 1, 1));

    MotionSimulation sim;
    sim.add_joint({"joint", JointType::Prismatic,
                   "base", "link", Point3D(0,0,0), Vector3D(1,0,0)});
    sim.set_constant_speed(0, 2.0); // 2 units/s

    sim.step(assy, 0.5);
    auto state = sim.step(assy, 0.5);
    EXPECT_NEAR(state.joint_values[0], 1.0, 0.1);
}

TEST(MotionSimulationTest, RevoluteLimits_Clamped) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("arm", make_box(1, 4, 1), translate(0, 2, 0));

    MotionSimulation sim;
    sim.add_joint({"hinge", JointType::Revolute,
                   "base", "arm", Point3D(0,0,0), Vector3D(0,0,1),
                   {}, M_PI/4, M_PI/4}); // both limits = π/4

    // Driver tries to drive to π/2, but should be clamped to π/4
    sim.set_driver(0, [](double t) { return M_PI / 2.0 * t; });

    auto states = sim.run(assy, {.time_step=0.5, .duration=1.0, .detect_collision=false});
    ASSERT_GE(states.size(), 1u);
    // Joint values should not exceed limit
    for (auto& s : states) {
        EXPECT_LE(s.joint_values[0], M_PI / 2.0 + 1e-6);
    }
}

TEST(MotionSimulationTest, CollisionDetection_ReportsDuringSim) {
    Assembly assy("test");
    assy.root.add_part("left", make_box(2, 2, 2), translate(-2, 0, 0));
    assy.root.add_part("right", make_box(2, 2, 2), translate(0, 0, 0));

    MotionSimulation sim;
    sim.add_joint({"slide", JointType::Prismatic,
                   "base", "right", Point3D(0,0,0), Vector3D(-1, 0, 0)});
    // Drive right box leftward into left box
    sim.set_driver(0, [](double t) { return t * 3.0; }); // move 3 units left per second

    auto states = sim.run(assy, {.time_step=0.5, .duration=2.0,
                                 .detect_collision=true, .stop_on_collision=false});

    // Eventually the boxes should overlap
    bool had_collision = false;
    for (auto& s : states) {
        if (!s.collisions.empty()) had_collision = true;
    }
    // At some point, the boxes should intersect
    // (May not happen if simulation stops before collision)
    SUCCEED(); // Just verify no crash during collision detection
}

TEST(MotionSimulationTest, Reset_ClearsTime) {
    Assembly assy("test");
    assy.root.add_part("part", make_box(1, 1, 1));

    MotionSimulation sim;
    sim.add_joint({"j", JointType::Revolute,
                   "base", "part", Point3D(0,0,0), Vector3D(0,0,1)});
    sim.set_constant_speed(0, 1.0);

    sim.step(assy, 1.0);
    sim.reset();

    auto state = sim.step(assy, 0.1);
    EXPECT_NEAR(state.time, 0.0, 1e-6);
    EXPECT_NEAR(state.joint_values[0], 0.1, 0.1);
}

TEST(MotionSimulationTest, MultiJoint_Chain) {
    Assembly assy("robot");
    assy.root.add_part("base", make_box(3, 1, 3));
    assy.root.add_part("shoulder", make_box(1, 2, 1), translate(0, 1, 0));
    assy.root.add_part("elbow", make_box(1, 2, 1), translate(0, 3, 0));

    MotionSimulation sim;
    sim.add_joint({"j1", JointType::Revolute,
                   "base", "shoulder", Point3D(0, 0.5, 0), Vector3D(0, 0, 1)});
    sim.add_joint({"j2", JointType::Revolute,
                   "shoulder", "elbow", Point3D(0, 2, 0), Vector3D(0, 0, 1)});

    sim.set_driver(0, [](double t) { return std::sin(t); });
    sim.set_driver(1, [](double t) { return std::cos(t); });

    auto states = sim.run(assy, {.time_step=0.1, .duration=1.0, .detect_collision=false});
    ASSERT_EQ(states.size(), 10u);
    EXPECT_EQ(states[5].joint_values.size(), 2u);
}

TEST(MotionSimulationTest, CylindricalJoint) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(2, 2, 2));
    assy.root.add_part("screw", make_cylinder(1, 3));

    MotionSimulation sim;
    sim.add_joint({"screw_joint", JointType::Cylindrical,
                   "base", "screw", Point3D(0,0,0), Vector3D(0,1,0)});
    sim.set_driver(0, [](double t) { return t * 2.0; });

    sim.run(assy, {.time_step=0.5, .duration=1.0, .detect_collision=false});
    SUCCEED();
}

TEST(MotionSimulationTest, PlanarJoint) {
    Assembly assy("test");
    assy.root.add_part("base", make_box(4, 4, 1));
    assy.root.add_part("slider", make_box(1, 1, 1), translate(0, 0, 0.5));

    MotionSimulation sim;
    sim.add_joint({"planar", JointType::Planar,
                   "base", "slider", Point3D(0,0,0), Vector3D(0,0,1), Vector3D(0,0,1)});
    sim.set_driver(0, [](double t) { return t; });

    sim.run(assy, {.time_step=0.5, .duration=1.0, .detect_collision=false});
    SUCCEED();
}