/*
 * xsens.cpp
 *
 *  Created on: 23/09/2014
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <xsens/xsens.h>

namespace casualty {

void XSens::connect() {
	//open port
	int commErr;

	ROS_INFO("XSens: Connecting to xSens on port %s.\n", port.c_str());
	if (commPort.openPort (port.c_str()) != MTRV_OK) {
		ROS_ERROR("XSens: Could not connect to xsens.\n");
		return;
	}
	commPort.flush();

//	tmplen = commPort.writeMessage(MID_RESETACK);
	if (commPort.writeMessage(MID_RESET) != MTRV_OK) {
		ROS_ERROR("Xsens did not ACK reset message.\n");
		return;
	}

	unsigned char msg;
	unsigned char msgdata[MAXMSGLEN];
	short msglen;
	while (commPort.readMessage(msg, msgdata, msglen) == MTRV_OK) {
		if (msg == MID_WAKEUP) {
			msglen = commPort.writeMessage(MID_WAKEUPACK);
			break;
		} else {
			ROS_INFO("XSens: wakeup received unexpected message %x\n", msg);
		}
	}

	// Put MTi/MTx in Config State
	if (commPort.writeMessage (MID_GOTOCONFIG) != MTRV_OK) {
		ROS_ERROR("XSens: No xsens device connected\n");
		return;
	}

	unsigned short numDevices;
	// Get current settings and check if Xbus Master is connected
	if (commPort.getDeviceMode (&numDevices) != MTRV_OK) {
		if (numDevices == 1) {
			ROS_ERROR("XSens: MTi / MTx has not been detected. Could not get device mode!\n");
			return;
		} else {
			ROS_ERROR("Not just MTi / MTx connected to Xbus. Could not get all device modes!\n");
			return;
		}
	}

	// Check if Xbus Master is connected
	unsigned long outputMode, outputSettings;
	unsigned short dataLength;
	commPort.getMode (outputMode, outputSettings, dataLength, BID_MASTER);
	if (outputMode == OUTPUTMODE_XM) {
		// If Xbus Master is connected, attached Motion Trackers should not send sample counter
		ROS_ERROR("XSens: Sorry, this driver only talks to one MTx/MTi device.\n");
		return;
	}

	outputMode = OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT;
	// Set output mode and output settings for the MTi/MTx
	if (commPort.setDeviceMode (outputMode, OUTPUTSETTINGS_ORIENTMODE_QUATERNION, BID_MASTER) != MTRV_OK) {
		ROS_ERROR("XSens: Could not set device mode(s)!\n");
		return;
	}

	// Put MTi/MTx in Measurement State
	commPort.writeMessage (MID_GOTOMEASUREMENT);

	if (resetOnStart) {
		ROS_INFO("XSens: Resetting orientation\n");
		commErr = commPort.writeMessage (MID_RESETORIENTATION, RESETORIENTATION_GLOBAL, LEN_RESETORIENTATION, BID_MASTER);
		if (commErr != MTRV_OK) {
			ROS_ERROR("Could not reset orientation on device! Error 0x%x\n", commErr);
		}
	}
}

void XSens::disconnect() {
    if (commPort.isPortOpen() || commPort.isFileOpen()) {
		// close port
    	commPort.writeMessage(MID_RESET);
        // Close the MTx device
        if (commPort.close () != MTRV_OK) {
    		ROS_ERROR("Could not close xsens device!\n");
        }
    }
}

tf::Quaternion XSens::getOrientation() {
	if (!(commPort.isFileOpen() || commPort.isPortOpen())) {
		usleep(10000);
		return orientation;
	}

	unsigned char data[MAXMSGLEN];
	short datalen;
	int commErr;
	float xSensValues[9];
	tf::Quaternion orientation;
	commPort.flush();
	commErr = commPort.readDataMessage (data, datalen);

	// Need to test this!
	if (commErr != MTRV_OK) {
		ROS_ERROR("XSens: Unable to read from device. (%d)\n", commErr);
		return this->orientation;
	}

	// Parse and get value (Quaternion orientation)
	commPort.getValue(VALUE_ORIENT_QUAT, xSensValues, data, BID_MASTER);

	switch (rotation) {
	case Forward:
		orientation = tf::Quaternion(xSensValues[1], xSensValues[2], xSensValues[3], xSensValues[0]); break;
	case Left:
		orientation = tf::Quaternion(-xSensValues[2], xSensValues[1], xSensValues[3], xSensValues[0]); break;
	case Back:
		orientation = tf::Quaternion(-xSensValues[1], -xSensValues[2], xSensValues[3], xSensValues[0]); break;
	case Right:
		orientation = tf::Quaternion(xSensValues[2], -xSensValues[1], xSensValues[3], xSensValues[0]); break;
	}

	this->orientation = orientation;

	return orientation;
}

} // namespace casualty
