#include "ivscheduler.h"

#include <iostream>


static ivscheduler * givs;

static QMutex gMutexGPSIMU;

static iv::gps::gpsimu ggpsimu;
static bool gbUpdate = false;

class xodrobj
{
public:
    double flatsrc;
    double flonsrc;
    double fhgdsrc;
    double flat;
    double flon;
    int lane;
};


void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//        ggpsimu->UpdateGPSIMU(strdata,nSize);

    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"Listengpsimu Parse error."<<std::endl;
        return;
    }
    (void)&index;
    (void)dt;
    (void)strmemname;

    gMutexGPSIMU.lock();
    ggpsimu.CopyFrom(xgpsimu);
    gbUpdate = true;
    gMutexGPSIMU.unlock();
}

void Listenscheduler(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//        ggpsimu->UpdateGPSIMU(strdata,nSize);

    iv::scheduler pscheduler;
    if(!pscheduler.ParseFromArray(strdata,nSize))
    {
        std::cout<<"Listenscheduler Parse error."<<std::endl;
        return;
    }

    (void)&index;
    (void)dt;
    (void)strmemname;

    givs->setscheduler(&pscheduler);

}

ivscheduler::ivscheduler(std::string strmemname,std::string strschedulername)
{
    givs = this;
    mstrmemname = strmemname;
    mstrschedulername = strschedulername;
    iv::modulecomm::RegisterRecv(mstrmemname.data(),Listengpsimu);
    iv::modulecomm::RegisterRecv(mstrschedulername.data(),Listenscheduler);
    mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);
}

void ivscheduler::run()
{
    iv::gps::gpsimu xgpsimu;
    while(!QThread::isInterruptionRequested())
    {
        if(gbUpdate)
        {
            gMutexGPSIMU.lock();
            xgpsimu.CopyFrom(ggpsimu);
            gbUpdate = false;
            gMutexGPSIMU.unlock();
            emit updategps(xgpsimu.lon(),xgpsimu.lat(),xgpsimu.heading());
            if(mpscheduler == 0)continue;
            mMutex.lock();
            if(mncycle<mncyclecount)
            {
                if(mnprocess<mpscheduler->mscheduler_unit_size())
                {
                    switch (mnstep) {
                    case 0:
                    {
                        mfLatInit = xgpsimu.lat();
                        mfLonInit = xgpsimu.lon();
                        mfHeadingInit = xgpsimu.heading();
                        mnTimeInit = QDateTime::currentMSecsSinceEpoch();

                        //Sending Obj;
                        iv::scheduler_unit * pscheduler_unit = mpscheduler->mutable_mscheduler_unit(mnprocess);
                        mfLatObj = pscheduler_unit->mflat();
                        mfLonObj = pscheduler_unit->mflon();
                        mfHeadingObj = pscheduler_unit->mfheading();
                        mnLastSendObj = QDateTime::currentMSecsSinceEpoch();
                        SendObj(mfLonObj,mfLatObj);
                        mnstep = 1;
                        emit updatestep(mnstep);
                    }
                        break;
                    case 1:
                        if(IsVehicleMoving(&xgpsimu))
                        {
                            mnstep = 2;
                            emit updatestep(mnstep);
                        }
                        else
                        {
                            if((QDateTime::currentMSecsSinceEpoch() - mnLastSendObj) > 3000)
                            {
                                //Send Obj
                                SendObj(mfLonObj,mfLatObj);
                                mnLastSendObj = QDateTime::currentMSecsSinceEpoch();
                            }
                        }
                        if(IsVehcileArrivingStation(&xgpsimu))
                        {
                            mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
                            mnstep = 3;
                            emit updatestep(mnstep);
                        }
                        //Decide Vechicle Running.
                        break;
                    case 2:
                        if(IsVehcileArrivingStation(&xgpsimu))
                        {
                            mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
                            mnstep = 3;
                            emit updatestep(mnstep);
                        }
                        //Decide Vehiclde Arrive End Point;
                        break;
                    case 3:
                    {
                        iv::scheduler_unit * pscheduler_unit = mpscheduler->mutable_mscheduler_unit(mnprocess);
                        if((QDateTime::currentMSecsSinceEpoch() - mnArrivingTime)>= (pscheduler_unit->mstopsecond()*1000) )
                        {
                            mnstep = 4;
                            emit updatestep(mnstep);
                        }
                    }
                        //Count Stop Time;
                        break;
                    case 4:
                        mnstep = 0;
                        emit updatestep(mnstep);
                        mnprocess++;
                        break;
                    default:
                        break;
                    }
                }
                else
                {
                    mncycle++;
                    mnprocess = 0;
                }
            }
            emit updatestate(mncycle,mncyclecount,mnprocess,mpscheduler->mscheduler_unit_size());
            mMutex.unlock();


        }
        else
        {
            msleep(10);
        }
    }
}

void ivscheduler::setscheduler(iv::scheduler *pscheduler)
{
    mMutex.lock();
    if(mpscheduler != 0)delete mpscheduler;
    mpscheduler = new iv::scheduler;
    mpscheduler->CopyFrom(*pscheduler);
    mncyclecount = pscheduler->mcyble();
    mncycle = 0;
    mnprocess = 0;
    mnstep = 0;
    mMutex.unlock();

}

void ivscheduler::GetProcess(int &nProc, int &nProcTotal)
{
    if(mpscheduler == 0)
    {
        nProc = 0;
        nProcTotal = 0;
        return;
    }

    mMutex.lock();
    nProcTotal = mpscheduler->mscheduler_unit_size();
    nProc = mnprocess;
    mMutex.unlock();
}

void ivscheduler::GetCycle(int &ncycle, int &ncyclecount)
{
    ncycle = mncycle;
    ncyclecount = mncyclecount;

}

bool ivscheduler::IsVehicleMoving(iv::gps::gpsimu * pgpsimu)
{
    double dis;
    double x1,y1;
    double x2,y2;
    GaussProjCal(mfLonInit,mfLatInit,&x1,&y1);
    GaussProjCal(pgpsimu->lon(),pgpsimu->lat(),&x2,&y2);
    dis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));
    if(dis > 10.0)return true;

    return false;
}

bool ivscheduler::IsVehcileArrivingStation(iv::gps::gpsimu *pgpsimu)
{
    double dis;
    double x1,y1;
    double x2,y2;
    GaussProjCal(mfLonObj,mfLatObj,&x1,&y1);
    GaussProjCal(pgpsimu->lon(),pgpsimu->lat(),&x2,&y2);
    dis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));
    if(dis < 10.0)
    {
        double fheaddiff = pgpsimu->heading() - mfHeadingObj;
        if(fheaddiff< 0)fheaddiff = fheaddiff + 360.0;
        if((fheaddiff>300)||(fheaddiff<60))
            return true;
    }

    return false;
}

void ivscheduler::SendObj(double flon, double flat)
{
    xodrobj xo;
    xo.flon = flon;
    xo.flat = flat;
    xo.lane = 3;

    iv::modulecomm::ModuleSendMsg(mpadst,(char *)&xo,sizeof(xodrobj));
}