/*
 * monitor.h
 *
 *  Created on: 08/04/2014
 *      Author: timothyw
 */

#ifndef NEGOTIATOR_MONITOR_H_
#define NEGOTIATOR_MONITOR_H_

#include "negotiator_drivers/state/state.h"

#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <tf/transform_listener.h>

#include <crosbot/thread.hpp>
#include "negotiator_drivers/NegotiatorStateService.h"

#include <pthread.h>
#include <deque>
#include <string>

namespace casualty {

class NegotiatorMonitor {
public:

    NegotiatorMonitor();
    virtual ~NegotiatorMonitor();

    void configure(ros::NodeHandle& nh);

    // Setup/close the monitor node
    void connect();
    void disconnect();

    // Run publisher is a loop until disconnect() is called
    void publish();

    // Get a new Negotiator State
    NegotiatorStatePtr getState();

    // Subscription callbacks
    void callbackFlipper(sensor_msgs::JointStateConstPtr jstate);
    void callbackFlipperGazebo(std_msgs::Float64 position);
    //void callbackTracks(nav_msgs::OdometryConstPtr tracks);
    void callbackTracks(geometry_msgs::TwistPtr tracks);
    void callbackXsens(geometry_msgs::QuaternionConstPtr xsens);
    void callbackXsensGazebo(sensor_msgs::ImuPtr xsens);

    // Service callback
    bool callbackStateService(negotiator_drivers::NegotiatorStateServiceRequest& request,
            negotiator_drivers::NegotiatorStateServiceResponse& response);

private:
    // Configuration parameters
    double frequency;        // Publish frequency in seconds
    std::string frameRobot;
    std::string frameIcp;
    std::string frameSlam;
    bool fromGazebo;
    int nFlipperReadings;
    bool publisherEnabled;
    bool slamEnabled;

    // Publisher Thread
    bool operating;
    pthread_t threadPublish;
    int publishSleep;

    // State
    NegotiatorState state;
    ros::Time timeFlippers;
    std::deque<double> flipperReadings;
    double flipperSum;

    // Data Mutex's
    crosbot::Mutex mutexFlippers;
    crosbot::Mutex mutexTracks;
    crosbot::Mutex mutexXsens;

    // Subscriptions
    ros::Subscriber subFlippers;
    ros::Subscriber subTracks;
    ros::Subscriber subXsense;
    tf::TransformListener* tfListener;
    ros::Publisher pubState;

    // Service
    ros::ServiceServer serviceState;

    // Error handling
    ros::Time lastICPError;
    ros::Time lastSlamError;
};

} // namespace casualty

#endif /* NEGOTIATOR_MONITOR_H_ */
