//    Creates a thread that runs the ros node.
//
//    Copyright (C) 2017  Angel Romero
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef ___ROSNODETHREAD_H___
#define ___ROSNODETHREAD_H___

#include <QtCore>
#include <QThread>
#include <QStringList>
#include <stdlib.h>
#include <QMutex>
#include <iostream>
#include "assert.h"

#include <ros/ros.h>
#include <ros/network.h>
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyfliePositionData.h"
#include "d_fall_pps/ViconData.h"

#include <std_msgs/Int32MultiArray.h>

using namespace d_fall_pps;

typedef ViconData::ConstPtr ptrToMessage;

Q_DECLARE_METATYPE(ptrToMessage)


class rosNodeThread : public QObject {
	Q_OBJECT
public:
    explicit rosNodeThread(int argc, char **pArgv, const char * node_name, QObject *parent = 0);
    virtual ~rosNodeThread();

    bool init();

    // void messageCallback(const ViconData& data);
    void messageCallback(const ptrToMessage& p_msg);
    void studentIDCallback(const std_msgs::Int32MultiArray &ids);
    void positionDataCallback(const CrazyfliePositionData& data);

    ros::ServiceClient m_read_db_client;
    ros::ServiceClient m_update_db_client;
    ros::ServiceClient m_command_db_client;

signals:

    void newViconData(const ptrToMessage& p_msg);
    void newStudentIDs(const std_msgs::Int32MultiArray &ids);
    void newPositionData(const CrazyfliePositionData& data);

public slots:
    void run();

private:
    int m_Init_argc;
    char** m_pInit_argv;
    const char * m_node_name;

    QThread * m_pThread;

    ViconData m_vicon_data;

    ros::NodeHandle* nh;
    ros::Subscriber m_vicon_subscriber;
    ros::Subscriber studentID_subscriber;
    ros::Subscriber positionData_subscriber;
    // ros::Publisher  sim_velocity;
};
#endif