#include "ANSOCSortTrack.h"
#include "OCSortObject.h"
#include "boost/property_tree/ptree.hpp"
#include "boost/property_tree/json_parser.hpp"
#include "boost/foreach.hpp"
#include "boost/optional.hpp"
#include <map>
namespace ANSCENTER {
  
    Eigen::MatrixXf GetOCSortInputs(const char* jsonString) {
        Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
        try {
            boost::property_tree::ptree pt;
            std::stringstream ss;
            ss << jsonString;
            boost::property_tree::read_json(ss, pt);
            boost::property_tree::ptree detections = pt.get_child("results");
            if (detections.size() > 0) {
                for (const auto& detection : detections) {
                    float xMin = detection.second.get<float>("x");
                    float yMin = detection.second.get<float>("y");
                    float width = detection.second.get<float>("width");
                    float height = detection.second.get<float>("height");
                    float conf = detection.second.get<float>("prob");
                    int classId = detection.second.get<int>("class_id");
                    std::string object_id = detection.second.get<std::string>("object_id");
                    std::stringstream ss(object_id);
                    std::vector<std::string> result;
                    int i = 0;
                    while (ss.good())
                    {
                        std::string substr;
                        getline(ss, substr, ';');
                        result.push_back(substr);
                        i++;
                        if (i > 5)break;
                    }
                    // We now need to parse object_id into DGId,CameraId,and ModelId
                    double xMax = xMin + width;
                    double yMax = yMin + height;
                    int DGId = 0;
                    int CameraId = 0;
                    int ModelId = 0;
                    if (result.size() >= 4) {
                        DGId = stoi(result[1]);
                        CameraId = stoi(result[2]);
                        ModelId = stoi(result[3]);
                    }
                    Eigen::MatrixXf newRow(1, ret.cols());
                    newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
                    ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
                    ret.row(ret.rows() - 1) = newRow;
                }
            } 
            return ret;
        }
        catch (const std::exception& e) {
            return ret;
        }
    }

    ANSOCSortTrack::ANSOCSortTrack()
    {
        _licenseValid = false;
        CheckLicense();

        float det_thresh = 0;
        int max_age = 50;
        int min_hits = 1;
        float iou_threshold = 0.22136877277096445;
        int delta_t = 1;
        std::string asso_func = "iou";//giou//"iou"
        float inertia = 0.3941737016672115;
        bool use_byte = true;


        tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
    }

    ANSOCSortTrack::~ANSOCSortTrack() {

    }

    bool ANSOCSortTrack::Destroy() {
        return true;
    }
    bool ANSOCSortTrack::UpdateParameters(const std::string& trackerParameters) {
        //// Use JSON Boost to parse paramter from trackerParameters
        try {
            float det_thresh = 0;
            int max_age = 50;
            int min_hits = 1;
            float iou_threshold = 0.22136877277096445;
            int delta_t = 1;
            int asso_func_in = 0;
            std::string asso_func = "giou";
            float inertia = 0.3941737016672115;
            bool use_byte = true;
            int use_byte_in = 0;
            if (!trackerParameters.empty()) {
                std::stringstream ss;
                ss << trackerParameters;
                boost::property_tree::ptree pt;
                boost::property_tree::read_json(ss, pt);
                auto rootNode = pt.get_child("parameters");

                auto childNode = rootNode.get_child("det_thresh");
                det_thresh = childNode.get_value<float>();

                childNode = rootNode.get_child("max_age");
                max_age = childNode.get_value<int>();

                childNode = rootNode.get_child("min_hits");
                min_hits = childNode.get_value<int>();

                childNode = rootNode.get_child("iou_threshold");
                iou_threshold = childNode.get_value<float>();

                childNode = rootNode.get_child("delta_t");
                delta_t = childNode.get_value<int>();

                childNode = rootNode.get_child("asso_func");
                asso_func_in = childNode.get_value<int>();

                if (asso_func_in == 1)asso_func = "giou";
                else asso_func = "iou";

                childNode = rootNode.get_child("inertia");
                inertia = childNode.get_value<float>();

                childNode = rootNode.get_child("use_byte");
                use_byte_in = childNode.get_value<int>();

                if (use_byte_in == 1) use_byte = true;
                else use_byte = false;
            }  
            tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
            return true;
        }
      
        catch (const std::exception& e) {  
            this->_logger.LogFatal("ANSOCSortTrack::UpdateParameters", e.what(), __FILE__, __LINE__);
            return false;
        }
    }

    std::string ANSOCSortTrack::Update(int modelId, const std::string& detectionData) {
        if (!_licenseValid) {
            this->_logger.LogFatal("ANSOCSortTrack::Update", "Invalid license", __FILE__, __LINE__);
            return "";
        }
        try {
            boost::property_tree::ptree root;
            boost::property_tree::ptree trackingObjects;
            boost::property_tree::ptree pt;
            std::vector<std::string> obj_ids;
            Eigen::MatrixXf objects = GetOCSortInputs(detectionData.c_str());
            //2. Do tracking
            const auto outputs = tracker.update(objects);
            if (outputs.size() > 0) {
                for (int i = 0; i < outputs.size(); i++) {
                    double x_min = outputs[i][0];
                    double y_min = outputs[i][1];
                    double x_max = outputs[i][2];
                    double y_max = outputs[i][3];
                    int trackId = static_cast<int>(outputs[i][4]);
                    int classId = static_cast<int>(outputs[i][5]);
                    double prob = static_cast<double>(outputs[i][6]);
                    int DGId = static_cast<int>(outputs[i][7]);
                    int CameraId = static_cast<int>(outputs[i][8]);
                    int MId = static_cast<int>(outputs[i][9]);
                    std::string name = std::to_string(MId) + ":" + std::to_string(classId);
                    std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
                    bool is_valid = true;

                    double width = x_max - x_min;
                    double heigh = y_max - y_min;
                    double left = x_min;
                    double top = y_min;
                    double right = x_max;
                    double bottom = y_max;

                    boost::property_tree::ptree trackingNode;
                    trackingNode.put("model_id", modelId);
                    trackingNode.put("track_id", trackId);
                    trackingNode.put("class_id", classId);
                    trackingNode.put("prob", prob);
                    trackingNode.put("x", x_min);
                    trackingNode.put("y", y_min);
                    trackingNode.put("width", width);
                    trackingNode.put("height", heigh);
                    trackingNode.put("left", left);
                    trackingNode.put("top", top);
                    trackingNode.put("right", right);
                    trackingNode.put("bottom", bottom);
                    trackingNode.put("valid", is_valid);
                    trackingNode.put("object_id", object_id);
                    trackingObjects.push_back(std::make_pair("", trackingNode));
                }
            }    
            //3. Convert result
            root.add_child("results", trackingObjects);
            std::ostringstream stream;
            boost::property_tree::write_json(stream, root,false);
            std::string st = stream.str();
            return st;

        }
        catch (const std::exception& e) {
            this->_logger.LogFatal("ANSOCSortTrack::Update", e.what(), __FILE__, __LINE__);
            return "";
        }
    }

    std::vector<TrackerObject> ANSOCSortTrack::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
        if (!_licenseValid) {
            this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", "Invalid license", __FILE__, __LINE__);
            return std::vector<TrackerObject>();
        }
        try {
            std::vector<TrackerObject> trackingResults;
            Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
            if (detectionObjects.size() > 0) {
                for (const auto& detection : detectionObjects) {
                    float xMin = detection.x;
                    float yMin = detection.y;
                    float width = detection.width;
                    float height = detection.height;
                    float conf = detection.prob;
                    int classId = detection.class_id;
                    std::string object_id = detection.object_id;
                   
                    // We now need to parse object_id into DGId,CameraId,and ModelId
                    double xMax = xMin + width;
                    double yMax = yMin + height;
                    int DGId = 0;
                    int CameraId = 0;
                    int ModelId = 0;
                    Eigen::MatrixXf newRow(1, ret.cols());
                    newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
                    ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
                    ret.row(ret.rows() - 1) = newRow;
                }
            }
            //2. Do tracking
            const auto outputs = tracker.update(ret);
            if (outputs.size() > 0) {
                for (int i = 0; i < outputs.size(); i++) {
                    TrackerObject trackObj;

                    double x_min = outputs[i][0];
                    double y_min = outputs[i][1];
                    double x_max = outputs[i][2];
                    double y_max = outputs[i][3];
                    int trackId = static_cast<int>(outputs[i][4]);
                    int classId = static_cast<int>(outputs[i][5]);
                    double prob = static_cast<double>(outputs[i][6]);
                    int DGId = static_cast<int>(outputs[i][7]);
                    int CameraId = static_cast<int>(outputs[i][8]);
                    int MId = static_cast<int>(outputs[i][9]);
                    std::string name = std::to_string(MId) + ":" + std::to_string(classId);
                    std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
                    bool is_valid = true;

                    double width = x_max - x_min;
                    double heigh = y_max - y_min;
                    double left = x_min;
                    double top = y_min;
                    double right = x_max;
                    double bottom = y_max;

                    trackObj.track_id = trackId;
                    trackObj.class_id = classId;
                    trackObj.prob = prob;
                    trackObj.x = x_min;
                    trackObj.y = y_min;
                    trackObj.width = width;
                    trackObj.height = heigh;
                    trackObj.left = left;
                    trackObj.top = top;
                    trackObj.right = right;
                    trackObj.bottom = bottom;
                    trackObj.object_id = object_id;
                    trackingResults.push_back(trackObj);
                }
            }
            return trackingResults;
        }
        catch (const std::exception& e) {
            this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", e.what(), __FILE__, __LINE__);
            return std::vector<TrackerObject>();
        }

    }


}