#include "sim/squad.h"

#include "ai/lane_registry.h"

namespace
{

// Локальные слоты строя: x = вперёд, y = вправо. Множатся на formationSpacing.
const float kWedge[NUM_AGENTS][2] = {
    { 1.0f,  0.0f},
    { 0.0f, -0.9f},
    { 0.0f,  0.9f},
    {-1.0f, -1.8f},
    {-1.0f,  1.8f},
};

const float kLine[NUM_AGENTS][2] = {
    {0.0f, -2.0f},
    {0.0f, -1.0f},
    {0.0f,  0.0f},
    {0.0f,  1.0f},
    {0.0f,  2.0f},
};

// «Недостижимо» в поле путей и потолок длины натягиваемой верёвки.
constexpr uint16_t NAV_FAR = 0xFFFF;
constexpr int      NAV_PATH_MAX = 64;

}   // namespace

const char* FormationName(Formation f)
{
    switch (f)
    {
        case Formation::WEDGE: return "WEDGE";
        case Formation::LINE:  return "LINE";
        case Formation::RING:  return "RING";
        default:               return "?";
    }
}

void Squad::Init(ecs::World& world, Vec2 at)
{
    anchor = at;
    anchorPrev = at;
    anchorVel = {};
    forward = {1.0f, 0.0f};
    state = AgentState::HOLDING;
    idleTimer = g_tune.holdDelay;
    holdElapsed = 0.0f;
    urgency = 0.0f;
    sprinting = false;
    formSpacing = 1.0f;
    formStretch = 1.0f;

    meleeCount = std::min(std::max(meleeCount, 0), NUM_AGENTS);
    pistolCount = std::min(std::max(pistolCount, 0), NUM_AGENTS - meleeCount);

    // Сначала создаём все сущности бойцов, и только потом берём указатель на
    // плотный массив пула: до конца заполнения он может переехать.
    ecs::Entity ids[NUM_AGENTS];
    for (int i = 0; i < NUM_AGENTS; ++i)
    {
        ids[i] = world.Create();
        world.Add<Agent>(ids[i], Agent{});
    }
    agents.p = world.All<Agent>().data();
    agents.n = NUM_AGENTS;

    for (int i = 0; i < NUM_AGENTS; ++i)
    {
        Agent& a = agents[i];
        a = Agent{};
        a.self = ids[i];
        a.slotIndex = i;

        // Клинки занимают первые слоты: в клине это остриё и левое крыло,
        // то есть ровно те, кто и так упирается в противника первым.
        // Пистолеты — последние слоты: короткий ствол уезжает в хвост клина.
        const bool isMelee = (i < meleeCount);
        const bool isPistol = !isMelee && (i >= NUM_AGENTS - pistolCount);

        // Стартовое оружие — то же, что и было: клинок, карабин, пистолет.
        // Снаряжения на бойце НЕТ: броня меняет скорость, а на скорости меряются
        // критерии приёмки. Комплекты раздаёт игровой режим (Game::EquipKits).
        const ItemId weapon = isMelee    ? ItemId::SABER
                              : isPistol ? ItemId::PISTOL
                                         : ItemId::RIFLE;

        a.weaponEntity = SpawnWeapon(world, weapon);
        world.Add<Inventory>(ids[i], Inventory{weapon, ItemId::NONE, ItemId::NONE});

        a.melee.sweepSign = (i & 1) ? -1.0f : 1.0f;
        a.trig.rng = 1u + uint32_t(i) * 2654435761u;   // свой поток на слот
        a.pos = SlotWorld(i);
        a.prevPos = a.pos;
        a.postPos = a.pos;
        a.facing = 0.0f;
        a.state = state;
        a.currentTargetId = -1;
        a.commitTimer = 0.0f;
        // Фазы решателя разведены: за кадр решает максимум один агент (6.5).
        a.solveTimer = g_tune.solveInterval * (float(i) / float(NUM_AGENTS));

        SyncLoadout(world, a);
        a.gun.ammo = a.gun.def.magazine;   // выходим с полным магазином

        // Незанятый слот: сущность есть (по ней ходят все циклы), человека нет.
        // Не alive — значит, ни решатель, ни твари, ни правила огня его не
        // видят; не present — значит, его не видит и игрок.
        a.present = (i < roster);
        a.alive = a.present;
        a.hp = a.alive ? a.maxHp : 0.0f;
    }
}

Vec2 Squad::SlotLocal(int i) const
{
    // Интервал и вытяжение — это ТЕКУЩЕЕ состояние строя, а не константа:
    // на стоянке отряд собирается, в беге растягивается вдоль движения.
    const float s = g_tune.formationSpacing * formSpacing;
    const float fwd = s * formStretch;

    switch (formation)
    {
        case Formation::LINE:
            return {kLine[i][0] * fwd, kLine[i][1] * s};
        case Formation::RING:
        {
            const float ang = float(i) * (TWO_PI_F / float(NUM_AGENTS));
            const Vec2 d = FromAngle(ang) * 1.5f;
            return {d.x * fwd, d.y * s};
        }
        case Formation::WEDGE:
        default:
            return {kWedge[i][0] * fwd, kWedge[i][1] * s};
    }
}

Vec2 Squad::SlotWorld(int i) const
{
    const Vec2 local = SlotLocal(i);
    const Vec2 right = Perp(forward);
    return anchor + forward * local.x + right * local.y;
}

Vec2 Squad::GoalFor(int i) const
{
    // ОТСТАВШИЙ ВОЗВРАЩАЕТСЯ В СТРОЙ, что бы ни говорил решатель.
    //
    // Без этого правила отряд намертво вставал, и вот почему. Якорь упирается в
    // поводок до центра отряда -> отряд переходит в HOLDING -> отставшие в
    // HOLDING держат ОГНЕВУЮ ПОЗИЦИЮ вместо возврата в слот -> центр не
    // догоняет -> якорь не может идти дальше. Круг замкнулся, и снаружи это
    // выглядело как «пятеро не проходят там, где одиночка проходит».
    //
    // Проверено отдельно: ширина коридоров тут ни при чём — отряд вставал и на
    // проходах в три, четыре и пять клеток, и даже в пустых комнатах.
    //
    // Правило заодно честное по игре: отряд, растянувшийся на полэтажа, — это
    // не строй, а цепочка одиночек, и собраться он обязан сам.
    const float lag = Distance(agents[i].pos, SlotWorld(i));
    if (lag > g_tune.straggleDist) return SlotWorld(i);

    // Боец ближнего боя ведёт себя одинаково в обоих состояниях: пока цель
    // внутри его круга, он идёт к ней, а не в слот. Иначе клинок оказывался бы
    // полезен только на стоянке — а он нужен как раз в движении.
    // Но в спринте цель отменяется: бежим, а не деремся.
    if (agents[i].IsMelee() && !sprinting) return agents[i].postPos;
    if (agents[i].IsMelee()) return SlotWorld(i);

    // В движении — слот строя, на месте — выбранная решателем огневая позиция.
    return (state == AgentState::ADVANCING) ? SlotWorld(i) : agents[i].postPos;
}

// -----------------------------------------------------------------------------
// Поле путей: BFS от клетки цели по проходимым клеткам, четыре соседа.
// Пересчёт только когда цель сменила клетку — это несколько тысяч клеток и
// стоит меньше одного кадра рендера.
// -----------------------------------------------------------------------------
void Squad::RebuildNav(NavField& field, const Tilemap& map, Vec2 goal) const
{
    int gx = int(std::floor(goal.x));
    int gy = int(std::floor(goal.y));

    // Якорь игрок может завести в стену — поле строим от ближайшей свободной
    // клетки, иначе отряд остался бы вообще без путей.
    if (!Tilemap::InBounds(gx, gy) || map.IsWall(gx, gy))
    {
        bool found = false;
        for (int r = 1; r <= 5 && !found; ++r)
            for (int dy = -r; dy <= r && !found; ++dy)
                for (int dx = -r; dx <= r && !found; ++dx)
                {
                    const int x = gx + dx, y = gy + dy;
                    if (!Tilemap::InBounds(x, y) || map.IsWall(x, y)) continue;
                    gx = x; gy = y; found = true;
                }
        if (!found) return;
    }

    if (gx == field.goalX && gy == field.goalY && !field.cost.empty()) return;
    field.goalX = gx;
    field.goalY = gy;

    field.cost.assign(size_t(MAP_W) * MAP_H, NAV_FAR);
    std::vector<int> queue;
    queue.reserve(size_t(MAP_W) * MAP_H);

    field.cost[size_t(gy) * MAP_W + gx] = 0;
    queue.push_back(gy * MAP_W + gx);

    static const int DX[4] = {1, -1, 0, 0};
    static const int DY[4] = {0, 0, 1, -1};
    for (size_t head = 0; head < queue.size(); ++head)
    {
        const int c = queue[head];
        const int cx = c % MAP_W;
        const int cy = c / MAP_W;
        const uint16_t nd = uint16_t(field.cost[size_t(c)] + 1);
        for (int k = 0; k < 4; ++k)
        {
            const int x = cx + DX[k], y = cy + DY[k];
            if (!Tilemap::InBounds(x, y) || map.IsWall(x, y)) continue;
            const size_t n = size_t(y) * MAP_W + x;
            if (field.cost[n] <= nd) continue;
            field.cost[n] = nd;
            queue.push_back(int(n));
        }
    }
}

Vec2 Squad::NavWaypoint(const NavField& field, const Tilemap& map, Vec2 from, Vec2 goal) const
{
    // Видно цель — идём в цель. Поле нужно ровно там, где прямой не хватает.
    if (field.cost.empty() || !map.RaycastBlocked(from, goal)) return goal;

    int cx = int(std::floor(from.x));
    int cy = int(std::floor(from.y));
    if (!Tilemap::InBounds(cx, cy)) return goal;

    uint16_t here = field.cost[size_t(cy) * MAP_W + cx];
    if (here == NAV_FAR || here == 0) return goal;   // вне поля или уже пришли

    // Спуск по градиенту. Длина ограничена: дальше горизонта верёвка всё равно
    // не натягивается, а лишние клетки — лишняя работа на каждого бойца.
    Vec2 path[NAV_PATH_MAX];
    int n = 0;
    static const int DX[4] = {1, -1, 0, 0};
    static const int DY[4] = {0, 0, 1, -1};
    while (n < NAV_PATH_MAX && here > 0)
    {
        int bx = -1, by = -1;
        uint16_t best = here;
        for (int k = 0; k < 4; ++k)
        {
            const int x = cx + DX[k], y = cy + DY[k];
            if (!Tilemap::InBounds(x, y)) continue;
            const uint16_t d = field.cost[size_t(y) * MAP_W + x];
            if (d >= best) continue;
            best = d; bx = x; by = y;
        }
        if (bx < 0) break;
        cx = bx; cy = by; here = best;
        path[n++] = Vec2{float(cx) + 0.5f, float(cy) + 0.5f};
    }
    if (n == 0) return goal;

    // Натягиваем верёвку: самая дальняя точка пути, которую боец ещё видит.
    for (int i = n - 1; i >= 1; --i)
        if (!map.RaycastBlocked(from, path[i])) return path[i];
    return path[0];
}

Vec2 Squad::Centroid() const
{
    // Центр масс считается по ЖИВЫМ: поводок якоря не должен тянуть отряд
    // к телу павшего.
    Vec2 c{};
    int n = 0;
    for (int i = 0; i < NUM_AGENTS; ++i)
    {
        if (!agents[i].alive) continue;
        c += agents[i].pos;
        ++n;
    }
    return (n > 0) ? c / float(n) : anchor;
}

void Squad::UpdateAnchorAndState(Vec2 moveWorldDir, bool sprint, const Tilemap& map, float dt)
{
    anchorPrev = anchor;

    // --- теснота ---------------------------------------------------------------
    // Сколько свободного места вокруг ТЕЛ. Считается грубо: доля проходимых
    // клеток в квадрате рядом. Этого хватает, чтобы отличить зал от коридора и
    // от дверного проёма, а честный замер ширины прохода стоил бы поиска пути
    // там, где его нет.
    //
    // Считается ЗДЕСЬ, до всего остального: от тесноты зависят и доворот строя,
    // и его ширина, и обе вещи нужны в этом же шаге.
    int freeTiles = 0, seenTiles = 0;
    {
        const Vec2 body = Centroid();
        const int ax = int(body.x), ay = int(body.y);
        for (int dy = -2; dy <= 2; ++dy)
            for (int dx = -2; dx <= 2; ++dx)
            {
                const int nx = ax + dx, ny = ay + dy;
                if (nx < 0 || ny < 0 || nx >= MAP_W || ny >= MAP_H) continue;
                ++seenTiles;
                if (!map.IsWall(nx, ny)) ++freeTiles;
            }
    }
    const float openness = (seenTiles > 0) ? float(freeTiles) / float(seenTiles) : 1.0f;
    const float tight = Clampf((0.72f - openness) / 0.32f, 0.0f, 1.0f);

    const bool moving = LengthSq(moveWorldDir) > 1e-6f;
    const AgentState prevState = state;
    const Vec2 c = Centroid();

    // --- натяжение поводка ----------------------------------------------------
    // Поводок УПРУГИЙ, а не жёсткий. За мягкой границей курсор продолжает
    // уходить, но идёт всё тяжелее, и до жёсткого предела не доходит никогда.
    //
    // Жёсткий поводок здесь не годился принципиально: он мгновенно упирается,
    // дистанция намертво встаёт в одно значение, и по ней уже нельзя отличить
    // «подправил направление» от «тяну отряд вперёд изо всех сил». Упругий
    // поводок превращает дистанцию в непрерывную величину: чем сильнее тянешь,
    // тем дальше уезжает курсор и тем быстрее бегут за ним.
    const float soft = std::max(0.2f, g_tune.anchorLeash);
    const float hard = std::max(soft + 0.2f, g_tune.anchorLeashMax);
    const float start = soft * Clampf(g_tune.urgeStartFrac, 0.0f, 0.95f);
    const float dist = Distance(anchor, c);

    float pull = 0.0f;
    if (moving) pull = Clampf((dist - start) / (hard - start), 0.0f, 1.0f);
    if (moving && sprint) pull = 1.0f;   // Shift — приказ бежать, без уговоров

    const float rate = (pull > urgency) ? g_tune.urgeAttack : g_tune.urgeRelease;
    urgency += (pull - urgency) * (1.0f - std::exp(-std::max(0.01f, rate) * dt));
    urgency = Clampf(urgency, 0.0f, 1.0f);
    // Бег без стрельбы — либо по прямому приказу, либо когда курсор реально
    // уехал: отряд догоняет и ему не до целей.
    sprinting = (moving && sprint) || (urgency >= g_tune.sprintAt);

    // --- сам курсор -----------------------------------------------------------
    // У него есть разгон и торможение: мгновенная смена скорости — это и есть
    // «топорно». Отпустил клавиши — курсор доезжает, а не втыкается в грунт.
    const float over = Clampf((dist - soft) / (hard - soft), 0.0f, 1.0f);
    const float dragK = std::max(0.08f, 1.0f - Clampf(g_tune.leashDrag, 0.0f, 1.0f) * over);
    const float speed = (sprint ? g_tune.anchorSpeedFast : g_tune.anchorSpeed) * dragK;

    const Vec2 want = moving ? Normalized(moveWorldDir, forward) * speed : Vec2{};
    const Vec2 dv = want - anchorVel;
    anchorVel += ClampLength(dv, std::max(0.1f, g_tune.anchorAccel) * dt);
    anchor += anchorVel * dt;

    // Ориентация строя ПОВОРАЧИВАЕТСЯ, а не щёлкает. Раньше она мгновенно
    // равнялась направлению ввода, и на каждом повороте слоты клина
    // перепрыгивали через весь отряд: бойцы бросали дорогу и бежали вбок,
    // занимать новые места. Снаружи это и читалось как «строй тормозит» —
    // отряд был занят перестроением, а не движением.
    if (moving)
    {
        const Vec2 want = Normalized(moveWorldDir, forward);

        // В ЗАЛЕ доворот плавный: мгновенный заставлял бойцов бросать дорогу и
        // бежать вбок за перепрыгнувшими слотами. В ТЕСНОТЕ наоборот — почти
        // мгновенный: там строй и так колонна, и колонна, смотрящая в прошлую
        // сторону, упирается в стену. Замер поймал это сразу: со сглаживанием
        // на всех случаях автопилот перестал проходить даже первый этаж.
        const float rate = Lerpf(g_tune.formTurnRate, 40.0f, tight);
        const float k = 1.0f - std::exp(-rate * dt);
        forward = Normalized(forward + (want - forward) * k, want);
    }

    // Жёсткий предел. Сопротивление обычно не даёт до него дойти, но отряд может
    // и застрять в стене — тогда курсор без предела уехал бы за полкарты.
    const Vec2 d = anchor - c;
    const float len = Length(d);
    if (len > hard && len > 1e-5f)
    {
        const Vec2 n = d / len;
        anchor = c + n * hard;
        const float vn = Dot(anchorVel, n);
        if (vn > 0.0f) anchorVel -= n * vn;
    }

    map.ResolveCircle(anchor, g_tune.anchorRadius);

    // --- состояние отряда ------------------------------------------------------
    if (moving)
    {
        idleTimer = 0.0f;
        state = AgentState::ADVANCING;
        holdElapsed = 0.0f;
    }
    else
    {
        idleTimer += dt;
        if (idleTimer >= g_tune.holdDelay) state = AgentState::HOLDING;
        if (state == AgentState::HOLDING) holdElapsed += dt;
    }

    // --- дыхание строя ---------------------------------------------------------
    const float wantSpacing = (state == AgentState::HOLDING)
                                  ? g_tune.holdSpacing
                                  : Lerpf(1.0f, g_tune.sprintSpacing, urgency);
    // В тесноте строй И сжимается вбок, И вытягивается вдоль хода: это и есть
    // колонна. Числа в tuning, чтобы крутились на живой игре.
    const float wantStretch = Lerpf(1.0f, g_tune.sprintStretch, urgency);
    const float tightSpacing = Lerpf(wantSpacing, g_tune.tightSpacing, tight);
    const float tightStretch = Lerpf(wantStretch, g_tune.tightStretch, tight);
    const float k = 1.0f - std::exp(-std::max(0.01f, g_tune.formMorphRate) * dt);
    formSpacing += (tightSpacing - formSpacing) * k;
    formStretch += (tightStretch - formStretch) * k;

    if (state != prevState)
    {
        for (int i = 0; i < NUM_AGENTS; ++i)
        {
            Agent& a = agents[i];
            a.commitTimer = 0.0f;
            // Встали — остаёмся на месте, пока решатель не найдёт лучше (6.5).
            if (state == AgentState::HOLDING) a.postPos = a.pos;
        }
        if (state == AgentState::HOLDING) holdElapsed = 0.0f;
    }

    for (int i = 0; i < NUM_AGENTS; ++i) agents[i].state = state;
}

void Squad::UpdateMovement(const Tilemap& map, const LaneRegistry* lanes,
                           const std::vector<Target>& targets, float dt)
{
    // Скорость — не тумблер «шаг/рывок», а непрерывная величина от натяжения.
    const float baseSpeed = MoveSpeed();


    for (int i = 0; i < NUM_AGENTS; ++i)
    {
        Agent& a = agents[i];
        a.prevPos = a.pos;
        if (!a.alive) { a.vel = {}; continue; }   // упавший лежит там, где упал

        // Захваченного не двигает НИЧТО: ни строй, ни решатель, ни расступание
        // из чужой линии. В этом вся цена захвата — боец выпал из геометрии
        // отряда и стоит там, где его взяли, пока тварь не отпустит или не умрёт.
        // Оружие при этом работает: захват держит ноги, а не руки.
        if (a.Grabbed()) { a.vel = {}; a.stuckFor = 0.0f; continue; }

        const Vec2 rawGoal = GoalFor(i);

        // Упёрся — веди по геометрии. Боец хочет идти (цель не под ногами), но
        // скорость почти нулевая: значит прямая упирается в стену, и обход
        // стены толкает обратно. Именно так отставший за углом замирал навсегда.
        //
        // Счётчик сбрасывается НЕ по факту «поехал», а по факту «цель видно».
        // Разница принципиальная: сброс по скорости давал дребезг — обход
        // трогал бойца с места, поле тут же выключалось, боец упирался в ту же
        // стену, и так по кругу со средней скоростью в шаг за две секунды.
        // Пока цель за стеной, режим обхода ДЕРЖИТСЯ, и боец доходит по дуге.
        const bool wantsMove = DistanceSq(a.pos, rawGoal) > g_tune.navGoalReached *
                                                            g_tune.navGoalReached;
        if (!wantsMove || !map.RaycastBlocked(a.pos, rawGoal))
            a.stuckFor = 0.0f;
        else if (Length(a.vel) < g_tune.navStuckSpeed * baseSpeed)
            a.stuckFor += dt;

        // ЧТО именно держит бойца — вот вся развилка. Стена держит навсегда, и
        // из-под неё надо выводить по геометрии. Спина соседа держит на секунду,
        // и обходить её нельзя: в проходе очередь стоит именно так, и отряд,
        // отправленный в обход своей же колонны, входит в горловину шеренгой
        // (критерий 11.6 ловит это сразу).
        //
        // Пробовал развилку по расстоянию до цели — это угадайка: у очереди в
        // проходе и у отставшего одиночки дистанции пересекаются, и любой порог
        // ломал либо строй, либо проходимость.
        // Кому обход положен: тому, кто УПЁРСЯ и при этом ПОТЕРЯЛ СТРОЙ.
        // Второе условие важно не меньше первого — боец, стоящий на своём месте,
        // обходить ничего не должен: он не потерялся, его держит бой.
        Vec2 goal = rawGoal;
        if (a.stuckFor > g_tune.navStuckTime &&
            Distance(a.pos, SlotWorld(i)) > g_tune.straggleDist)
        {
            ++navUses;
            RebuildNav(nav[i], map, rawGoal);
            goal = NavWaypoint(nav[i], map, a.pos, rawGoal);
        }

        const bool lunging = a.IsMelee() && a.melee.state == MeleeState::STRIKE;

        // --- рывок клинка --------------------------------------------------
        // В активном окне удара ось уже зафиксирована, и боец едет по ней.
        // Seek/separation/расступание тут молчат: удар — это обязательство,
        // а не предложение. Стены остаются: сквозь них не рвётся никто.
        if (lunging)
        {
            const float speed = g_tune.meleeLungeSpeed;
            const Vec2 dir = FromAngle(a.melee.dir);
            Vec2 desired = dir * speed;
            desired += WallAvoidForce(a, map, dir, speed) * g_tune.wWallAvoid;
            IntegrateAgent(a, desired, speed, dt, map, g_tune.meleeLungeAccel);
            continue;
        }

        // --- обычные слои, порядок 6.7 --------------------------------------
        // speedMul — это снаряжение: плита тормозит, шпоры разгоняют. Без
        // снаряжения он равен 1, и движение остаётся прежним до последнего бита.
        const float maxSpeed = baseSpeed * a.SpeedScale() * a.speedMul;

        Vec2 desired = SeekForce(a, goal, maxSpeed) * g_tune.wSeek;
        desired += SeparationForce(a, agents.data(), NUM_AGENTS, maxSpeed) * g_tune.wSeparation;
        // Замах — это обязательство. Выдёргивать бойца из чужой линии огня
        // посреди него бессмысленно: стрелок всё равно не выстрелит (правило
        // 6.3 запрещает), а удар сорвётся. Расступаться боец будет до и после.
        if (lanes && !(a.IsMelee() && a.melee.Committed()))
            desired += lanes->AvoidanceForce(i, a.pos, a.bodyRadius, maxSpeed) * g_tune.wLaneAvoid;
        desired += TargetPushForce(a, targets, maxSpeed) * g_tune.wTargetPush;
        desired += WallAvoidForce(a, map, Normalized(goal - a.pos), maxSpeed) * g_tune.wWallAvoid;

        IntegrateAgent(a, desired, maxSpeed, dt, map);
    }
}