#include "game/sim/movement.h"

#include "check.h"

using namespace sots::sim;

static void test_vectors() {
    CHECK_NEAR(Distance({0, 0, 0}, {3, 4, 0}), 5.0, 1e-12);
    Vec3 p = AdvanceToward({0, 0, 0}, {10, 0, 0}, 4);
    CHECK_NEAR(p.x, 4.0, 1e-12);
    CHECK_NEAR(p.y, 0.0, 0.0);
    p = AdvanceToward({0, 0, 0}, {10, 0, 0}, 20);        // snaps to the destination
    CHECK_NEAR(p.x, 10.0, 0.0);
    p = AdvanceToward({1, 2, 3}, {1, 2, 3}, 5);          // already there
    CHECK_NEAR(p.z, 3.0, 0.0);
    p = AdvanceToward({0, 0, 0}, {3, 4, 0}, 2.5);        // half way along a 3-4-5
    CHECK_NEAR(p.x, 1.5, 1e-12);
    CHECK_NEAR(p.y, 2.0, 1e-12);
}

static void test_steps() {
    CHECK_NEAR(StraightStep(10, 0.5), 5.0, 0.0);
    CHECK_NEAR(StraightStep(10, kFullStep), 10.0, 0.0);
    CHECK_NEAR(StraightStep(0, kHalfStep), 0.0, 0.0);

    TuningTable t;
    t.STUTTER_SYSTEM_INFLUENCE_RADIUS = 100;
    t.STUTTER_MIN_SPEED = 0.2;
    t.STUTTER_MAX_SPEED = 1.0;
    CHECK_NEAR(NodeLineSpeed(10, 50, t), 6.0, 1e-12);     // 10 x (0.8 x 0.5 + 0.2)
    CHECK_NEAR(NodeLineSpeed(10, 0, t), 2.0, 1e-12);      // at a system: min profile
    CHECK_NEAR(NodeLineSpeed(10, 100, t), 10.0, 1e-12);   // at the radius: max profile
    CHECK_NEAR(NodeLineSpeed(10, 200, t), 18.0, 1e-12);   // no clamp (never reached in practice)
    TuningTable zero;
    CHECK_NEAR(NodeLineSpeed(10, 50, zero), 0.0, 0.0);    // no tuning -> no speed
}

static void test_stutter_segments() {
    CHECK_NEAR(DistPointToSegment({5, 3, 0}, {0, 0, 0}, {10, 0, 0}), 3.0, 1e-12);
    CHECK_NEAR(DistPointToSegment({-5, 3, 0}, {0, 0, 0}, {10, 0, 0}), std::sqrt(34.0), 1e-12);
    CHECK_NEAR(DistPointToSegment({15, 0, 0}, {0, 0, 0}, {10, 0, 0}), 5.0, 1e-12);
    CHECK_NEAR(DistPointToSegment({3, 4, 0}, {1, 1, 1}, {1, 1, 1}), std::sqrt(4 + 9 + 1), 1e-12);

    TuningTable t;
    t.STUTTER_SYSTEM_INFLUENCE_RADIUS = 50;
    t.STUTTER_MIN_SPEED = 0.2;
    t.STUTTER_MAX_SPEED = 1.0;
    const Vec3 from{0, 0, 0}, to{100, 0, 0};

    // One system 30 off the line: chord where (x-50)^2 + 900 <= 2500 -> x in [10, 90]
    std::vector<StutterSegment> s = BuildStutterSegments(from, to, {{50, 30, 0}}, t);
    CHECK_EQ(s.size(), std::size_t{1});
    CHECK_NEAR(s[0].start, 10.0, 1e-9);
    CHECK_NEAR(s[0].end, 90.0, 1e-9);
    CHECK_EQ(s[0].systemIndex, 0);
    CHECK_NEAR(s[0].speedFactor, 0.8 * 30 / 50 + 0.2, 1e-12);        // 0.68 for the whole chord

    // Out of reach, and exactly tangent (zero-length chord): no segments
    CHECK(BuildStutterSegments(from, to, {{50, 200, 0}}, t).empty());
    CHECK(BuildStutterSegments(from, to, {{50, 50, 0}}, t).empty());

    // Second system on the line near the end: chord [35, 135] clipped to [35, 100];
    // the overlap with [10, 90] is split at 62.5.
    s = BuildStutterSegments(from, to, {{50, 30, 0}, {85, 0, 0}}, t);
    CHECK_EQ(s.size(), std::size_t{2});
    CHECK_NEAR(s[0].start, 10.0, 1e-9);
    CHECK_NEAR(s[0].end, 62.5, 1e-9);
    CHECK_EQ(s[0].systemIndex, 0);
    CHECK_NEAR(s[0].speedFactor, 0.68, 1e-12);
    CHECK_NEAR(s[1].start, 62.5, 1e-9);
    CHECK_NEAR(s[1].end, 100.0, 1e-9);
    CHECK_EQ(s[1].systemIndex, 1);
    CHECK_NEAR(s[1].speedFactor, 0.2, 1e-12);                          // system on the line: min

    // Input order does not matter: segments come back sorted
    std::vector<StutterSegment> r = BuildStutterSegments(from, to, {{85, 0, 0}, {50, 30, 0}}, t);
    CHECK_EQ(r.size(), std::size_t{2});
    CHECK_EQ(r[0].systemIndex, 1);
    CHECK_EQ(r[1].systemIndex, 0);

    // A chord contained in an earlier one is split at the midpoint of the overlap
    s = BuildStutterSegments(from, to, {{50, 0, 0}, {50, 40, 0}}, t);   // [0,100] and [20,80]
    CHECK_EQ(s.size(), std::size_t{2});
    CHECK_NEAR(s[0].end, 60.0, 1e-9);
    CHECK_NEAR(s[1].start, 60.0, 1e-9);
    CHECK_NEAR(s[1].end, 80.0, 1e-9);

    TuningTable zero;
    CHECK(BuildStutterSegments(from, to, {{50, 0, 0}}, zero).empty());  // no radius: plain line

    // Advance along the [10, 90] x0.68 profile at node speed 20
    s = BuildStutterSegments(from, to, {{50, 30, 0}}, t);
    // half a turn to reach 10 at speed 20, then 0.5 x 13.6
    CHECK_NEAR(AdvanceAlongNodeLine(0, 1.0, 20, 100, s), 16.8, 1e-9);
    // from 85: 5 units at 13.6 take 0.36765 turns; the rest at 20
    CHECK_NEAR(AdvanceAlongNodeLine(85, 0.5, 20, 100, s), 90.0 + (0.5 - 5.0 / 13.6) * 20.0, 1e-9);
    CHECK_NEAR(AdvanceAlongNodeLine(0, 10.0, 20, 100, s), 100.0, 0.0);  // never past the end
    CHECK_NEAR(AdvanceAlongNodeLine(0, 1.0, 20, 100, {}), 20.0, 1e-12); // no spheres: plain speed
    CHECK_NEAR(AdvanceAlongNodeLine(30, 1.0, 0, 100, s), 30.0, 0.0);    // no speed: no movement
    CHECK_NEAR(AdvanceAlongNodeLine(30, 0.0, 20, 100, s), 30.0, 0.0);
}

static void test_resolve() {
    MoveStepResult r = ResolveMoveStep(5, 10, 20);
    CHECK_NEAR(r.moved, 5.0, 0.0);
    CHECK_NEAR(r.fraction, 1.0, 0.0);
    CHECK(!r.arrived);
    CHECK(!r.outOfFuel);

    r = ResolveMoveStep(5, 3, 20);                        // range 2.95 limits the step
    CHECK_NEAR(r.moved, 2.95, 1e-12);
    CHECK_NEAR(r.fraction, 0.59, 1e-12);

    r = ResolveMoveStep(5, 0, 20);                        // no fuel at all
    CHECK_NEAR(r.moved, 0.0, 0.0);
    CHECK(r.outOfFuel);
    CHECK(!r.arrived);

    r = ResolveMoveStep(5, 0, 0.01);                      // even a tiny hop needs range
    CHECK(r.outOfFuel);

    r = ResolveMoveStep(50, 100, 20);                     // arrives with step to spare
    CHECK_NEAR(r.moved, 20.0, 0.0);
    CHECK(r.arrived);
    CHECK_NEAR(r.fraction, 0.4, 1e-12);

    r = ResolveMoveStep(5, 0.02, 20);                     // range below the margin: stuck
    CHECK_NEAR(r.moved, 0.0, 0.0);
    CHECK(!r.outOfFuel);

    r = ResolveMoveStep(0, 10, 20);                       // zero step
    CHECK_NEAR(r.moved, 0.0, 0.0);
    CHECK_NEAR(r.fraction, 1.0, 0.0);

    r = ResolveMoveStep(5, 10, 0);                        // already at the destination
    CHECK(r.arrived);

    CHECK_NEAR(ConsumeShipRange(10, 3, false), 7.0, 0.0);
    CHECK_NEAR(ConsumeShipRange(2, 3, false), 0.0, 0.0);
    CHECK_NEAR(ConsumeShipRange(10, 3, true), 10.0, 0.0);

    CHECK_NEAR(RemainingPassTime(0.4, 1.0), 0.6, 1e-12);
    CHECK_NEAR(RemainingPassTime(0.4, 0.5), 0.3, 1e-12);
    CHECK_NEAR(RemainingPassTime(0.99995, 1.0), 0.0, 0.0);
    CHECK_NEAR(RemainingPassTime(1.0, 1.0), 0.0, 0.0);
}

static void test_multi_waypoint_turn() {
    // A fleet with speed 10 and plenty of range covers a 4-unit leg, then continues
    // with the remaining 0.6 of the turn onto the next leg.
    double dt = kFullStep;
    MoveStepResult a = ResolveMoveStep(StraightStep(10, dt), 100, 4);
    CHECK(a.arrived);
    dt = RemainingPassTime(a.fraction, dt);
    CHECK_NEAR(dt, 0.6, 1e-12);
    MoveStepResult b = ResolveMoveStep(StraightStep(10, dt), 96, 20);
    CHECK_NEAR(b.moved, 6.0, 1e-12);
    CHECK(!b.arrived);
    CHECK_NEAR(RemainingPassTime(b.fraction, dt), 0.0, 0.0);
}

static void test_jump() {
    {
        simtest::ScriptedRng rng({0.7f});
        JumpResult j = RollProbabilisticJump(1.0, 0.5, rng);
        CHECK(!j.arrived);
        CHECK_NEAR(j.stopFraction, 0.7, 1e-7);
        CHECK_EQ(rng.floatDraws(), std::size_t{1});
    }
    {
        simtest::ScriptedRng rng({0.3f});
        JumpResult j = RollProbabilisticJump(1.0, 0.5, rng);
        CHECK(j.arrived);
        CHECK_NEAR(j.stopFraction, 1.0, 0.0);
    }
    {   // efficiency scales the roll: 0.9 x 0.5 = 0.45 <= 0.5 arrives
        simtest::ScriptedRng rng({0.9f});
        JumpResult j = RollProbabilisticJump(0.5, 0.5, rng);
        CHECK(j.arrived);
    }
    {   // roll equal to the threshold is not "greater": arrives
        simtest::ScriptedRng rng({0.5f});
        JumpResult j = RollProbabilisticJump(1.0, 0.5, rng);
        CHECK(j.arrived);
    }
    {   // determinism: the same script gives the same outcome
        auto run = [] {
            simtest::ScriptedRng rng({0.6f, 0.2f, 0.95f});
            std::vector<double> out;
            for (int i = 0; i < 3; ++i) out.push_back(RollProbabilisticJump(1.0, 0.5, rng).stopFraction);
            return out;
        };
        CHECK(run() == run());
    }
}

int main() {
    test_vectors();
    test_steps();
    test_stutter_segments();
    test_resolve();
    test_multi_waypoint_turn();
    test_jump();
    return simtest::finish("test_movement");
}