/*
  Copyright (C) 1997-2024 Sam Lantinga <slouken@libsdl.org>

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely.
*/

#include <SDL3/SDL.h>

#include "testyuv_cvt.h"

#define YUV_SD_THRESHOLD 576

static YUV_CONVERSION_MODE YUV_ConversionMode = YUV_CONVERSION_BT601;

void SetYUVConversionMode(YUV_CONVERSION_MODE mode)
{
    YUV_ConversionMode = mode;
}

YUV_CONVERSION_MODE GetYUVConversionMode(void)
{
    return YUV_ConversionMode;
}

YUV_CONVERSION_MODE GetYUVConversionModeForResolution(int width, int height)
{
    YUV_CONVERSION_MODE mode = GetYUVConversionMode();
    if (mode == YUV_CONVERSION_AUTOMATIC) {
        if (height <= YUV_SD_THRESHOLD) {
            mode = YUV_CONVERSION_BT601;
        } else {
            mode = YUV_CONVERSION_BT709;
        }
    }
    return mode;
}

static float clip3(float x, float y, float z)
{
    return (z < x) ? x : ((z > y) ? y : z);
}

static void RGBtoYUV(const Uint8 *rgb, int *yuv, YUV_CONVERSION_MODE mode, int monochrome, int luminance)
{
    /**
     * This formula is from Microsoft's documentation:
     * https://msdn.microsoft.com/en-us/library/windows/desktop/dd206750(v=vs.85).aspx
     * L = Kr * R + Kb * B + (1 - Kr - Kb) * G
     * Y =                   floor(2^(M-8) * (219*(L-Z)/S + 16) + 0.5);
     * U = clip3(0, (2^M)-1, floor(2^(M-8) * (112*(B-L) / ((1-Kb)*S) + 128) + 0.5));
     * V = clip3(0, (2^M)-1, floor(2^(M-8) * (112*(R-L) / ((1-Kr)*S) + 128) + 0.5));
     */
    SDL_bool studio_RGB = SDL_FALSE;
    SDL_bool studio_YUV = SDL_FALSE;
    float N, M, S, Z, R, G, B, L, Kr, Kb, Y, U, V;

    N = 8.0f; /* 8 bit RGB */
    M = 8.0f; /* 8 bit YUV */
    if (mode == YUV_CONVERSION_BT709) {
        /* BT.709 */
        Kr = 0.2126f;
        Kb = 0.0722f;
    } else {
        /* BT.601 */
        Kr = 0.299f;
        Kb = 0.114f;
    }

    if (mode == YUV_CONVERSION_JPEG) {
        studio_YUV = SDL_FALSE;
    } else {
        studio_YUV = SDL_TRUE;
    }

    if (studio_RGB || !studio_YUV) {
        S = 219.0f * SDL_powf(2.0f, N - 8);
        Z = 16.0f * SDL_powf(2.0f, N - 8);
    } else {
        S = 255.0f;
        Z = 0.0f;
    }
    R = rgb[0];
    G = rgb[1];
    B = rgb[2];
    L = Kr * R + Kb * B + (1 - Kr - Kb) * G;
    Y =                                 SDL_floorf(SDL_powf(2.0f, (M - 8)) * (219.0f * (L - Z) / S + 16) + 0.5f);
    U = clip3(0, SDL_powf(2.0f, M) - 1, SDL_floorf(SDL_powf(2.0f, (M - 8)) * (112.0f * (B - L) / ((1.0f - Kb) * S) + 128) + 0.5f));
    V = clip3(0, SDL_powf(2.0f, M) - 1, SDL_floorf(SDL_powf(2.0f, (M - 8)) * (112.0f * (R - L) / ((1.0f - Kr) * S) + 128) + 0.5f));

    yuv[0] = (Uint8)Y;
    yuv[1] = (Uint8)U;
    yuv[2] = (Uint8)V;

    if (monochrome) {
        yuv[1] = 128;
        yuv[2] = 128;
    }

    if (luminance != 100) {
        yuv[0] = (Uint8)SDL_roundf(yuv[0] * (luminance / 100.0f));
        if (yuv[0] > 255) {
            yuv[0] = 255;
        }
    }
}

static void ConvertRGBtoPlanar2x2(Uint32 format, Uint8 *src, int pitch, Uint8 *out, int w, int h, YUV_CONVERSION_MODE mode, int monochrome, int luminance)
{
    int x, y;
    int yuv[4][3];
    Uint8 *Y1, *Y2, *U, *V;
    Uint8 *rgb1, *rgb2;
    int rgb_row_advance = (pitch - w * 3) + pitch;
    int UV_advance;

    rgb1 = src;
    rgb2 = src + pitch;

    Y1 = out;
    Y2 = Y1 + w;
    switch (format) {
    case SDL_PIXELFORMAT_YV12:
        V = (Y1 + h * w);
        U = V + ((h + 1) / 2) * ((w + 1) / 2);
        UV_advance = 1;
        break;
    case SDL_PIXELFORMAT_IYUV:
        U = (Y1 + h * w);
        V = U + ((h + 1) / 2) * ((w + 1) / 2);
        UV_advance = 1;
        break;
    case SDL_PIXELFORMAT_NV12:
        U = (Y1 + h * w);
        V = U + 1;
        UV_advance = 2;
        break;
    case SDL_PIXELFORMAT_NV21:
        V = (Y1 + h * w);
        U = V + 1;
        UV_advance = 2;
        break;
    default:
        SDL_assert(!"Unsupported planar YUV format");
        return;
    }

    for (y = 0; y < (h - 1); y += 2) {
        for (x = 0; x < (w - 1); x += 2) {
            RGBtoYUV(rgb1, yuv[0], mode, monochrome, luminance);
            rgb1 += 3;
            *Y1++ = (Uint8)yuv[0][0];

            RGBtoYUV(rgb1, yuv[1], mode, monochrome, luminance);
            rgb1 += 3;
            *Y1++ = (Uint8)yuv[1][0];

            RGBtoYUV(rgb2, yuv[2], mode, monochrome, luminance);
            rgb2 += 3;
            *Y2++ = (Uint8)yuv[2][0];

            RGBtoYUV(rgb2, yuv[3], mode, monochrome, luminance);
            rgb2 += 3;
            *Y2++ = (Uint8)yuv[3][0];

            *U = (Uint8)SDL_floorf((yuv[0][1] + yuv[1][1] + yuv[2][1] + yuv[3][1]) / 4.0f + 0.5f);
            U += UV_advance;

            *V = (Uint8)SDL_floorf((yuv[0][2] + yuv[1][2] + yuv[2][2] + yuv[3][2]) / 4.0f + 0.5f);
            V += UV_advance;
        }
        /* Last column */
        if (x == (w - 1)) {
            RGBtoYUV(rgb1, yuv[0], mode, monochrome, luminance);
            rgb1 += 3;
            *Y1++ = (Uint8)yuv[0][0];

            RGBtoYUV(rgb2, yuv[2], mode, monochrome, luminance);
            rgb2 += 3;
            *Y2++ = (Uint8)yuv[2][0];

            *U = (Uint8)SDL_floorf((yuv[0][1] + yuv[2][1]) / 2.0f + 0.5f);
            U += UV_advance;

            *V = (Uint8)SDL_floorf((yuv[0][2] + yuv[2][2]) / 2.0f + 0.5f);
            V += UV_advance;
        }
        Y1 += w;
        Y2 += w;
        rgb1 += rgb_row_advance;
        rgb2 += rgb_row_advance;
    }
    /* Last row */
    if (y == (h - 1)) {
        for (x = 0; x < (w - 1); x += 2) {
            RGBtoYUV(rgb1, yuv[0], mode, monochrome, luminance);
            rgb1 += 3;
            *Y1++ = (Uint8)yuv[0][0];

            RGBtoYUV(rgb1, yuv[1], mode, monochrome, luminance);
            rgb1 += 3;
            *Y1++ = (Uint8)yuv[1][0];

            *U = (Uint8)SDL_floorf((yuv[0][1] + yuv[1][1]) / 2.0f + 0.5f);
            U += UV_advance;

            *V = (Uint8)SDL_floorf((yuv[0][2] + yuv[1][2]) / 2.0f + 0.5f);
            V += UV_advance;
        }
        /* Last column */
        if (x == (w - 1)) {
            RGBtoYUV(rgb1, yuv[0], mode, monochrome, luminance);
            *Y1++ = (Uint8)yuv[0][0];

            *U = (Uint8)yuv[0][1];
            U += UV_advance;

            *V = (Uint8)yuv[0][2];
            V += UV_advance;
        }
    }
}

static void ConvertRGBtoPacked4(Uint32 format, Uint8 *src, int pitch, Uint8 *out, int w, int h, YUV_CONVERSION_MODE mode, int monochrome, int luminance)
{
    int x, y;
    int yuv[2][3];
    Uint8 *Y1, *Y2, *U, *V;
    Uint8 *rgb;
    int rgb_row_advance = (pitch - w * 3);

    rgb = src;

    switch (format) {
    case SDL_PIXELFORMAT_YUY2:
        Y1 = out;
        U = out + 1;
        Y2 = out + 2;
        V = out + 3;
        break;
    case SDL_PIXELFORMAT_UYVY:
        U = out;
        Y1 = out + 1;
        V = out + 2;
        Y2 = out + 3;
        break;
    case SDL_PIXELFORMAT_YVYU:
        Y1 = out;
        V = out + 1;
        Y2 = out + 2;
        U = out + 3;
        break;
    default:
        SDL_assert(!"Unsupported packed YUV format");
        return;
    }

    for (y = 0; y < h; ++y) {
        for (x = 0; x < (w - 1); x += 2) {
            RGBtoYUV(rgb, yuv[0], mode, monochrome, luminance);
            rgb += 3;
            *Y1 = (Uint8)yuv[0][0];
            Y1 += 4;

            RGBtoYUV(rgb, yuv[1], mode, monochrome, luminance);
            rgb += 3;
            *Y2 = (Uint8)yuv[1][0];
            Y2 += 4;

            *U = (Uint8)SDL_floorf((yuv[0][1] + yuv[1][1]) / 2.0f + 0.5f);
            U += 4;

            *V = (Uint8)SDL_floorf((yuv[0][2] + yuv[1][2]) / 2.0f + 0.5f);
            V += 4;
        }
        /* Last column */
        if (x == (w - 1)) {
            RGBtoYUV(rgb, yuv[0], mode, monochrome, luminance);
            rgb += 3;
            *Y2 = *Y1 = (Uint8)yuv[0][0];
            Y1 += 4;
            Y2 += 4;

            *U = (Uint8)yuv[0][1];
            U += 4;

            *V = (Uint8)yuv[0][2];
            V += 4;
        }
        rgb += rgb_row_advance;
    }
}

SDL_bool ConvertRGBtoYUV(Uint32 format, Uint8 *src, int pitch, Uint8 *out, int w, int h, YUV_CONVERSION_MODE mode, int monochrome, int luminance)
{
    switch (format) {
    case SDL_PIXELFORMAT_YV12:
    case SDL_PIXELFORMAT_IYUV:
    case SDL_PIXELFORMAT_NV12:
    case SDL_PIXELFORMAT_NV21:
        ConvertRGBtoPlanar2x2(format, src, pitch, out, w, h, mode, monochrome, luminance);
        return SDL_TRUE;
    case SDL_PIXELFORMAT_YUY2:
    case SDL_PIXELFORMAT_UYVY:
    case SDL_PIXELFORMAT_YVYU:
        ConvertRGBtoPacked4(format, src, pitch, out, w, h, mode, monochrome, luminance);
        return SDL_TRUE;
    default:
        return SDL_FALSE;
    }
}

int CalculateYUVPitch(Uint32 format, int width)
{
    switch (format) {
    case SDL_PIXELFORMAT_YV12:
    case SDL_PIXELFORMAT_IYUV:
    case SDL_PIXELFORMAT_NV12:
    case SDL_PIXELFORMAT_NV21:
        return width;
    case SDL_PIXELFORMAT_YUY2:
    case SDL_PIXELFORMAT_UYVY:
    case SDL_PIXELFORMAT_YVYU:
        return 4 * ((width + 1) / 2);
    default:
        return 0;
    }
}