#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/dnn.hpp>
#include <iostream>
#include "Inference.h"
#include "boost/property_tree/ptree.hpp"
#include "boost/property_tree/json_parser.hpp"
#include "boost/foreach.hpp"
#include "boost/optional.hpp"
#include "ANSMOT.h"
#include "ANSODEngine.h"

template <typename T>
T get_data(const boost::property_tree::ptree& pt, const std::string& key)
{
    T ret;
    if (boost::optional<T> data = pt.get_optional<T>(key))
    {
        ret = data.get();
    }
    return ret;
}

//  /*{
//    "results": [
//    {
//        "object_id":"871:2;567;702;871;\r\n",
//            "class_id" : "2",
//            "prob" : "0.774",
//            "x" : "0.000",
//            "y" : "127.000",
//            "width" : "130.000",
//            "height" : "57.000",
//            "left" : "0",
//            "top" : "127",
//            "right" : "130",
//            "bottom" : "184"
//    }
//    ]
//}*/
int ObjectTrackingUsingByteTrack() {
    boost::property_tree::ptree root;
    boost::property_tree::ptree detectionObjects;
    boost::property_tree::ptree pt;

    bool runOnGPU = true;

    ANSCENTER::ANSMOT* ansByteTrackHandle;
    ANSCENTER::ANSODBase* infHandle;    
    std::string labelMap;
    std::string licenseKey = "";
    std::string modelZipFilePassword = "";
    float modelThreshold = 0.5;
    float modelConfThreshold = 0.5;
    float modelNMSThreshold = 0.5;
    int modelType = 14;//14TENSORRT V10;//12 SAM
    int detectorType = 1;
    std::string optmizedModelFolder;
    //std::string gpumodelFilePath = "C:\\ProgramData\\ANSCENTER\\Shared\\ANS_SAM_v1.0.zip";// "C:\\Projects\\ANSVIS\\Models\\ANS_VehicleDetection(GPU)_v1.0.zip";
    //std::string gpumodelFilePath = "C:\\Projects\\ANSVIS\\Models\\ANS_VehicleDetection(GPU)_v1.0.zip";
    std::string gpumodelFilePath = "C:\\ProgramData\\ANSCENTER\\Shared\\ANS_GenericOD(GPU)_v1.0.zip";

  
    //.1 Optimise detection model
    //std::cout << "Optimizing model, please wait...." << std::endl;
    //optmizedModelFolder = OptimizeModelStr(gpumodelFilePath.c_str(), modelZipFilePassword.c_str(), modelType, 1);
    //std::cout << "Model is optmized, run inference...." << std::endl;
    
	//2. Create detection model
    labelMap = CreateANSODHandle(&infHandle, licenseKey.c_str(), gpumodelFilePath.c_str(), modelZipFilePassword.c_str(), modelThreshold, modelConfThreshold, modelNMSThreshold, 1, modelType, detectorType);
    std::stringstream ss(labelMap);

	//3. Create Tracker
	int btHandle = CreateANSMOTHandle(&ansByteTrackHandle, 1);//ByteTrack
    std::cout << "ByteTrack Handle: " << ansByteTrackHandle << std::endl;
    std::string params = "{\"parameters\":{\"frame_rate\":\"15\",\"track_buffer\" : \"300\",\"track_threshold\" : \"0.500000\",\"high_threshold\" : \"0.600000\",\"match_thresold\" : \"0.980000\"}}";
    //std::string params = "{\"parameters\":{\"det_thresh\":\"0.000000\",\"max_age\" : \"30\",\"min_hits\" : \"3\",\"iou_threshold\" : \"0.300000\",\"delta_t\" : \"3\",\"asso_func\" : \"0\",\"inertia\":\"3.000000\",\"use_byte\":\"0\"}}";
    UpdateANSMOTParameters(&ansByteTrackHandle, params.c_str());

	// 4. Read video and run inference
    std::cout << "begin read video" << std::endl;
    std::string videoFile = "E:\\Programs\\DemoAssets\\Videos\\road.mp4";
    //std::string videoFile = "E:\\Programs\\DemoAssets\\Videos\\Luxirot\\LXTest.mp4";

    cv::VideoCapture capture(videoFile);

    if (!capture.isOpened()) {
        printf("could not read this video file...\n");
        return -1;
    }

    int num_frames = 0;
    cv::VideoWriter video("C:\\Projects\\Research\\Models\\out.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 10, cv::Size(1920, 1080));
    while (true)
    {
        cv::Mat frame;
        if (!capture.read(frame)) // if not success, break loop
        {
            std::cout << "\n Cannot read the video file. please check your video.\n";
            break;
        }
        cv::Mat** inputBlob = new cv::Mat * [1];
        inputBlob[0] = new cv::Mat(frame.clone());

        num_frames++;
        std::vector<ANSCENTER::Object> results;
        auto startInference = std::chrono::system_clock::now();
        RunInference_CPP(& infHandle, inputBlob,"id", results);
        auto endInference = std::chrono::system_clock::now();
        auto infereneTime = std::chrono::duration_cast<std::chrono::milliseconds>(endInference - startInference).count();//ms
        std::cout << "Inference time: " << infereneTime << std::endl;
		root.clear();
		detectionObjects.clear();
    
        for (int i = 0; i < results.size(); i++) {
            boost::property_tree::ptree detectionNode;
            detectionNode.put("object_id", "345:5;883;231;345;\r\n");
            detectionNode.put("class_id", results[i].classId);
            detectionNode.put("prob", results[i].confidence);
            detectionNode.put("x", results[i].box.x);
            detectionNode.put("y", results[i].box.y);
            detectionNode.put("width", results[i].box.width);
            detectionNode.put("height", results[i].box.height);
            detectionNode.put("left", results[i].box.x);
            detectionNode.put("top", results[i].box.y);
            detectionNode.put("right", results[i].box.x + results[i].box.width);
            detectionNode.put("bottom", results[i].box.y + results[i].box.height);
            // Add this node to the list.
            detectionObjects.push_back(std::make_pair("", detectionNode));

        }
        root.add_child("results", detectionObjects);
        std::ostringstream stream;
        boost::property_tree::write_json(stream, root,false);
        std::string st = stream.str();
        int lenght = st.length();
        const char* detectionResult = st.c_str();

        auto startTrack = std::chrono::system_clock::now();
        std::string trackerResult;
        UpdateANSMOT(&ansByteTrackHandle, num_frames, detectionResult, trackerResult);
        auto endTrack = std::chrono::system_clock::now();
        auto track_time = std::chrono::duration_cast<std::chrono::milliseconds>(endTrack - startTrack).count();//ms
        std::cout << "Tracking time: " << track_time << std::endl;
        //std::cout << trackerResult << std::endl;

        pt.clear();
        std::stringstream ss;
        ss.clear();
        ss << trackerResult;
        boost::property_tree::read_json(ss, pt);
        BOOST_FOREACH(const boost::property_tree::ptree::value_type & child, pt.get_child("results"))
        {
            const boost::property_tree::ptree& result = child.second;
            const auto track_id = get_data<int>(result, "track_id");
            const auto class_id = get_data<int>(result, "class_id");
            const auto conf = get_data<float>(result, "prob");
            const auto x = get_data<float>(result, "x");
            const auto y = get_data<float>(result, "y");
            const auto width = get_data<float>(result, "width");
            const auto height = get_data<float>(result, "height");
            const auto left = get_data<float>(result, "left");
            const auto top = get_data<float>(result, "top");
            const auto right = get_data<float>(result, "right");
            const auto bottom = get_data<float>(result, "bottom");
            cv::rectangle(frame, cv::Rect(left, top, right - left, bottom - top), 30, 2);
            cv::putText(frame, cv::format("%d-%d", track_id,class_id), cv::Point(left, top - 5),
                0, 0.6, cv::Scalar(0, 0, 255), 1, cv::LINE_AA);
        }

        cv::imshow("ANS ByteTrack", frame);
        video.write(frame);
        if (cv::waitKey(30) == 27) // Wait for 'esc' key press to exit
        {
            break;
        }
		results.clear();
    }
    capture.release();
    video.release();
    cv::destroyAllWindows();
    ReleaseANSMOTHandle(&ansByteTrackHandle);
    ReleaseANSODHandle(&infHandle);
    std::cout << "End of program.\n";
    return 0;
}


int main()
{
    ObjectTrackingUsingByteTrack();
    std::cout << "End of program." << std::endl;
}

