/* Author: Jake Drahos
 *
 * VPRN tracker module header file.
 */

#ifndef _MICROCART_VRPN_TRACKER_HPP
#define _MICROCART_VRPN_TRACKER_HPP

#ifdef __cplusplus
#include <mutex>
#include <thread>
#include <functional>
#include <vector>

#include "vrpn_Tracker.h"
#include "config.h"

#endif

#include <sys/time.h>

#ifdef __cplusplus
extern "C" 
{
#endif
    struct ucart_vrpn_tracker;
    struct ucart_vrpn_TrackerData {
        float x;
        float y;
        float z;

        float pitch;
        float roll;
        float yaw;

        double fps;
        struct timeval timestamp;
        char server_name[256];
    };

    struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
            const char * server);

    void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);

    int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
            void (*cb)(struct ucart_vrpn_TrackerData *));

    int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
            struct ucart_vrpn_TrackerData * td);

    int ucart_vrpn_start();
    int ucart_vrpn_stop();
#ifdef __cplusplus
}
#endif

#ifdef __cplusplus
namespace microcart
{

    struct TrackerData {
    public:
        float x;
        float y;
        float z;

        float pitch;
        float roll;
        float yaw;

        double fps;
        timeval timestamp;
        char server_name[256];

        TrackerData(const char * server);
        TrackerData(std::string server);
    };

    class Tracker {
    public:
        const struct TrackerData getData(void);
        void callback(const vrpn_TRACKERCB t);

        Tracker(std::string server);
        Tracker(const char * server);
        ~Tracker();
        
        vrpn_Tracker_Remote remote;
        std::mutex vrpn_mutex;

        void addCallback(std::function<void(const TrackerData &)> cb);
    private:
        TrackerData trackerData;
        std::vector<std::function<void(const TrackerData &)>> cb_vector;
    };


    class Tracker_handler {
    public:
        Tracker_handler();
        ~Tracker_handler();
        void vrpn_start();
        void vrpn_stop();
        int num_trackers;
        int stop_flag;
        Tracker * trackers[NUM_TRACKABLES];
    private:
        std::thread vrpn_thread;
        void vrpn_loop(void);
    };
}
/* __cplusplus */
#endif

#endif