#pragma once
#include "Ctracker.h"

#include <iostream>
#include <vector>
#include "dataReader.h"

bool InitTracker(cv::UMat frame, CTracker& tracker)
{
    TrackerSettings settings;
    settings.SetDistance(tracking::DistRects);
    settings.m_kalmanType = tracking::KalmanLinear;
    settings.m_filterGoal = tracking::FilterRect;
    settings.m_lostTrackType = tracking::TrackNone;       // Use visual objects tracker for collisions resolving
    settings.m_matchType = tracking::MatchHungrian;
    settings.m_useAcceleration = false;                   // Use constant acceleration motion model
    settings.m_dt = settings.m_useAcceleration ? 0.05f : 0.2f; // Delta time for Kalman filter
    settings.m_accelNoiseMag = 0.2f;                  // Accel noise magnitude for Kalman filter
    settings.m_distThres = 0.95f;                    // Distance threshold between region and object on two frames
#if 1
    settings.m_minAreaRadiusPix = frame.rows / 20.f;
#else
    settings.m_minAreaRadiusPix = -1.f;
#endif
    settings.m_minAreaRadiusK = 0.8f;

    settings.m_useAbandonedDetection = true;
    if (settings.m_useAbandonedDetection)
    {
        settings.m_minStaticTime = 5;
        settings.m_maxStaticTime = 10;
        settings.m_maximumAllowedSkippedFrames = 30; // Maximum allowed skipped frames
        settings.m_maxTraceLength = 60;        // Maximum trace length
    }
    else
    {
        settings.m_maximumAllowedSkippedFrames = 30; // Maximum allowed skipped frames
        settings.m_maxTraceLength = 60;              // Maximum trace length
    }

    tracker.setSettings(settings);

    return true;
}
void Track(cv::Mat frame, const regions_t& regions, CTracker& tracker)
{
    cv::UMat uframe;
    if (tracker.CanColorFrameToTrack())
    {
        uframe = frame.getUMat(cv::ACCESS_READ);
    }
    else
    {
        cv::cvtColor(frame, uframe, cv::COLOR_BGR2GRAY);
    }

    tracker.Update(regions, uframe, 30);
}
// ----------------------------------------------------------------------
void Tracking(cv::Mat frame, std::vector<Tn::Bbox>& boxes, CTracker& tracker)
{
    regions_t regions;
    for(int i=0;i<boxes.size();i++)
    {
        int lt_x = boxes[i].left;
        int lt_y = boxes[i].top;
        int w = abs(boxes[i].right-boxes[i].left);
        int h = abs(boxes[i].bot-boxes[i].top);
        cv::Rect rect(lt_x, lt_y, w>0?w:w+1,h>0?h:h+1);
        CRegion region = CRegion(rect,std::to_string(boxes[i].classId),boxes[i].score);
        regions.push_back(region);
        //std::cout<<"l,r,t,b:"<<boxes[i].left<<","<<boxes[i].right<<","<<boxes[i].top<<","<<boxes[i].bot<<std::endl;
    }
    Track(frame, regions,tracker);
    auto tracks = tracker.GetTracks();

    for (size_t i = 0; i < tracks.size(); i++)
    {
        const auto& track = tracks[i];
        int obj_id = track.m_regionID;
        if(obj_id != -1)
        {
            boxes[obj_id].id = track.m_ID;
//            cv::RotatedRect rrect = track.m_rrect;
//            Point_t c = rrect.center;
//            int w = rrect.size.width;
//            int h = rrect.size.height;
//            boxes[obj_id].left = c.x - w / 2;
//            boxes[obj_id].right = c.x + w / 2;
//            boxes[obj_id].top = c.y - h / 2;
//            boxes[obj_id].bot = c.y + h / 2;
            boxes[obj_id].speedx = track.m_velocity[0];//x
            boxes[obj_id].speedy = track.m_velocity[1];//y
        }

    }
//    for (size_t i = 0; i < boxes.size(); i++)
//    {
//        std::cout<<"id:"<<boxes[i].id<<std::endl;
//        std::cout<<"speed:"<<boxes[i].speedx<<","<<boxes[i].speedy<<","<<std::endl;
//    }
    regions.clear();
    tracks.clear();
}