//   Copyright (c) 2021 PaddlePaddle Authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>

#include "include/trajectory.h"

namespace PaddleDetection {

void TKalmanFilter::init(const cv::Mat &measurement)
{
    measurement.copyTo(statePost(cv::Rect(0, 0, 1, 4)));
    statePost(cv::Rect(0, 4, 1, 4)).setTo(0);
    statePost.copyTo(statePre);

    float varpos = 2 *  std_weight_position * (*measurement.ptr<float>(3));
    varpos *= varpos;
    float varvel = 10 * std_weight_velocity * (*measurement.ptr<float>(3));
    varvel *= varvel;
    
    errorCovPost.setTo(0);
    *errorCovPost.ptr<float>(0, 0) = varpos;
    *errorCovPost.ptr<float>(1, 1) = varpos;
    *errorCovPost.ptr<float>(2, 2) = 1e-4f;
    *errorCovPost.ptr<float>(3, 3) = varpos;
    *errorCovPost.ptr<float>(4, 4) = varvel;
    *errorCovPost.ptr<float>(5, 5) = varvel;
    *errorCovPost.ptr<float>(6, 6) = 1e-10f;
    *errorCovPost.ptr<float>(7, 7) = varvel;  
    errorCovPost.copyTo(errorCovPre);
}

const cv::Mat &TKalmanFilter::predict()
{
    float varpos = std_weight_position * (*statePre.ptr<float>(3));
    varpos *= varpos;
    float varvel = std_weight_velocity * (*statePre.ptr<float>(3));
    varvel *= varvel;
    
    processNoiseCov.setTo(0);
    *processNoiseCov.ptr<float>(0, 0) = varpos;
    *processNoiseCov.ptr<float>(1, 1) = varpos;
    *processNoiseCov.ptr<float>(2, 2) = 1e-4f;
    *processNoiseCov.ptr<float>(3, 3) = varpos;
    *processNoiseCov.ptr<float>(4, 4) = varvel;
    *processNoiseCov.ptr<float>(5, 5) = varvel;
    *processNoiseCov.ptr<float>(6, 6) = 1e-10f;
    *processNoiseCov.ptr<float>(7, 7) = varvel;
    
    return cv::KalmanFilter::predict();
}

const cv::Mat &TKalmanFilter::correct(const cv::Mat &measurement)
{
    float varpos = std_weight_position * (*measurement.ptr<float>(3));
    varpos *= varpos;

    measurementNoiseCov.setTo(0);
    *measurementNoiseCov.ptr<float>(0, 0) = varpos;
    *measurementNoiseCov.ptr<float>(1, 1) = varpos;
    *measurementNoiseCov.ptr<float>(2, 2) = 1e-2f;
    *measurementNoiseCov.ptr<float>(3, 3) = varpos;
    
    return cv::KalmanFilter::correct(measurement);
}

void TKalmanFilter::project(cv::Mat &mean, cv::Mat &covariance) const
{    
    float varpos = std_weight_position * (*statePost.ptr<float>(3));
    varpos *= varpos;
    
    cv::Mat measurementNoiseCov_ = cv::Mat::eye(4, 4, CV_32F);
    *measurementNoiseCov_.ptr<float>(0, 0) = varpos;
    *measurementNoiseCov_.ptr<float>(1, 1) = varpos;
    *measurementNoiseCov_.ptr<float>(2, 2) = 1e-2f;
    *measurementNoiseCov_.ptr<float>(3, 3) = varpos;
        
    mean = measurementMatrix * statePost;
    cv::Mat temp = measurementMatrix * errorCovPost;
    gemm(temp, measurementMatrix, 1, measurementNoiseCov_, 1, covariance, cv::GEMM_2_T);
}

int Trajectory::count = 0;

const cv::Mat &Trajectory::predict(void)
{
    if (state != Tracked)
        *cv::KalmanFilter::statePost.ptr<float>(7) = 0;
    return TKalmanFilter::predict();
}

void Trajectory::update(Trajectory &traj, int timestamp_, bool update_embedding_)
{
    timestamp = timestamp_;
    ++length;
    ltrb = traj.ltrb;
    xyah = traj.xyah;    
    TKalmanFilter::correct(cv::Mat(traj.xyah));    
    state = Tracked;
    is_activated = true;
    score = traj.score;   
    if (update_embedding_)
        update_embedding(traj.current_embedding);
}

void Trajectory::activate(int timestamp_)
{
    id = next_id();
    TKalmanFilter::init(cv::Mat(xyah));    
    length = 0;
    state = Tracked;
    if (timestamp_ == 1) {
      is_activated = true;
    }
    timestamp = timestamp_;
    starttime = timestamp_;
}

void Trajectory::reactivate(Trajectory &traj, int timestamp_, bool newid)
{
    TKalmanFilter::correct(cv::Mat(traj.xyah));
    update_embedding(traj.current_embedding);
    length = 0;
    state = Tracked;
    is_activated = true;
    timestamp = timestamp_;
    if (newid)
        id = next_id();
}

void Trajectory::update_embedding(const cv::Mat &embedding)
{
    current_embedding = embedding / cv::norm(embedding);
    if (smooth_embedding.empty())
    {
        smooth_embedding = current_embedding;
    }
    else
    {
        smooth_embedding = eta * smooth_embedding + (1 - eta) * current_embedding;
    }
    smooth_embedding = smooth_embedding / cv::norm(smooth_embedding);
}

TrajectoryPool operator+(const TrajectoryPool &a, const TrajectoryPool &b)
{
    TrajectoryPool sum;
    sum.insert(sum.end(), a.begin(), a.end());
    
    std::vector<int> ids(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        ids[i] = a[i].id;
    
    for (size_t i = 0; i < b.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), b[i].id);
        if (iter == ids.end())
        {
            sum.push_back(b[i]);
            ids.push_back(b[i].id);
        }
    }
    
    return sum;
}

TrajectoryPool operator+(const TrajectoryPool &a, const TrajectoryPtrPool &b)
{
    TrajectoryPool sum;
    sum.insert(sum.end(), a.begin(), a.end());
    
    std::vector<int> ids(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        ids[i] = a[i].id;
    
    for (size_t i = 0; i < b.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), b[i]->id);
        if (iter == ids.end())
        {
            sum.push_back(*b[i]);
            ids.push_back(b[i]->id);
        }
    }
    
    return sum;
}

TrajectoryPool &operator+=(TrajectoryPool &a, const TrajectoryPtrPool &b)
{    
    std::vector<int> ids(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        ids[i] = a[i].id;
    
    for (size_t i = 0; i < b.size(); ++i)
    {
        if (b[i]->smooth_embedding.empty())
            continue;
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), b[i]->id);
        if (iter == ids.end())
        {
            a.push_back(*b[i]);
            ids.push_back(b[i]->id);
        }
    }
    
    return a;
}

TrajectoryPool operator-(const TrajectoryPool &a, const TrajectoryPool &b)
{
    TrajectoryPool dif;
    std::vector<int> ids(b.size());
    for (size_t i = 0; i < b.size(); ++i)
        ids[i] = b[i].id;
    
    for (size_t i = 0; i < a.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), a[i].id);
        if (iter == ids.end())
            dif.push_back(a[i]);
    }
    
    return dif;
}

TrajectoryPool &operator-=(TrajectoryPool &a, const TrajectoryPool &b)
{
    std::vector<int> ids(b.size());
    for (size_t i = 0; i < b.size(); ++i)
        ids[i] = b[i].id;
    
    TrajectoryPoolIterator piter;
    for (piter = a.begin(); piter != a.end(); )
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), piter->id);
        if (iter == ids.end())
            ++piter;
        else
            piter = a.erase(piter);
    }
    
    return a;
}

TrajectoryPtrPool operator+(const TrajectoryPtrPool &a, const TrajectoryPtrPool &b)
{
    TrajectoryPtrPool sum;
    sum.insert(sum.end(), a.begin(), a.end());
    
    std::vector<int> ids(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        ids[i] = a[i]->id;
    
    for (size_t i = 0; i < b.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), b[i]->id);
        if (iter == ids.end())
        {
            sum.push_back(b[i]);
            ids.push_back(b[i]->id);
        }
    }
    
    return sum;
}

TrajectoryPtrPool operator+(const TrajectoryPtrPool &a, TrajectoryPool &b)
{
    TrajectoryPtrPool sum;
    sum.insert(sum.end(), a.begin(), a.end());
    
    std::vector<int> ids(a.size());
    for (size_t i = 0; i < a.size(); ++i)
        ids[i] = a[i]->id;
    
    for (size_t i = 0; i < b.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), b[i].id);
        if (iter == ids.end())
        {
            sum.push_back(&b[i]);
            ids.push_back(b[i].id);
        }
    }
    
    return sum;
}

TrajectoryPtrPool operator-(const TrajectoryPtrPool &a, const TrajectoryPtrPool &b)
{
    TrajectoryPtrPool dif;
    std::vector<int> ids(b.size());
    for (size_t i = 0; i < b.size(); ++i)
        ids[i] = b[i]->id;
    
    for (size_t i = 0; i < a.size(); ++i)
    {
        std::vector<int>::iterator iter = find(ids.begin(), ids.end(), a[i]->id);
        if (iter == ids.end())
            dif.push_back(a[i]);
    }
    
    return dif;
}

cv::Mat embedding_distance(const TrajectoryPool &a, const TrajectoryPool &b)
{

    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            cv::Mat u = a[i].smooth_embedding;
            cv::Mat v = b[j].smooth_embedding;
            double uv = u.dot(v);
            double uu = u.dot(u);
            double vv = v.dot(v);
            double dist = std::abs(1. - uv / std::sqrt(uu * vv));
            //double dist = cv::norm(a[i].smooth_embedding, b[j].smooth_embedding, cv::NORM_L2);
            distsi[j] = static_cast<float>(std::max(std::min(dist, 2.), 0.));
        }
    }
    return dists;
}

cv::Mat embedding_distance(const TrajectoryPtrPool &a, const TrajectoryPtrPool &b)
{
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            //double dist = cv::norm(a[i]->smooth_embedding, b[j]->smooth_embedding, cv::NORM_L2);
            //distsi[j] = static_cast<float>(dist);
            cv::Mat u = a[i]->smooth_embedding;
            cv::Mat v = b[j]->smooth_embedding;
            double uv = u.dot(v);
            double uu = u.dot(u);
            double vv = v.dot(v);
            double dist = std::abs(1. - uv / std::sqrt(uu * vv));
            distsi[j] = static_cast<float>(std::max(std::min(dist, 2.), 0.));

        }
    }
    
    return dists;
}

cv::Mat embedding_distance(const TrajectoryPtrPool &a, const TrajectoryPool &b)
{
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            //double dist = cv::norm(a[i]->smooth_embedding, b[j].smooth_embedding, cv::NORM_L2);
            //distsi[j] = static_cast<float>(dist);
            cv::Mat u = a[i]->smooth_embedding;
            cv::Mat v = b[j].smooth_embedding;
            double uv = u.dot(v);
            double uu = u.dot(u);
            double vv = v.dot(v);
            double dist = std::abs(1. - uv / std::sqrt(uu * vv));
            distsi[j] = static_cast<float>(std::max(std::min(dist, 2.), 0.));

        }
    }
    
    return dists;
}

cv::Mat mahalanobis_distance(const TrajectoryPool &a, const TrajectoryPool &b)
{
    std::vector<cv::Mat> means(a.size());
    std::vector<cv::Mat> icovariances(a.size());
    for (size_t i = 0; i < a.size(); ++i)
    {
        cv::Mat covariance;
        a[i].project(means[i], covariance);
        cv::invert(covariance, icovariances[i]);
    }

    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Mat x(b[j].xyah);
            float dist = static_cast<float>(cv::Mahalanobis(x, means[i], icovariances[i]));
            distsi[j] = dist * dist;
        }
    }
    
    return dists;
}

cv::Mat mahalanobis_distance(const TrajectoryPtrPool &a, const TrajectoryPtrPool &b)
{
    std::vector<cv::Mat> means(a.size());
    std::vector<cv::Mat> icovariances(a.size());
    for (size_t i = 0; i < a.size(); ++i)
    {
        cv::Mat covariance;
        a[i]->project(means[i], covariance);
        cv::invert(covariance, icovariances[i]);
    }

    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Mat x(b[j]->xyah);
            float dist = static_cast<float>(cv::Mahalanobis(x, means[i], icovariances[i]));
            distsi[j] = dist * dist;
        }
    }
    
    return dists;
}

cv::Mat mahalanobis_distance(const TrajectoryPtrPool &a, const TrajectoryPool &b)
{
    std::vector<cv::Mat> means(a.size());
    std::vector<cv::Mat> icovariances(a.size());
 
    for (size_t i = 0; i < a.size(); ++i)
    {
        cv::Mat covariance;
        a[i]->project(means[i], covariance);
        cv::invert(covariance, icovariances[i]);
    }
    
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Mat x(b[j].xyah);
            float dist = static_cast<float>(cv::Mahalanobis(x, means[i], icovariances[i]));
            distsi[j] = dist * dist;
        }
    }
    
    return dists;
}

static inline float calc_inter_area(const cv::Vec4f &a, const cv::Vec4f &b)
{
    if (a[2] < b[0] || a[0] > b[2] || a[3] < b[1] || a[1] > b[3])
        return 0.f;
    
    float w = std::min(a[2], b[2]) - std::max(a[0], b[0]);
    float h = std::min(a[3], b[3]) - std::max(a[1], b[1]);
    return w * h;
}

cv::Mat iou_distance(const TrajectoryPool &a, const TrajectoryPool &b)
{
    std::vector<float> areaa(a.size());
    for (size_t i = 0; i < a.size(); ++i)
    {
        float w = a[i].ltrb[2] - a[i].ltrb[0];
        float h = a[i].ltrb[3] - a[i].ltrb[1];
        areaa[i] = w * h;
    }
    
    std::vector<float> areab(b.size());
    for (size_t j = 0; j < b.size(); ++j)
    {
        float w = b[j].ltrb[2] - b[j].ltrb[0];
        float h = b[j].ltrb[3] - b[j].ltrb[1];
        areab[j] = w * h;
    }
    
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        const cv::Vec4f &boxa = a[i].ltrb;
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Vec4f &boxb = b[j].ltrb;
            float inters = calc_inter_area(boxa, boxb);
            distsi[j] = 1.f - inters / (areaa[i] + areab[j] - inters);
        }
    }
    
    return dists;
}

cv::Mat iou_distance(const TrajectoryPtrPool &a, const TrajectoryPtrPool &b)
{
    std::vector<float> areaa(a.size());
    for (size_t i = 0; i < a.size(); ++i)
    {
        float w = a[i]->ltrb[2] - a[i]->ltrb[0];
        float h = a[i]->ltrb[3] - a[i]->ltrb[1];
        areaa[i] = w * h;
    }
    
    std::vector<float> areab(b.size());
    for (size_t j = 0; j < b.size(); ++j)
    {
        float w = b[j]->ltrb[2] - b[j]->ltrb[0];
        float h = b[j]->ltrb[3] - b[j]->ltrb[1];
        areab[j] = w * h;
    }
    
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        const cv::Vec4f &boxa = a[i]->ltrb;
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Vec4f &boxb = b[j]->ltrb;
            float inters = calc_inter_area(boxa, boxb);
            distsi[j] = 1.f - inters / (areaa[i] + areab[j] - inters);
        }
    }
    
    return dists;
}

cv::Mat iou_distance(const TrajectoryPtrPool &a, const TrajectoryPool &b)
{
    std::vector<float> areaa(a.size());
    for (size_t i = 0; i < a.size(); ++i)
    {
        float w = a[i]->ltrb[2] - a[i]->ltrb[0];
        float h = a[i]->ltrb[3] - a[i]->ltrb[1];
        areaa[i] = w * h;
    }
    
    std::vector<float> areab(b.size());
    for (size_t j = 0; j < b.size(); ++j)
    {
        float w = b[j].ltrb[2] - b[j].ltrb[0];
        float h = b[j].ltrb[3] - b[j].ltrb[1];
        areab[j] = w * h;
    }
    
    cv::Mat dists(a.size(), b.size(), CV_32F);
    for (size_t i = 0; i < a.size(); ++i)
    {
        const cv::Vec4f &boxa = a[i]->ltrb;
        float *distsi = dists.ptr<float>(i);
        for (size_t j = 0; j < b.size(); ++j)
        {
            const cv::Vec4f &boxb = b[j].ltrb;
            float inters = calc_inter_area(boxa, boxb);
            distsi[j] = 1.f - inters / (areaa[i] + areab[j] - inters);
        }
    }
    
    return dists;
}

}   // namespace PaddleDetection