#ifndef SIDEPARKCALC_H
#define SIDEPARKCALC_H


#include <math.h>
#include <vector>


enum SideParkMode
{
    ParkAtLeft =1,
    ParkAtRight =2
};

enum SideParkType
{
    FiveStep = 1,  //Wh
    TwoStep = 2,  //When hdg is big, y is small, use 2 steps is ok. 1 point
    NoSolution = 3
};


namespace iv {
struct SideParkPoint
{
    SideParkPoint(double x,double y,double fhdg)
    {
        mx = x;
        my = y;
        mhdg = fhdg;
    }
    double mx;
    double my;
    double mhdg;
};

}


class SideParkCalc
{
public:
    SideParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle  = 45.0,double fLastDirectDis = 0.3);

public:
    void CalcPark();
    SideParkType GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);

private:
    double mfLastDirectDis = 0.3;
    double mfRaidus = 5.0;
    double mfMaxAngle = 45.0 *M_PI/180.0;
    double mfMaxWheel = 430.0;

    std::vector<iv::SideParkPoint> mvectorpoint;
    SideParkType mSideParkType;   //five steps / 2 steps / no solution
    std::vector<double> mvectorWheel; // 5 values / 2 values /no value
    std::vector<double> mvectorlen; //5 values / 2 values /no value
    double mfTotalLen = 0.0;



private:
    double mx,my,mhdg;

    bool mbPark = false;

private:
    SideParkMode CalcParkMode();

    void ParkAtRightCalc();
    void ParkAtRightCalc_Model1();
    void ParkAtRightCalc_Model2();

    void ParkAtLeftCalc();
    void ParkAtLeftCalc_Model1();
    void ParkAtLeftCalc_Model2();

    void normalhdg(double & fhdg);
};

#endif // SIDEPARKCALC_H