modularization/src/test/testsideparkcalc/sideparkcalc.cpp

#include "sideparkcalc.h"

#include "math.h"
#include <iostream>

SideParkCalc::SideParkCalc(double x,double y,double hdg,double fRadius,double MaxWheel ,double MaxAngle ,double fLastDirectDis )
{
    mfRaidus = fRadius;
    mfMaxWheel = MaxWheel;
    mfMaxAngle = MaxAngle * M_PI/180.0;
    mfLastDirectDis = fLastDirectDis;

    mx = x;
    my = y;
    mhdg =  hdg;
    normalhdg(mhdg);

    mSideParkType = SideParkType::NoSolution;
}


void SideParkCalc::CalcPark()
{
    if(mx< 0.5)
    {
        return;
    }

    if(fabs(mhdg) > M_PI/6.0)
    {
        return;
    }

    SideParkMode parkmode = CalcParkMode();

    if(parkmode == SideParkMode::ParkAtRight)
    {
        ParkAtRightCalc();
    }
    else
        ParkAtLeftCalc();
}

SideParkMode SideParkCalc::CalcParkMode()
{


    if(my>=0)return SideParkMode::ParkAtRight;

    return SideParkMode::ParkAtLeft;
}

void SideParkCalc::ParkAtRightCalc()
{
    if(mhdg<=0.0)
    {
        ParkAtRightCalc_Model1();
    }
    else
    {
        ParkAtRightCalc_Model2();
    }
}

void SideParkCalc::ParkAtRightCalc_Model1()
{
    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
    dy6 = 0;
    dx6 = mfLastDirectDis;
    //    double fMaxdx5 = mfRaidus * sin(mfMaxAngle );
    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));

    //    double fMaxdx3 = fMaxdx5;
    //    double fMaxdy3 = fMaxdy5;

    dx2 = mfRaidus * sin(fabs(mhdg));
    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));


    double fang ;//= mfMaxAngle;
    if((my+dy2) >= (2*fMaxdy5) )
    {
        fang = mfMaxAngle;
    }
    else
    {
        fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
    }

    dx5 = mfRaidus * sin(fang);
    dy5 = mfRaidus * (1.0 - cos(fang));

    dx3 = dx5;
    dy3 = dy5;

    double thetax = mx - dx6 - dx3 - dx5 - dx2;

    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
    {
        std::cout<<" divide error."<<std::endl;
        return;
    }
    dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
    dx1 = thetax - dx4;
    dy1 = dx1 * tan(fabs(mhdg));
    dy4 = my + dy1 +dy2 -dy3-dy5;

    std::cout<<" compute.  "<<std::endl;

    if(dx1>(-0.1)&&(dx4>(-0.1)))
    {
        mSideParkType = SideParkType::FiveStep;
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel * (-1.0));
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel);
        mvectorWheel.push_back(0.0);

        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
        x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
        x3 = x4 + dx5;y3 = y4 + dy5;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
        hdg2 = hdg3;
        if(dx4>0.01)
        {
            x2 = x3 + dx4;y2 = y3 + dy4;
            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
        }
        else
        {
            x2 = x3;y2 = y3;flen3 = 0;
        }

        flen2 = mfRaidus * (fang - mhdg);
        x1 = mx - dx1;
        y1 = my + dy1;
        hdg1 = mhdg;

        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));

        mvectorlen.push_back(flen1);
        mvectorlen.push_back(flen2);
        mvectorlen.push_back(flen3);
        mvectorlen.push_back(flen4);
        mvectorlen.push_back(flen5);
        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));


    }


    //    if((my+dy2) > (2*fMaxdy3) )
    //    {
    //        dy3 = fMaxdy3;
    //        dx3 = fMaxdx3;
    //        dx5 = fMaxdx5;
    //        dy5 = fMaxdy5;
    //        double thetax = mx - dx6 - fMaxdx3 - fMaxdx5 - dx2;
    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - fMaxdy3 - fMaxdy5)/(tan(fabs(mhdg ) + tan(mfMaxAngle )));
    //        dx1 = thetax - dx4;
    //        dy1 = dx1 * tan(fabs(mhdg));
    //        dy4 = my + dy1 + dy2 -dy3-dy5;

    //        std::cout<<" compute.  "<<std::endl;
    //    }
    //    else
    //    {
    //        double fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
    //        dx5 = mfRaidus * sin(fang);
    //        dy5 = mfRaidus * (1.0 - cos(fang));

    //        dx3 = dx5;
    //        dy3 = dy5;

    //        double thetax = mx - dx6 - dx3 - dx5 - dx2;
    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
    //        dx1 = thetax - dx4;
    //        dy1 = dx1 * tan(fabs(mhdg));
    //        dy4 = my + dy1 +dy2 -dy3-dy5;

    //        std::cout<<" compute.  "<<std::endl;
    //     }
}

void SideParkCalc::ParkAtRightCalc_Model2()
{
    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
    dy5 = 0;
    dx5 = mfLastDirectDis;

    bool bFirstCalc = true;

    double dx2fix = mfRaidus * sin(mhdg);
    double dy2fix = mfRaidus * (1.0 - cos(mhdg));

    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));

    double fang ;
    if((my+dy2fix) >= (2*fMaxdy4) )
    {
        fang = mfMaxAngle;
    }
    else
    {
        fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
    }

TwoCalc:
    dx4 = mfRaidus * sin(fang);
    dy4 = mfRaidus * (1.0 - cos(fang));

    dx2 = dx4 - dx2fix;
    dy2 = dy4 - dy2fix;

    double thetax = mx - dx2 -dx4 -dx5;

    double k1 = tan(mhdg);
    double k3 = tan(fang);

    if(fabs(k1-k3)<0.001)
    {
        dy1 = 0;
        dx1 = 0;
        dy3 = my - dy2 - dy4;
        if(k3 > 0.001)
        {
            dx3 = dy3/k3;
        }
        else
        {
            dx3 = 0;
        }

        if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
        {
            std::cout<<" no solve"<<std::endl;
            return;
        }
    }
    else
    {
        dx3 = (my - dy2 - dy4 - k1 * thetax)/(k3 - k1);
        dy3 = k3 * dx3;
        dx1 = thetax - dx3;
        dy1 = k1 * dx1;
    }

    if((dx3<-0.1) || (dx1<-0.1))
    {
        if((dx1>0.5)&&(bFirstCalc))
        {
            double x_b = mx - mfLastDirectDis;
            double y_b = my - 0;

            double k = tan(mhdg);
            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);

            double a = 1 +k*k;
            double b = 2*k*C *(-1.0);
            double c = C*C-1;
            double d = b*b - 4*a*c;
            if(d >= 0)
            {
                double xr1,xr2;
                xr1 = (-b + sqrt(d))/(2*a);
                xr2 = (-b - sqrt(d))/(2*a);
                double fa = 0;
                if((xr1>=0)&&(xr1<1))
                {
                    fa = asin(xr1);
                }
                else
                {
                    if((xr2>=0)&&(xr2<1))
                    {
                        fa = asin(xr2);
                    }
                }

                if((fa>0.001) && (fa<mfMaxAngle))
                {
                    fang = fa;
                    bFirstCalc = false;
                    goto TwoCalc;
                }
            }
        }
        else
        {
            if(cos(mhdg)> 0.001)
            {
                double fR = my/(1 - cos(mhdg));
                double Rx = mx - fR*sin(mhdg);
                if((Rx>0.3) && (fR >= mfRaidus))
                {
                    mSideParkType = SideParkType::TwoStep;
                    mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
                    mvectorWheel.push_back(0.0);

                    double flen1,flen2;
                    flen2 = Rx;
                    flen1 = fR*fabs(mhdg);
                    mvectorlen.push_back(flen1);
                    mvectorlen.push_back(flen2);
                    mfTotalLen = flen1 + flen2;
                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
                    std::cout<<" Have One Step soluton."<<std::endl;
                }
            }
        }
    }
    else
    {
        mSideParkType = SideParkType::FiveStep;
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel * (-1.0));
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel);
        mvectorWheel.push_back(0.0);

        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
        x4 = dx5;y4 = dy5;hdg4 = 0;flen5 = x4;
        x3 = x4 + dx4;y3 = y4 + dy4;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
        hdg2 = hdg3;
        if(dx3>0.1)
        {
            x2 = x3 + dx3;y2 = y3 + dy3;
            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
        }
        else
        {
            x2 = x3;y2 = y3;flen3 = 0;
        }

        flen2 = mfRaidus * (fang - mhdg);
        x1 = mx - dx1;
        y1 = my - dy1;
        hdg1 = mhdg;

        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));

        mvectorlen.push_back(flen1);
        mvectorlen.push_back(flen2);
        mvectorlen.push_back(flen3);
        mvectorlen.push_back(flen4);
        mvectorlen.push_back(flen5);
        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
        std::cout<<" have compute."<<std::endl;
    }

    std::cout<<" compute. "<<std::endl;
}

void SideParkCalc::ParkAtLeftCalc()
{
    if(mhdg>=0.0)
    {
        ParkAtLeftCalc_Model1();
    }
    else
    {
        ParkAtLeftCalc_Model2();
    }
}

void SideParkCalc::ParkAtLeftCalc_Model1()
{
    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
    dy6 = 0;
    dx6 = mfLastDirectDis;

    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));


    dx2 = mfRaidus * sin(fabs(mhdg));
    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));

 //   double x = mx;
    double y = fabs(my);

    double fang ;//= mfMaxAngle;
    if((y+dy2) >= (2*fMaxdy5) )
    {
        fang = mfMaxAngle;
    }
    else
    {
        fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
    }

    dx5 = mfRaidus * sin(fang);
    dy5 = mfRaidus * (1.0 - cos(fang));

    dx3 = dx5;
    dy3 = dy5;

    double thetax = mx - dx6 - dx3 - dx5 - dx2;

    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
    {
        std::cout<<" divide error."<<std::endl;
        return;
    }
    dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
    dx1 = thetax - dx4;
    dy1 = dx1 * tan(fabs(mhdg));
    dy4 = y + dy1 +dy2 -dy3-dy5;

    std::cout<<" compute.  "<<std::endl;

    if(dx1>(-0.1)&&(dx4>(-0.1)))
    {
        mSideParkType = SideParkType::FiveStep;
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel * (1.0));
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel * (-1.0));
        mvectorWheel.push_back(0.0);

        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
        x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
        x3 = x4 + dx5;y3 = y4 - dy5;hdg3 = fang * (-1);flen4 = fabs(fang) *mfRaidus;
        hdg2 = hdg3;
        if(dx4>0.1)
        {
            x2 = x3 + dx4;y2 = y3 - dy4;
            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
        }
        else
        {
            x2 = x3;y2 = y3;flen3 = 0;
        }

        flen2 = mfRaidus * (mhdg + fang);
        x1 = mx - dx1;
        y1 = my - dy1;
        hdg1 = mhdg;

        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));

        mvectorlen.push_back(flen1);
        mvectorlen.push_back(flen2);
        mvectorlen.push_back(flen3);
        mvectorlen.push_back(flen4);
        mvectorlen.push_back(flen5);
        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));


    }



}

void SideParkCalc::ParkAtLeftCalc_Model2()
{
    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
    dy5 = 0;
    dx5 = mfLastDirectDis;
    bool bFirstCalc = true;

    double dx2fix = mfRaidus * sin(fabs(mhdg));
    double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));

    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));

    double fang ;
    double y = fabs(my);
    if((y+dy2fix) >= (2*fMaxdy4) )
    {
        fang = mfMaxAngle;
    }
    else
    {
        fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
    }
TwoCalc:

    dx4 = mfRaidus * sin(fang);
    dy4 = mfRaidus * (1.0 - cos(fang));

    dx2 = dx4 - dx2fix;
    dy2 = dy4 - dy2fix;

    double thetax = mx - dx2 -dx4 -dx5;

    double k1 = tan(fabs(mhdg));
    double k3 = tan(fang);

    if(fabs(k1-k3)<0.001)
    {
        dy1 = 0;
        dx1 = 0;
        dy3 = y - dy2 - dy4;
        if(k3 > 0.001)
        {
            dx3 = dy3/k3;
        }
        else
        {
            dx3 = 0;
        }

        if(fabs(mx - dx1 - dx2 - dx3 -dx4 - dx5) > 0.1)
        {
            std::cout<<" no solve"<<std::endl;
            return;
        }
    }
    else
    {
        dx3 = (y - dy2 - dy4 - k1 * thetax)/(k3 - k1);
        dy3 = k3 * dx3;
        dx1 = thetax - dx3;
        dy1 = k1 * dx1;
    }

    if((dx3<-0.1) || (dx1<-0.1))
    {
        if((dx1>0.5)&&(bFirstCalc))
        {
            double x_b = mx - mfLastDirectDis;
            double y_b = y - 0;

            double k = tan(fabs(mhdg));
            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);

            double a = 1 +k*k;
            double b = 2*k*C *(-1.0);
            double c = C*C-1;
            double d = b*b - 4*a*c;
            if(d >= 0)
            {
                double xr1,xr2;
                xr1 = (-b + sqrt(d))/(2*a);
                xr2 = (-b - sqrt(d))/(2*a);
                double fa = 0;
                if((xr1>=0)&&(xr1<1))
                {
                    fa = asin(xr1);
                }
                else
                {
                    if((xr2>=0)&&(xr2<1))
                    {
                        fa = asin(xr2);
                    }
                }

                if((fa>0.001) && (fa<mfMaxAngle))
                {
                    fang = fa;
                    bFirstCalc = false;
                    goto TwoCalc;
                }
            }
        }
        else
        {
            if(cos(fabs(mhdg))> 0.001)
            {
                double fR = y/(1 - cos(fabs(mhdg)));
                double Rx = mx - fR*sin(fabs(mhdg));
                if((Rx>0.3) && (fR >= mfRaidus))
                {
                    std::cout<<" Have One Step soluton."<<std::endl;

                    mSideParkType = SideParkType::TwoStep;
                    mvectorWheel.push_back((-1.0*mfMaxWheel * mfRaidus/fR));
                    mvectorWheel.push_back(0.0);
                    double flen1,flen2;
                    flen2 = Rx;
                    flen1 = fR*fabs(mhdg);
                    mvectorlen.push_back(flen1);
                    mvectorlen.push_back(flen2);
                    mfTotalLen = flen1 + flen2;
                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
                    std::cout<<" Have One Step soluton."<<std::endl;
                }
            }
        }
    }
    else
    {
        mSideParkType = SideParkType::FiveStep;
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel * (1.0));
        mvectorWheel.push_back(0.0);
        mvectorWheel.push_back(mfMaxWheel *(-1.0));
        mvectorWheel.push_back(0.0);

        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
        x4 = dx5;y4 = dy5 * (-1.0);hdg4 = 0;flen5 = x4;
        x3 = x4 + dx4;y3 = y4 - dy4;hdg3 = fang * (-1.0);flen4 = fabs(fang) *mfRaidus;
        hdg2 = hdg3;
        if(dx3>0.1)
        {
            x2 = x3 + dx3;y2 = y3 - dy3;
            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
        }
        else
        {
            x2 = x3;y2 = y3;flen3 = 0;
        }

        flen2 = mfRaidus * (fang + mhdg);
        x1 = mx - dx1;
        y1 = my + dy1;
        hdg1 = mhdg;

        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));

        mvectorlen.push_back(flen1);
        mvectorlen.push_back(flen2);
        mvectorlen.push_back(flen3);
        mvectorlen.push_back(flen4);
        mvectorlen.push_back(flen5);
        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
        std::cout<<" have compute."<<std::endl;
    }

    std::cout<<" compute. "<<std::endl;
}

void SideParkCalc::normalhdg(double & fhdg)
{
    while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
    while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
}

SideParkType SideParkCalc::GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
{
    if(mSideParkType == SideParkType::NoSolution)
    {
        return SideParkType::NoSolution;
    }

    xvectorpoint = mvectorpoint;
    xvectorWheel = mvectorWheel;
    xvectorlen = mvectorlen;
    fTotalLen = mfTotalLen;
    return mSideParkType;
}