#include <string>
#include <sstream>
#include <cerrno>
#include <sys/types.h>
#include <sys/stat.h>

#include "vi_out_v4l2sink.h"


#define ELOG(log) \
    printf("E %s:%d %s\n", __func__, __LINE__, log);

#define VIDEO_DEV_PATH  "/dev/video"


bool ViOutV4l2Sink::Init(EGLDisplay egl_disp,
                         Argus::Size2D<uint32_t> resolution,
                         uint8_t video_dev_num)
{
    bool src_set_ok = false;
    bool nvvidconv_ok = false;
    bool identity_ok = false;
    bool video_out_ok = false;
    std::stringstream video_dev_ss;
    struct stat sb;

    video_dev_ss << VIDEO_DEV_PATH << (unsigned int) video_dev_num;
    video_out_dev_ = video_dev_ss.str().c_str();
    resolution_ = resolution;

    if (stat(video_out_dev_, &sb) == 0)
    {
        if((sb.st_mode & S_IFMT) == S_IFCHR)
        {
            video_out_ok = true;
        }
        else
        {
            ELOG("The device %s is not a char device, verify the v4l2loopback"
                 "module was correctly loaded");
        }
    }
    else
    {
        printf("An error occurred while accessing %s: %s\n", video_out_dev_,
               strerror(errno));
    }

    if(video_out_ok)
    {
        // Initialize GStreamer.
        gst_init(NULL, NULL);

        // Create pipeline.
        pipeline_ = gst_pipeline_new("video_pipeline");

        // Create source.
        if(pipeline_ != NULL)
        {
            gst_src_ = gst_element_factory_make("nveglstreamsrc", NULL);
            if(gst_src_ != NULL)
            {
                if (gst_bin_add(GST_BIN(pipeline_), gst_src_))
                {
                    g_object_set(G_OBJECT(gst_src_), "display", egl_disp, NULL);
                    src_set_ok = true;
                }
                else
                {
                    gst_object_unref(gst_src_);
                    ELOG("Couldn't add gstreamer source to the pipeline")
                }
            }
            else
            {
                ELOG("Couldn't create gstreamer source")
            }
        }
        else
        {
            ELOG("Couldn't create pipeline")
        }

        // Create nvvidconv
        if(src_set_ok)
        {
            gst_nvvidconv_ = gst_element_factory_make("nvvidconv", NULL);
            if(gst_nvvidconv_ != NULL)
            {
                if (gst_bin_add(GST_BIN(pipeline_), gst_nvvidconv_))
                {
                    nvvidconv_ok = true;
                }
                else
                {
                    gst_object_unref(gst_nvvidconv_);
                    ELOG("Couldn't add gstreamer nvvidconv to the pipeline")
                }
            }
            else
            {
                ELOG("Couldn't create gstreamer nvvidconv")
            }
        }

        // Create identity
        if(nvvidconv_ok)
        {
            gst_identity_ = gst_element_factory_make("identity", NULL);
            if(gst_identity_ != NULL)
            {
                if (gst_bin_add(GST_BIN(pipeline_), gst_identity_))
                {
                    g_object_set(G_OBJECT(gst_identity_), "drop-allocation",
                                 true, NULL);
                    identity_ok = true;
                }
                else
                {
                    gst_object_unref(gst_identity_);
                    ELOG("Couldn't add gstreamer identity to the pipeline")
                }
            }
            else
            {
                ELOG("Couldn't create gstreamer identity")
            }
        }

        // Create v4l2sink
        if(identity_ok)
        {
            gst_sink_ = gst_element_factory_make("v4l2sink", NULL);

            if (gst_sink_ != NULL)
            {
                if (gst_bin_add(GST_BIN(pipeline_), gst_sink_))
                {
                    g_object_set(G_OBJECT(gst_sink_), "device", video_out_dev_,
                                 NULL);
                    init_ = true;
                }
                else
                {
                    gst_object_unref(gst_sink_);
                    ELOG("Failed to add file sink to pipeline")
                }
            }
            else
            {
                ELOG("Failed to create file sink")
            }
        }
    }

    return init_;
}


bool ViOutV4l2Sink::Initialized(void)
{
    return init_;
}


bool ViOutV4l2Sink::SetInStream(EGLStreamKHR in_stream)
{
    bool set_ok = false;

    if(init_ == true)
    {
        g_object_set(G_OBJECT(gst_src_), "eglstream", in_stream, NULL);
        set_ok = true;
    }

    return set_ok;
}


bool ViOutV4l2Sink::ConsumerConnect(void)
{
    bool connected_ok = false;
    bool ready_to_connect = false;
    bool caps_ok = false;
    GstCaps *caps = NULL;

    if(init_)
    {
        if(gst_state_ == GST_STATE_NULL)
        {
            ready_to_connect = true;
        }
        else
        {
            ELOG("GStreamer is already connected");
        }
    }
    else
    {
        ELOG("GStreamer output has not initialized correctly");
    }

    if(ready_to_connect)
    {
        // Create caps filter to describe EGLStream image format.
        caps = gst_caps_new_simple("video/x-raw",
                                   "format", G_TYPE_STRING, "NV12",
                                   "width", G_TYPE_INT, resolution_.width(),
                                   "height", G_TYPE_INT, resolution_.height(),
                                   "framerate", GST_TYPE_FRACTION, 10, 1,
                                   NULL);

        if (caps != NULL)
        {
            GstCapsFeatures *features = gst_caps_features_new("memory:NVMM",
                                                              NULL);
            if (features != NULL)
            {
                gst_caps_set_features(caps, 0, features);
                caps_ok = true;
            }
            else
            {
                gst_caps_unref(caps);
                ELOG("Failed to create caps feature")
            }
        }
        else
        {
            ELOG("Failed to create caps")
        }
    }

    if(caps_ok)
    {
        if (gst_element_link_filtered(gst_src_, gst_nvvidconv_, caps))
        {
            if (gst_element_link(gst_nvvidconv_, gst_identity_))
            {
                if (gst_element_link(gst_identity_, gst_sink_))
                {
                    if (gst_element_set_state(pipeline_, GST_STATE_PLAYING)
                        != GST_STATE_CHANGE_FAILURE)
                    {
                        connected_ok = true;
                        gst_state_ = GST_STATE_PLAYING;
                    }
                    else
                    {
                        ELOG("Failed to set the playing state")
                    }
                }
                else
                {
                    ELOG("Failed to link identity to sink")
                }
            }
            else
            {
                ELOG("Failed to link nvvidconv to identity")
            }
        }
        else
        {
            ELOG("Failed to link EGLStream source")
        }
        gst_caps_unref(caps);
    }

    return connected_ok;
}


bool ViOutV4l2Sink::ConsumerDisconnect(void)
{
    bool ready_to_disconnect = false;
    bool disconnected_ok = false;

    if(init_)
    {
        if(gst_state_ == GST_STATE_PLAYING)
        {
            ready_to_disconnect = true;
        }
        else
        {
            ELOG("GStreamer is already disconnected");
        }
    }
    else
    {
        ELOG("GStreamer output has not initialized correctly");
    }

    if(ready_to_disconnect)
    {
        // Send the end-of-stream event.
        GstPad *pad = gst_element_get_static_pad(gst_identity_, "sink");

        if (pad != NULL)
        {
            bool eos_ok = gst_pad_send_event(pad, gst_event_new_eos());

            gst_object_unref(pad);

            if (eos_ok)
            {
                // Wait for the event to complete.
                GstBus *bus = gst_pipeline_get_bus(GST_PIPELINE(pipeline_));
                if (bus != NULL)
                {
                    bool result = gst_bus_poll(bus, GST_MESSAGE_EOS,
                                               GST_CLOCK_TIME_NONE);
                    gst_object_unref(bus);

                    if (result)
                    {
                        // Stop the pipeline.
                        if (gst_element_set_state(pipeline_, GST_STATE_NULL)
                            == GST_STATE_CHANGE_FAILURE)
                        {
                            ELOG("Failed to stop recording.")
                        }
                        else
                        {
                            gst_state_ = GST_STATE_NULL;
                            disconnected_ok = true;
                        }
                    }
                    else
                    {
                        ELOG("Timeout while waiting for end of stream")
                    }
                }
                else
                {
                    ELOG("Failed to get bus")
                }
            }
            else
            {
                ELOG("Failed to send end of stream event to encoder")
            }
        }
        else
        {
            ELOG("Failed to get 'sink' pad")
        }
    }

    return disconnected_ok;
}