#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <string>


class VelocityController {
public:
    VelocityController() :
        move_base_action_("move_base", true),
        standard_vel_(1.0),
        current_max_vel_(1.0),
        min_vel_(0.0)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle nh;
        max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this);
        cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this);
        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_out", 10);

        std::string planner = "base_local_planner/TrajectoryPlannerROS";
        std::string param_max = "/move_base/TrajectoryPlannerROS/max_vel_x";
        std::string param_min = "/move_base/TrajectoryPlannerROS/min_vel_x";
        nh.param("/move_base/base_local_planner", planner, planner);
        if (planner == "dwa_local_planner/DWAPlannerROS") {
            param_max = "/move_base/DWAPlannerROS/max_vel_trans";
            param_min = "/move_base/DWAPlannerROS/min_vel_trans";
        }
        nh.param(param_max, standard_vel_, standard_vel_);
        nh.param(param_min, min_vel_, min_vel_);
        ROS_INFO_STREAM("Standard max vel: " << standard_vel_);
    }



    void maxVelCallback(const std_msgs::Float32 &msg)
    {
        current_max_vel_ = standard_vel_ * msg.data;
        current_max_vel_ = std::max(current_max_vel_, min_vel_);
        ROS_INFO_STREAM("Set max vel: " << current_max_vel_);
    }



    void cmdVelCallback(const geometry_msgs::Twist &msg)
    {
        pub_msg_.linear.x = std::min(float(msg.linear.x), current_max_vel_);
        pub_msg_.angular = msg.angular;
        cmd_vel_pub_.publish(pub_msg_);
    }



private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    ros::Subscriber max_vel_sub_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher cmd_vel_pub_;
    float standard_vel_, current_max_vel_, min_vel_;
    geometry_msgs::Twist pub_msg_;
};







int main(int argc, char *argv[]) {
    ros::init(argc, argv, "velocity_controller");
    VelocityController vc;

    ros::spin();
    return 0;
}