#ifndef vrpn_POSER_H
#define vrpn_POSER_H
#include <stdio.h> // for NULL

// NOTE: the poser class borrows heavily from the vrpn_Tracker code.
//       The poser is basically the inverse of a tracker.
//       We are only handling pose and velocity updates for now...acceleration
//       will come later, as needed.

#include "vrpn_BaseClass.h" // for vrpn_Callback_List, etc
#include "vrpn_Configure.h" // for VRPN_CALLBACK, VRPN_API
#include "vrpn_Shared.h"    // for timeval
#include "vrpn_Types.h"     // for vrpn_float64, vrpn_int32

class VRPN_API vrpn_Connection;
struct vrpn_HANDLERPARAM;

class VRPN_API vrpn_Poser : public vrpn_BaseClass {
public:
    vrpn_Poser(const char* name, vrpn_Connection* c = NULL);

    virtual ~vrpn_Poser(void);

    void p_print();     // print the current pose
    void p_print_vel(); // print the current velocity

    // a poser server should call the following to register the
    // default xform and workspace request handlers
    //        int register_server_handlers(void);

protected:
    // client-->server
    vrpn_int32 req_position_m_id;          // ID of poser position message
    vrpn_int32 req_position_relative_m_id; // ID of poser position delta message
    vrpn_int32 req_velocity_m_id;          // ID of poser velocity message
    vrpn_int32 req_velocity_relative_m_id; // ID of poser velocity delta message

    // Description of current state
    vrpn_float64 p_pos[3], p_quat[4]; // Current pose, (x,y,z), (qx,qy,qz,qw)
    vrpn_float64 p_vel[3],
        p_vel_quat[4];          // Current velocity and dQuat/vel_quat_dt
    vrpn_float64 p_vel_quat_dt; // delta time (in secs) for vel_quat
    struct timeval p_timestamp; // Current timestamp

    // Minimum and maximum values available for the position and velocity values
    // of the poser.
    vrpn_float64 p_pos_min[3], p_pos_max[3], p_pos_rot_min[3], p_pos_rot_max[3],
        p_vel_min[3], p_vel_max[3], p_vel_rot_min[3], p_vel_rot_max[3];

    virtual int register_types(void); // Called by BaseClass init()

    virtual int encode_to(char* buf);     // Encodes the position
    virtual int encode_vel_to(char* buf); // Encodes the velocity

    virtual void set_pose(const struct timeval t, // Sets the pose internally
                          const vrpn_float64 position[3],
                          const vrpn_float64 quaternion[4]);
    virtual void set_pose_relative(
        const struct timeval t, // Increments the pose internally
        const vrpn_float64
            position_delta[3],             // pos_new = position_delta + pos_old
        const vrpn_float64 quaternion[4]); // q_new = quaternion * q_old
    virtual void
    set_pose_velocity(const struct timeval t, // Sets the velocity internally
                      const vrpn_float64 position[3],
                      const vrpn_float64 quaternion[4],
                      const vrpn_float64 interval);
    virtual void set_pose_velocity_relative(
        const struct timeval t, // Increments the velocity internally
        const vrpn_float64
            velocity_delta[3],            // vel_new = velocity_delta + vel_old
        const vrpn_float64 quaternion[4], // q_new = quaternion * q_old
        const vrpn_float64
            interval_delta); // interval_new = interval_delta + interval_old
};

//------------------------------------------------------------------------------------
// Server Code

/// A structure for Call-Backs related to Vrpn Poser Server
typedef struct _vrpn_POSERCB {
    struct timeval msg_time; // Timestamp
    /// NOTE: I think since we have different routines for handling velocity and
    /// position poser requests,
    /// putting poser and quaternions for both doesn't make sense. Instead, the
    /// change handler should
    /// take care of packing correct poser and quaternion.
    vrpn_float64 pos[3];
    vrpn_float64 quat[4];
} vrpn_POSERCB;

typedef void(VRPN_CALLBACK* vrpn_POSERHANDLER)(void* userdata,
                                               const vrpn_POSERCB info);

//------------------------------------------------------------------------------------
// Server Code
// Users supply the routines to handle requests from the client

// This is a sample basic poser server
//

class VRPN_API vrpn_Poser_Server : public vrpn_Poser {
public:
    vrpn_Poser_Server(const char* name, vrpn_Connection* c);

    /// This function should be called each time through app mainloop.
    virtual void mainloop();

    int register_change_handler(void* userdata, vrpn_POSERHANDLER handler)
    {
        return d_callback_list.register_handler(userdata, handler);
    };
    int unregister_change_handler(void* userdata, vrpn_POSERHANDLER handler)
    {
        return d_callback_list.unregister_handler(userdata, handler);
    }

    int register_relative_change_handler(void* userdata,
                                         vrpn_POSERHANDLER handler)
    {
        return d_relative_callback_list.register_handler(userdata, handler);
    }
    int unregister_relative_change_handler(void* userdata,
                                           vrpn_POSERHANDLER handler)
    {
        return d_relative_callback_list.unregister_handler(userdata, handler);
    }

protected:
    static int VRPN_CALLBACK
    handle_change_message(void* userdata, vrpn_HANDLERPARAM p);
    static int VRPN_CALLBACK
    handle_relative_change_message(void* userdata, vrpn_HANDLERPARAM p);
    static int VRPN_CALLBACK
    handle_vel_change_message(void* userdata, vrpn_HANDLERPARAM p);
    static int VRPN_CALLBACK
    handle_relative_vel_change_message(void* userdata, vrpn_HANDLERPARAM p);
    vrpn_Callback_List<vrpn_POSERCB> d_callback_list;
    vrpn_Callback_List<vrpn_POSERCB> d_relative_callback_list;
};

//------------------------------------------------------------------------------------
// Client Code

// Open a poser that is on the other end of a connection for sending updates to
// it.
class VRPN_API vrpn_Poser_Remote : public vrpn_Poser {
public:
    // The name of the poser to connect to, including connection name,
    // for example "poser@magnesium.cs.unc.edu". If you already
    // have the connection open, you can specify it as the second parameter.
    // This allows both servers and clients in the same thread, for example.
    // If it is not specified, then the connection will be looked up based
    // on the name passed in.
    vrpn_Poser_Remote(const char* name, vrpn_Connection* c = NULL);

    // unregister all of the handlers registered with the connection
    virtual ~vrpn_Poser_Remote(void);

    // This routine calls the mainloop of the connection it's on
    virtual void mainloop();

    // Routines to set the state of the poser
    int request_pose(const struct timeval t, const vrpn_float64 position[3],
                     const vrpn_float64 quaternion[4]);
    int request_pose_relative(const struct timeval t,
                              const vrpn_float64 position_delta[3],
                              const vrpn_float64 quaternion[4]);
    int request_pose_velocity(const struct timeval t,
                              const vrpn_float64 velocity[3],
                              const vrpn_float64 quaternion[4],
                              const vrpn_float64 interval);
    int request_pose_velocity_relative(const struct timeval t,
                                       const vrpn_float64 velocity_delta[3],
                                       const vrpn_float64 quaternion[4],
                                       const vrpn_float64 interval_delta);

protected:
    virtual int
    client_send_pose(); // Sends the current pose.  Called by request_pose
    virtual int client_send_pose_relative(); // Sends the current pose delta.
                                             // Called by request_pose_relative
    virtual int client_send_pose_velocity(); // Sends the current velocity.
                                             // Called by request_pose_velocity
    virtual int
    client_send_pose_velocity_relative(); // Sends the current velocity delta.
                                          // Called by
                                          // request_pose_velocity_relative
};

#endif
