#include "ivpark_simple.h"

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

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<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);
                xvectortype.push_back(1);
            }
        }
        if(xpark.mnParkType == 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;
    }

    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;
    }



    return true;
}