#include "track.h"
Track::Track()
{

}
Track::~Track()
{

}
Track::Track(KAL_MEAN& mean, KAL_COVA& covariance, uint64_t track_id, int n_init, int max_age, const FEATURE& feature,char* img_time)
{
    this->mean = mean;
    this->covariance = covariance;
    this->track_id = track_id;
    this->hits = 1;
    this->age = 1;
    this->time_since_update = 0;
    this->state = TrackState::Tentative;
    features = FEATURESS(1, 128);
    features.row(0) = feature;//features.rows() must = 0;

    this->_n_init = n_init;
    this->_max_age = max_age;
    this->init_t = true;
    this->isCurrent = true;

    //初始化徘徊参数
    this->last_time = 0;
//    gettimeofday(&this->last_s_time,0); // 徘徊计时开启
    this->last_s_time = char_2_unix(img_time);
    std::cout<< "==track img time=%s：" << img_time << std::endl;

    this->isWander = false;

    //初始化跌倒的参数
    this->rate = 100;
    this->isFall = false;
    this->first_Fall = true;
    this->rateScale = 0;
    this->minRate = 0;
    this->center_point.x = 0;
    this->center_point.y = 0;
    this->fall_total_time = 0;
    this->hisMinRate = 100;
    this->human_h = 0;
    this->human_w = 0;


    //初始化奔跑的参数
    this->last_point.x = -1;
    this->last_point.y = -1;
    this->is_runing_p = true;
    this->is_runing_t = true;
    this->isRuning = false;
    this->sum_velocity = 0;
    this->confidence = 0;
    this->run_rate = 0;
    this->last_rate = 0;
    this->rate_area = 0.0;

    this->is_hat = false;
    this->hatScore = 0;
    this->headScore = 0;
    this->helmetScore = 0;

    this->is_mask=false;
    this->maskScore = 0;

    this->is_smoke=false;
    this->smokeScore = 0;

    // 添加当前的xy坐标
//    memset(&this->start,0,sizeof(struct this->timeval));
//    memset(&this->stop,0,sizeof(struct this->timeval));
//    memset(&this->diff,0,sizeof(struct this->timeval));
}

void Track::predit(KalmFilter *kf)
{
    /*Propagate the state distribution to the current time step using a
        Kalman filter prediction step.

        Parameters
        ----------
        kf : kalman_filter.KalmFilter
            The Kalman filter.
        */

    kf->predict(this->mean, this->covariance);
    this->age += 1;
    this->time_since_update += 1;
}

void Track::update(KalmFilter * const kf, const DETECTION_ROW& detection, char* img_time)
{
    // 判断消失三秒后删除该轨迹
    if(this->init_t){
//        gettimeofday(&this->start,0);
        this->start = char_2_unix(img_time);
        std::cout << "tracks start:: " << this->start << std::endl;
        this->init_t = false;
    }

//    gettimeofday(&this->stop,0);
    this->stop = char_2_unix(img_time);
//    this->time_substract(&this->diff, &this->start, &this->stop);
    this->diff = this->stop - this->start;
    std::cout << "tracks curr time:: " << this->stop << " tracks curr diff:: "<<(double)this->diff <<std::endl;

//    this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
    this->sum_timer = (double)(this->diff)/1000;
    printf("----Track update time------%f\n\n\n\n\n\n", this->sum_timer);
//    std::cout<< this->sum_timer  << "::" <<  MAX_OUT_TIME<<std::endl;

    if(this->sum_timer > MAX_OUT_TIME)
    {
        this->state = TrackState::Deleted;
        this->_max_age = 0;
        this->last_time = 0;

//        gettimeofday(&this->last_s_time,0); // 重新徘徊计时（这是离开不到三秒都算是徘徊）
        this->last_s_time = char_2_unix(img_time);
        std::cout<< "wander start "<< this->last_s_time << " : " << img_time <<std::endl;
//        std::cout<< "--------------------------delete -----------------------";
    }
//    gettimeofday(&this->start,0);
    this->start = char_2_unix(img_time);

    // 计算徘徊的时长
//    gettimeofday(&this->last_e_time,0);
    this->last_e_time = char_2_unix(img_time);
    std::cout<< "wander end "<< this->last_e_time << " : " << img_time <<std::endl;
//    this->time_substract(&this->last_diff_time,&this->last_s_time,&this->last_e_time);
    this->last_diff_time = this->last_e_time - this->last_s_time;
//    this->last_time = (int)(this->last_diff_time.tv_sec+this->last_diff_time.tv_usec/1000000);
    this->last_time = (int)(this->last_diff_time/1000);
    std::cout<< "wander time "<<  this->last_time << "::" <<  this->last_diff_time << "last_s_time" << this->last_s_time << " e: "<< this->last_e_time <<std::endl;

    // 奔跑记录单帧时间
    if(this->is_runing_t)
    {
//        gettimeofday(&this->single_s_time, 0);
        this->single_s_time = char_2_unix(img_time);

        this->is_runing_t = false;
    }else{
//        this->time_substract(&this->single_diff_time,&this->single_s_time,&this->last_e_time);
        this->single_diff_time = this->last_e_time - this->single_s_time;
//        this->single_time = (int)(this->single_diff_time.tv_sec*1000 + this->single_diff_time.tv_usec/1000);
        this->single_time = (int)(this->single_diff_time);
//        gettimeofday(&this->single_s_time, 0);
        this->single_s_time = char_2_unix(img_time);
    }


    KAL_DATA pa = kf->update(this->mean, this->covariance, detection.to_xyah());
    this->mean = pa.first;
    this->covariance = pa.second;

    // 添加当前的xy坐标
    this->xywh = detection.tlwh;
    this->confidence = (this->confidence*4+detection.confidence)/5;

    if(this->is_hat)
    {
        this->is_hat = detection.is_hat;
        this->hatScore =(int)((this->hatScore*9 + detection.hatScore)/10);
        this->helmetScore =(int)((this->helmetScore*9 + detection.helmetScore)/10);
        this->headScore =(int)((this->headScore*9 + detection.headScore)/10);
    }else
    {
        this->is_hat = detection.is_hat;
        this->hatScore = detection.hatScore;
        this->helmetScore = detection.helmetScore;
        this->headScore = detection.headScore;
    }


    this->is_mask = detection.is_mask;
    this->maskScore = detection.maskScore;
    this->is_smoke = detection.is_smoke;
    this->smokeScore = detection.smokeScore;
    /***yolo 直接测跌倒代码更新 s*/
    if(detection.obj_id==0)
    {
        this->fall_total_time = 0 ;
        this->first_Fall = true;
        this->rateScale = 0;
        this->isRuning = false;
    }else if(detection.obj_id==1)
    {
         /**-- start计算累积跌倒时间*/
        if(this->first_Fall) //是否开始计算跌倒时间
        {
            this->first_Fall = false;
//            gettimeofday(&this->fall_s_time,0);
            this->fall_s_time = char_2_unix(img_time);
        }
//        gettimeofday(&this->fall_e_time,0);
        this->fall_e_time = char_2_unix(img_time);
//        this->time_substract(&this->fall_diff_time,&this->fall_s_time,&this->fall_e_time); //距离最小比例的时间
        this->fall_diff_time = this->fall_e_time - this->fall_s_time;
//        this->fall_total_time = (int)(this->fall_diff_time.tv_sec + this->fall_diff_time.tv_usec/1000000);
        this->fall_total_time = (int)(this->fall_diff_time/1000);

        /** -- end计算累积跌倒时间*/

        this->rateScale = detection.confidence;
        this->isRuning = false;

    }else if(detection.obj_id==2)
    {
        this->isRuning = true;
    }
    /***yolo 直接测跌倒代码更新 end*/

    featuresAppendOne(detection.feature);
    //    this->features.row(features.rows()) = detection.feature;
    this->hits += 1;
    this->time_since_update = 0;
    if(this->state == TrackState::Tentative && this->hits >= this->_n_init) {
        this->state = TrackState::Confirmed;
    }
    this->isCurrent = true;
}

bool Track::mark_missed(char* img_time)
{
    if(this->init_t)
    {
//        gettimeofday(&this->start,0);
        this->start = char_2_unix(img_time);
        this->init_t = false;
    }
//    gettimeofday(&this->stop,0);
    this->stop = char_2_unix(img_time);

//    this->time_substract(&this->diff,&this->start,&this->stop);
    this->diff = this->stop - this->start;

//    this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
    this->sum_timer = (double)(this->diff)/1000;

    if(this->state == TrackState::Tentative) {
        this->state = TrackState::Deleted;
        return true;
    } else if(this->time_since_update > this->_max_age) {
        this->state = TrackState::Deleted;
        return true;
    } else if(this->sum_timer > MAX_OUT_TIME)
    {
        this->state = TrackState::Deleted;
        this->_max_age = 0;
        return true;
    }
    this->isCurrent = false;
    return false;
}

bool Track::is_confirmed()
{
    return this->state == TrackState::Confirmed;
}

bool Track::is_deleted()
{
    return this->state == TrackState::Deleted;
}

bool Track::is_tentative()
{
    return this->state == TrackState::Tentative;
}

DETECTBOX Track::to_tlwh()
{
    DETECTBOX ret = mean.leftCols(4);
    ret(2) *= ret(3);
    ret.leftCols(2) -= (ret.rightCols(2)/2);
    return ret;
}

DETECTBOX Track::to_xywh()
{
    return this->xywh;
}

void Track::featuresAppendOne(const FEATURE &f)
{
    int size = this->features.rows();
    FEATURESS newfeatures = FEATURESS(size+1, 128);
    newfeatures.block(0, 0, size, 128) = this->features;
    newfeatures.row(size) = f;
    features = newfeatures;
//    printf("--features size::---%d---%d--\n",size,features.rows());
}

int Track::time_substract(struct timeval *result, struct timeval *begin, struct timeval *end)
{
    if(begin->tv_sec > end->tv_sec)    return -1;
    if((begin->tv_sec == end->tv_sec) && (begin->tv_usec > end->tv_usec))    return -2;
    result->tv_sec    = (end->tv_sec - begin->tv_sec);
    result->tv_usec    = (end->tv_usec - begin->tv_usec);

    if(result->tv_usec < 0)
    {
        result->tv_sec--;
        result->tv_usec += 1000000;
    }
    return 0;

}