#ifndef _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_
#define _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_

#include <ros/ros.h>
#include <ros_kinematics/base_drive.h>
#include <ros_kinematics/odometry.h>
#include <ros_kinematics/speed_limiter.h>
#include <ros_kinematics/ros_diff_drive_parameters.h>

#include <memory>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <control_msgs/JointTrajectoryControllerState.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
// Boost
#include <boost/thread.hpp>
// plugin
#include <models/BaseSteerDrive.h>
#include <models/BaseAbsoluteEncoder.h>
#include <pluginlib/class_loader.h>

namespace ros_kinematics
{
  class RosDiffDrive : public BaseDrive
  {
  public:
    /**
     * @brief Constructor
     */
    RosDiffDrive(ros::NodeHandle& nh, const std::string& name);

    /**
     * @brief Deconstructor
     */
    virtual ~RosDiffDrive();

  private:

    void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
    /**
     * @brief returns true, if publishing of a cmd_vel topic is publishing
     * 
    */
    bool isCmdVelTriggered(void);

    /**
     *  @brief returns true, if not subcribe cmd_vel
    */
    bool isCmdVelLastestTriggered(void);

    /**
     * @brief schedules MotorController function 
     * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
    */
    void scheduleMotorController(bool schedule);
    
    void updateDynamicParams();
    void updateChild(void);
    void updateOdometryEncoder(const ros::Time& current_time);
    void publishOdometry(const ros::Time& current_time);
    void brake();
    bool allReady();

    /**
     * @brief Sets the odometry publishing fields
     * @param root_nh Root node handle
     * @param controller_nh Node handle inside the controller namespace
     */
    void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
    
    /**
     * @brief Get the wheel names from a wheel param
     * @param [in]  controller_nh Controller node handler
     * @param [in]  wheel_param   Param name
     * @param [out] wheel_names   Vector with the whel names
     * @return true if the wheel_param is available and the wheel_names are
     *        retrieved successfully from the param server; false otherwise
     */
    bool getWheelNames(ros::NodeHandle& controller_nh, const std::string& wheel_param, std::vector<std::string>& wheel_names);

    bool getWheelParams(ros::NodeHandle& controller_nh, const std::vector<std::string>& wheel_names, 
                          std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map);

    // properties
    bool initialized_;
    std::string name_;
    ros::NodeHandle nh_;
    ros::NodeHandle nh_priv_;

    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
    std::vector<boost::shared_ptr<models::BaseSteerDrive> > left_drive_;
    std::vector<boost::shared_ptr<models::BaseSteerDrive> > right_drive_;
    
    std::map<std::string, XmlRpc::XmlRpcValue> left_drive_param_map_;
    std::map<std::string, XmlRpc::XmlRpcValue> right_drive_param_map_;
    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > left_wheel_tf_pub_;
    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > right_wheel_tf_pub_;

    bool use_abs_encoder;
    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > left_encoder_;
    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > right_encoder_;
    double left_wheel_curr_pos_;
    double right_wheel_curr_pos_;



    ros::Subscriber cmd_vel_subscriber_;
    int m_subscribe_queue_size_;
    boost::shared_ptr<boost::thread> callback_thread_;
    ros::Duration schedule_delay_;
    ros::Duration schedule_lastest_delay_;
    ros::Time publish_time_;
    ros::Time publish_lastest_time_;
    boost::mutex publish_mutex_;
    /// Publish executed commands
    boost::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > cmd_vel_pub_;

    ros_kinematics::Odometry odometry_;
    ros::Duration publish_period_;
    ros::Time last_odom_state_publish_time_;
    bool open_loop_;
    ros::Time last_odom_update_;
    std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
    std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;

    /// Wheel separation, wrt the midpoint of the wheel width:
    double wheel_separation_;

    /// Wheel radius (assuming it's the same for the left and right wheels):
    double wheel_radius_;

    /// Wheel separation and radius calibration multipliers:
    double wheel_separation_multiplier_;
    double left_wheel_radius_multiplier_;
    double right_wheel_radius_multiplier_;

    /// Timeout to consider cmd_vel commands old:
    double cmd_vel_timeout_;

    /// Whether to allow multiple publishers on cmd_vel topic or not:
    bool allow_multiple_cmd_vel_publishers_;

    /// Frame to use for the robot base:
    std::string base_frame_id_;

    /// Frame to use for odometry and odom tf:
    std::string odom_frame_id_;

    /// Whether to publish odometry to tf or not:
    bool enable_odom_tf_;

    /// Whether to publish wheel link to tf or not:
    bool enable_wheel_tf_;

    /// Number of wheel joints:
    size_t wheel_joints_size_;
    boost::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;

    /// Speed limiters:
    ros_kinematics::BaseDrive::DriveCmd last1_cmd_;
    ros_kinematics::BaseDrive::DriveCmd last0_cmd_;
    ros_kinematics::SpeedLimiter limiter_lin_;
    ros_kinematics::SpeedLimiter limiter_ang_;

    /// Publish limited velocity:
    bool publish_cmd_;
    // Update Rate
    double update_rate_;

    
    /// Dynamic Reconfigure server
    boost::shared_ptr<ros_kinematics::KinematicDiffDriveParameters> kinematics_param_ptr_;

    std::string odometry_topic_;
    std::string odometry_frame_;
    std::string odometry_enc_topic_;
    std::string odometry_enc_child_frame_;
    std::string command_topic_;

    // Flags
    bool publishOdomTF_;

    // bool publishWheelJointState_;
    bool odom_enc_initialized_;
  };
}
#endif // _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_