#ifndef _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_
#define _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_

#include <ros/ros.h>
#include <ros_kinematics/base_drive.h>
#include <ros_kinematics/ros_steer_drive_parameters.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>

// Boost
#include <boost/thread.hpp>
// plugin
#include <models/BaseSteerDrive.h>
#include <models/BaseAbsoluteEncoder.h>
#include <pluginlib/class_loader.h>

namespace ros_kinematics
{
  class RosSteerDrive : public BaseDrive
  {
  public:
    RosSteerDrive(ros::NodeHandle & nh, const std::string &name);
    virtual ~RosSteerDrive();
  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 MotorController(double target_speed, double target_steering_speed, double dt);

    void UpdateOdometryEncoder();
    void UpdateChild(void);
    void PublishOdometry(double step_time);
    void publishSteerJoint(double odom_alpha);

    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
    std::string steer_motor_mode_, traction_motor_mode_;
    boost::shared_ptr<models::BaseSteerDrive> steer_motor_;
    boost::shared_ptr<models::BaseSteerDrive> traction_motor_;

    bool use_abs_encoder;
    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
    boost::shared_ptr<models::BaseAbsoluteEncoder> absolute_encoder_;    

    ros::NodeHandle nh_;
    ros::NodeHandle nh_priv_;

    ros::Publisher odometry_publisher_;
    ros::Publisher odometry_enc_publisher_;
    ros::Publisher cmd_vel_feedback_;
    ros::Publisher steer_angle_pub_;
    ros::Publisher cmd_vel_rb;
    ros::Subscriber cmd_vel_subscriber_;
    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
    int m_subscribe_queue_size_;
    boost::mutex callback_mutex_;
    boost::thread *callback_thread_;

    nav_msgs::Odometry odom_;
    nav_msgs::Odometry odom_enc_;
    geometry_msgs::Pose2D pose_encoder_;
    ros::Time last_odom_update_;

    // Update Rate
    double update_rate_;
    double update_period_;
    ros::Time last_actuator_update_;
    ros::Time publish_time_;
    ros::Time publish_lastest_time_;
    ros::Duration schedule_delay_;
    ros::Duration schedule_lastest_delay_;
    boost::mutex publish_mutex_;

    boost::shared_ptr<ros_kinematics::KinematicSteerDriveParameters> kinematics_param_ptr;
    double steering_fix_wheel_distance_x_;
    double steering_fix_wheel_distance_y_;
    double odom_enc_steering_angle_offset_;

    double wheel_diameter_;
    double wheel_acceleration_;
    double steering_ratio_;
    double traction_ratio_;

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

    // Flags
    bool publishOdomTF_;
    // bool publishWheelJointState_;
    bool odom_enc_initialized_;
  };
}
#endif