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

}
Track::~Track()
{

}
Track::Track(KAL_MEAN& mean, KAL_COVA& covariance, int track_id, int n_init, int max_age, const FEATURE& feature)
{
    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;
}

void Track::predit(KalmFilter *kf)
{
    kf->predict(this->mean, this->covariance);
    this->age += 1;
    this->time_since_update += 1;
}

void Track::update(KalmFilter * const kf, const DETECTION_ROW& detection)
{
    if(this->init_t){
        gettimeofday(&this->start,0);
        this->init_t = false;
    }
    gettimeofday(&this->stop,0);
    this->time_substract(&this->diff,&this->start,&this->stop);

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

    if(this->sum_timer > MAX_OUT_TIME)
    {
        this->state = TrackState::Deleted;
        this->_max_age = 0;
    }
    gettimeofday(&this->start,0);



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

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

// 如果不匹配的情况超过max_age或者超时，则清除
bool Track::mark_missed()
{
    if(this->init_t)
    {
        gettimeofday(&this->start,0);
        this->init_t = false;
    }
    gettimeofday(&this->stop,0);
    this->time_substract(&this->diff,&this->start,&this->stop);

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

    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;
    }
    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;
}

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;
}

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;

}