Titano/Titano/Imaging/GeometryStage.cs

using Titano.Motion;

namespace Titano.Imaging;

/// <summary>
/// Trasformazione affine dal fotogramma d'uscita a quello sorgente: per ogni pixel di
/// destinazione dice da quali coordinate prelevare. È la forma inversa a quella naturale,
/// ed è voluta — percorrendo la destinazione ogni pixel viene scritto una volta sola e non
/// restano buchi, mentre percorrendo la sorgente ne resterebbero ovunque la scala aumenti.
/// </summary>
public readonly record struct SourceMapping(double Ax, double Bx, double Tx,
                                            double Ay, double By, double Ty)
{
    public static SourceMapping Identity => new(1, 0, 0, 0, 1, 0);

    public bool IsIdentity =>
        Math.Abs(Ax - 1) < 1e-9 && Math.Abs(Bx) < 1e-9 && Math.Abs(Tx) < 1e-6 &&
        Math.Abs(Ay) < 1e-9 && Math.Abs(By - 1) < 1e-9 && Math.Abs(Ty) < 1e-6;

    public (double X, double Y) Apply(double x, double y)
        => (Ax * x + Bx * y + Tx, Ay * x + By * y + Ty);
}

/// <summary>
/// Stadio geometrico finale: ritaglio del movimento virtuale e correzione della
/// stabilizzazione, applicati insieme in un unico ricampionamento.
///
/// Che siano insieme non è un dettaglio di efficienza. Ogni ricampionamento costa un po' di
/// nitidezza, perché ricostruisce il segnale su un reticolo diverso; farne due in fila —
/// prima raddrizzare il fotogramma, poi ritagliarlo — costa il doppio senza dare nulla in
/// cambio. Le due trasformazioni sono entrambe affini, quindi si compongono esattamente in
/// una sola, e il fotogramma viene interpolato una volta sola.
///
/// Il filtro di ricostruzione è la cubica di Catmull-Rom: passa per i campioni, ha derivata
/// continua e restituisce un'immagine più incisa di quanto farebbe una bilineare, che a ogni
/// panoramica virtuale lascerebbe un velo di morbidezza.
/// </summary>
public static class GeometryStage
{
    /// <summary>
    /// Compone il ritaglio dell'inquadratura con l'inversa della correzione di stabilizzazione.
    ///
    /// La correzione porta il fotogramma dalla posizione in cui è stato scattato a quella in cui
    /// avrebbe dovuto essere; qui serve il percorso opposto, perché si parte dalla destinazione
    /// e si va a cercare il pixel nella sorgente.
    /// </summary>
    public static SourceMapping Build(int sourceWidth, int sourceHeight, int outputWidth, int outputHeight,
                                      in CameraFraming framing, in SimilarityTransform stabilization)
    {
        var inverse = stabilization.Inverse;

        double zoom = Math.Max(1e-3, framing.Zoom);
        double cropWidth = sourceWidth / zoom;
        double cropHeight = sourceHeight / zoom;
        double left = framing.CentreX * sourceWidth - cropWidth * 0.5;
        double top = framing.CentreY * sourceHeight - cropHeight * 0.5;

        // La catena è affine, quindi tre punti la determinano per intero: valutarla e poi
        // ricavarne i coefficienti è più corto — e molto meno soggetto a errori di segno —
        // che moltiplicare le matrici a mano.
        var origin = Trace(0, 0);
        var alongX = Trace(1, 0);
        var alongY = Trace(0, 1);

        return new SourceMapping(alongX.X - origin.X, alongY.X - origin.X, origin.X,
                                 alongX.Y - origin.Y, alongY.Y - origin.Y, origin.Y);

        (double X, double Y) Trace(double x, double y)
        {
            // Pixel di destinazione → punto dentro il ritaglio, in pixel sorgente stabilizzati.
            double stabilizedX = left + (x + 0.5) * cropWidth / outputWidth;
            double stabilizedY = top + (y + 0.5) * cropHeight / outputHeight;

            if (inverse.IsIdentity) return (stabilizedX - 0.5, stabilizedY - 0.5);

            // Le similitudini vivono in coordinate normalizzate sulla larghezza e centrate.
            double u = (stabilizedX - sourceWidth * 0.5) / sourceWidth;
            double v = (stabilizedY - sourceHeight * 0.5) / sourceWidth;
            var (ru, rv) = inverse.Apply(u, v);

            return (ru * sourceWidth + sourceWidth * 0.5 - 0.5,
                    rv * sourceWidth + sourceHeight * 0.5 - 0.5);
        }
    }

    /// <summary>
    /// Mappatura fra due fotogrammi della stessa dimensione legati da una similitudine
    /// espressa in coordinate normalizzate. La usa lo stacking per sovrapporre fotogrammi
    /// che la stabilizzazione ha spostato di quantità diverse.
    /// </summary>
    public static SourceMapping FromSimilarity(int width, int height, in SimilarityTransform mapping)
    {
        if (mapping.IsIdentity) return SourceMapping.Identity;

        var similarity = mapping;
        var origin = Trace(0, 0);
        var alongX = Trace(1, 0);
        var alongY = Trace(0, 1);

        return new SourceMapping(alongX.X - origin.X, alongY.X - origin.X, origin.X,
                                 alongX.Y - origin.Y, alongY.Y - origin.Y, origin.Y);

        (double X, double Y) Trace(double x, double y)
        {
            double u = (x + 0.5 - width * 0.5) / width;
            double v = (y + 0.5 - height * 0.5) / width;
            var (ru, rv) = similarity.Apply(u, v);
            return (ru * width + width * 0.5 - 0.5, rv * width + height * 0.5 - 0.5);
        }
    }

    /// <summary>Vero se lo stadio non ha nulla da fare: stessa dimensione e nessuna trasformazione.</summary>
    public static bool IsPassThrough(in SourceMapping map, ImageBuffer source, ImageBuffer destination)
        => map.IsIdentity && source.Width == destination.Width && source.Height == destination.Height;

    /// <summary>Ricampiona la sorgente nella destinazione secondo la mappatura indicata.</summary>
    public static void Resample(ImageBuffer source, ImageBuffer destination, in SourceMapping map)
    {
        if (IsPassThrough(map, source, destination))
        {
            destination.CopyFrom(source);
            return;
        }

        int sourceWidth = source.Width;
        int sourceHeight = source.Height;
        int width = destination.Width;
        int height = destination.Height;
        var src = source.Data;
        var dst = destination.Data;

        double ax = map.Ax, bx = map.Bx, tx = map.Tx;
        double ay = map.Ay, by = map.By, ty = map.Ty;

        Parallel.For(0, height, y =>
        {
            double rowX = bx * y + tx;
            double rowY = by * y + ty;
            int rowBase = y * width * ImageBuffer.Channels;

            Span<float> weightX = stackalloc float[4];
            Span<float> weightY = stackalloc float[4];

            for (int x = 0; x < width; x++)
            {
                float sx = (float)(ax * x + rowX);
                float sy = (float)(ay * x + rowY);

                int x0 = (int)MathF.Floor(sx);
                int y0 = (int)MathF.Floor(sy);
                CatmullRom(sx - x0, weightX);
                CatmullRom(sy - y0, weightY);

                float r = 0, g = 0, b = 0;
                for (int j = 0; j < 4; j++)
                {
                    int py = Math.Clamp(y0 - 1 + j, 0, sourceHeight - 1);
                    int lineBase = py * sourceWidth * ImageBuffer.Channels;
                    float wy = weightY[j];
                    if (wy == 0f) continue;

                    for (int i = 0; i < 4; i++)
                    {
                        int px = Math.Clamp(x0 - 1 + i, 0, sourceWidth - 1);
                        float w = wy * weightX[i];
                        int index = lineBase + px * ImageBuffer.Channels;
                        r += src[index] * w;
                        g += src[index + 1] * w;
                        b += src[index + 2] * w;
                    }
                }

                // La cubica sovraelonga: sui bordi ad alto contrasto può scendere sotto zero,
                // che in luce lineare non significa nulla e all'encoder arriverebbe come rumore.
                int destination0 = rowBase + x * ImageBuffer.Channels;
                dst[destination0] = r > 0f ? r : 0f;
                dst[destination0 + 1] = g > 0f ? g : 0f;
                dst[destination0 + 2] = b > 0f ? b : 0f;
            }
        });
    }

    /// <summary>Pesi della cubica di Catmull-Rom per i quattro campioni attorno alla posizione.</summary>
    private static void CatmullRom(float t, Span<float> weights)
    {
        float t2 = t * t;
        float t3 = t2 * t;
        weights[0] = -0.5f * t3 + t2 - 0.5f * t;
        weights[1] = 1.5f * t3 - 2.5f * t2 + 1f;
        weights[2] = -1.5f * t3 + 2f * t2 + 0.5f * t;
        weights[3] = 0.5f * t3 - 0.5f * t2;
    }
}