#ifndef _IV_DECITION__PID_CONTROLLER_
#define _IV_DECITION__PID_CONTROLLER_

#include <gps_type.h>
#include <decition_type.h>
#include <obstacle_type.h>
#include<vector>
#include <gnss_coordinate_convert.h>
#include <adc_controller/base_controller.h>
#include <adc_tools/transfer.h>

namespace iv {
        namespace decition {
        //根据传感器传输来的信息作出决策
                    class PIDController: public BaseController {
                    public:

                        float lastEA;
                        float lastEP;
                        float lastAng;
                        float angleLimit=600;
                        float lastU ;
                        float lastEv ;
                        float lastVt ;

                        float Iv;
                        float eAngSum=0;
                        float ePosSum=0;

                        PIDController();
                        ~PIDController();

                        /**
                           * @brief getControlDecition
                           * pid方式计算横向方向盘和纵向加速度
                           *
                           * @param now_gps_ins          实时gps信息
                           * @param path                 目标路径
                           * @param dSpeed               决策速度
                           * @param obsDistacne          障碍物距离
                           * @param obsSpeed             障碍物速度
                           * @param computeAngle         是否要计算方向盘角度
                           * @param computeAcc           是否要计算纵向加速度
                           * @param decition             决策信息结构体的指针
                           * @return                     iv::decition::Decition
                           */


                        iv::decition::Decition getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path,float dSpeed, float obsDistacne,float  obsSpeed,
                                                                  bool computeAngle, bool computeAcc, Decition *decition);
                        float  getPidAngle (float realSpeed, std::vector<Point2D> trace,int roadMode);
                        float  getAveDef (std::vector<Point2D> farTrace);
                        float  getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistacne, float obsSpeed);
                        float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
                        float  limiteU(float u ,float ttc);


                    private:

                    };

        }
}












#endif // PID_CONTROLLER_H