#include "ivpark_simple.h"


#include "adc_tools/compute_00.h"
#include "adc_tools/transfer.h"

#include <chrono>

using namespace  iv;

ivpark_simple::ivpark_simple()
{

}

bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
{
    GPS_INS nowgps,aimgps;
    std::vector<std::vector<GPS_INS>> xvectordTPoint;
    std::vector<double> xvectorRearDis;
    std::vector<double> xvectorAngle;
    std::vector<int> xvectortype;
    std::vector<GPS_INS> xvectoraimgps;
    std::vector<iv::simpleparkspace> xvectorsimpleparkspace = GetParkSpace();
    xvectordTPoint.clear();
    xvectorRearDis.clear();
    xvectorAngle.clear();
    unsigned int i;
    unsigned int nsize = static_cast<unsigned int >(xvectorsimpleparkspace.size());
    nowgps.gps_lat = fLat;
    nowgps.gps_lng = fLon;
    nowgps.ins_heading_angle = fHeading;
    for(i=0;i<nsize;i++)
    {
        iv::simpleparkspace xpark = xvectorsimpleparkspace[i];
        iv::GPS_INS aimgps;
        aimgps.gps_lat = xpark.mfLat;
        aimgps.gps_lng = xpark.mfLon;
        aimgps.ins_heading_angle = xpark.mfHeading;
        std::vector<GPS_INS> TPoints;
        double fRearDis,fAngle;
        if(xpark.mnParkType == 1)  //side park
        {
            if(iv::decition::Compute00().bocheDirectCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
            {
                xvectordTPoint.push_back(TPoints);
                xvectorAngle.push_back(fAngle);
                xvectorRearDis.push_back(fRearDis);
                xvectoraimgps.push_back(aimgps);
                xvectortype.push_back(1);
            }
        }
        if(xpark.mnParkType == 0)
        {
            if(iv::decition::Compute00().bocheCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
            {
                xvectordTPoint.push_back(TPoints);
                xvectorAngle.push_back(fAngle);
                xvectorRearDis.push_back(fRearDis);
                xvectoraimgps.push_back(aimgps);
                xvectortype.push_back(0);
            }
        }
    }

    nsize = static_cast<unsigned int >(xvectordTPoint.size());

    if(nsize<1)
    {
        return false;
    }

    unsigned int nsel = 0;
    //Select rear dis >1.0 and small
    for(i =1;i<nsize;i++)
    {
        if((xvectorRearDis[i] < xvectorRearDis[nsel])&&(xvectorRearDis[i]>1.0))
        {
            nsel = i;
        }
        else
        {
            if(xvectorRearDis[i]<1.0)
            {
                if(xvectorRearDis[nsel]>5.0)
                {
                    nsel = i;
                }
            }
            else
            {
                if((xvectorRearDis[i]<3.0)&&(xvectorRearDis[nsel]<0.5))
                {
                    nsel = i;
                }
            }
        }
    }

    if(xvectortype[nsel] == 0)
    {
        iv::decition::Compute00().nearTpoint = xvectordTPoint[nsel].at(0);
        iv::decition::Compute00().farTpoint = xvectordTPoint[nsel].at(1);
        iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
        iv::decition::Compute00().nParkType = 0;
        maimgps = xvectoraimgps[nsel];
    }

    if(xvectortype[nsel] == 1)
    {
        iv::decition::Compute00().dTpoint0 = xvectordTPoint[nsel].at(0);
        iv::decition::Compute00().dTpoint1 = xvectordTPoint[nsel].at(0);
        iv::decition::Compute00().dTpoint2 = xvectordTPoint[nsel].at(0);
        iv::decition::Compute00().dTpoint3 = xvectordTPoint[nsel].at(0);
        iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
        iv::decition::Compute00().nParkType = 1;
        maimgps = xvectoraimgps[nsel];
    }



    GaussProjCal(maimgps.gps_lng,maimgps.gps_lat,&maimgps.gps_x,&maimgps.gps_y);
    return true;
}

double ivpark_simple::GetDistance(iv::GPS_INS p1,iv::GPS_INS p2)
{
    return sqrt(pow(p1.gps_x - p2.gps_x,2)+pow(p1.gps_y-p2.gps_y,2));
}



int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
{
    (void)fHeading;
    if(bbocheMode == false)
    {
        return 0; //Not in boche mode
    }

    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 1000; //every stop time is 3

    if(xvehstate!=dRever &&  xvehstate!=dRever0  &&  xvehstate!=dRever1 && xvehstate!=dRever2
                    &&  xvehstate!=dRever3 && xvehstate!=dRever4 &&  xvehstate!=reverseArr
                    && xvehstate!=reverseCar &&  xvehstate!=reversing  &&  xvehstate!=reverseCircle &&  xvehstate!=reverseDirect)
    {
        if(fSpeed>0.3)
        {
            fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
            fAcc = -0.5;
            fWheel = 0.0;
            nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
        }
        else
        {
            fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
            fAcc = -0.5;
            fWheel = 0.0;
            nshift = 2; //rear
            int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
            if(abs(nnow - nstoptiming_ms) >= nstoptime_ms)
            {
                if(iv::decition::Compute00().nParkType == 0)
                {
                    xvehstate = reversing;
                    mlastvehstate = xvehstate;
                }
                else
                {
                    xvehstate =  dRever;
                    mlastvehstate = xvehstate;
                }
            }


        }
        return 1;
    }

    iv::GPS_INS nowgps;
    nowgps.gps_lat = fLat;
    nowgps.gps_lng = fLon;
    GaussProjCal(nowgps.gps_lng,nowgps.gps_lat,&nowgps.gps_x,&nowgps.gps_y);

    switch (xvehstate) {
    case reversing:
        reversingcarFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case reverseCircle:
        reverseCircleFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case reverseDirect:
        reverseDirectFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case reverseArr:
        reverseArrFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case dRever0:
        dRever0Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case dRever1:
        dRever1Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case dRever2:
        dRever2Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    case dRever3:
        dRever3Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
        break;
    default:
        return 0;

    }

    return 0;

}

void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{

    mlastvehstate = xvehstate;

    double fdistonear = sqrt(pow(nowgps.gps_x - iv::decition::Compute00().nearTpoint.gps_x,2)+pow(nowgps.gps_y - iv::decition::Compute00().nearTpoint.gps_y,2));

    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);

    Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().nearTpoint.gps_x,iv::decition::Compute00().nearTpoint.gps_y, maimgps);

    fdistonear = fabs(pt.x - ptnear.x);

    nshift = 2;
    if(fdistonear>1.0)
    {
        fAcc = 0.0;  //acc calcutale by pid
        fWheel = 0.0;
        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;

    }
    else
    {
        if((fSpeed>0.3)&&(fdistonear>0.3))
        {
            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
        }
        else
        {
                fAcc = -0.5;
                xvehstate = reverseCircle;
        }
    }



    return;
}

void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{

    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.05);
        mCircleWheel = fWheel;
        return;

    }

    fWheel = mCircleWheel;

    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
    double angdis =fabs(nowgps.ins_heading_angle - maimgps.ins_heading_angle);

    if((fabs(pt.x)<2.0)&&(((angdis<5)||(angdis>355))))
    {
        xvehstate = reverseDirect;
        fAcc = -0.5;
        fdSpeed = 0.0;
        fWheel = 0.0;
        fdSecSpeed = 0.0;
    }
    else
    {
        fAcc = 0.0;
        fdSpeed = 2;
        fdSecSpeed = fdSecSpeed / 3.6;
    }

    return;

}

void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{
    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = 0.0;
        return;

    }

    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);

    if(pt.y<0.5)
    {
        xvehstate = reverseArr;
        fAcc = -0.5;
        fdSpeed = 0.0;
        fWheel = 0.0;
        fdSecSpeed = 0.0;
    }
    else
    {
        fAcc = 0.0;
        fdSpeed = 2;
        fdSecSpeed = fdSecSpeed / 3.6;
        fWheel = 0.0;
    }

    return;
}

void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{
    (void)nowgps;
    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = 0.0;
        return;

    }

    nshift = 1; //P shift

    fdSpeed = 0;
    fdSecSpeed = 0;
    fAcc = -0.5;
    fWheel = 0.0;

    xvehstate = normalRun;
}

void ivpark_simple::dRever0Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{

    mlastvehstate = xvehstate;

    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);

    Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);

    double fdistonear = fabs(pt.y - ptnear.y);

    nshift = 2;
    if(fdistonear>1.0)
    {
        fAcc = 0.0;  //acc calcutale by pid
        fWheel = 0.0;
        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;

    }
    else
    {
        if((fSpeed>0.3)&&(fdistonear>0.3))
        {
            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
            fdSpeed = 0.0;
            fdSecSpeed = fdSpeed/3.6;
        }
        else
        {
                fAcc = -0.5;
                xvehstate = dRever2;
                fdSpeed = 0.0;
                fdSecSpeed = fdSpeed/3.6;
        }
    }



    return;
}

void ivpark_simple::dRever1Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{
    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.0);
        mCircleWheel = fWheel;
        return;

    }

    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);

    double fdis = fabs(pt1.x - pt2.x);

    if(fdis>1.0)
    {
        fAcc = 0.0;  //acc calcutale by pid

        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
    }
    else
    {
        if((fSpeed>0.3)&&(fdis>0.3))
        {
            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdis);
            fdSpeed = 0.0;
            fdSecSpeed = fdSpeed/3.6;
        }
        else
        {
                fAcc = -0.5;
                fdSpeed = 0.0;
                fdSecSpeed = fdSpeed/3.6;
                xvehstate = dRever2;
        }
    }
    fWheel = mCircleWheel;
}

void ivpark_simple::dRever2Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{
    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = 0;
        return;

    }

    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, maimgps);

    double fdis = pt1.x - pt2.x;

    if(fdis>-1.0)
    {
        fAcc = 0.0;  //acc calcutale by pid

        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
    }
    else
    {
        if((fSpeed>0.3)&&(fabs(fdis)>0.3))
        {
            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
            fdSpeed = 0.0;
            fdSecSpeed = fdSpeed/3.6;
        }
        else
        {
                fAcc = -0.5;
                fdSpeed = 0.0;
                fdSecSpeed = fdSpeed/3.6;
                xvehstate = dRever3;
        }
    }
    fWheel = 0;
}

void ivpark_simple::dRever3Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
{
    (void)fSpeed;
    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    int64_t nstoptime_ms = 3000; //every stop time is 3

    if(mlastvehstate != xvehstate)
    {
        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    }

    mlastvehstate = xvehstate;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;


    nshift = 2;

    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
    {
        fdSpeed = 0;
        fdSecSpeed = 0;
        fAcc = -0.5;
        fWheel = iv::decition::Compute00().bocheAngle*16.5 *1.0;
        mCircleWheel = fWheel;
        return;

    }

    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint3.gps_x,iv::decition::Compute00().dTpoint3.gps_y, maimgps);

    double fdis = fabs(pt1.x - pt2.x);

    if(fdis>1.0)
    {
        fAcc = 0.0;  //acc calcutale by pid

        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
    }
    else
    {
        if((fSpeed>0.3)&&(fabs(fdis)>0.3))
        {
            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
            fdSpeed = 0.0;
            fdSecSpeed = fdSpeed/3.6;
        }
        else
        {
                fAcc = -0.5;
                fdSpeed = 0.0;
                fdSecSpeed = fdSpeed/3.6;
                xvehstate = reverseDirect;
        }
    }
    fWheel = mCircleWheel;
}