#include "usg_common.h"
#include "IndirectAlg.h"
#include "graph.h"
 
#define PI 3.14159265
 
using namespace std;


struct Status {
 
    // target id
    std::string id;
    //第一次出现的时间
    time_t firstTime;
    //上次出现的时间
    time_t lastTime;
    //第一次逆行的时间
    time_t firstInvalTime;
    //上次逆行的时间
    time_t lastInvalTime;
    //上一次出现的坐标
    Point lastCoordinate;
    // Status(std::string _id, time_t ftime, time_t ltime, Point coordinate): 
    //     id(_id), firstInvalTime(ftime), lastInvalTime(ltime), lastCoordinate(coordinate){}
};


IndirectAlg::IndirectAlg() {}

IndirectAlg::IndirectAlg(const initializer_list<Edge> & edges, const time_t & _keepTime, const Vector2 & _direction): 
    frame(edges), keepTime(_keepTime), direction(_direction), mterminate(false) {

    mthread = std::thread(std::bind(&IndirectAlg::threadRoutine, this));

}

IndirectAlg::IndirectAlg(const initializer_list<Point> & points, const time_t & _keepTime, const Vector2 & _direction): 
    keepTime(_keepTime), direction(_direction), mterminate(false) {
    std::vector<Edge> edges;
    initializer_list<Point>::iterator iter;
    for(iter = points.begin(); iter < points.end(); iter++ ) {
        edges.push_back({*iter, *(iter +1)});
    }
    edges.push_back({*(points.end()), *(points.begin())});
    frame = Figure(edges);

    mthread = std::thread(std::bind(&IndirectAlg::threadRoutine, this));

}

IndirectAlg::~IndirectAlg() {
    //销毁定时线程
    mterminate.store(true);
    mthread.join();
    //销毁map
    map<string, Status *>::iterator iter; 
    Status *status = nullptr;
    for(iter = statusMap.begin(); iter != statusMap.end(); iter++) {
        //cout<<iter->first<<' '<<iter->second<<endl; 
        status = iter->second;
        if (status != nullptr) {
            delete status;
            statusMap.erase(iter);
        }
    }
}

void IndirectAlg::threadRoutine() {
    //定时清理不在监控区的对象
    //sleep(this->keepTime);
    map<string, Status *>::iterator iter; 
    Status *status = nullptr;
    while (!mterminate.load()) {
       // err_msg(0, "===========thread==========run" );
        std::this_thread::sleep_for(std::chrono::seconds(this->keepTime));
        //sleep(this->keepTime);
        for(iter = statusMap.begin(); iter != statusMap.end(); iter++) {
            //cout<<iter->first<<' '<<iter->second<<endl; 
            status = iter->second;
            if (status != nullptr) {
                if(time_t(0) - status->lastTime > this->keepTime) {
                    delete status;
                    statusMap.erase(iter);
                }
            }
        }
    }
     
}

void IndirectAlg::printRecord(int tag, Record &record) {
     printf("%d  %ld : {%f, %f}\n", tag, record.timestamp, record.coordinate.x, record.coordinate.y);
}

/**
 * 是否逆行
*/
bool IndirectAlg::isRetrograde(Record &record) {
    if(record.id.length() == 0) {
        return false;
    }

    Status *status = nullptr;

    map<string, Status *>::iterator statusIter = statusMap.find(record.id);
    if( statusIter != statusMap.end() ) {
     status = statusIter->second;
    }

    if (status == nullptr) {
        // 第一次出现
        if(frame.contains(record.coordinate)) {
            status = new Status({ 
                .id = record.id, 
                .firstTime = record.timestamp, 
                .lastTime = record.timestamp,
                .firstInvalTime = 0,
                .lastInvalTime = 0,
                .lastCoordinate = record.coordinate});

            statusMap.insert({record.id, status});
            printRecord(1, record);
        }
        printRecord(2, record);
        return false;
    }

     
    status->lastTime = record.timestamp;

    Vector2 currentDirection = getVecor2(status -> lastCoordinate, record.coordinate);
    if (currentDirection.x == 0 && currentDirection.y == 0) {
        //没动
        printRecord(3, record);
        return false;
    }
    status -> lastCoordinate = record.coordinate;
    //逆行判断 cos小于0即两个向量的夹角大于90度,表示逆行了
    if(frame.contains(record.coordinate) && getVector2Angle(direction, currentDirection) <= 0) {
        if(status->firstInvalTime ==0) {
            //第一次发生逆行
            status->firstInvalTime = status->lastInvalTime = record.timestamp;
            printRecord(4, record);
            return false;
            
        } else {
            status->lastInvalTime = record.timestamp;
            //逆行时间超过keeptime
            if (status->lastInvalTime - status-> firstInvalTime > keepTime) {
                printRecord(5, record);
                return true;
            } 
            printRecord(6, record);
            return false;
        }
    } else {
        printRecord(7, record);
        status->firstInvalTime = status->lastInvalTime = 0;
        return false;
    }
     
}