#ifndef	vrpn_POSER_H
#define vrpn_POSER_H

#ifndef _WIN32_WCE
#include <time.h>
#endif
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#ifndef _WIN32
#ifndef _WIN32_WCE
#include <sys/time.h>
#endif
#endif

// 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_Connection.h"
#include "vrpn_BaseClass.h"

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_velocity_m_id;       // ID of poser velocity 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_velocity(const struct timeval t,        // Sets the velocity internally
                                    const vrpn_float64 position[3], 
                                    const vrpn_float64 quaternion[4], 
                                    const vrpn_float64 interval);
};

//------------------------------------------------------------------------------------
// 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();
	
	///-----------------KD adding (un)register_change_handler------------
	// (un)Register a callback handler to handle analog value change
        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);
	}
        ///----------------------------------------------------------------

    protected:
        static int VRPN_CALLBACK handle_change_message(void *userdata, vrpn_HANDLERPARAM p);
        static int VRPN_CALLBACK handle_vel_change_message(void *userdata, vrpn_HANDLERPARAM p);
	vrpn_Callback_List<vrpn_POSERCB> d_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_velocity(const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval);

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

#endif
