#ifndef __ROBOT__HH__
#define __ROBOT__HH__

#include <ros/ros.h>
#include <tf/tf.h>
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>

class RobotBase
{
protected:
    virtual bool get_odom(){return false;}    
    virtual bool post_tf(){return false;}
    virtual bool down_speed(){return false;}
public:
    RobotBase()
        :bExit_(false)
        ,bConnect_(false)
        ,cmd_vel_linear_(0)
        ,cmd_vel_angular_(0)
    {
        
        odom_publisher_ = nh_.advertise<nav_msgs::Odometry>("/odom",1, true);
        current_pos_publisher_=nh_.advertise<geometry_msgs::PoseStamped>("/robot_control/pose",1, true);
        cmd_vel_sub_ = nh_.subscribe<geometry_msgs::Twist>("cmd_vel", 1,
                        boost::bind(&RobotBase::move_base_cmd_vel_callback,_1,this));
        publish_odom_tf_thread_=new boost::thread(boost::bind(RobotBase::thread_odom_tf_publish,this));
        get_odom_tf_thread_=new boost::thread(boost::bind(RobotBase::thread_get_odom_tf,this));
        down_speed_thread_=new boost::thread(boost::bind(RobotBase::thread_down_speed,this));
    }
    virtual ~RobotBase()
    {
        bExit_=true;
        publish_odom_tf_thread_->join();
    }
    
protected:
    bool post_odom()
    {
        
        ros::Time current_time = ros::Time::now();
        tf::Pose base_link=base_link_pose_;
        tf::Quaternion q=base_link.getRotation();
        
        double theta=2*acos(q.w());
        if(base_link.getRotation().z()<0) theta=-theta;

        geometry_msgs::Quaternion odom_quat;
        odom_quat.x = q.x();
        odom_quat.y = q.y();
        odom_quat.z = q.z();
        odom_quat.w = q.w();
            
        nav_msgs::Odometry odom;
        odom.header.frame_id = "map";
        odom.header.stamp=current_time;
        odom.pose.pose.position.x = base_link.getOrigin().x();
        odom.pose.pose.position.y = base_link.getOrigin().y();
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
    
        odom.child_frame_id="base_link";
        odom.twist.twist.linear.x=current_linear_speed_;
        odom.twist.twist.linear.y=0;
        odom.twist.twist.angular.z=current_angular_speed_;
        odom_publisher_.publish(odom);

        std::cout<<"angular:"<<theta*180.0/M_PI<<" v_x:"<<odom.twist.twist.linear.x
                <<", v_y:"<<odom.twist.twist.linear.y
                <<" speed_angular:"<<odom.twist.twist.angular.z<<std::endl;

        return false;
    }
protected:
    static void thread_get_odom_tf(RobotBase* robot)
    {
        if(robot==0)
            return ;

        ros::Rate loop_rate(100);
        while(ros::ok()&&!robot->bExit_)
        {
            tf::Pose pose;
            robot->get_odom();
            loop_rate.sleep();
        }
    }

    
    static void thread_odom_tf_publish(RobotBase* robot)
    {
        if(robot==0)
            return ;

        ros::Rate loop_rate(30);
        while(ros::ok()&&!robot->bExit_)
        {
            tf::Pose pose;
            robot->post_tf();
    
            robot->post_odom();
        

            //发布 当前 pose 用于 rviz 显示
            tf::Pose base_link_pose=robot->base_link_pose_;
            tf::Quaternion q_base_link=base_link_pose.getRotation();
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.frame_id="map";
            pose_stamped.pose.position.x=base_link_pose.getOrigin().x();
            pose_stamped.pose.position.y=base_link_pose.getOrigin().y();
            pose_stamped.pose.position.z=0;
            pose_stamped.pose.orientation.x=q_base_link.x();
            pose_stamped.pose.orientation.y=q_base_link.y();
            pose_stamped.pose.orientation.z=q_base_link.z();
            pose_stamped.pose.orientation.w=q_base_link.w();
            robot->current_pos_publisher_.publish(pose_stamped);
           
            ros::spinOnce();
            loop_rate.sleep();
        }
    }


    static void thread_down_speed(RobotBase* robot) 
    {
        if(robot==0)
            return ;
        ros::Rate loop_rate(100);
        while(ros::ok()&&!robot->bExit_)
        {
            robot->down_speed();
            ros::spinOnce();
            loop_rate.sleep();
        }
    }

    static void move_base_cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg,RobotBase* robot)
    {
        /*std::cout<<" move_base_cmd_vel :"
        <<ros::Time::now()<<"\t"
        <<msg->linear.x<<","
        <<msg->linear.y<<","
        <<msg->linear.z<<","
        <<msg->angular.x<<","
        <<msg->angular.y<<","
        <<msg->angular.z<<std::endl;*/
        if(robot==0)
            return ;
        robot->cmd_vel_linear_=msg->linear.x;
        robot->cmd_vel_angular_=msg->angular.z;
    }
protected:
    bool            bConnect_;
    bool            bExit_;
    ros::NodeHandle nh_;
    boost::thread* 	get_odom_tf_thread_;
    boost::thread* 	publish_odom_tf_thread_;
    ros::Publisher  odom_publisher_;
    ros::Publisher  current_pos_publisher_;
    ros::Subscriber cmd_vel_sub_; 

    boost::thread* 	down_speed_thread_;;
    tf::Pose        base_link_pose_;
    double          current_linear_speed_;
    double          current_angular_speed_;
    double          cmd_vel_linear_;
    double          cmd_vel_angular_;
};

#endif