#include <gtest/gtest.h>
#include "vde/brep/kinematic_chain.h"
#include <cmath>

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

// ═══════════════════════════════════════════════════════════
//  Four-Bar Linkage — Classification
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, FourBar_CrankRocker) {
    // Ground=4, Crank=1, Coupler=3, Rocker=2
    // s=1(crank), l=4(ground), s+l=5 < p+q=5 → Grashof, crank is shortest → CrankRocker
    FourBarLinkage link{4, 1, 3, 2};
    EXPECT_EQ(link.classify(), FourBarType::CrankRocker);
    EXPECT_TRUE(link.is_grashof());
}

TEST(KinematicChainTest, FourBar_DoubleCrank) {
    // Ground=1 (shortest), Crank=3, Coupler=2, Rocker=4
    // s=1(ground), l=4(rocker), s+l=5 < p+q=5 → Grashof, ground is shortest → DoubleCrank
    FourBarLinkage link{1, 3, 2, 4};
    EXPECT_EQ(link.classify(), FourBarType::DoubleCrank);
}

TEST(KinematicChainTest, FourBar_DoubleRocker) {
    // Ground=3, Crank=2 (not shortest), Coupler=1 (shortest), Rocker=4
    // s=1(coupler), l=4(rocker), s+l=5 < p+q=5 → Grashof, coupler is shortest → DoubleRocker
    FourBarLinkage link{3, 2, 1, 4};
    EXPECT_EQ(link.classify(), FourBarType::DoubleRocker);
}

TEST(KinematicChainTest, FourBar_NonGrashof) {
    // s=1, l=5, s+l=6 > p+q=3+4=7? No, 6 < 7, so it IS Grashof.
    // Let's make: s=2, p=3, q=4, l=10 → s+l=12 > p+q=7 → NonGrashof
    FourBarLinkage link{2, 3, 4, 10};
    EXPECT_EQ(link.classify(), FourBarType::NonGrashof);
    EXPECT_FALSE(link.is_grashof());
}

TEST(KinematicChainTest, FourBar_ChangePoint) {
    // s+l = p+q exactly
    FourBarLinkage link{4, 1, 3, 2}; // 1+4=5, 2+3=5
    EXPECT_EQ(link.classify(), FourBarType::ChangePoint);
}

// ═══════════════════════════════════════════════════════════
//  Four-Bar Linkage — Position Solving
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, SolveFourBar_ValidAngle) {
    FourBarLinkage link{4, 1, 3, 2}; // CrankRocker
    auto sol = solve_fourbar(link, 0.5); // 0.5 rad input
    EXPECT_TRUE(sol.valid);
    EXPECT_NEAR(sol.input_angle, 0.5, 1e-9);
}

TEST(KinematicChainTest, SolveFourBar_ZeroAngle) {
    FourBarLinkage link{4, 1, 3, 2};
    auto sol = solve_fourbar(link, 0.0);
    EXPECT_TRUE(sol.valid);
}

TEST(KinematicChainTest, SolveFourBar_FullRotation) {
    // CrankRocker: input should rotate full 360°
    FourBarLinkage link{4, 1, 3, 2};
    int valid_count = 0;
    for (int i = 0; i < 36; ++i) {
        double angle = 2.0 * M_PI * i / 36.0;
        auto sol = solve_fourbar(link, angle, 0);
        if (sol.valid) valid_count++;
    }
    // CrankRocker: all positions should be valid
    EXPECT_EQ(valid_count, 36);
}

TEST(KinematicChainTest, SolveFourBar_BranchSwitch) {
    FourBarLinkage link{4, 1, 3, 2};
    auto sol0 = solve_fourbar(link, 0.5, 0); // open
    auto sol1 = solve_fourbar(link, 0.5, 1); // crossed
    EXPECT_TRUE(sol0.valid);
    EXPECT_TRUE(sol1.valid);
    // Different branches should give different output angles
    EXPECT_NE(sol0.output_angle, sol1.output_angle);
}

TEST(KinematicChainTest, SolveFourBar_TransmissionAngle) {
    FourBarLinkage link{4, 1, 3, 2};
    auto sol = solve_fourbar(link, 0.5);
    EXPECT_TRUE(sol.transmission_angle >= 0.0);
    EXPECT_TRUE(sol.transmission_angle <= M_PI_2 + 1e-9);
}

TEST(KinematicChainTest, SolveFourBar_DeadPoint) {
    // Design a mechanism with a dead point
    // When crank and coupler are collinear, transmission angle ≈ 0
    FourBarLinkage link{3, 1, 2, 2}; // s=1,l=3 → Grashof, crank is shortest
    auto sol = solve_fourbar(link, 0.0);
    // May or may not be dead point — just verify field exists
    EXPECT_TRUE(sol.dead_point || !sol.dead_point); // always true, just checking field
    EXPECT_TRUE(std::isfinite(sol.transmission_angle));
}

// ═══════════════════════════════════════════════════════════
//  Four-Bar — Full Analysis
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, AnalyzeFourBar_ReturnsResults) {
    FourBarLinkage link{4, 1, 3, 2};
    auto results = analyze_fourbar(link, 72);
    EXPECT_GT(results.size(), 0u);
    for (const auto& r : results) {
        EXPECT_TRUE(r.valid);
    }
}

// ═══════════════════════════════════════════════════════════
//  Gear Train
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, GearPair_Ratio) {
    GearPair gear;
    gear.teeth_driver = 20;
    gear.teeth_driven = 40;
    EXPECT_NEAR(gear.ratio(), 2.0, 1e-9);
}

TEST(KinematicChainTest, GearPair_CenterDistance) {
    GearPair gear;
    gear.teeth_driver = 20;
    gear.teeth_driven = 40;
    gear.module = 2.0;
    double cd = gear.compute_center_distance();
    EXPECT_NEAR(cd, 60.0, 1e-9); // (20+40)*2/2 = 60
}

TEST(KinematicChainTest, SolveGear_QuarterTurn) {
    GearPair gear{20, 40};
    auto sol = solve_gear(gear, M_PI_2); // 90° input
    EXPECT_NEAR(sol.driver_angle, M_PI_2, 1e-9);
    EXPECT_NEAR(sol.driven_angle, -M_PI_4, 1e-9); // -45° (opposite direction, half speed)
    EXPECT_NEAR(sol.angular_velocity_ratio, 2.0, 1e-9);
}

TEST(KinematicChainTest, SolveGear_FullRotation) {
    GearPair gear{20, 20}; // 1:1 ratio
    auto sol = solve_gear(gear, 2.0 * M_PI);
    EXPECT_NEAR(sol.driven_angle, -2.0 * M_PI, 1e-9);
}

TEST(KinematicChainTest, SolveGearTrain_TwoStage) {
    std::vector<GearPair> stages = {
        {20, 40},  // 2:1
        {20, 60},  // 3:1
    };
    auto results = solve_gear_train(stages, M_PI);
    EXPECT_EQ(results.size(), 2u);
    EXPECT_NEAR(results[0].driven_angle, -M_PI / 2.0, 1e-9);
    EXPECT_NEAR(results[1].driven_angle, M_PI / 6.0, 1e-9); // negative of previous / 3
}

// ═══════════════════════════════════════════════════════════
//  Cam-Follower
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, CamFollower_Dwell) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI, 0, 0, CamMotionType::Dwell}};

    auto state = solve_cam(cam, 0.5);
    EXPECT_NEAR(state.displacement, 0.0, 1e-9);
    EXPECT_NEAR(state.velocity, 0.0, 1e-9);
}

TEST(KinematicChainTest, CamFollower_ConstantVelocity) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI, 0, 10, CamMotionType::ConstantVelocity}};

    auto state = solve_cam(cam, M_PI_2); // halfway
    EXPECT_NEAR(state.displacement, 5.0, 1e-6);
}

TEST(KinematicChainTest, CamFollower_Cycloidal) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI, 0, 10, CamMotionType::Cycloidal}};

    auto start = solve_cam(cam, 0.0);
    EXPECT_NEAR(start.displacement, 0.0, 1e-9);

    auto end = solve_cam(cam, M_PI);
    EXPECT_NEAR(end.displacement, 10.0, 1e-6);

    // Cycloidal has zero velocity at endpoints
    EXPECT_NEAR(start.velocity, 0.0, 1e-9);
    EXPECT_NEAR(end.velocity, 0.0, 1e-9);
}

TEST(KinematicChainTest, CamFollower_Polynomial345) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI, 0, 10, CamMotionType::Polynomial345}};

    auto start = solve_cam(cam, 0.0);
    EXPECT_NEAR(start.displacement, 0.0, 1e-9);
    EXPECT_NEAR(start.velocity, 0.0, 1e-9);
    EXPECT_NEAR(start.acceleration, 0.0, 1e-9);

    auto end = solve_cam(cam, M_PI);
    EXPECT_NEAR(end.displacement, 10.0, 1e-6);
    EXPECT_NEAR(end.velocity, 0.0, 1e-9);
    EXPECT_NEAR(end.acceleration, 0.0, 1e-9);
}

TEST(KinematicChainTest, CamFollower_MultiSegment) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {
        {0, M_PI_2, 0, 10, CamMotionType::Cycloidal},     // rise
        {M_PI_2, M_PI, 10, 10, CamMotionType::Dwell},     // dwell
        {M_PI, 1.5 * M_PI, 10, 0, CamMotionType::Cycloidal}, // fall
        {1.5 * M_PI, 2 * M_PI, 0, 0, CamMotionType::Dwell},   // dwell
    };

    EXPECT_NEAR(cam.total_lift(), 10.0, 1e-9);

    // At 45°: midpoint of rise
    auto mid_rise = solve_cam(cam, M_PI_4);
    EXPECT_NEAR(mid_rise.displacement, 5.0, 1e-6);

    // At 135°: dwell at top
    auto top_dwell = solve_cam(cam, 1.35 * M_PI);
    EXPECT_NEAR(top_dwell.displacement, 10.0, 1e-6);
    EXPECT_NEAR(top_dwell.velocity, 0.0, 1e-9);
}

TEST(KinematicChainTest, CamFollower_PressureAngle) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI_2, 0, 20, CamMotionType::ConstantVelocity}};

    auto state = solve_cam(cam, M_PI_4);
    EXPECT_TRUE(state.pressure_angle >= 0.0);
    EXPECT_TRUE(state.pressure_angle < M_PI_2);
}

TEST(KinematicChainTest, CamFollower_TotalLift) {
    CamFollowerSystem cam;
    cam.segments = {
        {0, M_PI, 0, 5, CamMotionType::SimpleHarmonic},
        {M_PI, 2 * M_PI, 5, 15, CamMotionType::Cycloidal},
    };
    EXPECT_NEAR(cam.total_lift(), 15.0, 1e-9);
}

// ═══════════════════════════════════════════════════════════
//  Full Analysis
// ═══════════════════════════════════════════════════════════

TEST(KinematicChainTest, AnalyzeCam_FullCycle) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, 2 * M_PI, 0, 10, CamMotionType::Cycloidal}};

    auto results = analyze_cam(cam, 36);
    EXPECT_EQ(results.size(), 36u);
    EXPECT_NEAR(results.front().displacement, 0.0, 1e-9);
    EXPECT_NEAR(results.back().displacement, 0.0, 1e-6);
}

TEST(KinematicChainTest, CamFollower_AngleWraparound) {
    CamFollowerSystem cam;
    cam.base_radius = 20.0;
    cam.segments = {{0, M_PI, 0, 5, CamMotionType::ConstantVelocity}};

    // 3π (360° + 180°) should wrap to π
    auto state = solve_cam(cam, 3.0 * M_PI);
    EXPECT_NEAR(state.displacement, 5.0, 1e-6);
}