/*
 * flipper_plugin.h
 *
 * Gazebo plugin to listen to joint controls and publish to transmission the joint states
 *
 *  Created on: 17 Oct 2014
 *      Author: timothyw
 */

#ifndef NEGOTIATOR_GAZEBO_FLIPPER_NODE_H_
#define NEGOTIATOR_GAZEBO_FLIPPER_NODE_H_

#include <ros/ros.h>
#include <crosbot/thread.hpp>
#include <sensor_msgs/JointState.h>

namespace negotiator {
namespace gazebo {

class FlipperVelocityGazebo;
class FVGThread : public crosbot::Thread {
public:
    FVGThread(FlipperVelocityGazebo& control) : control(control) {};
    void run();

private:
    FlipperVelocityGazebo& control;
};

class FlipperVelocityGazebo {
private:
    ros::Subscriber jointControlSub;
    ros::Publisher transmissionLeftPub;
    ros::Publisher transmissionRightPub;

    // Control mode
    bool operating;
    double frequency;

    // Last position
    ros::Time lastTime;
    float velocity;
    float leftPosition;
    float rightPosition;

    // Thread
    FVGThread thread;

public:
    FlipperVelocityGazebo();
    ~FlipperVelocityGazebo();

    void start();
    void stop();

    // Subscriber callback
    void callbackJointControl(sensor_msgs::JointStateConstPtr joints);

    // Callback thread for publish
    void threadLoop();
};

} // namespace gazebo
} // namespace negotiator

#endif /* NEGOTIATOR_GAZEBO_FLIPPER_NODE_H_ */
