#include "tracker.h"
#include "nn_matching.h"
#include "model.h"
#include "linear_assignment.h"
#include <iostream>

using namespace std;

//#define MY_inner_DEBUG
#ifdef MY_inner_DEBUG
#include <string>
#include <iostream>
#endif

tracker::tracker(
		float max_cosine_distance, int nn_budget,
		float max_iou_distance, int max_age, int n_init)
{
    this->metric = new NearNeighborDisMetric(
    		NearNeighborDisMetric::METRIC_TYPE::cosine,
    		max_cosine_distance, nn_budget);
    this->max_iou_distance = max_iou_distance;
    this->max_age = max_age;
    this->n_init = n_init;

    this->kf = new KalmFilter();
    this->tracks.clear();
    this->_next_idx = 1;
}

tracker::~tracker()
{
    //    delete this->metric;
    //    delete this->kf;
}

void tracker::predict()
{
    for(auto& track:tracks)
    {
        track->predit(this->kf);
    }
}

void tracker::update(std::vector<std::shared_ptr<tracker>> &CAMERAS_VCT, const DETECTIONS &detections, int cam_id, char* img_time)
{
    TRACHER_MATCHD res;
//    for (map<uint64_t, std::shared_ptr<Track>>::iterator iter=candidate_track_map.begin();iter!=candidate_track_map.end();iter++)
//    {
//        this->tracks.push_back(iter->second);
//        printf("-------------------this->tracks.size:%d\n",this->tracks.size());
//    }

//    std::cout<< "size1:"<< detections.size() << std::endl;
//    if (detections.size()==2){
//        for (DETECTION_ROW det : detections)
//        {
//            std::cout<< det.feature <<std::endl;
//        }
//        exit(0);
//    }

    _match(detections, res);

    vector<MATCH_DATA>& matches = res.matches;
    //#ifdef MY_inner_DEBUG
    //    printf("res.matches size = %d:\n", matches.size());
    //#endif
    for(MATCH_DATA& data:matches) {
        int track_idx = data.first;
        int detection_idx = data.second;
        //#ifdef MY_inner_DEBUG
        //        printf("\t%d == %d;\n", track_idx, detection_idx);
        //#endif
        tracks[track_idx]->update(this->kf, detections[detection_idx], img_time);
//        LOG(INFO) << "=======match-track_idx::" << tracks[track_idx]->track_id;
//        DEBUG( "=======match-track_idx::" + to_string(tracks[track_idx]->track_id));
    }

    vector<int>& unmatched_tracks = res.unmatched_tracks;
    //#ifdef MY_inner_DEBUG
    //    printf("res.unmatched_tracks size = %d\n", unmatched_tracks.size());
    //#endif
    for(int& track_idx:unmatched_tracks)
    {
        this->tracks[track_idx]->mark_missed(img_time);
    }
    vector<int>& unmatched_detections = res.unmatched_detections;
    //#ifdef MY_inner_DEBUG
    //    printf("res.unmatched_detections size = %d\n", unmatched_detections.size());
    //#endif
    for(int& detection_idx:unmatched_detections) {
        this->_initiate_track(detections[detection_idx], cam_id, img_time);
    }
    //#ifdef MY_inner_DEBUG
    //    int size = tracks.size();
    //    printf("now track size = %d\n", size);
    //#endif
    vector<std::shared_ptr<Track>>::iterator it;
    for(it = tracks.begin(); it != tracks.end();) {
        if((*it)->is_deleted()) it = tracks.erase(it);
        else ++it;
    }
    //#ifdef MY_inner_DEBUG
    //    printf("update track size = %d\n", tracks.size());
    //#endif

    /* old version:
    //update distance metric:
    FEATURESS features;
    vector<int> targets;
    vector<int> active_targets;
    int pos = 0;
    for(Track track:tracks) {
        if(track.is_confirmed() == false) continue;
        active_targets.push_back(track.track_id);
        features.row(pos) = track.features;
        int rows = track.features.rows();
        pos += rows;
        for(int i = 0; i < rows; i++) targets.push_back(track.track_id);
        //attention!!!
        //track.features.clear();
        track.features = Eigen::Matrix<float, -1, 128, Eigen::RowMajor>(0,128);
    }
    this->metric->partial_fit(features, targets, active_targets);
    */

    vector<uint64_t> active_targets;
    vector<TRACKER_DATA> tid_features;
    for (auto& track:tracks)
    {
        if(track->is_confirmed() == false) continue;
        active_targets.push_back(track->track_id);
        tid_features.push_back(std::make_pair(track->track_id, track->features));
        FEATURESS t = FEATURESS(0, 128);
        track->features = t;
    }

    this->metric->partial_fit(tid_features, active_targets);

}

void tracker::_match(const DETECTIONS &detections, TRACHER_MATCHD &res)
{
    DEBUG("tracker.cpp _match start 147:: ");
    vector<int> confirmed_tracks;
    vector<int> unconfirmed_tracks;
    int idx = 0;
    for(auto& t:tracks) {
        if(t->is_confirmed()) confirmed_tracks.push_back(idx);
        else unconfirmed_tracks.push_back(idx);
        idx++;
    }

    TRACHER_MATCHD matcha = linear_assignment::getInstance()->matching_cascade(
                this, &tracker::gated_matric,
                this->metric->mating_threshold,
                this->max_age,
                this->tracks,
                detections,
                confirmed_tracks);
    vector<int> iou_track_candidates;
    iou_track_candidates.assign(unconfirmed_tracks.begin(), unconfirmed_tracks.end());
    vector<int>::iterator it;

    for(it = matcha.unmatched_tracks.begin(); it != matcha.unmatched_tracks.end();) {
        int idx = *it;
        if(tracks[idx]->time_since_update == 1) { //push into unconfirmed
            iou_track_candidates.push_back(idx);
            it = matcha.unmatched_tracks.erase(it);
            continue;
        }
        ++it;
    }

    TRACHER_MATCHD matchb = linear_assignment::getInstance()->min_cost_matching(
                this, &tracker::iou_cost,
                this->max_iou_distance,
                this->tracks,
                detections,
                iou_track_candidates,
                matcha.unmatched_detections);

    //get result:
    res.matches.assign(matcha.matches.begin(), matcha.matches.end());
    res.matches.insert(res.matches.end(), matchb.matches.begin(), matchb.matches.end());
    //unmatched_tracks;
    res.unmatched_tracks.assign(
                matcha.unmatched_tracks.begin(),
                matcha.unmatched_tracks.end());
    res.unmatched_tracks.insert(
                res.unmatched_tracks.end(),
                matchb.unmatched_tracks.begin(),
                matchb.unmatched_tracks.end());
    res.unmatched_detections.assign(
                matchb.unmatched_detections.begin(),
                matchb.unmatched_detections.end());
    DEBUG("tracker.cpp _match end 207:: ");

}


void tracker::_initiate_track(const DETECTION_ROW &detection, int cam_id, char* img_time)
{
    KAL_DATA data = kf->initiate(detection.to_xyah());
    KAL_MEAN mean = data.first;
    KAL_COVA covariance = data.second;
    uint64_t n_track_id;
    stringstream strValue;
    strValue << random_int(8);
    strValue >> n_track_id;
    // n_track_id = m_staticStruct::human_ids++;
    auto t = std::make_shared<Track>(mean, covariance, n_track_id, this->n_init, this->max_age, detection.feature, img_time);
    t->xywh = detection.tlwh;
    this->tracks.push_back(t);

//    auto t = std::make_shared<Track>(mean, covariance, this->_next_idx, this->n_init,this->max_age, detection.feature);
//    this->tracks.push_back(t);
//
//    _next_idx += 1;
}


DYNAMICM tracker::gated_matric(
        std::vector<std::shared_ptr<Track>> &tracks,
        const DETECTIONS &dets,
        const std::vector<int>& track_indices,
        const std::vector<int>& detection_indices)
{
    // FEATURESS features(detection_indices.size(), 128);
    // int pos = 0;
    // for(int i:detection_indices) {
    //     features.row(pos++) = dets[i].feature;
    // }
    vector<uint64_t> targets;
    for(int i:track_indices) {
        targets.push_back(tracks[i]->track_id);
    }
    // DYNAMICM cost_matrix = this->metric->distance(features, targets);
    DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(targets.size(), detection_indices.size());
    DYNAMICM res = linear_assignment::getInstance()->gate_cost_matrix(
                this->kf, cost_matrix, tracks, dets, track_indices,
                detection_indices);
    return res;
}

DYNAMICM
tracker::cos_matric(
    std::vector<std::shared_ptr<Track> > &tracks,
    const DETECTIONS &dets, const std::vector<int>
    &track_indices, const std::vector<int>
    &detection_indices)
{
    FEATURESS features(detection_indices.size(), 128);
    int pos = 0;
    for(int i : detection_indices) {
        features.row(pos++) = dets[i].feature;
    }
    std::map<uint64_t, FEATURESS> simples;
    for(auto i : track_indices)
    {
        simples[tracks[i]->track_id] = tracks[i]->features;
    }

    DYNAMICM res = this->metric->cos_distance(features, simples);

    return res;
}


DYNAMICM
tracker::iou_cost(
        std::vector<std::shared_ptr<Track>> &tracks,
        const DETECTIONS &dets,
        const std::vector<int>& track_indices,
        const std::vector<int>& detection_indices)
{
    //!!!python diff: track_indices && detection_indices will never be None.
    //    if(track_indices.empty() == true) {
    //        for(size_t i = 0; i < tracks.size(); i++) {
    //            track_indices.push_back(i);
    //        }
    //    }
    //    if(detection_indices.empty() == true) {
    //        for(size_t i = 0; i < dets.size(); i++) {
    //            detection_indices.push_back(i);
    //        }
    //    }
    int rows = track_indices.size();
    int cols = detection_indices.size();
    DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(rows, cols);
    for(int i = 0; i < rows; i++) {
        int track_idx = track_indices[i];
        if(tracks[track_idx]->time_since_update > 1) {
            cost_matrix.row(i) = Eigen::RowVectorXf::Constant(cols, INFTY_COST);
            continue;
        }
        DETECTBOX bbox = tracks[track_idx]->to_tlwh();
        int csize = detection_indices.size();
        DETECTBOXSS candidates(csize, 4);
        for(int k = 0; k < csize; k++) candidates.row(k) = dets[detection_indices[k]].tlwh;
        Eigen::RowVectorXf rowV = (1. - iou(bbox, candidates).array()).matrix().transpose();
        cost_matrix.row(i) = rowV;
    }
    return cost_matrix;
}


Eigen::VectorXf
tracker::iou(DETECTBOX& bbox, DETECTBOXSS& candidates)
{
    float bbox_tl_1 = bbox[0];
    float bbox_tl_2 = bbox[1];
    float bbox_br_1 = bbox[0] + bbox[2];
    float bbox_br_2 = bbox[1] + bbox[3];
    float area_bbox = bbox[2] * bbox[3];

    Eigen::Matrix<float, -1, 2> candidates_tl;
    Eigen::Matrix<float, -1, 2> candidates_br;
    candidates_tl = candidates.leftCols(2) ;
    candidates_br = candidates.rightCols(2) + candidates_tl;

    int size = int(candidates.rows());
    //    Eigen::VectorXf area_intersection(size);
    //    Eigen::VectorXf area_candidates(size);
    Eigen::VectorXf res(size);
    for(int i = 0; i < size; i++) {
        float tl_1 = std::max(bbox_tl_1, candidates_tl(i, 0));
        float tl_2 = std::max(bbox_tl_2, candidates_tl(i, 1));
        float br_1 = std::min(bbox_br_1, candidates_br(i, 0));
        float br_2 = std::min(bbox_br_2, candidates_br(i, 1));

        float w = br_1 - tl_1; w = (w < 0? 0: w);
        float h = br_2 - tl_2; h = (h < 0? 0: h);
        float area_intersection = w * h;
        float area_candidates = candidates(i, 2) * candidates(i, 3);
        res[i] = area_intersection/(area_bbox + area_candidates - area_intersection);
    }
    //#ifdef MY_inner_DEBUG
    //        std::cout << res << std::endl;
    //#endif
    return res;
}