/*
 * xsens.h
 *
 *  Created on: 04/09/2012
 *      Author: mmcgill
 */

#ifndef CASUALTY_XSENS_H_
#define CASUALTY_XSENS_H_

#include <xsens/MTComm.h>

#include <crosbot/handle.hpp>
#include <tf/tf.h>

namespace casualty {

#define DEFAULT_XSENS_PORT "/dev/ttyUSB0"

class XSens : public crosbot::HandledObject {
protected:
	enum Rotation { Forward, Left, Back, Right };

	tf::Quaternion orientation;
	Rotation rotation;
	CMTComm commPort;

	std::string port;
	bool resetOnStart;

public:
	XSens(std::string port = DEFAULT_XSENS_PORT, bool resetOnStart = false) :
		rotation(Forward), port(port), resetOnStart(resetOnStart)
    {}
	virtual ~XSens() {};

	virtual void connect();
	virtual void disconnect();

	std::string getPort() { return port; };
	bool getResetOnStart() { return resetOnStart; };
	virtual tf::Quaternion getOrientation();

	void setPort(std::string port) { this->port = port; };
	void setResetOnStart(bool resetOnStart) { this->resetOnStart = resetOnStart; };
};
typedef crosbot::Handle<XSens> XSensPtr;

} // namespace casualty

#endif /* CASUALTY_XSENS_H_ */
