#include "nv_infer_server_manager.hpp"

// #define NVDS_USER_OBJECT_META_LANDMARKS_AND_SOURCE_ID
// (nvds_get_user_meta_type("NVIDIA.NVINFER.USER_META"))
#define NVDS_USER_OBJECT_META_LANDMARKS_AND_SOURCE_ID \
    (nvds_get_user_meta_type(const_cast<gchar *>("NVIDIA.NVINFER.USER_META")))

#define MAX_DISPLAY_LEN 64
#define PGIE_CLASS_ID_PERSON 0
#define IMPRECISE_FACE_CLASS_ID 1
#define PGIE_DETECTED_CLASS_NUM 1
#define BODY_COMPONENT_ID 1
#define IMPRECISE_FACE_COMPONENT_ID 2
#define BODY_TENSOR_SIZE 57
#define MAX_BODY_PER_FRAME 100

gint NvInferServerManager::frame_number = 0;
unsigned int NvInferServerManager::PGIE_NET_WIDTH = 1;
unsigned int NvInferServerManager::PGIE_NET_HEIGHT = 1;
unsigned int NvInferServerManager::MUXER_OUTPUT_WIDTH = 1;
unsigned int NvInferServerManager::MUXER_OUTPUT_HEIGHT = 1;
guint NvInferServerManager::use_device_mem = 0;
float NvInferServerManager::threshold_body_detection = 0;
unsigned int NvInferServerManager::nvds_lib_major_version = NVDS_VERSION_MAJOR;
unsigned int NvInferServerManager::nvds_lib_minor_version = NVDS_VERSION_MINOR;

const gchar pgie_class_str[PGIE_DETECTED_CLASS_NUM][32] = {"Person_NVINFER"};
const gchar imprecise_face_str[PGIE_DETECTED_CLASS_NUM][32] = {
    "ImpreciseFace_NVINFER"};

/* nvds_lib_major_version and nvds_lib_minor_version is the version number of
 * deepstream sdk */

unsigned int nvds_lib_major_version = NVDS_VERSION_MAJOR;
unsigned int nvds_lib_minor_version = NVDS_VERSION_MINOR;

NvInferServerManager::NvInferServerManager() {
    const auto &config = ConfigManager::get_instance().get_config();
    pgie_batch_size = config["pgie_batch_size"];
    inferserver_pgie_config_file =
        config["inferserver_pgie_config_file"].get<std::string>();
    PGIE_NET_WIDTH = config["PGIE_NET_WIDTH"];
    PGIE_NET_HEIGHT = config["PGIE_NET_HEIGHT"];
    MUXER_OUTPUT_WIDTH = config["MUXER_OUTPUT_WIDTH"];
    MUXER_OUTPUT_HEIGHT = config["MUXER_OUTPUT_HEIGHT"];
    threshold_body_detection = config["threshold_body_detection"];
}

bool NvInferServerManager::create_nv_infer_server(int num_sources) {
    /* Configure the nvinferserver element using the config file. */
    primary_detector =
        gst_element_factory_make("nvinferserver", "primary-nvinference-engine");
    g_object_set(G_OBJECT(primary_detector), "config-file-path",
                 inferserver_pgie_config_file.c_str(), "unique-id", 1, NULL);

    /* Override the batch-size set in the config file with the number of
     * sources. */
    g_object_get(G_OBJECT(primary_detector), "batch-size", &pgie_batch_size,
                 NULL);
    if (pgie_batch_size != num_sources) {
        g_printerr(
            "WARNING: Overriding infer-config batch-size (%d) with number of "
            "sources (%d)\n",
            pgie_batch_size, num_sources);
        g_object_set(G_OBJECT(primary_detector), "batch-size", num_sources,
                     NULL);
    }
    if (!primary_detector) {
        g_printerr("Could not create primary detector. Exiting.\n");
        return false;
    }
    return true;
}

// Probe function to inspect NvDsObjectMeta

// GstPadProbeReturn NvInferServerManager::osd_sink_pad_buffer_probe(
//     GstPad *pad, GstPadProbeInfo *info, gpointer user_data) {
//     (void)pad;
//     (void)user_data;
//     GstBuffer *buf = (GstBuffer *)info->data;

//     // Retrieve batch metadata from buffer
//     NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta(buf);
//     if (!batch_meta) {
//         std::cerr << "No batch metadata found\n";
//         return GST_PAD_PROBE_OK;
//     }

//     // probe sees the frame metadata (NvDsFrameMeta) —
//     // but no object metadata (NvDsObjectMeta) was attached to that frame.

//     for (NvDsMetaList *l_frame = batch_meta->frame_meta_list; l_frame !=
//     NULL;
//          l_frame = l_frame->next) {
//         NvDsFrameMeta *frame_meta = (NvDsFrameMeta *)(l_frame->data);
//         // std::cout << "Frame number: " << frame_meta->frame_num <<
//         std::endl;
//         // if (frame_meta->obj_meta_list == NULL) {
//         //     std::cout << "  ⚠️ No object metadata for this frame.\n";
//         // }

//         for (NvDsMetaList *l_obj = frame_meta->obj_meta_list; l_obj != NULL;
//              l_obj = l_obj->next) {
//             NvDsObjectMeta *obj_meta = (NvDsObjectMeta *)(l_obj->data);

//             std::cout << "  Object ID: " << obj_meta->object_id << std::endl;
//             std::cout << "  Class ID: " << obj_meta->class_id << std::endl;
//             std::cout << "  Label: "
//                       << (obj_meta->obj_label ? obj_meta->obj_label : "N/A")
//                       << std::endl;
//             std::cout << "  BBox: x=" << obj_meta->rect_params.left
//                       << " y=" << obj_meta->rect_params.top
//                       << " w=" << obj_meta->rect_params.width
//                       << " h=" << obj_meta->rect_params.height << std::endl;
//         }
//     }
//     return GST_PAD_PROBE_OK;
// }

// // Attach probe to a pad in the pipeline
// void NvInferServerManager::attach_probe_to_element(GstElement *nvosd) {
//     GstPad *sink_pad = gst_element_get_static_pad(nvosd, "src");
//     if (!sink_pad) {
//         std::cerr << "Unable to get nvosd sink pad\n";
//         return;
//     }

//     gst_pad_add_probe(sink_pad, GST_PAD_PROBE_TYPE_BUFFER,
//                       osd_sink_pad_buffer_probe_new, NULL, NULL);
//     gst_object_unref(sink_pad);
// }

// /* This is the buffer probe function that we have registered on the sink pad
//  * of the OSD element. All the infer elements in the pipeline shall attach
//  * their metadata to the GstBuffer, here we will iterate & process the
//  metadata
//  * forex: class ids to strings, counting of class_id objects etc. */
// GstPadProbeReturn NvInferServerManager::osd_sink_pad_buffer_probe_new(
//     GstPad *pad, GstPadProbeInfo *info, gpointer u_data) {
//     (void)pad;
//     (void)u_data;
//     GstBuffer *buf = (GstBuffer *)info->data;
//     guint num_rects = 0;
//     guint person_count = 0;
//     NvDsObjectMeta *obj_meta = NULL;
//     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_PERSON) {
//                 person_count++;
//                 num_rects++;
//                 std::cout << "In OSD sink "
//                           << "x = " << obj_meta->rect_params.left
//                           << " y = " << obj_meta->rect_params.top
//                           << " w = " << obj_meta->rect_params.width
//                           << " h = " << obj_meta->rect_params.height
//                           << " score = " << obj_meta->confidence
//                           << "  Object ID: " << obj_meta->object_id
//                           << std::endl;
//             }
//         }
//         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);
//         (void)offset;

//         /* 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(
//         "In OSD sink "
//         "Frame Number = %d "
//         "Person Count = %d\n",
//         frame_number, person_count);

//     frame_number++;
//     return GST_PAD_PROBE_OK;
// }

/* This is the buffer probe function that we have registered on the src pad
 * of the PGIE's next queue element. PGIE element in the pipeline shall attach
 * its NvDsInferTensorMeta to each frame metadata on GstBuffer, here we will
 * iterate & parse the tensor data to get detection bounding boxes. The result
 * would be attached as object-meta(NvDsObjectMeta) into the same frame
 * metadata.
 */
GstPadProbeReturn NvInferServerManager::pgie_pad_buffer_probe(
    GstPad *pad, GstPadProbeInfo *info, gpointer u_data) {
    (void)pad;
    gboolean *use_new_mux = (gboolean *)u_data;
    guint stream_width = 0, stream_height = 0;

    static NvDsInferNetworkInfo networkInfo{PGIE_NET_WIDTH, PGIE_NET_HEIGHT, 3};
    NvDsBatchMeta *batch_meta =
        gst_buffer_get_nvds_batch_meta(GST_BUFFER(info->data));

    /* Iterate each frame metadata in batch */
    for (NvDsMetaList *l_frame = batch_meta->frame_meta_list; l_frame != NULL;
         l_frame = l_frame->next) {
        NvDsFrameMeta *frame_meta = (NvDsFrameMeta *)l_frame->data;

        // to solve track not showing up issue
        nvds_acquire_meta_lock(batch_meta);
        frame_meta->bInferDone = TRUE;
        nvds_release_meta_lock(batch_meta);
        if (*use_new_mux) {
            stream_width = frame_meta->source_frame_width;
            stream_height = frame_meta->source_frame_height;
            // These values come from the actual source (decoder) frame size
            // before batching/muxing. They tell us the native resolution of the
            // incoming camera/RTSP/file. If have multiple sources with
            // different resolutions, these values can differ per source/frame.
            // Use this if need the original stream resolution (e.g., for
            // scaling bounding boxes back to source coordinates).
        } else {
            stream_width = MUXER_OUTPUT_WIDTH;
            stream_height = MUXER_OUTPUT_HEIGHT;
            // These are the dimensions configured in nvstreammux (width /
            // height). All sources fed into the muxer get scaled/padded to this
            // resolution before being passed to downstream elements (like
            // PGIE). So PGIE always “sees” frames at muxer resolution, not the
            // raw input resolution. Use this if need the effective frame size
            // that PGIE is processing (i.e., what TensorRT sees).
        }
        // Inside PGIE (nvinfer), the correct dimensions are the muxer output
        // width/height, because frames are resized by nvstreammux before
        // inference. If want the original camera’s resolution, use
        // frame_meta->source_frame_width / source_frame_height.
        // nvmultiurisrcbin internally creates a nvstreammux before sending
        // buffers downstream. That means by the time PGIE sees frames, they are
        // already scaled to the muxer’s output size.

        // At PGIE input, the frame resolution is the muxer’s configured output
        // size. Therefore the correct dimensions for PGIE are: stream_width  =
        // MUXER_OUTPUT_WIDTH; stream_height = MUXER_OUTPUT_HEIGHT; Why not
        // frame_meta->source_frame_width? Those fields still exist in
        // frame_meta, but they represent the original source stream resolution
        // (camera/file). Since PGIE never directly sees that resolution (it
        // only sees muxed frames), using these values inside PGIE would be
        // misleading.

        // For this pipeline, use MUXER_OUTPUT_WIDTH and MUXER_OUTPUT_HEIGHT to
        // represent what PGIE actually processes. If later need to map
        // detections back to the original stream resolution (e.g., for saving
        // cropped images or re-streaming), then use
        // frame_meta->source_frame_width and source_frame_height for scaling.

        (void)stream_height;
        (void)stream_width;
        uint detected_persons = 0;

        /* Iterate user metadata in frames to search PGIE's tensor metadata */
        for (NvDsMetaList *l_user = frame_meta->frame_user_meta_list;
             l_user != NULL; l_user = l_user->next) {
            NvDsUserMeta *user_meta = (NvDsUserMeta *)l_user->data;
            if (user_meta->base_meta.meta_type != NVDSINFER_TENSOR_OUTPUT_META)
                continue;
            detected_persons = extract_tensor_metadata(user_meta, networkInfo,
                                                       batch_meta, frame_meta);
        }

        NvDsDisplayMeta *display_meta = NULL;
        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);
        int offset = 0;
        offset = snprintf(txt_params->display_text, MAX_DISPLAY_LEN,
                          "Person_NVInfer = %d ", detected_persons);
        (void)offset;

        /* 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);
    }
    // use_device_mem = 1 - use_device_mem;
    return GST_PAD_PROBE_OK;
}

uint NvInferServerManager::extract_tensor_metadata(
    NvDsUserMeta *user_meta, NvDsInferNetworkInfo networkInfo,
    NvDsBatchMeta *batch_meta, NvDsFrameMeta *frame_meta) {
    /* convert to tensor metadata */
    NvDsInferTensorMeta *meta =
        (NvDsInferTensorMeta *)user_meta->user_meta_data;
    for (unsigned int i = 0; i < meta->num_output_layers; i++) {
        NvDsInferLayerInfo *info = &meta->output_layers_info[i];
        info->buffer = meta->out_buf_ptrs_host[i];
        if (use_device_mem && meta->out_buf_ptrs_dev[i]) {
            cudaMemcpy(meta->out_buf_ptrs_host[i], meta->out_buf_ptrs_dev[i],
                       info->inferDims.numElements * 4, cudaMemcpyDeviceToHost);
        }
    }
    /* Parse output tensor and fill detection results into objectList.
     */
    std::vector<NvDsInferLayerInfo> outputLayersInfo(
        meta->output_layers_info,
        meta->output_layers_info + meta->num_output_layers);
#if NVDS_VERSION_MAJOR >= 5
    if (nvds_lib_major_version >= 5) {
        if (meta->network_info.width != networkInfo.width ||
            meta->network_info.height != networkInfo.height ||
            meta->network_info.channels != networkInfo.channels) {
            g_error("failed to check pgie network info\n");
        }
    }
#endif

    float *outputBuffer = (float *)outputLayersInfo[0].buffer;
    (void)outputBuffer;
    // NvDsInferDims dims = outputLayersInfo[0].inferDims;

    for (size_t jkl = 0; jkl < outputLayersInfo.size(); jkl++) {
        const NvDsInferLayerInfo &layer = outputLayersInfo[jkl];

        unsigned int numDims = layer.inferDims.numDims;
        unsigned int numElements = layer.inferDims.numElements;
        (void)numElements;
        (void)numDims;

        // std::cout << "Layer " << jkl << " (" << layer.layerName <<
        // "):\n"; std::cout << "  Num Dims: " << numDims << "\n";
        // std::cout << "  Num Elements: " << numElements << "\n";
        // std::cout << "  Dims: [";
        // for (unsigned int mno = 0; mno < numDims; ++mno) {
        //     std::cout << layer.inferDims.d[mno];
        //     // layer.inferDims.d[0] = MAX_BODY_PER_FRAME;
        //     // layer.inferDims.d[1] = BODY_TENSOR_SIZE;
        //     if (mno < numDims - 1)
        //        std::cout << ", ";
        // }
        // std::cout << "]\n";
    }
    const NvDsInferLayerInfo &layer = outputLayersInfo[0];  // or loop over all

    uint detected_persons = 0;
    float *data = static_cast<float *>(layer.buffer);
    for (unsigned int jkl = 0; jkl < MAX_BODY_PER_FRAME; jkl++) {
        if (data[jkl * BODY_TENSOR_SIZE + 4] > threshold_body_detection) {
            detected_persons++;
        }
    }
    update_frame_with_face_body_meta(detected_persons, batch_meta, data,
                                     frame_meta);
    return detected_persons;
}

void NvInferServerManager::update_frame_with_face_body_meta(
    uint detected_persons, NvDsBatchMeta *batch_meta, float *data,
    NvDsFrameMeta *frame_meta) {
    for (uint index = 0; index < detected_persons; index++) {
        // imprecise_face_obj_meta is the imprecise face
        NvDsObjectMeta *imprecise_face_obj_meta =
            nvds_acquire_obj_meta_from_pool(batch_meta);

        // meta->unique_id in NvDsInferTensorMeta
        // This is the unique ID of the inference component (PGIE/SGIE)
        // that produced the tensor output. It comes directly from the
        // unique-id property in the [property] section of your
        // config_infer_primary.txt or config_infer_secondary.txt. A
        // pipeline can have multiple inference components (1 PGIE +
        // many SGIEs). Each inference element might output tensors
        // (NvDsInferTensorMeta) that are attached as user metadata.
        // unique_id lets you know which inference element the tensor
        // belongs to. meta->unique_id → The unique-id you assigned in
        // the config for the inference component that produced these
        // tensor outputs.

        strncpy(imprecise_face_obj_meta->obj_label, imprecise_face_str[0],
                sizeof(imprecise_face_obj_meta->obj_label) - 1);
        imprecise_face_obj_meta
            ->obj_label[sizeof(imprecise_face_obj_meta->obj_label) - 1] =
            '\0';  // Ensure null-termination
        imprecise_face_obj_meta->unique_component_id =
            IMPRECISE_FACE_COMPONENT_ID;  // meta->unique_id

        // imprecise_face_obj_meta->unique_component_id
        // Meaning: The ID of the component (PGIE, SGIE, Tracker,
        // Custom, etc.) that generated this metadata. Source: Assigned
        // by DeepStream when metadata is attached by a specific element
        // in the pipeline. Example: PGIE might be assigned
        // unique_component_id = 1 SGIE might be assigned
        // unique_component_id = 2 Tracker usually doesn’t overwrite
        // PGIE’s class_id but may extend metadata (like assigning
        // object_id). You normally don’t set this manually. DeepStream
        // sets it when a particular component (PGIE/SGIE) attaches
        // object metadata. You might override it only if you’re
        // injecting your own custom objects into the pipeline and need
        // to differentiate your component from PGIE/SGIE.

        imprecise_face_obj_meta->confidence =
            data[index * BODY_TENSOR_SIZE + 4];
        // imprecise_face_obj_meta->object_id = UNTRACKED_OBJECT_ID;
        imprecise_face_obj_meta->class_id =
            IMPRECISE_FACE_CLASS_ID;  // 0 for body detection
        NvOSD_RectParams &rect_params_imprecise_face =
            imprecise_face_obj_meta->rect_params;
        NvOSD_TextParams &text_params_imprecise_face =
            imprecise_face_obj_meta->text_params;
        /* Assign bounding box coordinates. */
        rect_params_imprecise_face.left = (data[index * BODY_TENSOR_SIZE + 0] *
                                           MUXER_OUTPUT_WIDTH / PGIE_NET_WIDTH);
        rect_params_imprecise_face.top =
            (data[index * BODY_TENSOR_SIZE + 1] * MUXER_OUTPUT_HEIGHT /
             PGIE_NET_HEIGHT);
        Point2D left_down_shoulder =
            find_left_down_corner_shoulder(data, index);
        rect_params_imprecise_face.width =
            ((left_down_shoulder.x - data[index * BODY_TENSOR_SIZE + 0]) *
             MUXER_OUTPUT_WIDTH / PGIE_NET_WIDTH);
        rect_params_imprecise_face.height =
            ((left_down_shoulder.y - data[index * BODY_TENSOR_SIZE + 1]) *
             MUXER_OUTPUT_HEIGHT / PGIE_NET_HEIGHT);

        clamp_rect_params(frame_meta, &rect_params_imprecise_face, "FACE");

        /* Border of width 3. */
        rect_params_imprecise_face.border_width = 3;
        rect_params_imprecise_face.has_bg_color = 0;
        rect_params_imprecise_face.border_color =
            NvOSD_ColorParams{0, 0, 1, 1};  // Blue box
        /* display_text requires heap allocated memory. */
        text_params_imprecise_face.display_text =
            g_strdup(imprecise_face_str[0]);  // g_strdup(pgie_class_str[0]);
        /* Display text above the left top corner of the object. */
        text_params_imprecise_face.x_offset =
            (rect_params_imprecise_face.left - 15 < 0)
                ? 15
                : rect_params_imprecise_face.left - 15;
        text_params_imprecise_face.y_offset =
            (rect_params_imprecise_face.top - 15 < 0)
                ? 15
                : rect_params_imprecise_face.top - 15;
        /* Set black background for the text. */
        text_params_imprecise_face.set_bg_clr = 1;
        text_params_imprecise_face.text_bg_clr = NvOSD_ColorParams{0, 0, 0, 1};
        /* Font face, size and color. */
        text_params_imprecise_face.font_params.font_name = (gchar *)"Serif";
        text_params_imprecise_face.font_params.font_size = 11;
        text_params_imprecise_face.font_params.font_color =
            NvOSD_ColorParams{1, 1, 1, 1};
        // adding landmarks to imprecise_face_obj_meta as user_meta
        NvDsUserMeta *um1 = nvds_acquire_user_meta_from_pool(batch_meta);
        assert(um1 != NULL);

        um1->user_meta_data = set_metadata_ptr(
            &(data[index * BODY_TENSOR_SIZE]));  // Add landmarks here

        um1->base_meta.meta_type =
            NVDS_USER_OBJECT_META_LANDMARKS_AND_SOURCE_ID;
        um1->base_meta.copy_func = (NvDsMetaCopyFunc)copy_user_meta;
        um1->base_meta.release_func = (NvDsMetaReleaseFunc)release_user_meta;
        nvds_add_user_meta_to_obj(imprecise_face_obj_meta, um1);
        nvds_add_obj_meta_to_frame(frame_meta, imprecise_face_obj_meta, NULL);

        NvDsObjectMeta *body_obj_meta =
            nvds_acquire_obj_meta_from_pool(batch_meta);

        strncpy(body_obj_meta->obj_label, pgie_class_str[0],
                sizeof(body_obj_meta->obj_label) - 1);
        body_obj_meta->obj_label[sizeof(body_obj_meta->obj_label) - 1] =
            '\0';  // Ensure null-termination
        body_obj_meta->unique_component_id =
            BODY_COMPONENT_ID;  // meta->unique_id;
        body_obj_meta->confidence = data[index * BODY_TENSOR_SIZE + 4];
        // body_obj_meta->object_id = UNTRACKED_OBJECT_ID;
        body_obj_meta->class_id = PGIE_CLASS_ID_PERSON;  // 0 for body detection
        NvOSD_RectParams &rect_params_body = body_obj_meta->rect_params;
        NvOSD_TextParams &text_params_body = body_obj_meta->text_params;
        /* Assign bounding box coordinates. */
        rect_params_body.left = (data[index * BODY_TENSOR_SIZE + 0] *
                                 MUXER_OUTPUT_WIDTH / PGIE_NET_WIDTH);
        rect_params_body.top = (data[index * BODY_TENSOR_SIZE + 1] *
                                MUXER_OUTPUT_HEIGHT / PGIE_NET_HEIGHT);
        rect_params_body.width = ((data[index * BODY_TENSOR_SIZE + 2] -
                                   data[index * BODY_TENSOR_SIZE + 0]) *
                                  MUXER_OUTPUT_WIDTH / PGIE_NET_WIDTH);
        rect_params_body.height = ((data[index * BODY_TENSOR_SIZE + 3] -
                                    data[index * BODY_TENSOR_SIZE + 1]) *
                                   MUXER_OUTPUT_HEIGHT / PGIE_NET_HEIGHT);

        clamp_rect_params(frame_meta, &rect_params_body, "BODY");

        /* Border of width 3. */
        rect_params_body.border_width = 3;
        rect_params_body.has_bg_color = 0;
        rect_params_body.border_color =
            NvOSD_ColorParams{1, 0, 0, 1};  // Red box
        /* display_text requires heap allocated memory. */
        text_params_body.display_text = g_strdup(pgie_class_str[0]);
        // text_params.display_text = g_strdup_printf("ImpreciseFace %lu",
        //     face_obj->object_id);
        /* Display text above the left top corner of the object. */
        text_params_body.x_offset =
            (rect_params_body.left - 30 < 0) ? 10 : rect_params_body.left - 30;
        text_params_body.y_offset =
            (rect_params_body.top - 30 < 0) ? 10 : rect_params_body.top - 30;

        /* Set black background for the text. */
        text_params_body.set_bg_clr = 1;
        text_params_body.text_bg_clr = NvOSD_ColorParams{0, 0, 0, 1};
        /* Font face, size and color. */
        text_params_body.font_params.font_name = (gchar *)"Serif";
        text_params_body.font_params.font_size = 11;
        text_params_body.font_params.font_color = NvOSD_ColorParams{1, 1, 1, 1};
        // // adding landmarks to body_obj_meta as user_meta
        // NvDsUserMeta *um1 =
        //     nvds_acquire_user_meta_from_pool(batch_meta);
        // um1->user_meta_data = set_metadata_ptr(
        //     &(data[index * BODY_TENSOR_SIZE]));  // Add landmarks here
        // um1->base_meta.meta_type =
        //     NVDS_USER_OBJECT_META_LANDMARKS_AND_SOURCE_ID;
        // um1->base_meta.copy_func = (NvDsMetaCopyFunc)copy_user_meta;
        // um1->base_meta.release_func =
        //     (NvDsMetaReleaseFunc)release_user_meta;
        // nvds_add_user_meta_to_obj(body_obj_meta, um1);
        nvds_add_obj_meta_to_frame(frame_meta, body_obj_meta, NULL);
    }
}

void NvInferServerManager::clamp_rect_params(NvDsFrameMeta *frame_meta,
                                             NvOSD_RectParams *rect_params,
                                             std::string type) {
    (void)type;
    guint frame_width = frame_meta->source_frame_width;
    guint frame_height = frame_meta->source_frame_height;

    // read values (DeepStream stores rect params as floats)
    float left = rect_params->left;
    float top = rect_params->top;
    float width = rect_params->width;
    float height = rect_params->height;
    float right = left + width;
    float bottom = top + height;

    // CHECK for invalid numbers (NaN/inf) or out-of-bounds
    bool invalid = false;
    if (!std::isfinite(left) || !std::isfinite(top) || !std::isfinite(width) ||
        !std::isfinite(height)) {
        invalid = true;
    } else if (width <= 0.0f || height <= 0.0f) {
        invalid = true;
    }

    // clamp coordinates into frame (clip)
    float clamped_left =
        std::max(0.0f, std::min(left, (float)frame_width - 1.0f));
    float clamped_top =
        std::max(0.0f, std::min(top, (float)frame_height - 1.0f));
    float clamped_right = abs(std::min(right, (float)frame_width - 1.0f));
    float clamped_bottom = abs(std::min(bottom, (float)frame_height - 1.0f));

    float clamped_w = clamped_right - clamped_left;
    float clamped_h = clamped_bottom - clamped_top;
    if (clamped_w <= 0.0f || clamped_h <= 0.0f) {
        invalid = true;
    }
    (void)invalid;

    rect_params->left = clamped_left;
    rect_params->top = clamped_top;
    rect_params->width = clamped_w;
    rect_params->height = clamped_h;
    return;
}

NvInferServerManager::Point2D
NvInferServerManager::find_left_down_corner_shoulder(float *data, uint index) {
    Point2D left_down_shoulder;
    // rightmost shoulder point in the BODY!
    if (data[index * BODY_TENSOR_SIZE + 21] >
        data[index * BODY_TENSOR_SIZE + 24]) {
        left_down_shoulder.x = data[index * BODY_TENSOR_SIZE + 21];
        left_down_shoulder.y = data[index * BODY_TENSOR_SIZE + 22];
    } else {
        left_down_shoulder.x = data[index * BODY_TENSOR_SIZE + 24];
        left_down_shoulder.y = data[index * BODY_TENSOR_SIZE + 25];
    }
    return left_down_shoulder;
}

// add custom infromation to metadata by: set_metadata_ptr, copy_user_meta,
// release_user_meta
void *NvInferServerManager::set_metadata_ptr(float *arr) {
    float *user_metadata = (float *)g_malloc0(BODY_TENSOR_SIZE * sizeof(float));
    std::memcpy(user_metadata, &arr[0], BODY_TENSOR_SIZE * sizeof(float));
    return (void *)user_metadata;
}

gpointer NvInferServerManager::copy_user_meta(gpointer data,
                                              gpointer user_data) {
    (void)user_data;
    NvDsUserMeta *user_meta = (NvDsUserMeta *)data;
    gfloat *src_user_metadata = (gfloat *)user_meta->user_meta_data;
    gfloat *dst_user_metadata =
        (gfloat *)g_malloc0(BODY_TENSOR_SIZE * sizeof(gfloat));
    memcpy(dst_user_metadata, src_user_metadata,
           BODY_TENSOR_SIZE * sizeof(gfloat));
    return (gpointer)dst_user_metadata;
}

void NvInferServerManager::release_user_meta(gpointer data,
                                             gpointer user_data) {
    (void)user_data;
    NvDsUserMeta *user_meta = (NvDsUserMeta *)data;
    if (user_meta->user_meta_data) {
        g_free(user_meta->user_meta_data);
        user_meta->user_meta_data = NULL;
    }
}