#include "ParkDetProc.h"

using namespace std;
using namespace cv;

extern std::shared_ptr<spdlog::logger> kylog;

ParkDetProc::ParkDetProc(void) 
{
    status = 0;
    adjestStatus = -1;
    memset(roadName, 0, sizeof(roadName));
    memset(startParkid, 0, sizeof(startParkid));
    memset(endParkid, 0, sizeof(endParkid));
    memset(weightPathFront, 0, sizeof(weightPathFront));
    memset(weightPathBack, 0, sizeof(weightPathBack));
    memset(classesidTxtPath, 0, sizeof(classesidTxtPath));

    fXInMapStart = 0;
    fYInMapStart = 0;
    fXInMapEnd = 0;
    fYInMapEnd = 0;

    pYoloFront = nullptr;
    pYoloBack = nullptr;

    pthread_mutex_init(&mutex, NULL);
}

ParkDetProc::~ParkDetProc() 
{

}

void ParkDetProc::InitParkidStr(void)
{
    char buf[64];
    FILE *fp = fopen(this->classesidTxtPath, "r");
    while (!feof(fp))
    {
        int id;
        char parkid[32];
        int park;

        if (!fgets(buf, 64, fp)) // read one line
        {
            continue;
        }

        sscanf(buf, "%d: %s", &id, parkid);
        // printf("%d %s\r\n", id, parkid);

        park = atoi(parkid);
        this->parkClasses.push_back(park);
    }
    fclose(fp);
}

int ParkDetProc::GetParkid(int id)
{
    int ret = -1;

    if (id >= 0 && id < this->parkClasses.size())
    {
        ret = this->parkClasses[id];
    }

    return ret;
}

void ParkDetProc::checkStatus(void)
{
    if(this->status != this->adjestStatus && this->adjestStatus != -1)
    {
        kylog->info("start parkid = {}, ori status = {}, adjest to {}", this->startParkid,
                this->status, this->adjestStatus);

        if(this->status == 0 && this->adjestStatus > 0)
        {
            if(this->pYoloFront != nullptr)
            {
                delete this->pYoloFront;
                this->pYoloFront = nullptr;
            }
            this->pYoloFront = new Yolo();
            this->pYoloFront->Init(this->weightPathFront);

            if(this->pYoloBack != nullptr)
            {
                delete this->pYoloBack;
                this->pYoloBack = nullptr;
            }
            this->pYoloBack = new Yolo();
            this->pYoloBack->Init(this->weightPathBack);

            this->parkClasses.clear();
            this->parkClasses.shrink_to_fit();
            this->InitParkidStr();

            this->status = this->adjestStatus;
            this->adjestStatus = -1;

            kylog->info("{} start!", this->startParkid);
        }
        else if(this->status >= 1 && this->adjestStatus == 0)
        {
            kylog->info("{} prepare stop!", this->startParkid);
            if(this->pYoloFront != nullptr)
            {
                delete this->pYoloFront;
                this->pYoloFront = nullptr;
            }

            if(this->pYoloBack != nullptr)
            {
                delete this->pYoloBack;
                this->pYoloBack = nullptr;
            }

            kylog->info("{} before clear vector!", this->startParkid);
            this->parkClasses.clear();
            this->parkClasses.shrink_to_fit();
            kylog->info("{} after clear vector!", this->startParkid);

            this->status = this->adjestStatus;
            this->adjestStatus = -1;

            kylog->info("{} stop!", this->startParkid);
        }
        else if(this->status == 2 && this->adjestStatus == 1)
        {
            this->status = this->adjestStatus;
            this->adjestStatus = -1;

            kylog->info("{} to main detect!", this->startParkid);
        }
        else 
        {
            kylog->info("illegal status! status = {}, adjestStatus = {}", this->status
                    , this->adjestStatus);
            this->adjestStatus = -1;
        }
    }
}

int ParkDetProc::YoloParkDet(cv::Mat &img, int &number, Yolo *yolo)
{
    int ret = -1;
    int hisMaxNum = -1;
    int hisid = -1;
    int maxNum = 1;
    int maxid = 0;

    std::vector<YoloBox> parkBoxes;

    parkBoxes = yolo->DetectWithTimeOutMs(img, 500);
    if (parkBoxes.size() > 0)
    {
        std::vector<int> p;
        for (int a = 0; a < parkBoxes.size(); a++)
        {
            p.push_back(parkBoxes[a].id);
        }

        std::sort(p.begin(), p.end());

        maxid = this->GetParkid(p[0]);
        hisid = maxid;
        for (int a = 1; a < p.size(); a++)
        {
            int tempParkid = this->GetParkid(p[a]);

            int t = tempParkid;
            printf("id = %d\r\n", tempParkid);
            kylog->info("id = {}", t);
            if (maxid != tempParkid)
            {
                if (maxNum > hisMaxNum)
                {
                    hisMaxNum = maxNum;

                    hisid = maxid;
                    maxid = tempParkid;
                    maxNum = 1;
                }
                else
                {
                    maxid = tempParkid;
                    maxNum = 1;
                }
            }
            else
            {
                maxNum++;
                if (maxNum > hisMaxNum)
                {
                    hisMaxNum = maxNum;

                    hisid = maxid;
                    maxid = tempParkid;
                }
            }
        }

        int parkid = hisid;
        // kylog->info("find parkid = {}, maxNum = {}, {}", parkid, maxNum, hisMaxNum);

        if (hisMaxNum > 4 || maxNum > 4)
        {
            ret = parkid;
            number = hisMaxNum > maxNum ? hisMaxNum : maxNum;
            kylog->info("find parkid = {}, number = {}", parkid, number);
        }
        else
        {
            
        }
        p.clear();
    }
    parkBoxes.clear();
    parkBoxes.shrink_to_fit();

    return ret;
}

int ParkDetProc::YoloParkDetFront(cv::Mat &img, int &number)
{
    if(img.empty())
    {
        return -1;
    }
    return YoloParkDet(img, number, this->pYoloFront);
}

int ParkDetProc::YoloParkDetBack(cv::Mat &img, int &number)
{
    if(img.empty())
    {
        return -1;
    }
    return YoloParkDet(img, number, this->pYoloBack);
}

void ParkDetProc::lock(void)
{
    pthread_mutex_lock(&mutex);
}

bool ParkDetProc::trylock(void)
{
    int ret = pthread_mutex_trylock(&mutex);
    if(ret != 0)
    {
        return false;
    }
    else 
    {
        return true;
    }

}

void ParkDetProc::unlock(void)
{
    pthread_mutex_unlock(&mutex);
}