/*
 * Copyright (c) 2018-2022, NVIDIA CORPORATION. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#include <gst/gst.h>
#include <glib.h>
#include <stdio.h>
#include <cuda_runtime_api.h>
#include "gstnvdsmeta.h"
#include "nvds_yml_parser.h"

#define MAX_DISPLAY_LEN 64

#define PGIE_CLASS_ID_VEHICLE 0
#define PGIE_CLASS_ID_PERSON 2

/* The muxer output resolution must be set if the input streams will be of
 * different resolution. The muxer will scale all the input frames to this
 * resolution. */
#define MUXER_OUTPUT_WIDTH 1920
#define MUXER_OUTPUT_HEIGHT 1080

/* Muxer batch formation timeout, for e.g. 40 millisec. Should ideally be set
 * based on the fastest source's framerate. */
#define MUXER_BATCH_TIMEOUT_USEC 40000

gint frame_number = 0;
gchar pgie_classes_str[4][32] = { "Vehicle", "TwoWheeler", "Person",
  "Roadsign"
};

/* osd_sink_pad_buffer_probe  will extract metadata received on OSD sink pad
 * and update params for drawing rectangle, object information etc. */

static GstPadProbeReturn
osd_sink_pad_buffer_probe (GstPad * pad, GstPadProbeInfo * info,
    gpointer u_data)
{
    GstBuffer *buf = (GstBuffer *) info->data;
    guint num_rects = 0; 
    NvDsObjectMeta *obj_meta = NULL;
    guint vehicle_count = 0;
    guint person_count = 0;
    NvDsMetaList * l_frame = NULL;
    NvDsMetaList * l_obj = NULL;
    NvDsDisplayMeta *display_meta = NULL;

    NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta (buf);

    for (l_frame = batch_meta->frame_meta_list; l_frame != NULL;
      l_frame = l_frame->next) {
        NvDsFrameMeta *frame_meta = (NvDsFrameMeta *) (l_frame->data);
        int offset = 0;
        for (l_obj = frame_meta->obj_meta_list; l_obj != NULL;
                l_obj = l_obj->next) {
            obj_meta = (NvDsObjectMeta *) (l_obj->data);
            if (obj_meta->class_id == PGIE_CLASS_ID_VEHICLE) {
                vehicle_count++;
                num_rects++;
            }
            if (obj_meta->class_id == PGIE_CLASS_ID_PERSON) {
                person_count++;
                num_rects++;
            }
        }
        display_meta = nvds_acquire_display_meta_from_pool(batch_meta);
        NvOSD_TextParams *txt_params  = &display_meta->text_params[0];
        display_meta->num_labels = 1;
        txt_params->display_text = g_malloc0 (MAX_DISPLAY_LEN);
        offset = snprintf(txt_params->display_text, MAX_DISPLAY_LEN, "Person = %d ", person_count);
        offset = snprintf(txt_params->display_text + offset , MAX_DISPLAY_LEN, "Vehicle = %d ", vehicle_count);

        /* Now set the offsets where the string should appear */
        txt_params->x_offset = 10;
        txt_params->y_offset = 12;

        /* Font , font-color and font-size */
        txt_params->font_params.font_name = "Serif";
        txt_params->font_params.font_size = 10;
        txt_params->font_params.font_color.red = 1.0;
        txt_params->font_params.font_color.green = 1.0;
        txt_params->font_params.font_color.blue = 1.0;
        txt_params->font_params.font_color.alpha = 1.0;

        /* Text background color */
        txt_params->set_bg_clr = 1;
        txt_params->text_bg_clr.red = 0.0;
        txt_params->text_bg_clr.green = 0.0;
        txt_params->text_bg_clr.blue = 0.0;
        txt_params->text_bg_clr.alpha = 1.0;

        nvds_add_display_meta_to_frame(frame_meta, display_meta);
    }
    
    frame_number++;
    return GST_PAD_PROBE_OK;
}

static gboolean
bus_call (GstBus * bus, GstMessage * msg, gpointer data)
{
  GMainLoop *loop = (GMainLoop *) data;
  switch (GST_MESSAGE_TYPE (msg)) {
    case GST_MESSAGE_EOS:
      g_print ("End of stream\n");
      g_main_loop_quit (loop);
      break;
    case GST_MESSAGE_ERROR:{
      gchar *debug;
      GError *error;
      gst_message_parse_error (msg, &error, &debug);
      g_printerr ("ERROR from element %s: %s\n",
          GST_OBJECT_NAME (msg->src), error->message);
      if (debug)
        g_printerr ("Error details: %s\n", debug);
      g_free (debug);
      g_error_free (error);
      g_main_loop_quit (loop);
      break;
    }
    default:
      break;
  }
  return TRUE;
}

static GstElement *
create_source_bin(guint index)
{
  GstElement *bin = NULL,
             *source = NULL,
             *vidconv = NULL,
             *nvvidconv = NULL,
             *cap_filter1 = NULL,
             *cap_filter2 = NULL;
  GstPad *real_pad, *ghost_pad;
  GstCaps *caps1, *caps2 = NULL;

  /* Create a source GstBin to abstract this bin's content from the rest of the pipeline */
  gchar bin_name[16] = {};
  g_snprintf(bin_name, 15, "source-bin-%02d", index);
  bin = gst_bin_new(bin_name);

  source = gst_element_factory_make("v4l2src", "camera");
  cap_filter1 = gst_element_factory_make("capsfilter", "cap_filter1");
  vidconv = gst_element_factory_make("videoconvert", "vidconv");
  nvvidconv = gst_element_factory_make("nvvideoconvert", "nvvidconv");
  cap_filter2 = gst_element_factory_make("capsfilter", "cap_filter2");

  if (!bin || !source || !cap_filter1 || !vidconv || !nvvidconv || !cap_filter2)
  {
      g_printerr("One element in source bin could not be created.\n");
      return NULL;
  }

  gchar device_name[16] = {};
  g_snprintf(device_name, 15, "/dev/video%d", 2 * index);
  g_object_set(G_OBJECT(source), "device", device_name, NULL);
  caps1 = gst_caps_from_string("video/x-raw, width=640, height=480, framerate=30/1");
  g_object_set(G_OBJECT(cap_filter1), "caps", caps1, NULL);
  gst_caps_unref(caps1);
  g_object_set(G_OBJECT(nvvidconv), "nvbuf-memory-type", 3, NULL);
  caps2 = gst_caps_from_string("video/x-raw(memory:NVMM), format=NV12");
  g_object_set(G_OBJECT(cap_filter2), "caps", caps2, NULL);
  gst_caps_unref(caps2);

  /* Add elements to the bin */
  gst_bin_add_many(GST_BIN(bin), source, cap_filter1, vidconv, nvvidconv, cap_filter2, NULL);

  /* Link elements in the bin*/
  if (!gst_element_link_many(source, cap_filter1, vidconv, nvvidconv, cap_filter2, NULL))
  {
      g_printerr("One element in source bin could not be linked.\n");
      return NULL;
  }
    
  real_pad = gst_element_get_static_pad(cap_filter2, "src");
  ghost_pad = gst_ghost_pad_new("src", real_pad);
  gst_pad_set_active(ghost_pad, TRUE);
  if (!gst_element_add_pad(bin, ghost_pad))
  {
      g_printerr("Failed to add ghost pad in source bin\n");
      return NULL;
  }
  gst_object_unref(real_pad);

  return bin;
}

GstPadProbeReturn demux_sink_probe(
    GstPad *pad, 
    GstPadProbeInfo *info,
    gpointer u_data)
{
    GstBuffer *buf = (GstBuffer *)info->data;
    NvDsMetaList *l_frame = NULL;
    NvDsFrameMeta *frame_meta = NULL;

    NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta(buf);

    static guint batch_id = 0;
    g_message("==============BATCH[%d]==============", batch_id);
    
    for (l_frame = batch_meta->frame_meta_list; l_frame != NULL; l_frame = l_frame->next)
    {
        frame_meta = (NvDsFrameMeta *)(l_frame->data);
        g_message("%s: num frames=%d, frame_meta->batch_id=%d, frame_meta->source_id=%d, frame_meta->buf_pts=%ld", __func__, g_list_length(batch_meta->frame_meta_list), frame_meta->batch_id, frame_meta->source_id, frame_meta->buf_pts);
    }

    batch_id++;

    return GST_PAD_PROBE_OK;
}

GstPadProbeReturn mux_src_probe(
    GstPad *pad, 
    GstPadProbeInfo *info,
    gpointer u_data)
{
    GstBuffer *buf = (GstBuffer *)info->data;
    NvDsMetaList *l_frame = NULL;
    NvDsFrameMeta *frame_meta = NULL;

    NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta(buf);

    static guint batch_id = 0;
    
    for (l_frame = batch_meta->frame_meta_list; l_frame != NULL; l_frame = l_frame->next)
    {
        frame_meta = (NvDsFrameMeta *)(l_frame->data);
        if(frame_meta->batch_id >= g_list_length(batch_meta->frame_meta_list))
        {
            g_message("==============BATCH[%d]==============", batch_id);
            g_error("%s: num frames=%d, frame_meta->batch_id=%d, frame_meta->source_id=%d, frame_meta->buf_pts=%ld", __func__, g_list_length(batch_meta->frame_meta_list), frame_meta->batch_id, frame_meta->source_id, frame_meta->buf_pts);
            exit(1);
        }
    }

    batch_id++;

    return GST_PAD_PROBE_OK;
}

int
main (int argc, char *argv[])
{
  GMainLoop *loop = NULL;
  GstElement *pipeline = NULL, *streammux1 = NULL, *sink = NULL, *pgie = NULL, *tee_demux = NULL, *queue_demux = NULL, *streamdemux = NULL, *streammux2 = NULL, *tiler = NULL, *nvvidconv = NULL, *nvosd = NULL;
  GstBus *bus = NULL;
  guint bus_watch_id;
  GstPad *osd_sink_pad = NULL;
  GstPad *demux_sink_pad = NULL;
  GstPad *mux_src_pad = NULL;

  /* Standard GStreamer initialization */
  gst_init (&argc, &argv);
  loop = g_main_loop_new (NULL, FALSE);

  /* Create gstreamer elements */
  /* Create Pipeline element that will form a connection of other elements */
  pipeline = gst_pipeline_new ("dstest1-pipeline");

  /* Create nvstreammux instance to form batches from one or more sources. */
  streammux1 = gst_element_factory_make ("nvstreammux", "stream-muxer1");

  if (!pipeline || !streammux1) {
    g_printerr ("One element could not be created. Exiting.\n");
    return -1;
  }

  pgie = gst_element_factory_make ("nvinfer", "primary-nvinference-engine");
  tee_demux = gst_element_factory_make("tee", "tee_demux");
  queue_demux = gst_element_factory_make("queue", "queue_demux");
  streamdemux = gst_element_factory_make("nvstreamdemux", "stream_demuxer");
  streammux2 = gst_element_factory_make ("nvstreammux", "stream-muxer2");
  tiler = gst_element_factory_make("nvmultistreamtiler", "nvtiler");
  nvvidconv = gst_element_factory_make ("nvvideoconvert", "nvvideo-converter");
  nvosd = gst_element_factory_make ("nvdsosd", "nv-onscreendisplay");
  sink = gst_element_factory_make("nveglglessink", "nvvideo-renderer");

  if (!pgie || !tee_demux || !queue_demux || !streamdemux || !streammux2 || !tiler || !nvvidconv || !nvosd || !sink) {
    g_printerr ("One element could not be created. Exiting.\n");
    return -1;
  }

  /* we set the input filename to the source element */
  g_object_set (G_OBJECT (streammux1),
  "batch-size", 2, 
  "width", MUXER_OUTPUT_WIDTH, 
  "height", MUXER_OUTPUT_HEIGHT,
  "batched-push-timeout", MUXER_BATCH_TIMEOUT_USEC, 
  "live-source", TRUE,
  "nvbuf-memory-type", 3,
  NULL);

  g_object_set (G_OBJECT (streammux2),
  "batch-size", 2, 
  "width", MUXER_OUTPUT_WIDTH, 
  "height", MUXER_OUTPUT_HEIGHT,
  "batched-push-timeout", MUXER_BATCH_TIMEOUT_USEC, 
  "live-source", TRUE,
  "nvbuf-memory-type", 3,
  NULL);

  g_object_set (G_OBJECT (pgie), "config-file-path", "dstest1_pgie_config.txt", NULL);

  g_object_set(G_OBJECT(tiler),
            "rows", 1,
            "columns", 2,
            "width", 1280,
            "height", 480,
            NULL);

  g_object_set(G_OBJECT(sink), "sync", 0, NULL);

  /* we add a message handler */
  bus = gst_pipeline_get_bus (GST_PIPELINE (pipeline));
  bus_watch_id = gst_bus_add_watch (bus, bus_call, loop);
  gst_object_unref (bus);

  /* Set up the pipeline */
  /* we add all elements into the pipeline */
  gst_bin_add_many (GST_BIN (pipeline), streammux1, pgie, 
  tee_demux, queue_demux, streamdemux, 
  streammux2, tiler, nvvidconv, nvosd, sink, NULL);

  /* we link the elements together */
  if (!gst_element_link_many (streammux1, pgie, tee_demux, queue_demux, streamdemux, NULL)) {
    g_printerr ("Elements could not be linked: 1. Exiting.\n");
    return -1;
  }

  if (!gst_element_link_many (streammux2, tiler, nvvidconv, nvosd, sink, NULL)) {
    g_printerr ("Elements could not be linked: 2. Exiting.\n");
    return -1;
  }
  
  for(int i = 0; i < 2; i++)
  {
    GstElement *source_bin = create_source_bin(i);
    GstElement *tee = gst_element_factory_make ("tee", NULL);
    GstElement *queue = gst_element_factory_make ("queue", NULL);
    gst_bin_add_many(GST_BIN(pipeline), source_bin, tee, queue, NULL);

    GstPad *sinkpad, *srcpad;
    gchar pad_name[16] = {};
    g_snprintf(pad_name, 15, "sink_%u", i);
    sinkpad = gst_element_get_request_pad(streammux1, pad_name);
    srcpad = gst_element_get_static_pad(source_bin, "src");
    gst_pad_link(srcpad, sinkpad);
    gst_object_unref(srcpad);
    gst_object_unref(sinkpad);

    GstPad *demux_src_pad = NULL;
    g_snprintf(pad_name, 15, "src_%u", i);
    demux_src_pad = gst_element_get_request_pad (streamdemux, pad_name);
    sinkpad = gst_element_get_static_pad (tee, "sink");
    gst_pad_link (demux_src_pad, sinkpad);
    gst_object_unref(demux_src_pad);
    gst_object_unref(sinkpad);

    GstPad *tee_src_pad = NULL;
    GstPadTemplate *padtemplate = (GstPadTemplate *) gst_element_class_get_pad_template (GST_ELEMENT_GET_CLASS (tee), "src_%u");
    tee_src_pad = gst_element_request_pad (tee, padtemplate, NULL, NULL);
    sinkpad = gst_element_get_static_pad (queue, "sink");
    gst_pad_link (tee_src_pad, sinkpad);
    gst_object_unref(tee_src_pad);
    gst_object_unref(sinkpad);

    GstPad *mux_sink_pad = NULL;
    g_snprintf (pad_name, 15, "sink_%u", i);
    mux_sink_pad = gst_element_get_request_pad (streammux2, pad_name);
    srcpad = gst_element_get_static_pad (queue, "src");
    gst_pad_link (srcpad, mux_sink_pad);
    gst_object_unref(mux_sink_pad);
    gst_object_unref(srcpad);
  }

  /* Lets add probe to get informed of the meta data generated, we add probe to
   * the sink pad of the osd element, since by that time, the buffer would have
   * had got all the metadata. */
  osd_sink_pad = gst_element_get_static_pad (nvosd, "sink");
  if (!osd_sink_pad)
    g_print ("Unable to get sink pad\n");
  else
    gst_pad_add_probe (osd_sink_pad, GST_PAD_PROBE_TYPE_BUFFER, osd_sink_pad_buffer_probe, NULL, NULL);
  gst_object_unref (osd_sink_pad);

  demux_sink_pad = gst_element_get_static_pad (streamdemux, "sink");
  if (!demux_sink_pad)
    g_print ("Unable to get sink pad\n");
  else
    gst_pad_add_probe (demux_sink_pad, GST_PAD_PROBE_TYPE_BUFFER, demux_sink_probe, NULL, NULL);
  gst_object_unref (demux_sink_pad);

  mux_src_pad = gst_element_get_static_pad (streammux2, "src");
  if (!mux_src_pad)
    g_print ("Unable to get sink pad\n");
  else
    gst_pad_add_probe (mux_src_pad, GST_PAD_PROBE_TYPE_BUFFER, mux_src_probe, NULL, NULL);
  gst_object_unref (mux_src_pad);

  /* Set the pipeline to "playing" state */
  gst_element_set_state (pipeline, GST_STATE_PLAYING);

  /* Wait till pipeline encounters an error or EOS */
  g_print ("Running...\n");
  GST_DEBUG_BIN_TO_DOT_FILE_WITH_TS(GST_BIN(pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline");
  g_main_loop_run (loop);

  /* Out of the main loop, clean up nicely */
  g_print ("Returned, stopping playback\n");
  gst_element_set_state (pipeline, GST_STATE_NULL);
  g_print ("Deleting pipeline\n");
  gst_object_unref (GST_OBJECT (pipeline));
  g_source_remove (bus_watch_id);
  g_main_loop_unref (loop);
  return 0;
}