#include "vrpn_Freespace.h"

#ifdef VRPN_USE_FREESPACE

#include "quat.h"

#include <freespace/freespace_codecs.h>

#include <cstring>

static q_type gs_qIdent = Q_ID_QUAT;

// libfreespace provides linear acceleration values in (mm/(s^2))^-3

// VRPN wants these in meters/(s^2)

static vrpn_float64 convertFreespaceLinearAccelation(int16_t mmpss) {

return mmpss / ((vrpn_float64) 1000);

}

// freespace reports in milliradians per second, so need to convert

// to meters per second...

// The radius to use is a bit arbitrary, and would depend on

// what exactly this device is 'attached' to. Additionally, this may

// be different for each axis. In general, the radius shouldn't really

// matter since the device may be representing a larger/smaller object in

// the virtual world.

static vrpn_float64 convertFreespaceAngularVelocity(int16_t mrps, double radius) {

return mrps * radius / ((vrpn_float64) 1000);

}

#define MILLIS_TO_SECONDS(x) (x / ((vrpn_float64) 1000000))

vrpn_Freespace::vrpn_Freespace(FreespaceDeviceId freespaceId,

struct FreespaceDeviceInfo* deviceInfo,

const char *name, vrpn_Connection *conn):

vrpn_Tracker_Server(name, conn),

vrpn_Button(name, conn),

vrpn_Dial(name, conn),

_freespaceDevice(freespaceId)

{

memcpy(&_deviceInfo, deviceInfo, sizeof(_deviceInfo));

// 5 buttons + a scroll wheel

vrpn_Button::num_buttons = 5;

vrpn_Dial::num_dials = 1;

memset(&_lastBodyFrameTime, 0, sizeof(_lastBodyFrameTime));

_timestamp.tv_sec = 0;

}

void vrpn_Freespace::freespaceInit() {

static bool freespaceInit = false;

if (!freespaceInit) {

freespaceInit = true;

int rc = freespace_init();

if (rc != FREESPACE_SUCCESS) {

fprintf(stderr, "vrpn_Freespace::freespaceInit: failed to init freespace lib. rc=%d\n", rc);

}

}

}

void vrpn_Freespace::deviceSetConfiguration(bool send_body_frames, bool send_user_frames) {

_sendBodyFrames = send_body_frames;

_sendUserFrames = send_user_frames;

}

void vrpn_Freespace::deviceConfigure() {

int rc = 0;

// Send the data mode request

struct freespace_message message;

memset(&message, 0, sizeof(message));

if (_deviceInfo.hVer == 1) {

message.messageType = FREESPACE_MESSAGE_DATAMOTIONCONTROL;

struct freespace_DataMotionControl * req;

req = &message.dataMotionControl;

if (_sendBodyFrames) { req->enableBodyMotion = 1; }

if (_sendUserFrames) { req->enableUserPosition = 1; }

req->inhibitPowerManager = 1;

req->enableMouseMovement = 0;

req->disableFreespace = 0;

} else {

message.messageType = FREESPACE_MESSAGE_DATAMODEREQUEST;

struct freespace_DataModeRequest * req;

req = &message.dataModeRequest;

if (_sendBodyFrames) { req->enableBodyMotion = 1; }

if (_sendUserFrames) { req->enableUserPosition = 1; }

req->inhibitPowerManager = 1;

req->enableMouseMovement = 0;

req->disableFreespace = 0;

}

rc = freespace_sendMessage(_freespaceDevice, &message);

if (rc != FREESPACE_SUCCESS) {

fprintf(stderr, "vrpn_Freespace::deviceConfigure: Could not send message: %d.\n", rc);

}

}

void vrpn_Freespace::deviceUnconfigure() {

int rc = 0;

// Send the data mode request

struct freespace_message message;

memset(&message, 0, sizeof(message));

if (_deviceInfo.hVer == 1) {

message.messageType = FREESPACE_MESSAGE_DATAMOTIONCONTROL;

struct freespace_DataMotionControl * req;

req = &message.dataMotionControl;

req->enableBodyMotion = 0;

req->enableUserPosition = 0;

req->inhibitPowerManager = 0;

req->enableMouseMovement = 1;

req->disableFreespace = 0;

} else {

message.messageType = FREESPACE_MESSAGE_DATAMODEREQUEST;

struct freespace_DataModeRequest * req;

req = &message.dataModeRequest;

req->enableBodyMotion = 0;

req->enableUserPosition = 0;

req->inhibitPowerManager = 0;

req->enableMouseMovement = 1;

req->disableFreespace = 0;

}

rc = freespace_sendMessage(_freespaceDevice, &message);

if (rc != FREESPACE_SUCCESS) {

fprintf(stderr, "vrpn_Freespace::deviceUnconfigure: Could not send message: %d.\n", rc);

}

}

vrpn_Freespace* vrpn_Freespace::create(const char *name, vrpn_Connection *conn,

int device_index,

bool send_body_frames, bool send_user_frames)

{

// initialize libfreespace

vrpn_Freespace::freespaceInit();

const unsigned MAX_DEVICES = 100;

FreespaceDeviceId devices[MAX_DEVICES];

int numIds = 0;

int rc;

rc = freespace_getDeviceList(devices, MAX_DEVICES, &numIds);

if ( (rc != FREESPACE_SUCCESS) || (numIds < (device_index+1)) ) {

fprintf(stderr, "vrpn_Freespace::create: Didn't find enough devices: %d.\n", numIds);

return NULL;

}

FreespaceDeviceId freespaceId = devices[device_index];

rc = freespace_openDevice(freespaceId);

if (rc != FREESPACE_SUCCESS) {

fprintf(stderr, "vrpn_Freespace::create: Could not open device %d.\n", device_index);

return NULL;

}

struct FreespaceDeviceInfo deviceInfo;

rc = freespace_getDeviceInfo(freespaceId, &deviceInfo);

if (rc != FREESPACE_SUCCESS) {

return NULL;

}

rc = freespace_flush(freespaceId);

if (rc != FREESPACE_SUCCESS) {

fprintf(stderr, "vrpn_Freespace::create: Error flushing device: %d\n", rc);

return NULL;

}

vrpn_Freespace* dev = new vrpn_Freespace(freespaceId, &deviceInfo, name, conn);

dev->deviceSetConfiguration(send_body_frames, send_user_frames);

printf("Added freespace device: %s : \n", name);

return dev;

}

void vrpn_Freespace::mainloop(void)

{

server_mainloop();

struct freespace_message s;

/*

* Send configuration information to the device repeatedly to force the

* device into the correct mode. This method ensures the correct mode

* even if the Freespace device was asleep during initialization or if

* the tracker resets.

*/

struct timeval timestamp;

vrpn_gettimeofday(&timestamp, NULL);

if (_timestamp.tv_sec != timestamp.tv_sec) {

_timestamp.tv_sec = timestamp.tv_sec;

deviceConfigure();

}

// Do not block, read as many messages as

while (FREESPACE_SUCCESS == freespace_readMessage(_freespaceDevice, &s, 0)) {

switch(s.messageType) {

case FREESPACE_MESSAGE_LINKSTATUS:

handleLinkStatus(s.linkStatus);

break;

case FREESPACE_MESSAGE_BODYFRAME:

handleBodyFrame(s.bodyFrame);

break;

case FREESPACE_MESSAGE_USERFRAME:

handleUserFrame(s.userFrame);

break;

case FREESPACE_MESSAGE_ALWAYSONRESPONSE:

break;

case FREESPACE_MESSAGE_DATAMODERESPONSE:

break;

default:

send_text_message("Received an unhandled message from freespace device", timestamp, vrpn_TEXT_WARNING);

break;

}

}

}

vrpn_Freespace::~vrpn_Freespace(void)

{

// as part of cleanup, disable user and body frame messages, and

// put back to just mouse movement.

int rc = freespace_flush(_freespaceDevice);

if (rc != FREESPACE_SUCCESS) {

printf("freespaceInputThread: Error flushing device: %d\n", rc);

}

deviceUnconfigure();

freespace_closeDevice(_freespaceDevice);

}

void vrpn_Freespace::handleBodyFrame(const struct freespace_BodyFrame& body) {

vrpn_gettimeofday(&(vrpn_Tracker::timestamp), NULL);

vrpn_float64 now = vrpn_Tracker::timestamp.tv_sec + MILLIS_TO_SECONDS(vrpn_Tracker::timestamp.tv_usec);

vrpn_float64 delta = now - _lastBodyFrameTime;

_lastBodyFrameTime = now;

vrpn_Tracker::acc[0] = convertFreespaceLinearAccelation(body.linearAccelX);

vrpn_Tracker::acc[1] = convertFreespaceLinearAccelation(body.linearAccelY);

vrpn_Tracker::acc[2] = convertFreespaceLinearAccelation(body.linearAccelZ);

q_copy(vrpn_Tracker::acc_quat, gs_qIdent);

vrpn_Tracker::acc_quat_dt = delta;

vrpn_Tracker_Server::report_pose_acceleration(0, vrpn_Tracker::timestamp,

vrpn_Tracker::acc, vrpn_Tracker::acc_quat, delta);

vrpn_Tracker::vel[0] = convertFreespaceAngularVelocity(body.angularVelX, 1);

vrpn_Tracker::vel[1] = convertFreespaceAngularVelocity(body.angularVelY, 1);

vrpn_Tracker::vel[2] = convertFreespaceAngularVelocity(body.angularVelZ, 1);

q_copy(vrpn_Tracker::vel_quat, gs_qIdent);

vrpn_Tracker::vel_quat_dt = delta;

vrpn_Tracker_Server::report_pose_velocity(0, vrpn_Tracker::timestamp,

vrpn_Tracker::vel, vrpn_Tracker::vel_quat, vrpn_Tracker::vel_quat_dt);

vrpn_Button::buttons[0] = body.button1;

vrpn_Button::buttons[1] = body.button2;

vrpn_Button::buttons[2] = body.button3;

vrpn_Button::buttons[3] = body.button4;

vrpn_Button::buttons[4] = body.button5;

vrpn_Button::timestamp = vrpn_Tracker::timestamp;

vrpn_Button::report_changes();

// this is totally arbitrary

// I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems

// like it would be the number of 'clicks' while turning the scroll wheel...

// vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means

// the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full

// revolution....'

// this values should probably set in a configuration file.

vrpn_Dial::dials[0] = body.deltaWheel / (vrpn_float64) 16;

vrpn_Dial::timestamp = vrpn_Tracker::timestamp;

vrpn_Dial::report_changes();

}

void vrpn_Freespace::handleUserFrame(const struct freespace_UserFrame& user) {

vrpn_gettimeofday(&(vrpn_Tracker::timestamp), NULL);

// Translate the Freespace linear position into VRPN linear position.

vrpn_Tracker::pos[0] = user.linearPosY;

vrpn_Tracker::pos[1] = user.linearPosZ;

vrpn_Tracker::pos[2] = user.linearPosX;

// Transfer the Freespace coordinate system angular position quanternion

// into the VRPN coordinate system.

vrpn_Tracker::d_quat[Q_W] = user.angularPosA;

vrpn_Tracker::d_quat[Q_X] = user.angularPosC;

vrpn_Tracker::d_quat[Q_Y] = -user.angularPosD;

vrpn_Tracker::d_quat[Q_Z] = user.angularPosB;

q_normalize(vrpn_Tracker::d_quat, vrpn_Tracker::d_quat);

vrpn_Tracker_Server::report_pose(0, vrpn_Tracker::timestamp, vrpn_Tracker::pos, vrpn_Tracker::d_quat);

vrpn_Button::buttons[0] = user.button1;

vrpn_Button::buttons[1] = user.button2;

vrpn_Button::buttons[2] = user.button3;

vrpn_Button::buttons[3] = user.button4;

vrpn_Button::buttons[4] = user.button5;

vrpn_Button::timestamp = vrpn_Tracker::timestamp;

vrpn_Button::report_changes();

// this is totally arbitrary

// I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems

// like it would be the number of 'clicks' while turning the scroll wheel...

// vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means

// the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full

// revolution....'

// this values should probably set in a configuration file.

vrpn_Dial::dials[0] = user.deltaWheel / (vrpn_float64) 16;

vrpn_Dial::timestamp = vrpn_Tracker::timestamp;

vrpn_Dial::report_changes();

}

void vrpn_Freespace::handleLinkStatus(const struct freespace_LinkStatus& l) {

// Could be used to send messages when the loop is powered off/unavailable.

}

#endif //VRPN_USE_FREESPACE