#include "game/sim/movement.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <set>

#include "game/sim/numeric.h"

namespace sots::sim {

namespace {
constexpr double kMinChordLength = 0.01;

// The epsilon the engine's vector normalise uses: below it the direction is zeroed.
constexpr double kNormaliseEpsilon = 1.1920928955078125e-07;  // 2^-23, as a float32 literal

double Dot(const Vec3& a, const Vec3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
Vec3 Sub(const Vec3& a, const Vec3& b) { return Vec3{a.x - b.x, a.y - b.y, a.z - b.z}; }
Vec3 Lerp(const Vec3& a, const Vec3& b, double f) {
    return Vec3{a.x + (b.x - a.x) * f, a.y + (b.y - a.y) * f, a.z + (b.z - a.z) * f};
}
}  // namespace

double Distance(const Vec3& a, const Vec3& b) {
    const double dx = b.x - a.x, dy = b.y - a.y, dz = b.z - a.z;
    return std::sqrt(dx * dx + dy * dy + dz * dz);
}

Vec3 AdvanceToward(const Vec3& pos, const Vec3& dest, double amount) {
    const double d = Distance(pos, dest);
    if (amount >= d || d <= 0) return dest;
    return Lerp(pos, dest, amount / d);
}

double DistPointToSegment(const Vec3& p, const Vec3& a, const Vec3& b) {
    const Vec3 ab = Sub(b, a);
    const double len2 = Dot(ab, ab);
    double u = 0.0;
    if (len2 > 0) u = Clamp01(Dot(Sub(p, a), ab) / len2);
    return Distance(p, Lerp(a, b, u));
}

bool IsGateTransitWaypoint(int waypointType) { return waypointType == 4 || waypointType == 5; }
bool IsNodeWaypoint(int waypointType) { return waypointType == 3; }

double StraightStep(double speed, double dt) { return F32(speed * dt); }

double NodeLineSpeed(double nodeSpeed, double distToLineSystem, const TuningTable& t) {
    double ratio = 0.0;
    if (t.STUTTER_SYSTEM_INFLUENCE_RADIUS > 0) {
        ratio = distToLineSystem / t.STUTTER_SYSTEM_INFLUENCE_RADIUS;
    }
    return nodeSpeed * ((t.STUTTER_MAX_SPEED - t.STUTTER_MIN_SPEED) * ratio + t.STUTTER_MIN_SPEED);
}

bool SegmentSphereIntersect(const Vec3& from, const Vec3& to, const Vec3& centre, double radius,
                            double* tNear, double* tFar) {
    constexpr double kFltMax = 3.4028234663852886e38;
    const Vec3 d = Sub(to, from);
    const double a = Dot(d, d);
    if (a <= kNormaliseEpsilon) {
        *tNear = *tFar = kFltMax;
        return false;
    }
    const Vec3 fc = Sub(from, centre);
    const double b = 2.0 * Dot(d, fc);
    const double c = Dot(centre, centre) + Dot(from, from) - 2.0 * Dot(from, centre) - radius * radius;
    const double disc = b * b - 4.0 * a * c;
    double t0, t1;
    if (std::fabs(disc) < kNormaliseEpsilon) {
        t0 = t1 = -b / (2.0 * a);
        if (!(t0 >= 0.0 && t0 <= 1.0)) {
            *tNear = *tFar = kFltMax;
            return false;
        }
    } else if (disc < 0) {
        *tNear = *tFar = kFltMax;
        return false;
    } else {
        const double root = std::sqrt(disc);
        t0 = (-b - root) / (2.0 * a);
        t1 = (-b + root) / (2.0 * a);
    }
    // Reject a sphere the segment does not reach, and report an open end as the sentinel
    // the caller's clamp turns into the corresponding line end.
    if (t1 < 0.0) {
        *tNear = *tFar = -kFltMax;
        return false;
    }
    if (t0 > 1.0) {
        *tNear = *tFar = kFltMax;
        return false;
    }
    *tNear = t0 < 0.0 ? -kFltMax : t0;
    *tFar = t1 > 1.0 ? kFltMax : t1;
    return true;
}

std::vector<StutterSegment> BuildStutterSegments(const Vec3& from, const Vec3& to,
                                                 const std::vector<Vec3>& systems,
                                                 const TuningTable& t) {
    std::vector<StutterSegment> segs;
    const double radius = t.STUTTER_SYSTEM_INFLUENCE_RADIUS;
    const Vec3 d = Sub(to, from);
    const double len = F32(std::sqrt(F32(Dot(d, d))));
    if (len <= 0) return segs;

    for (std::size_t i = 0; i < systems.size(); ++i) {
        double t0 = 0, t1 = 0;
        if (!SegmentSphereIntersect(from, to, systems[i], radius, &t0, &t1)) continue;
        StutterSegment s;
        s.start = ClampT(F32(t0 * len), 0.0, len);
        s.end = ClampT(F32(t1 * len), 0.0, len);
        s.systemIndex = static_cast<int>(i);
        if (std::fabs(s.start - s.end) <= kMinStutterChord) continue;
        segs.push_back(s);
    }

    std::sort(segs.begin(), segs.end(),
              [](const StutterSegment& a, const StutterSegment& b) { return a.start < b.start; });

    // The overlap pass, reproduced exactly -- see the header. One forward sweep over
    // adjacent pairs, both boundaries set to the same value, nothing removed.
    for (std::size_t i = 0; i + 1 < segs.size(); ++i) {
        if (!(segs[i].end > segs[i + 1].start)) continue;
        const double x = F32(segs[i].end + 0.5 * (segs[i].end - segs[i + 1].start));
        segs[i].end = x;
        segs[i + 1].start = x;
    }

    // Per-segment profile: closest approach of the (post-merge) chord to its system. The
    // original computes this inside the step loop, after the merge, which is where the
    // absence of a clamp on dist/radius can bite.
    for (StutterSegment& s : segs) {
        const Vec3 a = Lerp(from, to, s.start / len);
        const Vec3 b = Lerp(from, to, s.end / len);
        const double dist = F32(DistPointToSegment(systems[static_cast<std::size_t>(s.systemIndex)], a, b));
        s.speedFactor = F32(NodeLineSpeed(1.0, dist, t));
    }
    return segs;
}

NodeLineStepResult NodeLineStep(double nodeSpeed, double dt, double lineLength,
                                const std::vector<StutterSegment>& segments) {
    NodeLineStepResult r;
    if (!(dt > 0.0)) return r;  // dt <= 0: nothing moves, and `time < dt` is false

    bool ranOut = false;
    for (const StutterSegment& s : segments) {
        if (s.start >= r.along) {
            if (r.time >= dt) break;
            const double tt = r.time + (s.start - r.along) / nodeSpeed;  // plain speed in the gap
            if (tt > dt) {
                r.along = r.along + (dt - r.time) * nodeSpeed;
                r.time = dt;
                ranOut = true;
                break;
            }
            r.time = tt;
            r.along = s.start;
        }
        const double v = F32(s.speedFactor * nodeSpeed);
        if (s.end < r.along) continue;  // swallowed or inverted by the overlap pass
        if (dt <= r.time) break;
        const double tt = r.time + (s.end - r.along) / v;
        if (tt > dt) {
            r.along = r.along + (dt - r.time) * v;
            r.time = dt;
            ranOut = true;
            break;
        }
        r.time = tt;
        r.along = s.end;
        if (r.time >= dt) break;
    }

    // Segments exhausted with time to spare: run straight to the end of the line.
    if (!ranOut && r.time < dt) {
        if (!(lineLength < r.along) && dt > r.time) {
            const double tt = r.time + (lineLength - r.along) / nodeSpeed;
            if (tt <= dt) {
                r.time = tt;
                r.along = lineLength;
            } else {
                r.along += nodeSpeed * (dt - r.time);
                r.time = dt;
            }
        }
    }

    r.arrived = r.time < dt || std::fabs(r.along - lineLength) < kNormaliseEpsilon;
    return r;
}

// ---------------------------------------------------------------------------------------
// Range, clamping and arrival
// ---------------------------------------------------------------------------------------

double FleetMinShipRange(const std::vector<double>& shipRanges, double bias) {
    // The accumulator starts at FLT_MAX, so an empty fleet is unconstrained rather than
    // stranded -- worth knowing, because the stranded test compares against exactly 0.
    double best = static_cast<double>(std::numeric_limits<float>::max());
    for (double r : shipRanges) best = std::min(best, r);
    return F32(best + bias);
}

MoveStepResult ResolveMoveStep(double step, double minShipRange, double distance) {
    MoveStepResult r;
    double range = F32(minShipRange + kRangeGrace);
    if (distance > range) {
        // The original asks for the minimum range a second time, with no grace margin, and
        // takes the whole budget away when it is exactly zero. It zeroes the *range*, not
        // the step: the step is still the divisor of the pass fraction.
        if (minShipRange == 0.0) {
            range = 0.0;
            r.stranded = true;
        }
    }
    const double move = std::min(std::min(range, step), distance);
    r.range = range;
    r.moved = move;  // deliberately not floored at 0
    r.arrived = move == distance;
    return r;
}

Vec3 AdvanceAlongDirection(const Vec3& pos, const Vec3& dest, double move) {
    const Vec3 delta = Sub(dest, pos);
    const double len = std::sqrt(Dot(delta, delta));
    if (!(len > kNormaliseEpsilon)) return pos;  // the direction is zeroed; nothing moves
    const Vec3 dir{delta.x / len, delta.y / len, delta.z / len};
    return Vec3{F32(pos.x + F32(dir.x * move)), F32(pos.y + F32(dir.y * move)),
                F32(pos.z + F32(dir.z * move))};
}

double ConsumeShipRange(double shipRange, double moved, bool exempt) {
    if (exempt) return shipRange;
    const double v = F32(shipRange - moved);
    return v < 0.0 ? 0.0 : v;
}

// ---------------------------------------------------------------------------------------
// Pass fraction
// ---------------------------------------------------------------------------------------

double PassFraction(int waypointType, double distance, double step) {
    if (!IsNodeWaypoint(waypointType)) return 1.0;
    if (step == 0) return 1.0;  // the clamp of an infinity/NaN quotient lands at the bound
    return Clamp01(F32(distance / step));
}

double BlockedPassFraction(double moved, double step) {
    if (step == 0) return 1.0;
    return Clamp01(F32(moved / step));
}

double RemainingPassTime(double fraction, double dt) {
    if (fraction < kPassCompleteFraction) return F32((1.0 - fraction) * dt);
    return 0.0;
}

// ---------------------------------------------------------------------------------------
// Probabilistic jump
// ---------------------------------------------------------------------------------------

JumpResult RollProbabilisticJump(double castEfficiency, double castThreshold, IRandom& rng) {
    JumpResult r;
    r.roll = rng.NextFloat();
    r.draws = 1;
    // The efficiency is read as a float32 and the product is stored back to a float32 slot
    // before the comparison, so both narrowings are real.
    const double v = F32(static_cast<double>(r.roll) * F32(castEfficiency));
    if (v > castThreshold) {
        r.arrived = false;
        r.scatter = v;
        rng.NextUInt32();  // the random direction the miss is scattered along
        r.draws = 2;
    } else {
        r.arrived = true;
        r.scatter = 0;
    }
    return r;
}

// ---------------------------------------------------------------------------------------
// The turn's pass schedule
// ---------------------------------------------------------------------------------------

std::vector<MovementPass> PlanFleetMovement(const std::vector<FleetMovementEntry>& fleets) {
    std::set<int> pursuers, prey, followers, resolved;
    std::map<int, int> preyOf;
    for (const FleetMovementEntry& f : fleets) {
        if (f.targetFleetId == 0) continue;
        if (f.relation != 0) {
            followers.insert(f.fleetId);
        } else {
            pursuers.insert(f.fleetId);
            prey.insert(f.targetFleetId);
            preyOf[f.fleetId] = f.targetFleetId;
        }
    }

    std::vector<MovementPass> out;
    // Pass 1: the prey move half a turn.
    for (int id : prey) out.push_back({id, kHalfStep, 1});

    // Pass 2: every pursuer (of the *original* set) re-validates and moves half a turn. A
    // pursuer that arrives retires itself and its prey from the rest of the schedule.
    const std::set<int> pursuersAtPass2 = pursuers;
    for (int id : pursuersAtPass2) {
        out.push_back({id, kHalfStep, 2});
        const FleetMovementEntry* e = nullptr;
        for (const FleetMovementEntry& f : fleets)
            if (f.fleetId == id) e = &f;
        if (!e || !e->caught) continue;
        const int target = preyOf[id];
        resolved.insert(id);
        resolved.insert(target);
        pursuers.erase(id);
        prey.erase(target);
        followers.erase(target);
    }

    // Pass 3: the prey that got away take their second half turn.
    for (int id : prey) out.push_back({id, kHalfStep, 3});

    // Pass 4: everyone else, in fleet order. A pursuer whose chase failed is still in the
    // pursuer set and gets a half turn; anything already scheduled is skipped.
    for (const FleetMovementEntry& f : fleets) {
        const int id = f.fleetId;
        if (resolved.count(id)) continue;
        if (pursuers.count(id)) {
            out.push_back({id, kHalfStep, 4});
            continue;
        }
        if (prey.count(id)) continue;
        if (followers.count(id)) continue;
        out.push_back({id, kFullStep, 4});
    }

    // Pass 5: the followers take a full turn.
    for (int id : followers) out.push_back({id, kFullStep, 5});
    return out;
}

// ---------------------------------------------------------------------------------------
// Gate traffic
// ---------------------------------------------------------------------------------------

std::vector<int> GateTrafficTotals(const std::vector<GateTrafficEntry>& fleets, int playerCount) {
    std::vector<int> totals(static_cast<std::size_t>(std::max(playerCount, 0)), 0);
    for (const GateTrafficEntry& f : fleets) {
        if (f.waypointType < 0) continue;  // no waypoints: the fleet is not in transit
        if (!IsGateTransitWaypoint(f.waypointType)) continue;
        if (f.ownerIndex < 0 || f.ownerIndex >= static_cast<int>(totals.size())) continue;
        totals[static_cast<std::size_t>(f.ownerIndex)] += f.traffic;
    }
    return totals;
}

}  // namespace sots::sim