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

#include <ros/ros.h>
#include <xsens/xsens.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <tf/transform_broadcaster.h>

#include <unistd.h>

class XSensNode : public casualty::XSens {
public:
	void connect(std::string port, bool reset) {
		this->port = port;
		resetOnStart = reset;

		casualty::XSens::connect();
	}
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "xsens");

    ros::NodeHandle nh, nhPriv("~");

    // Xsens with default params
    casualty::XSens xsens;

    // Load Config parameters
    std::string port, frame_id, base_frame, stabilized_frame, odom_frame;
    bool reset;
    nhPriv.param< std::string >("port", port, xsens.getPort());
    nhPriv.param< bool >("reset", reset, xsens.getResetOnStart());
    nhPriv.param< std::string >("frame_id", frame_id, "/xsens");
    nhPriv.param< std::string >("base_frame", base_frame, "");
    nhPriv.param< std::string >("stabilized_frame", stabilized_frame, "");
    nhPriv.param< std::string >("odom_frame", odom_frame, "");

    // Set xsens params
    xsens.setPort(port);
    xsens.setResetOnStart(reset);
    xsens.connect();

    ros::Publisher pub = nh.advertise< geometry_msgs::QuaternionStamped >("orientation", 1);
    tf::TransformBroadcaster* tfCaster = NULL;
    if (base_frame != "" && stabilized_frame != "")
    	tfCaster = new tf::TransformBroadcaster();

    uint32_t seq = 0;
    while (ros::ok()) {
		// read xSens values
		tf::Quaternion orientation = xsens.getOrientation(),
				compass;


		double r,p,y;
		tf::Matrix3x3 mat(orientation);
		mat.getEulerYPR(y, p, r);

		orientation.setRPY(0, p, r);
		compass.setRPY(y, 0, 0);

		geometry_msgs::QuaternionStamped quat;
		quat.header.stamp = ros::Time::now();
		quat.header.frame_id = frame_id;
		quat.header.seq = seq++;
		quat.quaternion.x = orientation.x();
		quat.quaternion.y = orientation.y();
		quat.quaternion.z = orientation.z();
		quat.quaternion.w = orientation.w();

		pub.publish(quat);

		if (tfCaster) {
			std::vector< tf::StampedTransform > transforms;

			tf::StampedTransform tf;
			tf.frame_id_ = stabilized_frame;
			tf.child_frame_id_ = base_frame;
			tf.stamp_ = quat.header.stamp;
			tf.setOrigin(tf::Vector3(0,0,0));
			tf.setRotation(orientation);
			transforms.push_back(tf);

			if (odom_frame != "") {
				tf.frame_id_ = odom_frame;
				tf.child_frame_id_ = stabilized_frame;
				tf.setRotation(compass);
				transforms.push_back(tf);
			}

			tfCaster->sendTransform(transforms);

		}
//		usleep(1000);
    }

	// close xSens
	ROS_INFO("XSens: Disconnecting from XSens\n");
	xsens.disconnect();

    return 0;
}


