SK-AM62A-LP: Switch between IR and RGB when using a Omnivision Ox05B1S sensor

#include <gst/gst.h>
#include <iostream>
#include <string>
#include <glib.h>  // For g_usleep

/**
 * @brief Enum for color schemes used in the video stream.
 */
enum GSM_ColorScheme_t
{
    GSM_ColorScheme_NV12, /**< NV12 color scheme (YUV 4:2:0). */
    GSM_ColorScheme_GRAY8 /**< GRAY8 color scheme (grayscale). */
};

// Global GStreamer elements and variables
static GstElement *pipeline = nullptr;
static GstElement *text_overlay = nullptr;
static GstElement *video_converter = nullptr;
static GstElement *video_encoder = nullptr;
static GstElement *isp = nullptr;
static GMainContext *context = nullptr;

static std::string Color = "";  // Current stream color (RGB or IR)

static int RTPStreamWidth = 1920;
static int RTPStreamHeight = 1080;
static bool isHighResolution = true;  // Toggle between high and low resolution
static GSM_ColorScheme_t ColorMode = GSM_ColorScheme_NV12;  // Default color scheme
static int switch_count = 0;  // Counter for how many times the resolution has been switched
static int iteration_count = 0;  // Counter for main loop iterations

/**
 * @brief Sets up the GStreamer pipeline for video processing.
 *
 * This function constructs the GStreamer pipeline according to the specified
 * width, height, and color scheme (RGB or IR).
 *
 * @param width The width of the video stream.
 * @param height The height of the video stream.
 * @param RTPStreamColorScheme The color scheme for the RTP stream (e.g., NV12 or GRAY8).
 */
static void GSM_CreatePipeline(int width, int height, GSM_ColorScheme_t RTPStreamColorScheme)
{
    GError *error = nullptr;
    std::string pipeline_desc;

    std::cout << "Creating the pipeline with video resolution " << width << "x" << height << "..." << std::endl;

    // Choose the pipeline description based on the color scheme.
    switch (RTPStreamColorScheme)
    {
        case GSM_ColorScheme_GRAY8:
            // IR grayscale processing.
            pipeline_desc =
                "v4l2src device=/dev/video3 io-mode=dmabuf-import ! "
                "video/x-bayer, width=2592, height=1944, framerate=30/1, format=bggi10 ! "
                "queue ! tiovxisp name=isp sink_0::pool-size=2 sensor-name=SENSOR_OX05B1S "
                "dcc-isp-file=/opt/imaging/ox05b1s/linear/dcc_viss.bin sink_0::dcc-2a-file=/opt/imaging/ox05b1s/linear/dcc_2a.bin format-msb=9 ! "
                "video/x-raw, format=GRAY8, width=2592, height=1944 ! tee name=t "
                "t. ! queue ! tiovxmultiscaler src_0::pool-size=2 target=1 ! video/x-raw, format=GRAY8, width=" + std::to_string(width) + ", height=" + std::to_string(height) + " ! "
                "queue ! textoverlay name=overlay ! videoconvert name=videoconverter ! v4l2h264enc name=videoencoder ! rtph264pay ! udpsink host=192.168.1.10 port=5000";
            break;

        case GSM_ColorScheme_NV12:
        default:
            // Default NV12 (YUV) color processing.
            pipeline_desc =
                "v4l2src device=/dev/video4 io-mode=dmabuf-import ! "
                "video/x-bayer, width=2592, height=1944, framerate=30/1, format=bggi10 ! "
                "queue ! tiovxisp name=isp sink_0::pool-size=2 sink_0::device=/dev/v4l-subdev2 sensor-name=SENSOR_OX05B1S "
                "dcc-isp-file=/opt/imaging/ox05b1s/linear/dcc_viss.bin sink_0::dcc-2a-file=/opt/imaging/ox05b1s/linear/dcc_2a.bin format-msb=9 ! "
                "queue ! tiovxldc dcc-file=/opt/imaging/ox05b1s/linear/dcc_ldc.bin sensor-name=SENSOR_OX05B1S ! "
                "video/x-raw, format=NV12, width=2592, height=1944 ! queue ! tiovxmultiscaler src_0::pool-size=4 target=1 ! video/x-raw, format=NV12, width=" + std::to_string(width) + ", height=" + std::to_string(height) + " ! "
                "queue ! textoverlay name=overlay ! videoconvert name=videoconverter ! v4l2h264enc name=videoencoder ! rtph264pay ! udpsink host=192.168.1.10 port=5000";
            break;
    }

    // Parse and create the pipeline from the description.
    pipeline = gst_parse_launch(pipeline_desc.c_str(), &error);
    if (error != nullptr)
    {
        std::cerr << "Error creating pipeline: " << error->message << std::endl;
        g_clear_error(&error);
        return;
    }

    // Retrieve key elements from the pipeline.
    text_overlay = gst_bin_get_by_name(GST_BIN(pipeline), "overlay");
    video_converter = gst_bin_get_by_name(GST_BIN(pipeline), "videoconverter");
    video_encoder = gst_bin_get_by_name(GST_BIN(pipeline), "videoencoder");
    isp = gst_bin_get_by_name(GST_BIN(pipeline), "isp");

    std::cout << "Pipeline created." << std::endl;
}

/**
 * @brief Sets the text overlay with the specified string.
 *
 * @param TextOverlay The text to be displayed on the overlay.
 */
int GSM_SetTextOverlay(const std::string& TextOverlay) {
    if (text_overlay != nullptr) {
        g_object_set(text_overlay, "text", TextOverlay.c_str(), nullptr);
    } else {
        std::cerr << "text_overlay element is nullptr" << std::endl;
    }

    return 0;
}

/**
 * @brief Stops and deallocates the GStreamer pipeline.
 *
 * This function ensures all pipeline elements are properly cleaned up and the pipeline
 * is set to the NULL state.
 */
void GSM_StopPipeline()
{
    if (pipeline)
    {
        // Set the pipeline to NULL state to ensure it stops.
        gst_element_set_state(pipeline, GST_STATE_NULL);
        
        // Safely unref all individual elements.
        if (isp) {
            gst_bin_remove(GST_BIN(pipeline), isp);
            gst_object_unref(isp);
            isp = nullptr;
        }
        if (text_overlay) {
            gst_bin_remove(GST_BIN(pipeline), text_overlay);
            gst_object_unref(text_overlay);
            text_overlay = nullptr;
        }
        if (video_converter) {
            gst_bin_remove(GST_BIN(pipeline), video_converter);
            gst_object_unref(video_converter);
            video_converter = nullptr;
        }
        if (video_encoder) {
            gst_bin_remove(GST_BIN(pipeline), video_encoder);
            gst_object_unref(video_encoder);
            video_encoder = nullptr;
        }

        // Finally, unref the entire pipeline.
        gst_object_unref(pipeline);
        pipeline = nullptr;
    }
    std::cout << "Pipeline and elements stopped and unrefed." << std::endl;
}

/**
 * @brief Toggles the video resolution between 1920x1080 and 800x600.
 */
void GSM_ToggleResolution()
{
    // Stop the current pipeline.
    GSM_StopPipeline();

    // Toggle the resolution between high and low.
    if (isHighResolution)
    {
        RTPStreamWidth = 800;
        RTPStreamHeight = 600;
    }
    else
    {
        RTPStreamWidth = 1920;
        RTPStreamHeight = 1080;
    }

    isHighResolution = !isHighResolution;
    switch_count++;  // Increment the switch counter.

    // Create and start the pipeline with the new resolution.
    GSM_CreatePipeline(RTPStreamWidth, RTPStreamHeight, GSM_ColorScheme_NV12);
    if (pipeline)
    {
        gst_element_set_state(pipeline, GST_STATE_PLAYING);

        // Update the text overlay with the current resolution, switch count, and color mode.
        std::string overlay_text = "Resolution: " + std::to_string(RTPStreamWidth) + "x" + std::to_string(RTPStreamHeight) +
                                   " | Switch count: " + std::to_string(switch_count) +
                                   " | Color: " + Color;
        GSM_SetTextOverlay(overlay_text);

        std::cout << "Started " << Color << " Stream with resolution: " << RTPStreamWidth << "x" << RTPStreamHeight << " | Switch count: " << switch_count << std::endl;
    }
}

/**
 * @brief Toggles between color modes (RGB and IR).
 */
void GSM_ToggleColorMode()
{
    // Stop the current pipeline.
    GSM_StopPipeline();
    
    // Toggle between NV12 (RGB) and GRAY8 (IR).
    if (ColorMode == GSM_ColorScheme_NV12)
    {
        ColorMode = GSM_ColorScheme_GRAY8;
        Color = "IR";
    }
    else
    {
        ColorMode = GSM_ColorScheme_NV12;
        Color = "RGB";
    }

    switch_count++;  // Increment the switch counter.

    // Create and start the pipeline with the new color mode.
    GSM_CreatePipeline(RTPStreamWidth, RTPStreamHeight, ColorMode);
    if (pipeline)
    {
        gst_element_set_state(pipeline, GST_STATE_PLAYING);

        // Update the text overlay with the current resolution, switch count, and color mode.
        std::string overlay_text = "Resolution: " + std::to_string(RTPStreamWidth) + "x" + std::to_string(RTPStreamHeight) +
                                   " | Switch count: " + std::to_string(switch_count) +
                                   " | Color: " + Color;
        GSM_SetTextOverlay(overlay_text);

        std::cout << "Started " << Color << " Stream with resolution: " << RTPStreamWidth << "x" << RTPStreamHeight << " | Switch count: " << switch_count << std::endl;
    }
}

/**
 * @brief Main function to initialize GStreamer and control the main loop.
 */
int main(int argc, char *argv[])
{
    std::cout << "Initializing GStreamer..." << std::endl;

    // Initialize GStreamer.
    gst_init(&argc, &argv);

    // Create a default GStreamer context.
    context = g_main_context_default();

    // Create and start the initial pipeline.
    GSM_CreatePipeline(RTPStreamWidth, RTPStreamHeight, ColorMode);
    gst_element_set_state(pipeline, GST_STATE_PLAYING);

    int switch_after_iterations = 30;  // Change resolution/color every 30 iterations.

    // Main loop to handle GStreamer events and toggle modes.
    while (true) {
        // Process GStreamer events in the main loop.
        g_main_context_iteration(context, FALSE);

        // Increment the iteration counter.
        iteration_count++;

        // Toggle color mode or resolution after a set number of iterations (~ 3 seconds).
        if (iteration_count % switch_after_iterations == 0) {
            GSM_ToggleColorMode();
            //GSM_ToggleResolution();
        }

        // Sleep for a short time between iterations (100 ms).
        g_usleep(100000);
    }

    return 0;
}