//+——————————————————————————————————————————————————————————————————+
//|                                                         C_AO_FBL |
//|                                  Copyright 2007-2025, Andrey Dik |
//|                                https://www.mql5.com/ru/users/joo |
//+——————————————————————————————————————————————————————————————————+
#include "#C_AO.mqh"


//────────────────────────────────────────────────────────────────────
class C_AO_FBL : public C_AO
{
public: //------------------------------------------------------------
  ~C_AO_FBL () { }
  C_AO_FBL ()
  {
    ao_name = "FBL";
    ao_desc = "Flock by Leader";
    ao_link = "https://www.mql5.com/ru/articles/21821";

    popSize = 50;
    k       = 5;
    phi     = 0.5;   // [F1]
    wSep    = 0.15;  // [F6]

    ArrayResize (params, 4);
    params [0].name = "popSize"; params [0].val = popSize;
    params [1].name = "k";       params [1].val = k;
    params [2].name = "phi";     params [2].val = phi;
    params [3].name = "wSep";    params [3].val = wSep;
  }

  void SetParams ()
  {
    popSize = (int)params [0].val;
    k       = (int)params [1].val;
    phi     =      params [2].val;
    wSep    =      params [3].val;
  }

  bool Init (const double &rangeMinP  [],
             const double &rangeMaxP  [],
             const double &rangeStepP [],
             const int     epochsP = 0);

  void Moving   ();
  void Revision ();

  //------------------------------------------------------------------
  int    k;    // k ближайших соседей для DkNB / DR-kNB / ARF
  double phi;  // коэффициент притяжения к gBest
  double wSep; // вес вектора separation                                [F6]

private: //---------------------------------------------------------
  double V              []; // скорости              [popSize * coords]
  double Vsnap          []; // снапшот скоростей     [popSize * coords]  [F4]
  double Vmax           []; // ограничение скорости  [coords]
  double cPsnap         []; // снапшот позиций       [popSize * coords]
  double sepVec         []; // вектор separation     [coords]
  double distM          []; // норм. матрица расст.  [popSize × popSize]
  double dMax           []; // d_max^i               [popSize]
  int    dknbSz         []; // |DkNB_t(x_i)|         [popSize]
  int    drknbSz        []; // |DR-kNB_t(x_i)|       [popSize]
  double arf            []; // ARF_t(A_i)             [popSize]
  int    knnIdx         []; // k-NN индексы           [popSize * kEff]
  int    subflockId     []; // субфлок агента (-1=выброс) [popSize]
  int    leaderIdx      []; // индекс лидера субфлока [popSize]
  int    role           []; // 0=лидер 1=послед. 2=выброс [popSize]
  int    prevRole       []; // роль предыдущей итерации [popSize]
  int    centroidBuf    []; // центроиды, sorted by ARF↓ [popSize]
  int    subflockLeader []; // лидер субфлока s       [popSize]
  double subflockBestF  []; // лучший fB в субфлоке s [popSize]
  double rankBuf        []; // Rank_t(A_i) Formula 5  [popSize]          [F7]
  double tmpDist        []; // буфер k-NN сортировки  [popSize]
  int    tmpIdx_        []; // буфер k-NN индексов    [popSize]
  int    kEff;              // min(k, popSize-1)
  int    nCentroids;        // число субфлоков на итерации

  int    Idx   (int i, int c)
  {
    return i * coords + c;
  }
  double Clamp (double v, double lo, double hi)
  {
    return v < lo ? lo : v > hi ? hi : v;
  }

  void   CalcSep (int i); // вычислить sepVec[] для агента i
};
//————————————————————————————————————————————————————————————————————

//————————————————————————————————————————————————————————————————————
bool C_AO_FBL::Init (const double &rangeMinP  [],
                     const double &rangeMaxP  [],
                     const double &rangeStepP [],
                     const int     epochsP = 0)
{
  if (!StandardInit (rangeMinP, rangeMaxP, rangeStepP)) return false;

  kEff = MathMin (k, popSize - 1);
  if (kEff < 1) kEff = 1;

  ArrayResize (V,              popSize * coords);
  ArrayResize (Vsnap,          popSize * coords);  // [F4]
  ArrayResize (Vmax,           coords);
  ArrayResize (cPsnap,         popSize * coords);
  ArrayResize (sepVec,         coords);
  ArrayResize (distM,          popSize * popSize);
  ArrayResize (dMax,           popSize);
  ArrayResize (dknbSz,         popSize);
  ArrayResize (drknbSz,        popSize);
  ArrayResize (arf,            popSize);
  ArrayResize (knnIdx,         popSize * kEff);
  ArrayResize (subflockId,     popSize);
  ArrayResize (leaderIdx,      popSize);
  ArrayResize (role,           popSize);
  ArrayResize (prevRole,       popSize);
  ArrayResize (centroidBuf,    popSize);
  ArrayResize (subflockLeader, popSize);
  ArrayResize (subflockBestF,  popSize);
  ArrayResize (rankBuf,        popSize);
  ArrayResize (tmpDist,        popSize);
  ArrayResize (tmpIdx_,        popSize);

  for (int c = 0; c < coords; c++)
    Vmax [c] = (rangeMax [c] - rangeMin [c]) * 0.5;

  for (int i = 0; i < popSize; i++)
  {
    for (int c = 0; c < coords; c++)
    {
      a [i].cP [c]    = u.RNDfromCI (rangeMin [c], rangeMax [c]);
      V    [Idx (i, c)] = 0.0;
      Vsnap[Idx (i, c)] = 0.0;  // [F4]
    }
    a [i].fB    = -DBL_MAX;
    role     [i] = 1;
    prevRole [i] = 1;
    leaderIdx[i] = -1;
  }
  nCentroids = 0;
  return true;
}
//————————————————————————————————————————————————————————————————————

//————————————————————————————————————————————————————————————————————
void C_AO_FBL::Moving ()
{
  for (int i = 0; i < popSize; i++)
    for (int c = 0; c < coords; c++)
      a [i].c [c] = u.SeInDiSp (a [i].cP [c], rangeMin [c], rangeMax [c], rangeStep [c]);
}
//————————————————————————————————————————————————————————————————————

//————————————————————————————————————————————————————————————————————
// [O2][F6] Separation-вектор для агента i → sepVec[].
// Отталкивание от k-NN соседей ближе dSep = 0.5 * dMax[i].
// sepVec[] в абсолютных единицах позиций, масштаб задаёт wSep.
void C_AO_FBL::CalcSep (int i)
{
  ArrayInitialize (sepVec, 0.0);
  double dSep = 0.5 * dMax [i];
  if (dSep < 1e-14) return;

  for (int m = 0; m < kEff; m++)
  {
    int    j     = knnIdx [i * kEff + m];
    double normD = distM  [i * popSize + j];
    if (normD >= dSep) continue;
    double w = (normD > 1e-12) ? (dSep - normD) / dSep : 1.0;
    for (int c = 0; c < coords; c++)
      sepVec [c] += w * (cPsnap [Idx (i, c)] - cPsnap [Idx (j, c)]);
  }
  for (int c = 0; c < coords; c++) sepVec [c] *= wSep;  // [F6]
}
//————————————————————————————————————————————————————————————————————

//————————————————————————————————————————————————————————————————————
void C_AO_FBL::Revision ()
{
  //─── 1. Личные и глобальный лучшие ───────────────────────────────
  for (int i = 0; i < popSize; i++)
  {
    if (a [i].f > a [i].fB)
    {
      a [i].fB = a [i].f;
      ArrayCopy (a [i].cB, a [i].c, 0, 0, coords);
    }
    if (a [i].f > fB)
    {
      fB       = a [i].f;
      ArrayCopy (cB,       a [i].c, 0, 0, coords);
    }
  }

  //─── 2. Нормированная матрица расстояний ─────────────────────────
  for (int i = 0; i < popSize; i++)
  {
    distM [i * popSize + i] = 0.0;
    for (int j = i + 1; j < popSize; j++)
    {
      double d = 0.0;
      for (int c = 0; c < coords; c++)
      {
        double rng = rangeMax [c] - rangeMin [c];
        double df  = (rng > 0.0) ? (a [i].cP [c] - a [j].cP [c]) / rng : 0.0;
        d += df * df;
      }
      d = MathSqrt (d);
      distM [i * popSize + j] = d;
      distM [j * popSize + i] = d;
    }
  }

  //─── 3. k-NN → d_max → |DkNB| (Def 5, Formula 3) ───────────────
  for (int i = 0; i < popSize; i++)
  {
    int cnt = 0;
    for (int j = 0; j < popSize; j++)
    {
      if (j == i) continue;
      tmpDist [cnt] = distM [i * popSize + j];
      tmpIdx_ [cnt] = j;
      cnt++;
    }
    for (int m = 0; m < kEff; m++)
    {
      int minPos = m;
      for (int n = m + 1; n < cnt; n++)
        if (tmpDist [n] < tmpDist [minPos]) minPos = n;
      if (minPos != m)
      {
        double td = tmpDist [m];
        tmpDist [m] = tmpDist [minPos];
        tmpDist [minPos] = td;
        int    ti = tmpIdx_ [m];
        tmpIdx_ [m] = tmpIdx_ [minPos];
        tmpIdx_ [minPos] = ti;
      }
    }
    dMax [i] = tmpDist [kEff - 1];
    for (int m = 0; m < kEff; m++) knnIdx [i * kEff + m] = tmpIdx_ [m];

    dknbSz [i] = 0;
    for (int j = 0; j < popSize; j++)
    {
      if (j == i) continue;
      if (distM [i * popSize + j] <= dMax [i]) dknbSz [i]++;
    }
  }

  //─── 4. |DR-kNB| (Definition 6) ─────────────────────────────────
  ArrayInitialize (drknbSz, 0);
  for (int i = 0; i < popSize; i++)
    for (int j = 0; j < popSize; j++)
    {
      if (j == i) continue;
      if (distM [i * popSize + j] <= dMax [j]) drknbSz [i]++;
    }

  //─── 5. ARF (Formula 4) ──────────────────────────────────────────
  for (int i = 0; i < popSize; i++)
  {
    int denom = drknbSz [i] + dknbSz [i];
    arf [i] = (denom > 0) ? (double)drknbSz [i] / (double)denom : 0.5;
  }

  //─── 6. Центроиды: ARF >= 0.5, сортировка по Rank↓ (Formula 5) ──
  nCentroids = 0;
  for (int i = 0; i < popSize; i++)
    if (arf [i] >= 0.5) centroidBuf [nCentroids++] = i;

  if (nCentroids == 0)
  {
    int bestI = 0;
    for (int i = 1; i < popSize; i++) if (arf [i] > arf [bestI]) bestI = i;
    centroidBuf [0] = bestI;
    nCentroids = 1;
  }

  // [F7] Formula 5: Rank_t(A_i) = Log(|N_i,t| / |N_t| * 10) * ARF_t(A_i)
  // |N_i,t| = dknbSz[i] (число соседей агента i)
  // |N_t|   = popSize   (число всех агентов)
  // Аргумент Log защищён от нуля: если dknbSz == 0 → rank = 0.
  for (int i = 0; i < popSize; i++)
  {
    double ratio = (double)dknbSz [i] / (double)popSize * 10.0;
    rankBuf [i]  = (ratio > 1e-12) ? MathLog (ratio) * arf [i] : 0.0;
  }

  // Insertion sort центроидов по Rank↓
  for (int a_ = 1; a_ < nCentroids; a_++)
  {
    int    key  = centroidBuf [a_];
    double keyR = rankBuf [key];
    int    b_   = a_ - 1;
    while (b_ >= 0 && rankBuf [centroidBuf [b_]] < keyR)
    {
      centroidBuf [b_ + 1] = centroidBuf [b_];
      b_--;
    }
    centroidBuf [b_ + 1] = key;
  }

  // [F9] Обрезка: оставляем не более popSize/k центроидов.
  // Сортировка уже выполнена → в начале массива самые значимые лидеры.
  int maxCentroids = MathMax (1, popSize / kEff);
  if (nCentroids > maxCentroids) nCentroids = maxCentroids;

  //─── 7. Назначение агентов в субфлоки ────────────────────────────
  for (int i = 0; i < popSize; i++) subflockId [i] = -1;

  for (int s = 0; s < nCentroids; s++)
    subflockId [centroidBuf [s]] = s;

  for (int i = 0; i < popSize; i++)
  {
    if (subflockId [i] >= 0) continue;
    for (int s = 0; s < nCentroids; s++)
    {
      if (distM [i * popSize + centroidBuf [s]] <= dMax [centroidBuf [s]])
      {
        subflockId [i] = s;
        break;
      }
    }
  }

  //─── 8. Лидер субфлока = агент с наилучшим fB в субфлоке ─────────
  // [F2] Используем fB (исторический лучший), а не f (текущий шумный).
  for (int s = 0; s < nCentroids; s++)
  {
    subflockLeader [s] = -1;
    subflockBestF  [s] = -DBL_MAX;
  }
  for (int i = 0; i < popSize; i++)
  {
    int s = subflockId [i];
    if (s < 0) continue;
    if (a [i].fB > subflockBestF [s])  // [F2] fB вместо f
    {
      subflockBestF  [s] = a [i].fB;
      subflockLeader [s] = i;
    }
  }

  // Страховка: если ни один агент субфлока не имел fB > -DBL_MAX
  // (например, первая итерация), назначаем центроид лидером,
  // чтобы leaderIdx последователей никогда не был -1.
  for (int s = 0; s < nCentroids; s++)
    if (subflockLeader [s] < 0) subflockLeader [s] = centroidBuf [s];

  //─── 9. Роли агентов ─────────────────────────────────────────────
  for (int i = 0; i < popSize; i++)
  {
    int s = subflockId [i];
    if      (s < 0)
    {
      role      [i] = 2;
      leaderIdx [i] = -1;
    }
    else if (subflockLeader [s] == i)
    {
      role      [i] = 0;
      leaderIdx [i] = -1;
    }
    else
    {
      role      [i] = 1;
      leaderIdx [i] = subflockLeader [s];
    }
  }

  //─── 10. Снапшоты позиций и скоростей перед обновлением ──────────
  // [F4] Vsnap снимается здесь — Pass 2 использует согласованные
  //      старые скорости лидеров, а не уже изменённые в Pass 1.
  for (int i = 0; i < popSize; i++)
    for (int c = 0; c < coords; c++)
    {
      cPsnap [Idx (i, c)] = a [i].cP [c];
      Vsnap  [Idx (i, c)] = V  [Idx (i, c)];  // [F4]
    }

  //─── 11. Pass 1: ЛИДЕРЫ ──────────────────────────────────────────
  // v = w·v + r1·(cB_i − cP_i) + φ·r2·(gBest − cP_i) + sep
  // [F10] w=0.729 — коэффициент сужения Clerc-Kennedy (фиксированный).
  for (int i = 0; i < popSize; i++)
  {
    if (role [i] != 0) continue;

    // [F3] Смягчённый сброс скорости при первом появлении в роли лидера.
    //      Накопленный импульс приглушается вдвое, а не обнуляется.
    if (prevRole [i] != 0)
      for (int c = 0; c < coords; c++) V [Idx (i, c)] *= 0.5;  // [F3]

    CalcSep (i);

    for (int c = 0; c < coords; c++)
    {
      double r1   = u.RNDfromCI (0.0, 1.0);
      double r2   = u.RNDfromCI (0.0, 1.0);
      int    idx  = Idx (i, c);
      double vNew = 0.729 * V [idx]                              // [F10] инерция
                    + r1 * (a [i].cB [c] - cPsnap [idx])         // когнитивный
                    + phi * r2 * (cB [c]  - cPsnap [idx])        // глобальный
                    + sepVec [c];                                // separation

      vNew         = Clamp (vNew, -Vmax [c], Vmax [c]);
      a [i].cP [c] = Clamp (cPsnap [idx] + vNew, rangeMin [c], rangeMax [c]);
      V [idx]      = vNew;
    }
  }

  //─── 12. Pass 2: ПОСЛЕДОВАТЕЛИ ───────────────────────────────────
  // v = w·v + r1·(cP_L − cP_i) + r2·(Vsnap_L − Vsnap_i)
  //       + r3·(cB_i − cP_i) + φ·r4·(gBest − cP_i) + sep
  // [F4]  Выравнивание идёт по Vsnap лидера (старая скорость).
  // [F10] w=0.729 — коэффициент сужения Clerc-Kennedy (фиксированный).
  for (int i = 0; i < popSize; i++)
  {
    if (role [i] != 1) continue;
    int wi = leaderIdx [i];

    CalcSep (i);

    for (int c = 0; c < coords; c++)
    {
      double r1   = u.RNDfromCI (0.0, 1.0);
      double r2   = u.RNDfromCI (0.0, 1.0);
      double r3   = u.RNDfromCI (0.0, 1.0);
      double r4   = u.RNDfromCI (0.0, 1.0);
      int    idx  = Idx (i, c);
      int    idxL = Idx (wi, c);
      double vNew = 0.729 * V [idx]                             // [F10] инерция
                    + r1 * (cPsnap [idxL] - cPsnap [idx])       // Cohesion
                    + r2 * (Vsnap  [idxL] - Vsnap  [idx])       // Alignment [F4]
                    + r3 * (a [i].cB [c]  - cPsnap [idx])       // Когнитивный
                    + phi * r4 * (cB [c]  - cPsnap [idx])       // Глобальный
                    + sepVec [c];                               // Separation

      vNew         = Clamp (vNew, -Vmax [c], Vmax [c]);
      a [i].cP [c] = Clamp (cPsnap [idx] + vNew, rangeMin [c], rangeMax [c]);
      V [idx]      = vNew;
    }
  }

  //─── 13. Pass 3: ВЫБРОСЫ ─────────────────────────────────────────
  // [F5] r независим для каждой координаты — в многомерном
  //      пространстве координаты прыгают независимо.
  for (int i = 0; i < popSize; i++)
  {
    if (role [i] != 2) continue;

    for (int c = 0; c < coords; c++)
    {
      double r    = u.RNDfromCI (0.0, 1.0);  // [F5] per-coordinate
      double xNew = (r > 0.5)
                    ? cB [c] + u.RNDfromCI (-1.0, 1.0) * (rangeMax [c] - rangeMin [c]) * 0.2
                    : u.RNDfromCI (rangeMin [c], rangeMax [c]);

      a [i].cP [c]   = Clamp (xNew, rangeMin [c], rangeMax [c]);
      V [Idx (i, c)] = 0.0;
    }
  }

  //─── 14. Сохранение ролей ────────────────────────────────────────
  for (int i = 0; i < popSize; i++) prevRole [i] = role [i];
}
//————————————————————————————————————————————————————————————————————