//
// Created by Scheaven on 2020/3/23.
//

#include "tracker_manager.h"
#include <thread>
#include <unistd.h>
#include <cstdlib>


TrackerManager::TrackerManager()
{
    cout << "---------loading detect ---------"  <<  endl;
}

TrackerManager::~TrackerManager()
{

}

//调用其他释放函数
void TrackerManager::release()
{
    DetecterManager::getInstance()->release();
    FeatureEncoder::getInstance()->release();
}

void TrackerManager::init_load_model()
{
    cout << "loading YOLO model ---------"  <<  endl;
    DetecterManager::getInstance();
    Detector::getInstance();
    FeatureEncoder::getInstance();
}

//添加待追踪摄像头
bool TrackerManager::add_cam(int cam_id){
    if (cam_id==(this->CAMERAS_VCT.size()+1))
    {
        auto cam_tracker = std::make_shared<tracker>(max_cosine_distance, nn_budget);
        this->CAMERAS_VCT.push_back(cam_tracker);
        return true;
    }else if(cam_id<this->CAMERAS_VCT.size())
    {
        cout << "-- The camera ID is occupied!"  <<  endl;
        return false;
    }else
    {
        cout << "-- Camera ID discontinuous!"  <<  endl;
        return false;
    }
}

// ------------------------------ SDK 代码封装部分 ----------------------------------
// 模型检测 + 编码器 + 追踪 SDK封装
void TrackerManager::single_SDK(const int cam_id, const void *img, TResult *t_result)
{
    TImage *frame_img = (TImage*)img;
    cv::Mat frame(Size(frame_img->width, frame_img->height), CV_8UC3);
    frame.data = (uchar*)frame_img->data;
    DETECTIONS detections;
    DetecterManager::getInstance()->detecter_main(frame, detections);
    if(detections.size()>0)
    {
        if(FeatureEncoder::getInstance()->getRectsFeature(frame , detections) == false)
        {
            std::cout<< " encoder code failed ----" << std::endl;
        }
        single_tracker(cam_id, detections);
        switch_SDK_TResult(cam_id, frame, detections, t_result);

    }

}

// 转换成SDK 想要的结果
void TrackerManager::switch_SDK_TResult(int cam_id, cv::Mat img, DETECTIONS detections, TResult *t_result)
{
    t_result->targets = (Target*)malloc(sizeof(Target) * this->CAMERAS_VCT[cam_id]->tracks.size());
    int w_count = 0;
    for(auto& track :this->CAMERAS_VCT[cam_id]->tracks)
    {
        Target target;
        init_target(&target);
        RESULT_STRUCT result;
        if(!track->is_confirmed() || track->time_since_update > 1) continue;
        

        if(detections.size() <= w_count){ // 极个别无法没有confidence的问题
            break;
        }else{
            DETECTBOX tmpbox = detections[w_count].tlwh; // 检测的框信息
            target.rect.left = tmpbox(0);
            target.rect.top =  tmpbox(1);
            target.rect.right = tmpbox(0) + tmpbox(2);
            target.rect.bottom = tmpbox(1) + tmpbox(3);
            target.id = track->track_id;
            target.confidence = detections[w_count].confidence*100;
        }

        t_result->targets[w_count] = target;
        w_count ++;
    }
    t_result->count = w_count;
}

// ------------------------------单目多线程摄像头追踪代码部分 ----------------------------
// 模型检测 + 编码器 + 追踪
 void TrackerManager::single_detect_tracking(int cam_id, cv::Mat frame, FRAME_RESULT result_vec)
{
    DETECTIONS detections;
    DetecterManager::getInstance()->detecter_main(frame, detections);

    if(FeatureEncoder::getInstance()->getRectsFeature(frame , detections) == false)
    {
        std::cout<< " encoder code failed ----" << std::endl;
    }
    single_tracker(cam_id, detections); //單目追踪
}

//一次性追踪多摄像头(添加的摄像头列表全部追踪)信息
void TrackerManager::mul_detect_tracking(std::vector<int> cam_ids, std::vector<cv::Mat> frame_vec, std::vector<FRAME_RESULT>&  results_vec) // 多摄像头追踪
{

    std::vector<std::thread> thread_vec;
    for (int i = 0; i < cam_ids.size(); ++i)
    {
        std::thread tracking_thread(&TrackerManager::single_detect_tracking, this,cam_ids[i], frame_vec[i], results_vec[i]);
        thread_vec.emplace_back(std::move(tracking_thread));
    }

    for (auto &sub_thread : thread_vec)
    {
        sub_thread.join();
    }


    for (int i = 0; i < cam_ids.size(); ++i)
    {
        single_draw(cam_ids[i], frame_vec[i], results_vec[i]); //绘制结果信息（展示图像）
    }

}


// 编码器 + 追踪
void TrackerManager::sub_thread_tracking(int cam_id, cv::Mat frame, FRAME_RESULT result_vec, DETECTIONS detections)
{
    std::clock_t t_strat2 = std::clock();
    if(FeatureEncoder::getInstance()->getRectsFeature(frame , detections) == false)
    {
        std::cout<< " encoder code failed ----" << std::endl;
    }
    std::clock_t t_strat3 = std::clock();
    single_tracker(cam_id, detections); //單目追踪
    std::clock_t t_strat4 = std::clock();
}

void TrackerManager::single_tracker(int cam_id, DETECTIONS& detections)
{
    auto cam_tracker = this->CAMERAS_VCT[cam_id];

    cam_tracker->predict();
    cam_tracker->update(detections);

    this->CAMERAS_VCT[cam_id] = cam_tracker;
}

void TrackerManager::single_draw(int cam_id, cv::Mat img, FRAME_RESULT& result_vec)
{
    for(auto& track : this->CAMERAS_VCT[cam_id]->tracks)
    {
        RESULT_STRUCT result;
        if(!track->is_confirmed() || track->time_since_update > 1) continue;
        DETECTBOX tmpbox = track->to_tlwh();
        result.rect = cv::Rect(tmpbox(0),tmpbox(1),tmpbox(2),tmpbox(3));
        result.human_id = track->track_id;
//        result.confidence = detection.confidence;
        result_vec.push_back(result);
    }

    draw_result(img, result_vec);
}



std::vector<DETECTION_ROW> TrackerManager::deal_features(std::vector<cv::Rect> boxes, std::vector<vector<float> > feats_vec)
{
    DETECTIONS detections;
    for (int i = 0; i < boxes.size(); ++i)
    {
        DETECTION_ROW tmpRow;

        tmpRow.tlwh = DETECTBOX(boxes[i].x, boxes[i].y, boxes[i].width, boxes[i].height);//DETECTBOX(x, y, w, h);
        tmpRow.confidence = 1;

        for(int j = 0; j < 128; j++)
        {
            tmpRow.feature[j] = feats_vec[i][j];
        }
        cout << tmpRow.tlwh << endl;
        cout << tmpRow.feature << endl;
        detections.push_back(tmpRow);
//        std::cout<<"Detections Size:::::"<<detections.size() <<std::endl;
    }
    return detections;
}

Mat TrackerManager:: h_tracker(int cam_id, Mat frame, DETECTIONS detections){
    auto cam_tracker = this->CAMERAS_VCT[cam_id];

    cam_tracker->predict();
    cam_tracker->update(detections);
    std::vector<RESULT_DATA> result;
//    std::cout << " tracks Size:::::" << cam_tracker->tracks.size() << std::endl;

    for(auto& track : cam_tracker->tracks) {
        if(track->is_confirmed() && track->time_since_update > 1) continue;
        result.push_back(std::make_pair(track->track_id, track->to_tlwh()));


        DETECTBOX tmpbox = track->to_tlwh();
        cv::Rect rect(tmpbox(0), tmpbox(1), tmpbox(2), tmpbox(3));
        cv::rectangle(frame, rect, cv::Scalar(0,0,255), 4);
        std::string label = to_string(track->track_id);
        cv::putText(frame, label, cv::Point(rect.x, rect.y), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255, 0, 0), 2);
    }

    this->CAMERAS_VCT[cam_id] = cam_tracker;

    return frame;
}


void TrackerManager::init_target(Target *t){
    t->attribute = NULL;
    t->feature = NULL;
    t->id = 0;
    t->rect = TRect{0,0,0,0};
    t->confidence = 0;
}