/*
 * 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 CASUALTY_GAZEBO_AUTOLEVEL_H_
#define CASUALTY_GAZEBO_AUTOLEVEL_H_

#include <ros/ros.h>
#include <autolevel/factories.hpp>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <xsens/xsens.h>


namespace casualty {
namespace gazebo {

/**
 * Gazebo Version of the XSens
 */
class GazeboXSens : public casualty::XSens  {
private:
    ros::Subscriber imuSub;
    ros::Publisher transmissionLevelRollPub;
    ros::Publisher transmissionLevelPitchPub;

    // Last position/data
    sensor_msgs::Imu imuData;
    float levelRollPosition;
    float levelPitchPosition;

public:
    GazeboXSens();
    virtual ~GazeboXSens();

    // XSens Virtual methods
    virtual void connect();
    virtual void disconnect();
    virtual tf::Quaternion getOrientation();

    // Subscriber callback
    void callbackImu(sensor_msgs::ImuPtr imu);
};

/**
 * GazeboXSens factory for autolevel
 */
class GazeboFactoryXSens : public casualty::FactoryXSens {
public:
    XSensPtr makeXSens() { return new GazeboXSens(); };
    XSensPtr makeXSens(std::string port, bool resetOnStart) { return new GazeboXSens(); };
};

/**
 * Gazebo version of the AutoLevelChain
 */
class GazeboAutoLevelChain : public AutoLevelChain {
public:
    GazeboAutoLevelChain();
    virtual ~GazeboAutoLevelChain();

    // Chain virtuals
    bool readServos();
    void connect();
    void disconnect();

protected:
    virtual Dynamixel* make(ConfigElementPtr cfg);
};

/**
 * Gazebo version of an AutoLevelServo
 */
class GazeboAutoLevelServo : public AutoLevelServo {
public:
    // TODO: load pub/sub for this servo
    GazeboAutoLevelServo(std::string pubName, AutoLevelChain& chain) :
        AutoLevelServo(chain), active(false), pubName(pubName) {};
    virtual ~GazeboAutoLevelServo() {};

    // Overridden virtual methods
    virtual bool readServo();
    virtual void connect();
    virtual void disconnect();
    virtual void setPos(double pos, double vel);
    virtual void setPos(double pos);

    // Subscriber callback
    void callbackServoPos(std_msgs::Float64 pos);
protected:
    bool active;

    //ros::Subscriber servoSub;
    ros::Publisher servoPub;

    // Config info
    //std::string subName;
    std::string pubName;
};


/**
 * GazeboAutoLevelChain factory
 */
class GazeboFactoryAutoLevelChain : public FactoryAutoLevelChain {
public:
    AutoLevelChainPtr makeAutoLevelChain() { return new GazeboAutoLevelChain(); };
};

} // namespace gazebo
} // namespace casualty

#endif /* CASUALTY_GAZEBO_AUTOLEVEL_H_ */
