#ifndef __FORKLIFT_H__
#define __FORKLIFT_H__

#include <ros/ros.h>
#include "std_msgs/String.h"

#include <mutex>

#include "cons_planner/cons_planner.h"
#include "common/common.h"

namespace forklift
{
struct RudderAnt
{
    RudderAnt() : angle(0), 
                  vel(0), 
                  offset(0),
                  p(3.0),
                  min_rudder_angle(0.),
                  max_rudder_angle(0.),
                  toler_angle(0.17),
                  t(ros::Time(0))
    {}
    float angle;
    float offset;
    float vel;
    float p;

    float min_rudder_angle;
    float max_rudder_angle;

    float toler_angle;
   
    ros::Time t;
};

class FL : public cons::consPlanner
{
    public:
        FL();
        ~FL();

        virtual common::NavState computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd);
        virtual void destroy();

    private:
        ros::NodeHandle nh_;
        ros::Subscriber sub_;

        void __rudderAngleCallback(const std_msgs::StringConstPtr &msg);
        bool __readConf();
        
        RudderAnt __getRudderAnt();
        RudderAnt rudder_ant_; 

        std::mutex mutex_;
        #define RUN_LOCK() std::lock_guard<std::mutex> lockGuard(mutex_);
};

} // namespace diff


#endif