unit BeaconLock;

{$IFDEF FPC}
  {$MODE Delphi}
{$ENDIF}

{
  QO-100 Beacon Lock — выделенный узкополосный трекер несущей опорного маяка.

  Задача: LNB-гетеродин (≈9750 МГц) дрейфует с температурой на единицы–десятки
  кГц → весь downlink плывёт. Маяк (нижний CW 10489.500 МГц) имеет ИЗВЕСТНУЮ
  частоту. Меряя, где он реально стоит, получаем ошибку LO и компенсируем её.

  Двухступенчатая схема:
    1. ГРУБЫЙ захват — снаружи (по display-FFT) даёт начальную оценку смещения
       маяка в пределах ~±300 Гц; передаётся через Seed.
    2. ТОЧНЫЙ лок (этот модуль) — комплексный NCO сносит ожидаемую частоту маяка
       к DC, boxcar-дециматор сужает полосу до ~2 кГц (большой processing gain),
       затем FLL по фазовому дискриминатору доводит NCO ровно на несущую.
       Лок-метрика — SNR через carrier-EMA / noise-EMA на децимированном потоке.

  Точность: единицы Гц при достаточном SNR (узкая полоса + длинное усреднение
  фазы), что и есть «железный» лок (в духе SDR Console).

  Потоки: Feed() зовётся из DSP-потока (дёшево, на сэмпл). Результаты читаются
  из управляющего потока через снапшот под критической секцией.
}

interface

uses
  SysUtils, Math, SyncObjs;

type
  TBeaconState = (bsIdle,     // выключен / нет данных
                  bsSearch,   // идёт захват (SNR ниже порога)
                  bsLock);    // несущая захвачена и стабильна

  TBeaconTracker = class
  private
    FLock:        TCriticalSection;
    // --- конфигурация ---
    FSampleRate:  Double;   // вход (Hz)
    FDecim:       Integer;  // коэффициент децимации (Fs/FDecim ≈ FDecimRate)
    FDecimRate:   Double;   // частота децимированного потока (Hz)
    FLoopGain:    Double;   // гейн FLL (0..1, малый)
    FLockSNRdB:   Double;   // порог объявления лока
    FTargetHz:    Double;   // ожидаемое смещение маяка от центра (Hz)
    // --- состояние NCO/децимации (DSP-поток) ---
    FNcoFreq:     Double;   // текущая частота NCO (быстрая, дёрганая)
    FNcoSmooth:   Double;   // сглаженная оценка смещения маяка (для коррекции/маркера)
    FSeedHz:      Double;   // якорь clamp: NCO не уходит дальше ±CLAMP от сида
    FPhCos, FPhSin: Double; // текущий фазор NCO (для сноса к DC)
    FStepCos, FStepSin: Double; // инкремент фазора за сэмпл (Δ = −2π·FNcoFreq/Fs)
    FNcoNorm:     Integer;  // счётчик до ренормализации фазора
    FAccI, FAccQ: Double;   // аккумулятор boxcar-дециматора
    FAccN:        Integer;
    FPrevI, FPrevQ: Double; // предыдущий децимированный сэмпл (для дискриминатора)
    FHasPrev:     Boolean;
    FMeanI, FMeanQ: Double; // EMA комплексного децим. потока → амплитуда несущей
    FNoisePow:    Double;   // EMA мощности шума |z-mean|^2
    FResAvg:      Double;   // EMA остаточной частоты (для контроля стабильности)
    FActive:      Boolean;  // трекинг включён
    // --- снапшот результата (под FLock) ---
    FResMeasHz:   Double;   // измеренное смещение маяка = FNcoFreq на момент снапшота
    FResSNRdB:    Double;
    FResState:    TBeaconState;
    procedure RecomputeStep;            // пересчёт шага фазора по FNcoFreq (фазу НЕ трогает)
    procedure ResetPhasor;              // сброс фазора в 0 + пересчёт шага
    procedure ProcessDecimated(const zi, zq: Double);
    procedure Snapshot;
  public
    constructor Create;
    destructor Destroy; override;

    // Настройка под текущий sample rate. DecimRateHz — целевая полоса
    // децимированного потока (по умолчанию ~2 кГц: ±1 кГц захват FLL).
    procedure Configure(ASampleRate: Double; ADecimRateHz: Double = 4000.0);
    // Ожидаемое смещение маяка от DDC-центра (Hz). Меняется при перестройке.
    procedure SetTarget(AOffsetHz: Double);
    // Грубая оценка от FFT — переинициализирует NCO (захват/переакквизиция).
    procedure Seed(AOffsetHz: Double);
    procedure Start;
    procedure Stop;
    procedure Reset;

    // DSP-поток: один комплексный сэмпл @ FSampleRate.
    procedure Feed(const I, Q: Double); inline;

    // Управляющий поток: снапшот результата.
    function  MeasuredOffsetHz: Double;  // измеренное смещение маяка от центра
    function  ErrorHz: Double;           // MeasuredOffset − Target (ошибка LO)
    function  SNRdB: Double;
    function  State: TBeaconState;
    function  Locked: Boolean;
    property  Active: Boolean read FActive;
  end;

implementation

const
  NCO_RENORM   = 2048;    // ренормализация фазора каждые N сэмплов
  EMA_MEAN     = 0.002;   // постоянная EMA несущей (медленная — узкая полоса)
  EMA_NOISE    = 0.002;
  EMA_RES      = 0.01;
  EMA_NCO      = 0.001;   // сглаживание оценки частоты (TC ~0.25 с @4кГц)
  CLAMP_HZ     = 1500.0;  // NCO держим в ±CLAMP от сида (без runaway на край)
  NOISE_FLOOR  = 1e-30;   // ТОЛЬКО защита от деления на ноль (не маскировать сигнал!)
  SNR_MAX      = 40.0;

constructor TBeaconTracker.Create;
begin
  inherited Create;
  FLock := TCriticalSection.Create;
  FSampleRate := 1536000.0;
  FDecimRate  := 4000.0;   // квадрат-домен → захват ±FDecimRate/4 = ±1 кГц
  FLoopGain   := 0.02;
  FLockSNRdB  := 3.0;
  Reset;
  Configure(FSampleRate, FDecimRate);
end;

destructor TBeaconTracker.Destroy;
begin
  FLock.Free;
  inherited Destroy;
end;

procedure TBeaconTracker.Configure(ASampleRate, ADecimRateHz: Double);
begin
  FLock.Enter;
  try
    if ASampleRate < 1.0 then ASampleRate := 1536000.0;
    if ADecimRateHz < 100.0 then ADecimRateHz := 4000.0;
    FSampleRate := ASampleRate;
    FDecim := Max(1, Round(ASampleRate / ADecimRateHz));
    FDecimRate := ASampleRate / FDecim;
    // сброс рабочих аккумуляторов (NCO частоту сохраняем — это оценка маяка)
    FAccI := 0; FAccQ := 0; FAccN := 0;
    FHasPrev := False;
    FMeanI := 0; FMeanQ := 0; FNoisePow := 0; FResAvg := 0;
    ResetPhasor;
  finally
    FLock.Leave;
  end;
end;

procedure TBeaconTracker.RecomputeStep;
// Пересчёт шага фазора под текущую FNcoFreq (снос на −FNcoFreq, маяк → DC).
// Фазу НЕ трогаем — смена частоты непрерывна по фазе.
var w: Double;
begin
  w := -2.0 * Pi * FNcoFreq / FSampleRate;
  FStepCos := Cos(w);
  FStepSin := Sin(w);
end;

procedure TBeaconTracker.ResetPhasor;
begin
  FPhCos := 1.0;
  FPhSin := 0.0;
  FNcoNorm := 0;
  RecomputeStep;
end;

procedure TBeaconTracker.SetTarget(AOffsetHz: Double);
begin
  FLock.Enter;
  try
    FTargetHz := AOffsetHz;
  finally
    FLock.Leave;
  end;
end;

procedure TBeaconTracker.Seed(AOffsetHz: Double);
begin
  FLock.Enter;
  try
    FNcoFreq   := AOffsetHz;
    FNcoSmooth := AOffsetHz;
    FSeedHz    := AOffsetHz;   // якорь clamp
    ResetPhasor;
    FHasPrev := False;
    FMeanI := 0; FMeanQ := 0; FNoisePow := 0; FResAvg := 0;
  finally
    FLock.Leave;
  end;
end;

procedure TBeaconTracker.Start;
begin
  FActive := True;
end;

procedure TBeaconTracker.Stop;
begin
  FActive := False;
end;

procedure TBeaconTracker.Reset;
begin
  FLock.Enter;
  try
    FNcoFreq := 0; FNcoSmooth := 0; FSeedHz := 0; FTargetHz := 0;
    FAccI := 0; FAccQ := 0; FAccN := 0;
    FHasPrev := False;
    FMeanI := 0; FMeanQ := 0; FNoisePow := 0; FResAvg := 0;
    FResMeasHz := 0; FResSNRdB := -99.0; FResState := bsIdle;
    FActive := False;
    ResetPhasor;
  finally
    FLock.Leave;
  end;
end;

procedure TBeaconTracker.Feed(const I, Q: Double);
var
  mi, mq, nc, ns: Double;
begin
  if not FActive then Exit;
  // снос к DC: z * conj(nco)  (nco = exp(+jωt), шаг отрицательной частоты)
  mi := I * FPhCos - Q * FPhSin;
  mq := I * FPhSin + Q * FPhCos;
  // продвинуть фазор: phase *= step
  nc := FPhCos * FStepCos - FPhSin * FStepSin;
  ns := FPhCos * FStepSin + FPhSin * FStepCos;
  FPhCos := nc; FPhSin := ns;
  Inc(FNcoNorm);
  if FNcoNorm >= NCO_RENORM then
  begin
    // ренормализация (борьба с накоплением ошибки округления)
    nc := 1.5 - 0.5 * (FPhCos * FPhCos + FPhSin * FPhSin);
    FPhCos := FPhCos * nc;
    FPhSin := FPhSin * nc;
    FNcoNorm := 0;
  end;
  // boxcar-дециматор
  FAccI := FAccI + mi;
  FAccQ := FAccQ + mq;
  Inc(FAccN);
  if FAccN >= FDecim then
  begin
    ProcessDecimated(FAccI / FDecim, FAccQ / FDecim);
    FAccI := 0; FAccQ := 0; FAccN := 0;
  end;
end;

procedure TBeaconTracker.ProcessDecimated(const zi, zq: Double);
// Работаем в КВАДРАТЕ сигнала w = z²: для BPSK (средний маяк) это снимает
// модуляцию ±180° (b²=1), оставляя чистую несущую на 2× частоте; для чистого
// CW (нижний/верхний) — тоже корректно. Дискриминатор по w даёт 2·остаток,
// делим на 2. Захват ±FDecimRate/4. carrier/noise-EMA считаем в w-домене —
// для BPSK среднее по z = 0, а по w = z² несущая когерентна.
var
  wi, wq, pr, pi, phase, res2, resHz, di, dq, np: Double;
begin
  wi := zi * zi - zq * zq;     // Re(z²)
  wq := 2.0 * zi * zq;         // Im(z²)
  if FHasPrev then
  begin
    // фазовый дискриминатор по квадрату: arg(w[n] * conj(w[n-1]))
    pr := wi * FPrevI + wq * FPrevQ;
    pi := wq * FPrevI - wi * FPrevQ;
    phase := ArcTan2(pi, pr);
    res2  := phase * FDecimRate / (2.0 * Pi);   // = 2 · остаточная частота
    resHz := 0.5 * res2;
    // FLL: подтянуть NCO на остаточную частоту, но удержать в окне ±CLAMP_HZ
    // вокруг сида — иначе на BPSK/шуме петля убегает на край спектра.
    FNcoFreq := FNcoFreq + FLoopGain * resHz;
    if FNcoFreq > FSeedHz + CLAMP_HZ then FNcoFreq := FSeedHz + CLAMP_HZ;
    if FNcoFreq < FSeedHz - CLAMP_HZ then FNcoFreq := FSeedHz - CLAMP_HZ;
    FResAvg := FResAvg + EMA_RES * (Abs(resHz) - FResAvg);
    FNcoSmooth := FNcoSmooth + EMA_NCO * (FNcoFreq - FNcoSmooth);  // стаб. оценка
    RecomputeStep;   // обновить шаг фазора под новую FNcoFreq (фаза непрерывна)
  end;
  FPrevI := wi; FPrevQ := wq; FHasPrev := True;   // FPrev* = предыдущий w

  // carrier-EMA в w-домене (несущая² у DC) и шум как отклонение от среднего
  FMeanI := FMeanI + EMA_MEAN * (wi - FMeanI);
  FMeanQ := FMeanQ + EMA_MEAN * (wq - FMeanQ);
  di := wi - FMeanI; dq := wq - FMeanQ;
  np := di * di + dq * dq;
  FNoisePow := FNoisePow + EMA_NOISE * (np - FNoisePow);

  Snapshot;
end;

procedure TBeaconTracker.Snapshot;
var
  sigPow, snr: Double;
  st: TBeaconState;
begin
  sigPow := FMeanI * FMeanI + FMeanQ * FMeanQ;
  snr := 10.0 * Log10((sigPow + 1e-30) / (FNoisePow + NOISE_FLOOR));
  if snr > SNR_MAX then snr := SNR_MAX;
  // Лок по когерентности несущей (SNR в квадрат-домене). Гейт по остаточной
  // частоте убран — на BPSK он давал ложный «не-лок».
  if snr >= FLockSNRdB then
    st := bsLock
  else
    st := bsSearch;
  FLock.Enter;
  try
    FResMeasHz := FNcoSmooth;   // сглаженная оценка → стабильная коррекция/маркер
    FResSNRdB  := snr;
    FResState  := st;
  finally
    FLock.Leave;
  end;
end;

function TBeaconTracker.MeasuredOffsetHz: Double;
begin
  FLock.Enter; try Result := FResMeasHz; finally FLock.Leave; end;
end;

function TBeaconTracker.ErrorHz: Double;
begin
  FLock.Enter; try Result := FResMeasHz - FTargetHz; finally FLock.Leave; end;
end;

function TBeaconTracker.SNRdB: Double;
begin
  FLock.Enter; try Result := FResSNRdB; finally FLock.Leave; end;
end;

function TBeaconTracker.State: TBeaconState;
begin
  if not FActive then Exit(bsIdle);
  FLock.Enter; try Result := FResState; finally FLock.Leave; end;
end;

function TBeaconTracker.Locked: Boolean;
begin
  Result := State = bsLock;
end;

end.