/*
 * flippers.h
 *
 *  Created on: 19 Aug 2015
 *      Author: timothyw
 */

#ifndef INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_FLIPPERS_H_
#define INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_FLIPPERS_H_

#include <crosbot/config.hpp>
#include <crosbot/handle.hpp>
#include "negotiator_drivers/flippers/hardware.h"

#include <sensor_msgs/JointState.h>
#include <pthread.h>

namespace casualty {

class FlippersROS : public crosbot::HandledObject {
public:
    FlippersROS(FlippersHardwareInterface *interface);
    virtual ~FlippersROS() {};

    void configure(crosbot::ConfigElementPtr cf);

    // Start/Stop
    void connect();
    void disconnect();

    // Thread loop
    void run();

    // ROS callbacks
    void callback_setJoint(sensor_msgs::JointStateConstPtr jstate);

    // Report flipper values via ROS
    void publishValues();

private:
    // Operation
    bool operating;

    // Joint Set points
    ros::Time lastCommand;
    double desiredPos;
    double velocity;
    bool stopped;
    double angleAccept;
    double maxCommandDelay;

    // Current Joint values
    double potentiometer;
    double setPoint;
    double battery;

    // ROS Publishers/Subscribers
    std::string     jointName;
    int seqNum;
    ros::Subscriber jointSub;
    ros::Publisher jointPub;

    // Hardware Interface
    FlippersHardwareInterface *interface;

    // Reading Thread
    pthread_t threadRead;


    /**
     * Monitor joint movement for set point mode and stop if necessary
     */
    void setJointSetPoint();
};
typedef crosbot::Handle<FlippersROS> FlippersROSPtr;

} // namespace casualty

#endif /* INCLUDE_NEGOTIATOR_DRIVERS_FLIPPERS_FLIPPERS_H_ */
