/*
 * Copyright (c) 2018-2020, 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 "gstnvdsmeta.h"
#include "nvbufsurface.h"
#include "nvbufsurftransform.h"
#include <opencv2/opencv.hpp>

#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 1280  //1920
#define MUXER_OUTPUT_HEIGHT 720 //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
decode_src_pad_buffer_probe (GstPad * pad, GstPadProbeInfo * info, gpointer u_data){
	static int flag = 1;
	GstBuffer *buf = (GstBuffer *) info->data;

	NvDsBatchMeta *batch_meta =
		// gst_buffer_get_nvds_batch_meta (GST_BUFFER (info->data));
		gst_buffer_get_nvds_batch_meta (buf);
	GstMapInfo in_map_info;
	NvBufSurface *surface = NULL;
	memset (&in_map_info, 0, sizeof (in_map_info));
	if (!gst_buffer_map (buf, &in_map_info, GST_MAP_READ)) {
		g_print ("Error: Failed to map gst buffer\n");
	}
	surface = (NvBufSurface *) in_map_info.data;

	int batch_size = surface->batchSize;
	printf("\nBatch Size : %d, resolution : %dx%d \n",batch_size,
		surface->surfaceList[0].width, surface->surfaceList[0].height);

	cudaError_t cuda_err;

    NvBufSurfTransformRect src_rect, dst_rect;
    src_rect.top   = 0;
    src_rect.left  = 0;
    src_rect.width = (guint) surface->surfaceList[0].width;
    src_rect.height= (guint) surface->surfaceList[0].height;

    dst_rect.top   = 0;
    dst_rect.left  = 0;
    dst_rect.width = (guint) surface->surfaceList[0].width;
    dst_rect.height= (guint) surface->surfaceList[0].height;

    NvBufSurfTransformParams nvbufsurface_params;
    nvbufsurface_params.src_rect = &src_rect;
    nvbufsurface_params.dst_rect = &dst_rect;
    nvbufsurface_params.transform_flag =  NVBUFSURF_TRANSFORM_CROP_SRC | NVBUFSURF_TRANSFORM_CROP_DST;
    nvbufsurface_params.transform_filter = NvBufSurfTransformInter_Default;
  
    NvBufSurface *dst_surface = NULL;
    NvBufSurfaceCreateParams nvbufsurface_create_params;

    /* An intermediate buffer for NV12/RGBA to BGR conversion  will be
     * required. Can be skipped if custom algorithm can work directly on NV12/RGBA. */
    nvbufsurface_create_params.gpuId  = surface->gpuId;
    nvbufsurface_create_params.width  = (gint) surface->surfaceList[0].width;
    nvbufsurface_create_params.height = (gint) surface->surfaceList[0].height;
    nvbufsurface_create_params.size = 0;
    nvbufsurface_create_params.colorFormat = NVBUF_COLOR_FORMAT_RGBA;
    nvbufsurface_create_params.layout = NVBUF_LAYOUT_PITCH;
    nvbufsurface_create_params.memType = NVBUF_MEM_DEFAULT;

    cuda_err = cudaSetDevice (surface->gpuId);

    cudaStream_t cuda_stream;

    cuda_err=cudaStreamCreate (&cuda_stream);

    int create_result = NvBufSurfaceCreate(&dst_surface,batch_size,&nvbufsurface_create_params);	

    NvBufSurfTransformConfigParams transform_config_params;
    NvBufSurfTransform_Error err;

    transform_config_params.compute_mode = NvBufSurfTransformCompute_Default;
    transform_config_params.gpu_id = surface->gpuId;
    transform_config_params.cuda_stream = cuda_stream;
    err = NvBufSurfTransformSetSessionParams (&transform_config_params);

    NvBufSurfaceMemSet (dst_surface, 0, 0, 0);
    err = NvBufSurfTransform (surface, dst_surface, &nvbufsurface_params);
    if (err != NvBufSurfTransformError_Success) {
  	  g_print ("NvBufSurfTransform failed with error %d while converting buffer\n", err);
    }
    NvBufSurfaceMap (dst_surface, 0, 0, NVBUF_MAP_READ);
    NvBufSurfaceSyncForCpu (dst_surface, 0, 0);

    cv::Mat bgr_frame = cv::Mat (cv::Size(nvbufsurface_create_params.width, nvbufsurface_create_params.height), CV_8UC3);

    cv::Mat in_mat =
        cv::Mat (nvbufsurface_create_params.height, nvbufsurface_create_params.width,
        CV_8UC4, dst_surface->surfaceList[0].mappedAddr.addr[0],
        dst_surface->surfaceList[0].pitch);

    cv::cvtColor (in_mat, bgr_frame, CV_RGBA2BGR);	

    NvBufSurfaceUnMap (dst_surface, 0, 0);
    NvBufSurfaceDestroy (dst_surface);
    cudaStreamDestroy (cuda_stream);
    gst_buffer_unmap (buf, &in_map_info);

	// NvBufSurfaceMap (surface, 0, 0, NVBUF_MAP_READ);
	// NvBufSurfaceSyncForCpu (surface, 0, 0);

	// cv::Mat in_mat =
	// 	cv::Mat (MUXER_OUTPUT_HEIGHT, MUXER_OUTPUT_WIDTH,
	// 	CV_8UC4, surface->surfaceList[0].mappedAddr.addr[0],
	// 	surface->surfaceList[0].pitch);

	// cv::Mat out_mat;
	// cv::cvtColor (in_mat, out_mat, CV_RGBA2BGR);
	// if(flag == 1){ flag = 0; cv::imwrite("out_buf.jpg",out_mat); }
	
	// NvBufSurfaceUnMap (surface, 0, 0);
	// NvBufSurfaceDestroy (surface);
	// gst_buffer_unmap (buf, &in_map_info);

	
	return GST_PAD_PROBE_OK;
}

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 = (gchar *) 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 = (gchar *) "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);
	}

	g_print ("Frame Number = %d Number of objects = %d "
			"Vehicle Count = %d Person Count = %d\n",
			frame_number, num_rects, vehicle_count, person_count);
	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;
}

int
main (int argc, char *argv[])
{
  GMainLoop *loop = NULL;
  GstElement *pipeline = NULL, *source = NULL, *h264parser = NULL,
	  *decoder = NULL, *streammux = NULL, *sink = NULL, *pgie = NULL, *nvvidconv = NULL,
	  *nvosd = NULL;
  GstElement *nvc;
#ifdef PLATFORM_TEGRA
  GstElement *transform = NULL;
#endif
  GstBus *bus = NULL;
  guint bus_watch_id;
  GstPad *osd_sink_pad = NULL;

  /* Check input arguments */
  if (argc != 2) {
	g_printerr ("Usage: %s <H264 filename>\n", argv[0]);
	return -1;
  }

  /* 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");

  /* 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 */
  h264parser = gst_element_factory_make ("h264parse", "h264-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");

  /* Use convertor to convert from NV12 to RGBA as required by nvosd */
  nvvidconv = gst_element_factory_make ("nvvideoconvert", "nvvideo-converter");
  // g_object_set ( G_OBJECT(nvvidconv), "nvbuf-memory-type", 3, NULL);
  nvc = gst_element_factory_make ("nvvideoconvert", "nvvideo-converter1");
  g_object_set ( G_OBJECT(nvc), "nvbuf-memory-type", 3, NULL);

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

  /* Finally render the osd output */
#ifdef PLATFORM_TEGRA
  transform = gst_element_factory_make ("nvegltransform", "nvegl-transform");
#endif
  sink = gst_element_factory_make ("fakesink", "nvvideo-renderer");

  if (!source || !h264parser || !decoder || !pgie
	  || !nvvidconv || !nvosd || !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", argv[1], NULL);

  g_object_set (G_OBJECT (streammux), "batch-size", 1, NULL);

  g_object_set (G_OBJECT (streammux), "width", MUXER_OUTPUT_WIDTH, "height",
	  MUXER_OUTPUT_HEIGHT,
	  "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", "dstest1_pgie_config.txt", 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 */
#ifdef PLATFORM_TEGRA
  gst_bin_add_many (GST_BIN (pipeline),
	  source, h264parser, decoder, streammux, pgie,
	  nvvidconv, nvosd, transform, sink, NULL);
#else
  gst_bin_add_many (GST_BIN (pipeline),
	  source, h264parser, decoder,     nvc,   streammux, pgie,
	  nvvidconv, nvosd, sink, NULL);
#endif

  GstPad *sinkpad, *srcpad;
  gchar pad_name_sink[16] = "sink_0";
  gchar pad_name_src[16] = "src";

  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_static_pad (decoder, pad_name_src);
  srcpad = gst_element_get_static_pad (nvc, pad_name_src);
  if (!srcpad) {
	g_printerr ("Decoder request src pad failed. Exiting.\n");
	return -1;
  }

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

  gst_object_unref (sinkpad);
  gst_object_unref (srcpad);

  /* we link the elements together */
  /* file-source -> h264-parser -> nvh264-decoder ->
   * nvinfer -> nvvidconv -> nvosd -> video-renderer */

  if (!gst_element_link_many (source, h264parser, decoder, nvc, NULL)) {
	g_printerr ("Elements could not be linked: 1. Exiting.\n");
	return -1;
  }

#ifdef PLATFORM_TEGRA
  if (!gst_element_link_many (streammux, pgie,
	  nvvidconv, nvosd, transform, sink, NULL)) {
	g_printerr ("Elements could not be linked: 2. Exiting.\n");
	return -1;
  }
#else
  if (!gst_element_link_many (streammux, pgie,
	  nvvidconv, nvosd, 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. */
  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);

  GstPad *decode_src_pad = gst_element_get_static_pad (decoder, "src");
  if (!decode_src_pad)
	g_print ("Unable to get sink pad\n");
  else
	gst_pad_add_probe (decode_src_pad, GST_PAD_PROBE_TYPE_BUFFER,
		decode_src_pad_buffer_probe, NULL, NULL);

  /* Set the pipeline to "playing" state */
  g_print ("Now playing: %s\n", argv[1]);
  gst_element_set_state (pipeline, GST_STATE_PLAYING);

  /* Wait till pipeline encounters an error or EOS */
  g_print ("Running...\n");
  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;
}