Titano/Titano/Motion/PhaseCorrelator.cs

namespace Titano.Motion;

/// <summary>Spostamento misurato su un riquadro, con l'indice di attendibilità del picco.</summary>
public readonly record struct PhaseShift(float Dx, float Dy, float Confidence)
{
    public static PhaseShift None => new(0, 0, 0);
}

/// <summary>
/// Correlazione di fase fra due riquadri omologhi di fotogrammi adiacenti.
///
/// Il principio: una traslazione nel dominio spaziale è uno sfasamento lineare nel dominio
/// di Fourier. Normalizzando lo spettro incrociato al modulo unitario resta solo la fase, e
/// l'antitrasformata di una fase lineare pura è un impulso posto esattamente sullo
/// spostamento. Il metodo ignora quindi per costruzione le differenze di luminosità e di
/// contrasto fra i due fotogrammi — che in un time-lapse ci sono sempre — e reagisce
/// unicamente alla geometria.
///
/// L'antitrasformata dà però il picco solo sui campioni interi. Per scendere sotto il pixel
/// si ricostruisce la superficie di correlazione a passo fine nell'intorno del massimo,
/// valutando direttamente la somma di Fourier sulle posizioni intermedie invece di
/// interpolare i campioni già calcolati. La differenza è sostanziale: interpolare con una
/// parabola tre campioni di una cresta che parabola non è introduce un errore sistematico
/// che cresce con l'entità dello spostamento, mentre la valutazione diretta è esatta a meno
/// del passo scelto, perché il segnale è a banda limitata per costruzione.
///
/// L'istanza possiede i propri buffer di lavoro e non è utilizzabile da più thread insieme.
/// </summary>
public sealed class PhaseCorrelator
{
    /// <summary>Campioni per lato della griglia di raffinamento, su un intorno di ±1 pixel.</summary>
    private const int RefineSteps = 17;

    private readonly int _size;
    private readonly float[] _aRe;
    private readonly float[] _aIm;
    private readonly float[] _bRe;
    private readonly float[] _bIm;
    private readonly float[] _window;

    /// <summary>Radici dell'unità di ordine N: servono a spostare la valutazione di un intero.</summary>
    private readonly float[] _rootRe;
    private readonly float[] _rootIm;

    /// <summary>Fattori di fase degli scostamenti frazionari, precalcolati una volta sola.</summary>
    private readonly float[] _deltaRe;
    private readonly float[] _deltaIm;

    private readonly float[] _rowRe;
    private readonly float[] _rowIm;
    private readonly int[] _frequency;

    public int Size => _size;

    public PhaseCorrelator(int size)
    {
        if (!Fourier.IsPowerOfTwo(size))
            throw new ArgumentException("La correlazione di fase richiede un lato potenza di due.", nameof(size));

        _size = size;
        int samples = size * size;
        _aRe = new float[samples];
        _aIm = new float[samples];
        _bRe = new float[samples];
        _bIm = new float[samples];
        _window = BuildWindow(size);

        _rootRe = new float[size];
        _rootIm = new float[size];
        for (int m = 0; m < size; m++)
        {
            double angle = 2.0 * Math.PI * m / size;
            _rootRe[m] = (float)Math.Cos(angle);
            _rootIm[m] = (float)Math.Sin(angle);
        }

        // Frequenza con segno: oltre metà spettro l'indice rappresenta una frequenza negativa.
        // Usare l'indice grezzo darebbe una interpolazione sbagliata sulle posizioni frazionarie.
        _frequency = new int[size];
        for (int k = 0; k < size; k++) _frequency[k] = k < size / 2 ? k : k - size;

        _deltaRe = new float[RefineSteps * size];
        _deltaIm = new float[RefineSteps * size];
        for (int step = 0; step < RefineSteps; step++)
        {
            double delta = -1.0 + 2.0 * step / (RefineSteps - 1);
            for (int k = 0; k < size; k++)
            {
                double angle = 2.0 * Math.PI * _frequency[k] * delta / size;
                _deltaRe[step * size + k] = (float)Math.Cos(angle);
                _deltaIm[step * size + k] = (float)Math.Sin(angle);
            }
        }

        _rowRe = new float[size * RefineSteps];
        _rowIm = new float[size * RefineSteps];
    }

    /// <summary>
    /// Finestra di Hann separabile. Senza di essa il bordo del riquadro si comporta come un
    /// gradino e la sua trasformata riempie di righe l'intero spettro, coprendo il picco.
    /// </summary>
    private static float[] BuildWindow(int size)
    {
        var line = new float[size];
        for (int i = 0; i < size; i++)
            line[i] = 0.5f * (1f - MathF.Cos(2f * MathF.PI * i / (size - 1)));

        var window = new float[size * size];
        for (int y = 0; y < size; y++)
        {
            for (int x = 0; x < size; x++) window[y * size + x] = line[y] * line[x];
        }
        return window;
    }

    /// <summary>
    /// Misura lo spostamento del contenuto del riquadro fra <paramref name="a"/> e
    /// <paramref name="b"/>: il risultato è il vettore d per cui b(p) ≈ a(p − d).
    /// </summary>
    /// <summary>
    /// Deviazione standard sotto la quale un riquadro si considera privo di tessitura.
    /// Corrisponde a mezzo livello su 255: sotto, non c'è nulla di cui misurare lo spostamento.
    /// </summary>
    private const float TextureFloor = 0.002f;

    public PhaseShift Correlate(GrayImage a, GrayImage b, int originX, int originY)
    {
        float deviationA = Load(a, originX, originY, _aRe, _aIm);
        float deviationB = Load(b, originX, originY, _bRe, _bIm);

        // Un riquadro uniforme non ha spostamento misurabile: la normalizzazione al modulo
        // unitario amplificherebbe il solo rumore numerico e l'antitrasformata darebbe un
        // picco qualunque, indistinguibile da uno vero. Succede davvero — su una ripresa che
        // attraversa il giorno con pose da trenta secondi il cielo esce bruciato e piatto —
        // e senza questo controllo la stabilizzazione inseguiva spostamenti inventati.
        if (deviationA < TextureFloor || deviationB < TextureFloor) return PhaseShift.None;

        Fourier.Transform2D(_aRe, _aIm, _size, false);
        Fourier.Transform2D(_bRe, _bIm, _size, false);

        // Spettro incrociato coniugato e normalizzato: conj(A)·B / |conj(A)·B|.
        // Con questa combinazione l'impulso dell'antitrasformata cade su +d.
        for (int i = 0; i < _aRe.Length; i++)
        {
            float cr = _aRe[i] * _bRe[i] + _aIm[i] * _bIm[i];
            float ci = _aRe[i] * _bIm[i] - _aIm[i] * _bRe[i];
            float magnitude = MathF.Sqrt(cr * cr + ci * ci);

            if (magnitude < 1e-12f) { _aRe[i] = 0; _aIm[i] = 0; continue; }
            _aRe[i] = cr / magnitude;
            _aIm[i] = ci / magnitude;
        }

        // Lo spettro normalizzato serve ancora al raffinamento: i buffer di B sono liberi.
        Array.Copy(_aRe, _bRe, _aRe.Length);
        Array.Copy(_aIm, _bIm, _aIm.Length);

        Fourier.Transform2D(_aRe, _aIm, _size, true);

        int peak = 0;
        float best = float.NegativeInfinity;
        double energy = 0;

        for (int i = 0; i < _aRe.Length; i++)
        {
            float value = _aRe[i];
            energy += Math.Abs(value);
            if (value <= best) continue;
            best = value;
            peak = i;
        }

        if (best <= 0) return PhaseShift.None;

        // Il piano della correlazione è periodico: la metà superiore rappresenta gli
        // spostamenti negativi.
        int px = peak % _size;
        int py = peak / _size;
        int ix = px > _size / 2 ? px - _size : px;
        int iy = py > _size / 2 ? py - _size : py;

        Refine(ix, iy, out float dx, out float dy);

        // Attendibilità: quanto il picco svetta sul livello medio del piano. Un riquadro
        // senza tessitura, o coperto da una nuvola che si è mossa da sola, produce una
        // collina larga e bassa, non un impulso.
        double mean = energy / _aRe.Length;
        float confidence = mean > 1e-9 ? (float)(best / mean) : 0;

        return new PhaseShift(dx, dy, confidence);
    }

    /// <summary>
    /// Ricostruisce la correlazione a passo fine su ±1 pixel attorno al massimo intero,
    /// valutando la somma di Fourier direttamente sulle posizioni intermedie. La somma è
    /// separabile: prima si risolve la direzione orizzontale per ogni riga dello spettro,
    /// poi si combinano le righe. Il costo resta dello stesso ordine della trasformata.
    /// </summary>
    private void Refine(int integerX, int integerY, out float dx, out float dy)
    {
        int n = _size;

        // Somma sulle frequenze orizzontali, per ogni riga dello spettro e ogni scostamento.
        for (int ky = 0; ky < n; ky++)
        {
            int spectrumRow = ky * n;
            for (int step = 0; step < RefineSteps; step++)
            {
                int deltaRow = step * n;
                float sumRe = 0, sumIm = 0;

                for (int kx = 0; kx < n; kx++)
                {
                    // Fase totale = parte intera (radice dell'unità) × parte frazionaria.
                    int rotation = ((_frequency[kx] * integerX) % n + n) % n;
                    float phaseRe = _rootRe[rotation] * _deltaRe[deltaRow + kx]
                                  - _rootIm[rotation] * _deltaIm[deltaRow + kx];
                    float phaseIm = _rootRe[rotation] * _deltaIm[deltaRow + kx]
                                  + _rootIm[rotation] * _deltaRe[deltaRow + kx];

                    float re = _bRe[spectrumRow + kx];
                    float im = _bIm[spectrumRow + kx];
                    sumRe += re * phaseRe - im * phaseIm;
                    sumIm += re * phaseIm + im * phaseRe;
                }

                _rowRe[ky * RefineSteps + step] = sumRe;
                _rowIm[ky * RefineSteps + step] = sumIm;
            }
        }

        float bestValue = float.NegativeInfinity;
        int bestX = RefineSteps / 2, bestY = RefineSteps / 2;
        Span<float> surface = stackalloc float[RefineSteps * RefineSteps];

        for (int stepY = 0; stepY < RefineSteps; stepY++)
        {
            int deltaRow = stepY * n;
            for (int stepX = 0; stepX < RefineSteps; stepX++)
            {
                float sum = 0;
                for (int ky = 0; ky < n; ky++)
                {
                    int rotation = ((_frequency[ky] * integerY) % n + n) % n;
                    float phaseRe = _rootRe[rotation] * _deltaRe[deltaRow + ky]
                                  - _rootIm[rotation] * _deltaIm[deltaRow + ky];
                    float phaseIm = _rootRe[rotation] * _deltaIm[deltaRow + ky]
                                  + _rootIm[rotation] * _deltaRe[deltaRow + ky];

                    float re = _rowRe[ky * RefineSteps + stepX];
                    float im = _rowIm[ky * RefineSteps + stepX];
                    sum += re * phaseRe - im * phaseIm;      // basta la parte reale
                }

                surface[stepY * RefineSteps + stepX] = sum;
                if (sum <= bestValue) continue;
                bestValue = sum;
                bestX = stepX;
                bestY = stepY;
            }
        }

        float span = 2f / (RefineSteps - 1);

        // Sulla griglia fine la cresta è ormai ben campionata: qui la parabola è legittima.
        float offsetX = bestX is > 0 and < RefineSteps - 1
            ? ParabolicOffset(surface[bestY * RefineSteps + bestX - 1], bestValue,
                              surface[bestY * RefineSteps + bestX + 1])
            : 0;
        float offsetY = bestY is > 0 and < RefineSteps - 1
            ? ParabolicOffset(surface[(bestY - 1) * RefineSteps + bestX], bestValue,
                              surface[(bestY + 1) * RefineSteps + bestX])
            : 0;

        dx = integerX + (-1f + bestX * span) + offsetX * span;
        dy = integerY + (-1f + bestY * span) + offsetY * span;
    }

    /// <summary>Vertice della parabola per i tre campioni attorno al picco, in [-0.5, 0.5].</summary>
    private static float ParabolicOffset(float left, float centre, float right)
    {
        float denominator = 2f * (2f * centre - left - right);
        if (MathF.Abs(denominator) < 1e-9f) return 0;
        float offset = (right - left) / denominator;
        return MathF.Abs(offset) > 0.5f ? 0 : offset;
    }

    /// <summary>
    /// Estrae il riquadro, ne toglie la media e vi applica la finestra. Restituisce la
    /// deviazione standard del riquadro, che dice se c'era qualcosa da misurare.
    /// </summary>
    private float Load(GrayImage image, int originX, int originY, float[] re, float[] im)
    {
        double sum = 0;
        double sumSquares = 0;

        for (int y = 0; y < _size; y++)
        {
            int rowBase = y * _size;
            for (int x = 0; x < _size; x++)
            {
                float value = image.At(originX + x, originY + y);
                re[rowBase + x] = value;
                sum += value;
                sumSquares += (double)value * value;
            }
        }

        double count = re.Length;
        float mean = (float)(sum / count);
        double variance = Math.Max(0, sumSquares / count - (double)mean * mean);

        for (int i = 0; i < re.Length; i++)
        {
            re[i] = (re[i] - mean) * _window[i];
            im[i] = 0f;
        }

        return (float)Math.Sqrt(variance);
    }
}