#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},
};

}   // 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.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
{
    // Боец ближнего боя ведёт себя одинаково в обоих состояниях: пока цель
    // внутри его круга, он идёт к ней, а не в слот. Иначе клинок оказывался бы
    // полезен только на стоянке — а он нужен как раз в движении.
    // Но в спринте цель отменяется: бежим, а не деремся.
    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;
}

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;

    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) forward = Normalized(moveWorldDir, forward);

    // Жёсткий предел. Сопротивление обычно не даёт до него дойти, но отряд может
    // и застрять в стене — тогда курсор без предела уехал бы за полкарты.
    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);
    const float wantStretch = Lerpf(1.0f, g_tune.sprintStretch, urgency);
    const float k = 1.0f - std::exp(-std::max(0.01f, g_tune.formMorphRate) * dt);
    formSpacing += (wantSpacing - formSpacing) * k;
    formStretch += (wantStretch - 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 = {}; continue; }

        const Vec2 goal = GoalFor(i);
        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);
    }
}