#ifndef __OMNI_PLANNER__
#define __OMNI_PLANNER__

#include "common/common.h"
#include "core/core.h"
#include "space_lib/space_lib.h"

namespace omni {

    using Index = unsigned int;
    const float __rlu = 0.01; // 路径分辨率

    class omniPlanner : public planner::Core
    {
        public:
            omniPlanner(/* args */)
            {}
            ~omniPlanner()
            {}

            virtual common::NavState computeVelocity(common::Point& sta_pose, common::Twist& cmd);
            virtual common::NavState computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd);
            virtual common::Path interPath(float a);

            virtual void setPath(common::Path& path);
            virtual void destroy();
            virtual void reset();
            
            virtual void setConfig(common::Config& conf);

            virtual common::Config getConfig();
            virtual common::Path getLocalPath();
            virtual common::Path getGlobalPath();
        private:
            bool __isArrv(common::Point p);
            bool __pathCheck();
            
            float __maxmalDist(common::Path& pt1, common::Path& pt2);
            float __pathTangent(common::Path& path, Index i);
            float __c(common::Point& start, common::Point& goal);

            common::Path __exten(common::Path& path, Index i);
            common::Path __getPah(const common::Point& start, const common::Point& end, const float rlu);
            common::NavState __rotaTangent(common::Point& base, common::Point& tip, common::Twist& twist);
            common::NavState __cal(common::Point& start, common::Point& goal, common::Twist& twist, float linear);
            common::NavState __rotaAngle(float base, float tip, common::Twist& twist);
            
            Index __nib(common::Point& p, Index ci, common::Path& path, float range);
            /* data */
            Index i_;
            
            common::NavStep step_;
        protected:
            common::Path local_path_, path_;
            
            common::Config config_;
    };
}




#endif