#include <gst/gst.h>
#include <gst/app/gstappsink.h>
//#include <gst/
#include <sstream>
//#include <format>
#include <gst/video/gstvideometa.h>
#include <cstdlib>
#include <deque>
#include <iostream>
#include <thread>
#include <mutex>
#include <fstream>
#include <chrono>

#include "osc/OscOutboundPacketStream.h"
#include "ip/UdpSocket.h"

#include "System.h"
using namespace std::chrono;

// CHANGE HERE TO MAKE PORT 
#define ADDRESS "127.0.0.1"
#define PORT 7000

#define OUTPUT_BUFFER_SIZE 1024

#define MY_GST_USE_OPENCV

#ifdef MY_GST_USE_OPENCV
#include "opencv2/opencv.hpp"
// TODO: use synchronized deque
std::mutex g_mutex;
std::deque<cv::Mat> frameQueue;
#endif // MY_GST_USE_OPENCV

cv::Mat& extract_rot(cv::Mat& rot, const cv::Mat& trans) {
    // cv::Mat rot(3, 3, CV_32F, 0.0);
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            rot.at<float>(row, col) = trans.at<float>(row, col);
        }
    }
    return rot;
}

pair<float, vector<float>> extract_deg(const cv::Mat& rotation_matrix) {
    // TODO: extract_deg is producing negative values. Fix it.
    float degrees;
    vector<float> myvec = { rotation_matrix.at<float>(1, 2) - rotation_matrix.at<float>(2,1), rotation_matrix.at<float>(2, 0) - rotation_matrix.at<float>(0, 2), rotation_matrix.at<float>(0, 1) - rotation_matrix.at<float>(1, 0) };

    float trace;
    trace = rotation_matrix.at<float>(0, 0) + rotation_matrix.at<float>(1, 1) + rotation_matrix.at<float>(2, 2);
    //    cout << "a11 is: " << rotation_matrix.at<float>(0, 0) << " a22 is: " << rotation_matrix.at<float>(1, 1) << " a33 is: " << rotation_matrix.at<float>(2, 2) << endl;
    //    cout << "x is: " << (trace - 1) / 2 << endl;
    degrees = acos((trace - 1) / 2);
    //    cout << "Calc degrees (from function) is: " << degrees << endl;

    pair<float, vector<float>> result = { degrees, myvec };
    return result;
}

vector<float> find_mode(const vector<vector<float>>& vec_of_rot_axes) {
    cout << "Hello! This is find_mode() function" << endl;
    int index = 0, counter = 0, max_counted = 0;
    vector<float> el;
    for (int i = 0; i < vec_of_rot_axes.size(); i++) {
        el = vec_of_rot_axes[i];

        cout << "Extracted el is: ";
        for (auto e : el) {
            cout << " " << e << " ";
        }
        cout << endl;

        for (const auto& vec_of_rot_axe : vec_of_rot_axes) {
            if (el == vec_of_rot_axe) {
                cout << "Entered if (el == vec_of_rot_axe) statement" << endl;
                counter += 1;
            }
        }
        if (counter > max_counted) {
            // cout << "Found new max element. Index is: " << index << "; i is: " << i << endl;
            index = i;
            max_counted = counter;
        }
    }
    cout << "Index is: " << index << "; And arr size is: " << vec_of_rot_axes.size() << endl;
    return vec_of_rot_axes[index];
}

GstFlowReturn new_preroll(GstAppSink *appsink, gpointer data) {
    g_print ("Got preroll!\n");
    return GST_FLOW_OK;
}

GstFlowReturn new_sample(GstAppSink *appsink, gpointer data) {
    static int framecount = 0;
    framecount++;
    static long long ms = 0;

    auto new_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    std::cout << "nnew frame " << (new_ms - ms) << " " << framecount << std::endl;
    ms = new_ms;

    GstSample *sample = gst_app_sink_pull_sample(appsink);
    GstCaps *caps = gst_sample_get_caps(sample);
    GstBuffer *buffer = gst_sample_get_buffer(sample);

    const auto& n_memory = gst_buffer_n_memory(buffer);
    std::cout << "n_memory = " << n_memory << std::endl;
    std::cout << "buffer->pts = " << buffer->pts << std::endl;
    std::cout << "buffer->dts = " << buffer->dts << std::endl;
    std::cout << "buffer->duration = " << buffer->duration << std::endl;
    std::cout << "buffer->offset = " << buffer->offset << std::endl;
    std::cout << "buffer->offset_end = " << buffer->offset_end << std::endl;

    const GstStructure *info = gst_sample_get_info(sample);

    GstMeta *gst_meta;
    gpointer state = nullptr;
    while ((gst_meta = gst_buffer_iterate_meta(buffer, &state))) {
        if (gst_meta->info == gst_video_caption_meta_get_info()) {
            auto specific_meta = (GstVideoCaptionMeta *) gst_meta;
            if (specific_meta) {
                auto x = (const char *) (specific_meta->data);
                std::cout << "MetaInfo is recognized to be [GstVideoCaptionMeta]"
                          << "caption = " << std::string(x, specific_meta->size)
                          << std::endl;
            }
        } else if (gst_meta->info == gst_video_time_code_meta_get_info()) {
            auto specific_meta = (GstVideoTimeCodeMeta *) gst_meta;
            if (specific_meta) {
                std::cout << "MetaInfo is recognized to be [GstVideoTimeCodeMeta]"
                          << " h = " << specific_meta->tc.hours
                          << " m = " << specific_meta->tc.minutes
                          << " s = " << specific_meta->tc.seconds
                          << " f = " << specific_meta->tc.frames
                          << std::endl;
            }
        } else if (gst_meta->info == gst_meta_get_info("GstNdiSrcMeta")) {
            std::cout << "MetaInfo is recognized to be [GstNdiSrcMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstNdiSinkAudioMeta")) {
            std::cout << "MetaInfo is recognized to be [GstNdiSinkAudioMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoCropMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoCropMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstFramePositionerMeta")) {
            std::cout << "MetaInfo is recognized to be [GstFramePositionerMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstMetaDfbSurface")) {
            std::cout << "MetaInfo is recognized to be [GstMetaDfbSurface]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstSubtitleMeta")) {
            std::cout << "MetaInfo is recognized to be [GstSubtitleMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstRtmpMeta")) {
            std::cout << "MetaInfo is recognized to be [GstRtmpMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstMpegVideoMeta")) {
            std::cout << "MetaInfo is recognized to be [GstMpegVideoMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstSctpReceiveMeta")) {
            std::cout << "MetaInfo is recognized to be [GstSctpReceiveMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstSctpSendMeta")) {
            std::cout << "MetaInfo is recognized to be [GstSctpSendMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstCoreMediaMeta")) {
            std::cout << "MetaInfo is recognized to be [GstCoreMediaMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstCoreVideoMeta")) {
            std::cout << "MetaInfo is recognized to be [GstCoreVideoMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstAudioDownmixMeta")) {
            std::cout << "MetaInfo is recognized to be [GstAudioDownmixMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstAudioClippingMeta")) {
            std::cout << "MetaInfo is recognized to be [GstAudioClippingMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstGLSyncMeta")) {
            std::cout << "MetaInfo is recognized to be [GstGLSyncMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstRTPSourceMeta")) {
            std::cout << "MetaInfo is recognized to be [GstRTPSourceMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstRTPSourceMeta")) {
            std::cout << "MetaInfo is recognized to be [GstRTPSourceMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoGLTextureUploadMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoGLTextureUploadMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoRegionOfInterestMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoRegionOfInterestMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoAFDMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoAFDMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoBarMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoBarMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoMultiviewMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoMultiviewMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoOverlayCompositionMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoOverlayCompositionMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstMetaXImage")) {
            std::cout << "MetaInfo is recognized to be [GstMetaXImage]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstProtectionMeta")) {
            std::cout << "MetaInfo is recognized to be [GstProtectionMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstNetControlMessageMeta")) {
            std::cout << "MetaInfo is recognized to be [GstNetControlMessageMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstMetaTest")) {
            std::cout << "MetaInfo is recognized to be [GstMetaTest]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstNVMMParentMeta")) {
            std::cout << "MetaInfo is recognized to be [GstNVMMParentMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstAudioMeta")) {
            std::cout << "MetaInfo is recognized to be [GstAudioMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstAudioLevelMeta")) {
            std::cout << "MetaInfo is recognized to be [GstAudioLevelMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoAffineTransformationMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoAffineTransformationMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("GstVideoCodecAlphaMeta")) {
            std::cout << "MetaInfo is recognized to be [GstVideoCodecAlphaMeta]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else if (gst_meta->info == gst_meta_get_info("XXX")) {
            std::cout << "MetaInfo is recognized to be [XXX]"
                      << std::endl;
        } else {
            std::cout << "GstMetaInfo is not recognized."
                      << " info = " << gst_meta->info
                      << " api = " << gst_meta->info->api
                      << std::endl;
        }
    }

    // ---- Read frame and convert to opencv format ---------------

//    return GST_FLOW_OK;

    GstMapInfo map;
    gst_buffer_map (buffer, &map, GST_MAP_READ);

#ifdef MY_GST_USE_OPENCV
    // convert gstreamer data to OpenCV Mat, you could actually
    // resolve height / width from caps...

    int width = 2560;
    int height = 1440;
    int depth = 4;
    int bpp = -1;

    GstStructure *s = gst_caps_get_structure(caps, 0);
    gboolean res = true;
    res &= gst_structure_get_int (s, "width", &width);
    res &= gst_structure_get_int (s, "height", &height);
//    res &= gst_structure_get_int (s, "depth", &depth);
//    res &= gst_structure_get_int (s, "bpp", &bpp);

    if (gst_structure_get_field_type (s, "format") == G_TYPE_STRING) {
        const char *string;
        string = gst_structure_get_string (s, "format");
        // std::cout << "flksjlskfjsjdlkf" << string << std::endl;
//		fourcc = GST_STR_FOURCC (string);
//	} else if (gst_structure_get_field_type (s, "format") == GST_TYPE_FOURCC) {
//		gst_structure_get_fourcc (s, "format", &fourcc);
    } else {
//		fourcc = 0;
    }

    assert (depth == 4);
    cv::Mat frame(cv::Size(width, height), CV_8UC4, (char*)map.data, cv::Mat::AUTO_STEP);

//    int frameSize = map.size;
    std::cout << "size from caps = (" << width << "," << height << "," << depth << "," << bpp << ")" << "res =" << res
              << " total size = " << map.size
              << std::endl;

//    if (res) {
//        std::fstream file("example.bin", std::ios::out  | std::ios::binary | std::ios::app);
//        file.write((char*)map.data, map.size);
//        file.close();
//    }
//    throw 1;
    {
        std::lock_guard<std::mutex> guard(g_mutex);
        frameQueue.push_back(frame.clone());
    }
#endif

    gst_buffer_unmap(buffer, &map);

    // ------------------------------------------------------------

    // print dot every 30 frames
    if (framecount%30 == 0) {
        g_print (".");
    }

    // show caps on first frame
    if (framecount == 1) {
        g_print ("%s\n", gst_caps_to_string(caps));
    }

    gst_sample_unref (sample);
    return GST_FLOW_OK;
}

static gboolean my_bus_callback (GstBus *bus, GstMessage *message, gpointer data) {
    g_print ("Got %s message\n", GST_MESSAGE_TYPE_NAME (message));
    switch (GST_MESSAGE_TYPE (message)) {
    case GST_MESSAGE_ERROR: {
        GError *err;
        gchar *debug;

        gst_message_parse_error (message, &err, &debug);
        g_print ("Error: %s\n", err->message);
        g_error_free (err);
        g_free (debug);
        break;
    }
    case GST_MESSAGE_EOS: {
        /* end-of-stream */
        break;
    } default: {
        /* unhandled message */
        break;
    }
    }
    /* we want to be notified again the next time there is a message
     * on the bus, so returning TRUE (FALSE means we want to stop watching
     * for messages on the bus and our callback should not be called again)
     */
    return TRUE;
}

static gboolean overrun_callback (GstElement * queue, gpointer udata) {
    std::cout << "hi from overrun" << std::endl;
    return TRUE;
}

static gchar* ndi_name = nullptr;
static gint use_opencv_preview = 0;
static gboolean use_gst_preview = FALSE;

static GOptionEntry entries[] =
        {
                { "ndi-name", 0, 0, G_OPTION_ARG_STRING, &ndi_name, "you can enter the string here (ndi-name)", "M" },
                { "opencv-preview", 0, 0, G_OPTION_ARG_INT, &use_opencv_preview,         "use opencv preview", NULL },
                { "gst-preview", 0, 0, G_OPTION_ARG_INT, &use_gst_preview,           "use gstreamer preview", NULL },
                { NULL }
        };

int main (int argc, char *argv[]) {
    std::cout << "argc = " << argc << std::endl;

    //for (int i = 0; i < argc; i++) {
    //    std::cout << argv[i] << std::endl;
    //}

    GError *error = nullptr;
    GOptionContext *context;

    context = g_option_context_new("- test tree model performance");
    g_option_context_add_main_entries(context, entries, "bla");

    char** argv_gst;
    argv_gst = new char* [4];
    argv_gst[0] = new char[200];
    argv_gst[1] = new char[200];
    argv_gst[2] = new char[200];
    argv_gst[3] = new char[200];

    std::strcpy(argv_gst[0], argv[0]);
    std::strcpy(argv_gst[1], argv[1]);
    std::strcpy(argv_gst[2], argv[2]);
    std::strcpy(argv_gst[3], argv[3]);

    for (int i = 0; i < argc-3; i++) {
        std::cout << argv_gst[i] << std::endl;
    }

    int argc_gst = argc - 3;

    // QUESTION 1.
    g_option_context_parse(context, &argc_gst, &argv_gst, &error);

    //g_option_context_parse(context, &argc, &argv, &error);

    if (!ndi_name) {
        std::cout << "ndi-name is not provided" << std::endl;
        // ndi_name = (char*)malloc(sizeof(char) * 100);
        ndi_name = "DESKTOP-O5PNOBN (Test Pattern)";
        std::cout << "ndi-name (default) = '" <<  ndi_name << "'" << std::endl;
    } else {
        std::cout << "ndi-name = '" <<  ndi_name << "'" << std::endl;
    }

    GstStateChangeReturn ret;

    int fake_argc = 1;
    gst_init (&fake_argc, &argv);
    /*
    s_pipeline = f'ndisrc ndi-name="{ndi_name}" ! ndisrcdemux name=demux   ' \
                 f'demux.video ! queue ! tee name=my_tee   ' \
                 f'my_tee. ! queue ! videoconvert ! autovideosink ' \
                 f'my_tee. ! queue ! videoconvert ! appsink name=my_sink'
    */
    auto sink_caps = "video/x-raw, format=(string)BGRA";
    std::stringstream ss;
    ss << "ndisrc ndi-name=\"" << ndi_name << "\" ! ndisrcdemux name=demux "
       << "demux.video ! queue ! tee name=my_tee ";
    if (use_gst_preview) {
        ss << "my_tee. ! queue name=q_show ! videoconvert ! autovideosink ";
    }
    ss << "my_tee. ! queue name=q_appsink ! videoconvert name=convert_slow ! appsink name=my_sink caps=\"" << sink_caps << "\"";
    // ss << "my_tee. ! queue name=q_appsink ! glupload ! glcolorconvert ! \"video/x-raw(memory:GLMemory),format=BGR\" ! appsink name=my_sink ";
    std::string my_pipeline = ss.str();

    std::cout << "We are about to launch the pipeline = [" << my_pipeline.c_str() << "]" << std::endl;
    GstElement *pipeline = gst_parse_launch(my_pipeline.c_str(), nullptr);

    std::cout << "Launching pipeline success" << std::endl;

    // g_object_set (my_src, "ndi-name", "ADMIN (lafvi 29.97fps)", NULL);
    /*
    [libndi_newtek @ 0x556ab959f400] Found 4 NDI sources:
    [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (CameraVegasAR)'       '185.41.112.227:5962'
    [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (NVIDIA Quadro RTX 5000 1)'    '185.41.112.227:5961'
    [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (NVIDIA Quadro RTX 5000 2)'    '185.41.112.227:5963'
    [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (Test Pattern)'        '185.41.112.227:5964'
     */
//    g_object_set (my_src, "ndi-name", ndi_name, NULL);

    /* get sink */
    GstElement *sink = gst_bin_get_by_name (GST_BIN (pipeline), "my_sink");
    gst_app_sink_set_emit_signals((GstAppSink*)sink, true);
    gst_app_sink_set_drop((GstAppSink*)sink, true);
    gst_app_sink_set_max_buffers((GstAppSink*)sink, 1);
    GstAppSinkCallbacks callbacks = { NULL, new_preroll, new_sample };
    gst_app_sink_set_callbacks (GST_APP_SINK(sink), &callbacks, NULL, NULL);

    std::cout << "Launching sink success" << std::endl;

    GstBus *bus;
    guint bus_watch_id;
    bus = gst_pipeline_get_bus (GST_PIPELINE (pipeline));
    bus_watch_id = gst_bus_add_watch (bus, my_bus_callback, NULL);
    gst_object_unref (bus);

    std::cout << "Launching bus success" << std::endl;

    {
        GstElement *e = gst_bin_get_by_name(GST_BIN (pipeline), "q_appsink");
        g_signal_connect (e, "overrun", G_CALLBACK(overrun_callback), NULL);
    }
    {
        GstElement *e = gst_bin_get_by_name(GST_BIN (pipeline), "convert_slow");
        g_object_set(e, "n-threads", 1, nullptr);
    }

    /* Start playing */
    ret = gst_element_set_state (pipeline, GST_STATE_PLAYING);
    if (ret == GST_STATE_CHANGE_FAILURE) {
        g_printerr ("Unable to set the pipeline to the playing state.\n");
        gst_object_unref (pipeline);
        return -1;
    }

    std::cout << "Start playing success" << std::endl;

#ifdef MY_GST_USE_OPENCV

    bool pangolin_window;
    std::cout << argv[6] << std::endl;
    string pangolin_choice(argv[6]);
    if (pangolin_choice == "--orb_slam_window=1") {
        pangolin_window = true;
    }
    else {
        pangolin_window = false;
    }

    std::cout << "cout success" << std::endl;
    std::cout << pangolin_window << std::endl;


    auto lambda_1 = [] (char** argv, bool pangolin_preview) {
    // --------------------------------- SLAM SYSTEM VARIABLES ---------------------------------   

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
     ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, pangolin_preview);

     std::printf("SLAM system initialized\n");

     // Main loop
     cv::Mat frame;
    
     // This is the queue, which will accumulate the frames.
     std::vector<cv::Mat> queue;

     int cnt = 0;
     const double time_step = 1.0;
     double ts = 0;
     char matrix_name[100];
     vector<float> vec_of_deg, values;
     vector<vector<float>> vec_of_rot_axis;

     // ---- INITIALIZE FOR THE PROCESSING OF AXIS LOSS AND FOR THE AXIS VECTOR INFORMATION ----
     float skew1 = 0.0;
     float DIFF_TO_CENTER = 0.0;
     float curr_deg; // later I'll assign the exact value
     vector<float> curr_vec;
     vector<float> mode1, mode2;
     int number_of_detected_frames = 0;

     vector<vector<float>> accum, accum2;
     int counter2, j = 0;
     std::cout << "J is: " << j;
     vector<float> mode_vec, mode_vec2; // 2 вектора, для аккумуляции слева и справа
     // zero_flag - индикатор, что текущий элемент пошёл в обратную сторону (около нуля)
     // mirror_flag - значения на данный момент должны отражаться
     bool zero_flag, mirror_flag = false;
     float mirror_point = 0.0;



     //// FOR TESTING
     //while (true) {
     //   cv::Mat frame;
     //   //char* buffer = nullptr;

     //   // EXTRACTING FRAME HERE.
     //   {
     //       std::lock_guard<std::mutex> guard(g_mutex);
     //       if (frameQueue.size() > 0) {
     //           frame = frameQueue.front();
     //           frameQueue.pop_front();
     //           std::cout << "we have a frame to process..." << std::endl;
     //       }
     //   }

     //   cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
     //   cv::Mat Rot(3, 3, CV_32F, 0.0);
     //   std::cout << Tcw << std::endl;
     //}
     //// FOR TESTING

     // --------------------------------- SLAM SYSTEM VARIABLES ---------------------------------

     /* Let's do two steps outside the loop.*/
     for (int i = 1; i <= 2; i++) {

         //if (use_gui) {
         //    cv::namedWindow("preview", 1);
         //}
         //else {
         //    // cv::namedWindow("no preview", 1);
         //}
         cv::Mat frame;

         char* buffer = nullptr;

         // EXTRACTING FRAME HERE.
         {
             std::lock_guard<std::mutex> guard(g_mutex);
             if (frameQueue.size() > 0) {
                 frame = frameQueue.front();
                 frameQueue.pop_front();
                 std::cout << "we have a frame to process..." << std::endl;
             }
         }

         cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
         cv::Mat Rot(3, 3, CV_32F, 0.0);
         std::cout << Tcw << std::endl;

         if (!Tcw.empty()) {
             sprintf(matrix_name, "matrix%d", cnt);
             extract_rot(Rot, Tcw);
             // cout << "Extracted rotation matrix is: " << Rot;
             auto deg_vec = extract_deg(Rot);

             // QUESTION 2.
             curr_deg = -deg_vec.first * 57.29;
             // TODO: Invert curr_vec too. (put the minus sign to each element). (You can define the - operator fot the vector).
             curr_vec = deg_vec.second;
             cout << "Successfully created curr_deg and curr_vec" << endl;

             // LET'S DEFINE CONSTANT TO ZERO OUT THE START
             if (i == 1) {
                 DIFF_TO_CENTER = 0.0;
             }

             vec_of_deg.push_back(curr_deg - DIFF_TO_CENTER);
             vec_of_rot_axis.push_back(curr_vec);
             values.push_back(curr_deg - DIFF_TO_CENTER);
             cout << "Successfully pushed to the vectors " << endl;

             //cout << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
             // SEND THE RESULT THROUGH OSC
             //outfile << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
             cout << "Successfully written to the file" << endl;
             j++;
         }
         cnt++;
         ts += time_step;
     }


     while (true) {

         cv::Mat frame;

         char* buffer = nullptr;

         {
             std::lock_guard<std::mutex> guard(g_mutex);
             if (frameQueue.size() > 0) {
                 frame = frameQueue.front();
                 frameQueue.pop_front();
                 std::cout << "we have a frame to process..." << std::endl;

                 if (!frame.empty()) {

                     cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
                     cv::Mat Rot(3, 3, CV_32F, 0.0);
                     std::cout << Tcw << std::endl;
                     if (!Tcw.empty()) {
                         sprintf(matrix_name, "matrix%d", cnt);
                         extract_rot(Rot, Tcw);
                         // cout << "Extracted rotation matrix is: " << Rot;
                         // Extract the degree and the vector from the rotation matrix.
                         auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.

                         float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
                         vector<float> new_vec = deg_vec.second;
                         cout << "Successfully created curr_deg and curr_vec" << endl;

                         vec_of_deg.push_back(new_deg);
                         vec_of_rot_axis.push_back(new_vec);
                         j++;
                         cout << "Pushed to the vectors. Line 207" << endl;

                         // ---- II PART OF THE PROCESSING ----

                         // TODO: II PART OF PROCESSING MIRRORED FIRST CHANGE, BUT NOT THE REST.

                         // Если текущий градус больше epsilon = 5, то zero_flag = false
                         // Can cause a problem, when accumulating values after turning on the zero_flag.
                         // TODO: accum2 is full when the zero_flag enables, which is bad. work on that.
                         if (zero_flag) {
                             if ((vec_of_deg[j - 1] < -5 || vec_of_deg[j - 1] > 5) && accum2.size() == 5) {
                                 zero_flag = false;
                             }
                         }

                         if (zero_flag) { cout << "Zero flag is: true" << endl; }
                         else { cout << "Zero flag is: false" << endl; }

                         // Если нет zero_flag, а в accum2 что-то есть, то опустошим его.
                         if (!(zero_flag) && !accum2.empty()) { accum2 = {}; }

                         // Сохраняем последние 5 значений векторов
                         if (!zero_flag) {
                             cout << "Line 211 ok..." << endl;
                             if (accum.size() == 5) {
                                 cout << "Accum size = 5." << endl;
                                 accum.erase(accum.begin());
                                 cout << "Line 215 ok..." << endl;
                                 accum.push_back(vec_of_rot_axis[j - 1]);
                                 cout << "Line 217 ok..." << endl;
                             }
                             else {
                                 cout << "Accum size != 5." << endl;
                                 cout << "j is: " << j << " len of vec_of_rot_axis is: " << vec_of_rot_axis.size() << endl;
                                 accum.push_back(vec_of_rot_axis[j - 1]);
                                 cout << "Line 223 ok..." << endl;
                             }
                         }
                         // Найдем элемент, который начал расти, а не убывать около нуля
                         if (!zero_flag) {
                             if (vec_of_deg[j - 1] > -5 && vec_of_deg[j - 1] < 5) {
                                 // Если нынешний элемент уже не меньше предыдущего, а предыдущая разность тоже около нуля, при этом абсолютная разность между градусами больше, чем 0.01
                                 if (abs(vec_of_deg[j - 1]) >= abs(vec_of_deg[j - 2]) && (abs(vec_of_deg[j - 2] - vec_of_deg[j - 3]) < 10) && (abs(vec_of_deg[j - 1] - vec_of_deg[j - 2]) > .3)) {
                                     zero_flag = true;
                                     cout << "Line 233 and 232 ok..." << endl;

                                 }
                                 //                    else {
                                 //                        zero_flag = false;
                                 //                    }
                             }
                         }

                         cout << "Accum size is: " << accum.size() << endl;
                         cout << "Accum2 size is: " << accum2.size() << endl;
                         if (zero_flag) {
                             // Если набрали 5 элементов
                             cout << "Entered in zero_flag if..." << endl;
                             cout << "Accum2.size() is: " << accum2.size() << endl;
                             if (accum2.size() == 5 && accum.size() == 5) {
                                 // Имеем массивы векторов. Найдём их моды и сравним.
                                 cout << "Accum size: " << accum.size() << endl;
                                 cout << "Accum2 size: " << accum2.size() << endl;
                                 mode1 = find_mode(accum);
                                 mode2 = find_mode(accum2);
                                 cout << "Line 246 and 245 ok..." << endl;

                                 bool compar_res = mode1 == mode2;
                                 cout << "Line 250 ok..." << endl;
                                 // Если градусы около нуля, а значения векторов поменялись, то отражаем
                                 // Input data leave it as it as, but the output data has to be processed.
                                 if (!(compar_res)) {
                                     // Если мы нашли ту самую точку, то отразим точки, которые мы накопили, и прибавим к ним точку
                                     // отражения, а также изменим точку отражения, и изменим флаг mirror_flag = True
                                     cout << "Нашли ту самую точку!" << endl;
                                     // mirror_point += values[j-6];
                                     // cout << "Mirror point after: " << mirror_point << endl;
                                     cout << "Line 255 ok..." << endl;

                                     if (mirror_flag) {
                                         mirror_flag = false;
                                     }
                                     else {
                                         mirror_flag = true;
                                     }

                                     //                        for (int i = j-6; i < j-1; i++){
                                     //                            values[i] = -values[i] + mirror_point;
                                     //                        }
                                                             // cout << "Lines 263 and 264 are ok" << "j is: " << j << endl;

                                 }
                                 accum2 = {};
                                 cout << "Making zero flag false..." << endl;
                                 zero_flag = false;
                             }
                             else {
                                 if (accum2.size() < 5) {
                                     accum2.push_back(vec_of_rot_axis[j - 1]);
                                     cout << "Line 274 ok..." << endl;
                                 }
                             }
                         }


                         // Сохраняем значения...
                         if (mirror_flag) {
                             ; cout << "Mirror flag is on;" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
                             values.push_back(-vec_of_deg[j - 1] + mirror_point);
                             // cout << "Line 281 ok..." << endl;
                         }
                         else {
                             cout << "Mirror flag is off" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
                             values.push_back(vec_of_deg[j - 1]);
                             // cout << "Line 284 ok..." << endl;
                         }
                         cout << "Processed value is: " << values[j - 1] << endl; cout << " " << endl;

                         // --------- I PART OF THE PROCESSING ---------
             //            values[j-1] += skew1;
             //            float diff = (values[j-2] - values[j-1]);
             //            cout << "New deg is: " << new_deg << "Diff is: " << diff << endl;
             //
             //
             //            // Если разница больше 10, то скорее всего произошла потеря.
             //            if (abs(diff) > 10) {
             //                cout << "Diff is more than 10; Correcting... " << endl;
             //                values[j-1] += diff;
             //                skew1 += diff;
             //            }
                         // --------- I PART OF THE PROCESSING ---------

                         // Запись в файл.
                         //outfile << values[j - 1] << " " << new_vec[0] << " " << new_vec[1] << " " << new_vec[2] << " " << cnt << endl;
                         // cout << "Successfully written to the file" << endl;

                         // Выполнить отправку в протокол OSC. 
                         //cv::Vec3d res(1., 1., 1.);
                         //std::cout << "defined Vector is: " << res[0] << res[1] << res[2] << std::endl;
                         std::cout << "message received!" << std::endl;

                         // UDP Variables
                         std::string str;
                         char msg[40];
                         UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));

                         char buffer[OUTPUT_BUFFER_SIZE];
                         osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); 
                         
                         str = std::to_string(values[j-1]) + " " + std::to_string(new_vec[0]) + " " + std::to_string(new_vec[1]) + " " + std::to_string(new_vec[2]) + " " + std::to_string(cnt);
                         strcpy(msg, str.c_str());

                         std::cout << "Message filled" << std::endl;

                         p << osc::BeginBundleImmediate
                             << osc::BeginMessage("/test3") << msg << osc::EndMessage
                             /* << osc::BeginMessage("/test2")
                              << true << 24 << (float)10.8 << "world" << osc::EndMessage*/
                             << osc::EndBundle;

                         std::cout << "Bundle filled" << std::endl;

                         transmitSocket.Send(p.Data(), p.Size());
                         std::cout << "Message sent!" << std::endl;

                         memset(msg, 0, 40);

                         //std::cout << "Memory freed" << std::endl;

                         // ---- II PART OF THE PROCESSING ----

                         curr_deg = new_deg;
                         curr_vec = new_vec;

                         number_of_detected_frames += 1;
                     }
                     else {
                         // UDP Variables
                         std::string str;
                         char msg[40];
                         UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));

                         char buffer[OUTPUT_BUFFER_SIZE];
                         osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);

                         str = "0 0 0 0" + std::to_string(cnt);
                         strcpy(msg, str.c_str());

                         p << osc::BeginBundleImmediate
                             << osc::BeginMessage("/test3") << msg << osc::EndMessage
                             /* << osc::BeginMessage("/test2")
                              << true << 24 << (float)10.8 << "world" << osc::EndMessage*/
                             << osc::EndBundle;

                         transmitSocket.Send(p.Data(), p.Size());
                         std::cout << "Message sent!" << std::endl;

                         memset(msg, 0, 40);
                     }
                     cnt++;
                     ts += time_step;

                 }
             }
             else {
                 //std::cout << "Don't have any frames yet ..." << std::endl;
                 //std::cout << "";
             }
         }


         delete[] buffer;
     }

     std::printf("End of video\n");
     // Stop all threads
     SLAM.Shutdown();

     std::printf("Done.\n");

     std::cout << "Number of detected frames: " << number_of_detected_frames << std::endl;

    };

    char** argv_orb;
    argv_orb = new char* [3];
    argv_orb[0] = new char[300];
    argv_orb[1] = new char[300];
    argv_orb[2] = new char[300];

    std::strcpy(argv_orb[0], argv[0]);
    std::strcpy(argv_orb[1], argv[4]);
    std::strcpy(argv_orb[2], argv[5]);

    std::thread t1(lambda_1, argv_orb, pangolin_window);


    bool is_terminated = false;
    while (!is_terminated) {
        // g_main_iteration(false);
        g_main_context_iteration(NULL, false);
    }
    t1.join();
#else
    bool is_terminated = false;
    while (!is_terminated) {
        g_main_context_iteration(NULL, false);
    }
#endif // MY_GST_USE_OPENCV
    gst_element_set_state (GST_ELEMENT (pipeline), GST_STATE_NULL);
    gst_object_unref (GST_OBJECT (pipeline));

    return 0;
}