/*
 * Copyright (c) 2019-2020 NVIDIA Corporation.  All rights reserved.
 *
 * NVIDIA Corporation and its licensors retain all intellectual property
 * and proprietary rights in and to this software, related documentation
 * and any modifications thereto.  Any use, reproduction, disclosure or
 * distribution of this software and related documentation without an express
 * license agreement from NVIDIA Corporation is strictly prohibited.
 *
 */

#include <gst/gst.h>
#include <glib.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <sys/time.h>
#include <iostream>
#include "gstnvdsmeta.h"

/* 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 TRACKER_CONFIG_FILE "tracker_config.txt"
#define BEARD_SGIE_CONFIG_FILE "beard_sgie_config.txt"

#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 4000000

#define TILED_OUTPUT_WIDTH 1920
#define TILED_OUTPUT_HEIGHT 1080

clock_t t_start; 
clock_t t_end;
gint frame_number=0;

static gchar *
get_absolute_file_path (gchar *cfg_file_path, gchar *file_path)
{
  gchar abs_cfg_path[PATH_MAX + 1];
  gchar *abs_file_path;
  gchar *delim;

  if (file_path && file_path[0] == '/') {
    return file_path;
  }

  if (!realpath (cfg_file_path, abs_cfg_path)) {
    g_free (file_path);
    return NULL;
  }

  // Return absolute path of config file if file_path is NULL.
  if (!file_path) {
    abs_file_path = g_strdup (abs_cfg_path);
    return abs_file_path;
  }

  delim = g_strrstr (abs_cfg_path, "/");
  *(delim + 1) = '\0';

  abs_file_path = g_strconcat (abs_cfg_path, file_path, NULL);
  g_free (file_path);

  return abs_file_path;
}


static GstPadProbeReturn
tiler_src_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) {
	        
        }
          frame_number++;
          g_print ("Frame Number = %d\n",
            frame_meta->frame_num);
#if 0
        display_meta = nvds_acquire_display_meta_from_pool(batch_meta);
        NvOSD_TextParams *txt_params  = &display_meta->text_params;
        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);
#endif

    }
    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");
        t_end = clock();
        clock_t t = t_end - t_start;
        double time_taken = ((double)t)/CLOCKS_PER_SEC; // in seconds
        double fps = frame_number/time_taken;
        g_print("\nThe program took %.2f seconds to detect %d frames, pref = %.2f fps \n\n", time_taken,frame_number,fps);
        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;
}


/* Tracker config parsing */

#define CHECK_ERROR(error) \
    if (error) { \
        g_printerr ("Error while parsing config file: %s\n", error->message); \
        goto done; \
    }

#define CONFIG_GROUP_TRACKER "tracker"
#define CONFIG_GROUP_TRACKER_WIDTH "tracker-width"
#define CONFIG_GROUP_TRACKER_HEIGHT "tracker-height"
#define CONFIG_GROUP_TRACKER_LL_CONFIG_FILE "ll-config-file"
#define CONFIG_GROUP_TRACKER_LL_LIB_FILE "ll-lib-file"
#define CONFIG_GROUP_TRACKER_ENABLE_BATCH_PROCESS "enable-batch-process"
#define CONFIG_GPU_ID "gpu-id"

static gboolean
set_tracker_properties (GstElement *nvtracker)
{
  gboolean ret = FALSE;
  GError *error = NULL;
  gchar **keys = NULL;
  gchar **key = NULL;
  GKeyFile *key_file = g_key_file_new ();

  if (!g_key_file_load_from_file (key_file, TRACKER_CONFIG_FILE, G_KEY_FILE_NONE,
          &error)) {
    g_printerr ("Failed to load config file: %s\n", error->message);
    return FALSE;
  }

  keys = g_key_file_get_keys (key_file, CONFIG_GROUP_TRACKER, NULL, &error);
  CHECK_ERROR (error);

  for (key = keys; *key; key++) {
    if (!g_strcmp0 (*key, CONFIG_GROUP_TRACKER_WIDTH)) {
      gint width =
          g_key_file_get_integer (key_file, CONFIG_GROUP_TRACKER,
          CONFIG_GROUP_TRACKER_WIDTH, &error);
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "tracker-width", width, NULL);
    } else if (!g_strcmp0 (*key, CONFIG_GROUP_TRACKER_HEIGHT)) {
      gint height =
          g_key_file_get_integer (key_file, CONFIG_GROUP_TRACKER,
          CONFIG_GROUP_TRACKER_HEIGHT, &error);
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "tracker-height", height, NULL);
    } else if (!g_strcmp0 (*key, CONFIG_GPU_ID)) {
      guint gpu_id =
          g_key_file_get_integer (key_file, CONFIG_GROUP_TRACKER,
          CONFIG_GPU_ID, &error);
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "gpu_id", gpu_id, NULL);
    } else if (!g_strcmp0 (*key, CONFIG_GROUP_TRACKER_LL_CONFIG_FILE)) {
      char* ll_config_file = get_absolute_file_path (TRACKER_CONFIG_FILE,
                g_key_file_get_string (key_file,
                    CONFIG_GROUP_TRACKER,
                    CONFIG_GROUP_TRACKER_LL_CONFIG_FILE, &error));
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "ll-config-file", ll_config_file, NULL);
    } else if (!g_strcmp0 (*key, CONFIG_GROUP_TRACKER_LL_LIB_FILE)) {
      char* ll_lib_file = get_absolute_file_path (TRACKER_CONFIG_FILE,
                g_key_file_get_string (key_file,
                    CONFIG_GROUP_TRACKER,
                    CONFIG_GROUP_TRACKER_LL_LIB_FILE, &error));
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "ll-lib-file", ll_lib_file, NULL);
    } else if (!g_strcmp0 (*key, CONFIG_GROUP_TRACKER_ENABLE_BATCH_PROCESS)) {
      gboolean enable_batch_process =
          g_key_file_get_integer (key_file, CONFIG_GROUP_TRACKER,
          CONFIG_GROUP_TRACKER_ENABLE_BATCH_PROCESS, &error);
      CHECK_ERROR (error);
      g_object_set (G_OBJECT (nvtracker), "enable_batch_process",
                    enable_batch_process, NULL);
    } else {
      g_printerr ("Unknown key '%s' for group [%s]", *key,
          CONFIG_GROUP_TRACKER);
    }
  }

  ret = TRUE;
done:
  if (error) {
    g_error_free (error);
  }
  if (keys) {
    g_strfreev (keys);
  }
  if (!ret) {
    g_printerr ("%s failed", __func__);
  }
  return ret;
}


static void printUsage(const char* cmd) {
    g_printerr ("\tUsage: %s -c pgie_config_file -i <H264 or JPEG filename> [-b BATCH] [-d]\n", cmd);
    g_printerr ("-h: \n\tprint help info \n");
    g_printerr ("-c: \n\tpgie config file, e.g. pgie_frcnn_tlt_config.txt  \n");
    g_printerr ("-i: \n\tH264 or JPEG input file  \n");
    g_printerr ("-b: \n\tbatch size, this will override the value of \"baitch-size\" in pgie config file  \n");
    g_printerr ("-d: \n\tenable display, otherwise dump to output H264 or JPEG file  \n");
}
int
main (int argc, char *argv[]) {
    GMainLoop *loop = NULL;
    GstElement *pipeline = NULL, *source = NULL, *parser = NULL,
               *decoder = NULL, *streammux = NULL, *sink = NULL,
               *pgie = NULL, *nvvidconv = NULL, *nvdsosd = NULL,
               *parser1 = NULL, *nvvidconv1 = NULL, *enc = NULL,
               *tiler = NULL, *tee = NULL, *nvtracker = NULL, *sgie=NULL;

#ifdef PLATFORM_TEGRA
    GstElement *transform = NULL;
#endif
    GstBus *bus = NULL;
    guint bus_watch_id;
    GstPad *osd_sink_pad = NULL;

    gboolean isH264 = FALSE;
    gboolean useDisplay = FALSE;
    guint tiler_rows, tiler_cols;
    guint batchSize = 1;
    guint pgie_batch_size, sgie_batch_size;
    guint c;
    const char* optStr = "b:c:dhi:";
    std::string pgie_config;
    std::string input_file;

    while ((c = getopt(argc, argv, optStr)) != -1) {
        switch (c) {
            case 'b':
                batchSize = std::atoi(optarg);
                batchSize = batchSize == 0 ? 1:batchSize;
                break;
            case 'c':
                pgie_config.assign(optarg);
                break;
            case 'd':
                useDisplay = TRUE;
                break;
            case 'i':
                input_file.assign(optarg);
                break;
            case 'h':
            default:
                printUsage(argv[0]);
                return -1;
          }
     }

    /* Check input arguments */
    if (argc == 1) {
        printUsage(argv[0]);
        return -1;
    }

    const gchar *p_end = input_file.c_str() + strlen(input_file.c_str());
    if(!strncmp(p_end - strlen("h264"), "h264", strlen("h264"))) {
        isH264 = TRUE;
    } else if(!strncmp(p_end - strlen("jpg"), "jpg", strlen("jpg")) || !strncmp(p_end - strlen("jpeg"), "jpeg", strlen("jpeg"))) {
        isH264 = FALSE;
    } else {
        g_printerr("input file only support H264 and JPEG\n");
        return -1;
    }

    const char* use_display = std::getenv("USE_DISPLAY");
    if(use_display != NULL && std::stoi(use_display) == 1) {
        useDisplay = true;
    }

    const char* batch_size = std::getenv("BATCH_SIZE");
    if(batch_size != NULL ) {
        batchSize = std::stoi(batch_size);
        g_printerr("batch size is %d \n", batchSize);
    }

    /* 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 ("ds-custom-pipeline");

    /* Source element for reading from the file */
    source = gst_element_factory_make ("filesrc", "file-source");

    /* Since the data format in the input file is elementary h264 stream,
     * we need a h264parser */
    if(isH264 == TRUE) {
        parser = gst_element_factory_make ("h264parse", "h264-parser");
    } else {
        parser = gst_element_factory_make ("jpegparse", "jpeg-parser");
    }

    /* Use nvdec_h264 for hardware accelerated decode on GPU */
    decoder = gst_element_factory_make ("nvv4l2decoder", "nvv4l2-decoder");

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

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

    /* Use nvinfer to run inferencing on decoder's output,
     * behaviour of inferencing is set through config file */
    pgie = gst_element_factory_make ("nvinfer", "primary-nvinference-engine");

    /* We need to have a tracker to track the identified objects */
    nvtracker = gst_element_factory_make ("nvtracker", "tracker");

    /* We need three secondary gies so lets create 3 more instances of
     nvinfer */
    sgie = gst_element_factory_make ("nvinfer", "secondary1-nvinference-engine");

    /* Use convertor to convert from NV12 to RGBA as required by nvdsosd */
    nvvidconv = gst_element_factory_make ("nvvideoconvert", "nvvideo-converter");

    /* Create OSD to draw on the converted RGBA buffer */
    nvdsosd = gst_element_factory_make ("nvdsosd", "nv-onscreendisplay");

    tee = gst_element_factory_make("tee", "tee");
    tiler = gst_element_factory_make ("nvmultistreamtiler", "nvtiler");

    /* Finally render the osd output */
#ifdef PLATFORM_TEGRA
    transform = gst_element_factory_make ("nvegltransform", "nvegl-transform");
#endif
    if(useDisplay == FALSE) {
        if(isH264 == TRUE){
            parser1 = gst_element_factory_make ("h264parse", "h264-parser1");
            enc = gst_element_factory_make ("nvv4l2h264enc", "h264-enc");
        } else{
            parser1 = gst_element_factory_make ("jpegparse", "jpeg-parser1");
            enc = gst_element_factory_make ("jpegenc", "jpeg-enc");
        }
        nvvidconv1 = gst_element_factory_make ("nvvideoconvert", "nvvideo-converter1");
        sink = gst_element_factory_make ("filesink", "file-sink");
        //if (!source || !parser || !parser1 || !decoder || !tee || !pgie
             //|| !nvtracker || !sgie || !tiler || !nvvidconv || !nvvidconv1 || !nvdsosd || !enc || !sink) {
        if (!source || !parser || !parser1 || !decoder || !tee || !pgie
         || !tiler || !nvvidconv || !nvvidconv1 || !nvdsosd || !enc || !sink) {
            g_printerr ("One element could not be created. Exiting.\n");
            return -1;
        }
    } else {
        sink = gst_element_factory_make ("nveglglessink", "nvvideo-renderer");
        if (!source || !parser || !decoder || !tee || !pgie
            || !nvtracker || !sgie || !tiler || !nvvidconv || !nvdsosd || !sink) {
            g_printerr ("One element could not be created. Exiting.\n");
            return -1;
        }
    }

#ifdef PLATFORM_TEGRA
    if(!transform) {
        g_printerr ("One tegra element could not be created. Exiting.\n");
        return -1;
    }
#endif

    /* we set the input filename to the source element */
    g_object_set (G_OBJECT (source), "location", input_file.c_str(), NULL);

    //save the file to local dir
    if(useDisplay == FALSE) {
        if(isH264 == TRUE)
            g_object_set (G_OBJECT (sink), "location", "./out.h264", NULL);
        else
            g_object_set (G_OBJECT (sink), "location", "./out.jpg", NULL);
    }
    g_object_set (G_OBJECT (streammux), "width", MUXER_OUTPUT_WIDTH, "height",
                  MUXER_OUTPUT_HEIGHT, "batch-size", batchSize,
                  "batched-push-timeout", MUXER_BATCH_TIMEOUT_USEC, NULL);

    /* Set all the necessary properties of the nvinfer element,
     * the necessary ones are : */
    g_object_set (G_OBJECT (pgie),
                  "config-file-path", pgie_config.c_str(), NULL);

    g_object_set(G_OBJECT (sgie), 
                  "config-file-path", BEARD_SGIE_CONFIG_FILE, NULL);

    /* Override the batch-size set in the config file with the number of sources. */
    g_object_get (G_OBJECT (pgie), "batch-size", &pgie_batch_size, NULL);
    if (pgie_batch_size != batchSize) {
    g_printerr
        ("WARNING: Overriding infer-config batch-size (%d) with number of sources (%d)\n",
        pgie_batch_size, batchSize);
    g_object_set (G_OBJECT (pgie), "batch-size", batchSize, NULL);
    }

     /* Override the batch-size set in the config file with the number of sources. */
    g_object_get (G_OBJECT (sgie), "batch-size", &sgie_batch_size, NULL);
    if (pgie_batch_size != batchSize) {
    g_printerr
        ("WARNING: Overriding secondary-infer-config batch-size (%d) with number of sources (%d)\n",
        sgie_batch_size, batchSize);
    g_object_set (G_OBJECT (sgie), "batch-size", batchSize, NULL);
    }

    if (!set_tracker_properties(nvtracker)) {
        g_printerr ("Failed to set tracker properties. Exiting.\n");
        return -1;
    }

    tiler_rows = (guint) sqrt (batchSize);
    tiler_cols = (guint) ceil (1.0 * batchSize / tiler_rows);
    /* we set the tiler properties here */
    g_object_set (G_OBJECT (tiler), "rows", tiler_rows, "columns", tiler_cols,
      "width", TILED_OUTPUT_WIDTH, "height", TILED_OUTPUT_HEIGHT, 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 */
    if(useDisplay == FALSE) {
        // gst_bin_add_many (GST_BIN (pipeline),
        //                   source, parser, decoder, tee, streammux, pgie, nvtracker, sgie, tiler,
        //                   nvvidconv, nvdsosd, nvvidconv1, enc, parser1, sink, NULL);
        gst_bin_add_many (GST_BIN (pipeline),
                          source, parser, decoder, tee, streammux, pgie, tiler,
                          nvvidconv, nvdsosd, nvvidconv1, enc, parser1, sink, NULL);    
    } else {
#ifdef PLATFORM_TEGRA
        gst_bin_add_many (GST_BIN (pipeline),
                          source, parser, decoder, tee, streammux, pgie, nvtracker, sgie,
                          tiler, nvvidconv, nvdsosd, transform, sink, NULL);
#else
        gst_bin_add_many (GST_BIN (pipeline),
                          source, parser, decoder, tee, streammux, pgie, nvtracker, sgie,
                          tiler, nvvidconv, nvdsosd, sink, NULL);
#endif
    }

    for(guint i = 0; i < batchSize; i++) {
        GstPad *sinkpad, *srcpad;
        gchar pad_name_sink[16] = {};
        gchar pad_name_src[16] = {};

        g_snprintf (pad_name_sink, 15, "sink_%u", i);
        g_snprintf (pad_name_src, 15, "src_%u", i);
        sinkpad = gst_element_get_request_pad (streammux, pad_name_sink);
        if (!sinkpad) {
            g_printerr ("Streammux request sink pad failed. Exiting.\n");
            return -1;
        }

        srcpad = gst_element_get_request_pad(tee, pad_name_src);
        if (!srcpad) {
            g_printerr ("tee request src pad failed. Exiting.\n");
            return -1;
        }

        if (gst_pad_link (srcpad, sinkpad) != GST_PAD_LINK_OK) {
            g_printerr ("Failed to link tee to stream muxer. Exiting.\n");
            return -1;
        }

        gst_object_unref (sinkpad);
        gst_object_unref (srcpad);
    }
    /* We link the elements together */
    /* file-source -> h264-parser -> nvv4l2decoder ->
     * nvinfer -> nvvideoconvert -> nvdsosd -> video-renderer */

    if (!gst_element_link_many (source, parser, decoder, tee, NULL)) {
        g_printerr ("Elements could not be linked: 1. Exiting.\n");
        return -1;
    }
    if (useDisplay == FALSE) {
        // if (!gst_element_link_many (streammux, pgie, nvtracker, sgie, tiler,
        //                             nvvidconv, nvdsosd, nvvidconv1, enc, parser1, sink, NULL)) {
        //     g_printerr ("Elements could not be linked: 2. Exiting.\n");
        //     return -1;
        // }
        if (!gst_element_link_many (streammux, pgie, tiler,
                                    nvvidconv, nvdsosd, nvvidconv1, enc, parser1, sink, NULL)) {
            g_printerr ("Elements could not be linked: 2. Exiting.\n");
            return -1;
        }
    } else {
#ifdef PLATFORM_TEGRA
        if (!gst_element_link_many (streammux, pgie, tiler, 
                                    nvvidconv, nvdsosd, transform, sink, NULL)) {
            g_printerr ("Elements could not be linked: 2. Exiting.\n");
            return -1;
        }
#else
        if (!gst_element_link_many (streammux, pgie, tiler,
                                    nvvidconv, nvdsosd, sink, NULL)) {
            g_printerr ("Elements could not be linked: 2. Exiting.\n");
            return -1;
        }
#endif
    }

    /* 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. */
    GstPad *tiler_src_pad = NULL;
    tiler_src_pad = gst_element_get_static_pad (pgie, "src");
    if (!tiler_src_pad)
        g_print ("Unable to get src pad\n");
    else
        gst_pad_add_probe (tiler_src_pad, GST_PAD_PROBE_TYPE_BUFFER,
            tiler_src_pad_buffer_probe, NULL, NULL);

    /* Set the pipeline to "playing" state */
    g_print ("Now playing: %s\n", pgie_config.c_str());
    gst_element_set_state (pipeline, GST_STATE_PLAYING);

    /* Wait till pipeline encounters an error or EOS */
    g_print ("Running...\n");
    t_start = clock();
    g_main_loop_run (loop);
    t_end = clock();
    clock_t t = t_end - t_start;
    double time_taken = ((double)t)/CLOCKS_PER_SEC; // in seconds
    g_print("\nThe program took %.2f seconds!", time_taken);

    /* 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;
}