/*
 * autolevel.cpp
 *
 *  Created on: 06/09/2012
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <autolevel/autolevel.hpp>
#include <crosbot/geometry.hpp>

#include <tf/transform_broadcaster.h>

#include <geometry_msgs/Quaternion.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Int32.h>

#define DEFAULT_CHILD_FRAME             "base_link"
#define DEFAULT_PARENT_FRAME            "base_footprint"
#define DEFAULT_LEVEL_SCAN_JOINT_NAME   "level_scan"
#define DEFAULT_PITCH_JOINT_NAME        "level_pitch"
#define DEFAULT_ROLL_JOINT_NAME         "level_roll"

using namespace casualty;

class ROSAutoLevel : public AutoLevel {
protected:
	// ROS subscribers/publishers
    ros::Subscriber laserSub;
	ros::Publisher orientPub, jointPub, laserPub, statePub;
	tf::TransformBroadcaster tfBroad;

	ros::Time firstReady;

	// Configuration
	std::string parentFrame;
	std::string childFrame;
	std::string levelScanJointName;
	std::string pitchJointName;
	std::string rollJointName;

	StowState prevPubState;

public:
	ROSAutoLevel() :
	    parentFrame(DEFAULT_PARENT_FRAME),
	    childFrame(DEFAULT_CHILD_FRAME),
	    prevPubState(StowState::Unknown)
    {}

	virtual ~ROSAutoLevel() {
		orientPub.shutdown();
		jointPub.shutdown();
	}

	void callbackLaser(sensor_msgs::LaserScanConstPtr scan) {
		if (state != Ready) {
			firstReady = ros::Time(0,0);
		} else {
			if (firstReady == ros::Time(0,0)) {
				firstReady = ros::Time::now();
			}

			if (firstReady < scan->header.stamp) {
				laserPub.publish(scan);
			}
		}
	}

	void connect(ros::NodeHandle nh) {
		orientPub = nh.advertise<geometry_msgs::Quaternion>("/orientation", 1);
		jointPub = nh.advertise<sensor_msgs::JointState>("/joint_states", 1);
		laserPub = nh.advertise<sensor_msgs::LaserScan>("valid_scan", 1);
		statePub = nh.advertise<std_msgs::Int32>("state", 1);

		laserSub = nh.subscribe("/scan", 1, &ROSAutoLevel::callbackLaser, this);

		// Configure
		ros::NodeHandle local("~");
		ConfigElementPtr localConfig = new ROSConfigElement(local);

		// Super-class configure
		configure(localConfig);

		// Configure
		parentFrame = localConfig->getParam("parent_frame", DEFAULT_PARENT_FRAME);
		childFrame = localConfig->getParam("child_frame", DEFAULT_CHILD_FRAME);
		levelScanJointName = localConfig->getParam("level_scan_joint_name", DEFAULT_LEVEL_SCAN_JOINT_NAME);
		pitchJointName = localConfig->getParam("pitch_joint_name", DEFAULT_PITCH_JOINT_NAME);
		rollJointName = localConfig->getParam("roll_joint_name", DEFAULT_ROLL_JOINT_NAME);

		start();
	}

	virtual void updateOrientation(const Quaternion& orientation) {
		orientPub.publish(orientation.toROS());
	}

	virtual void updateJoints(double pitch, double roll, double scan) {
//		ROS_INFO("Autolevel: Angles: pitch %.1lf, roll %.1lf, scan %.1lf\n", RAD2DEG(pitch), RAD2DEG(roll), RAD2DEG(scan));
		sensor_msgs::JointState jstate;
		ros::Time stamp = ros::Time::now();
		jstate.header.stamp = stamp;
		jstate.name.push_back(pitchJointName);
		jstate.position.push_back(pitch);
		jstate.name.push_back(rollJointName);
		jstate.position.push_back(roll);
		jstate.name.push_back(levelScanJointName);
		jstate.position.push_back(scan);

		jointPub.publish(jstate);


		tf::Quaternion orientation = xsens->getOrientation();
		tf::Matrix3x3 mat(orientation);
		double r,p,y;
		mat.getEulerYPR(y, p, r);

//		ROS_INFO("Autolevel: Angles: r %.1lf, p %.1lf, y %.1lf\n", RAD2DEG(r), RAD2DEG(p), RAD2DEG(y));
		tf::Quaternion tfq;
		tfq.setRPY(r,p,0);

		tf::StampedTransform st;
		st.stamp_ = stamp;
		st.child_frame_id_ = childFrame;
		st.frame_id_ = parentFrame;
		st.setIdentity();
//		st.setOrigin(tf::Vector3(0, 0, 1));
		st.setRotation(tfq);

		tfBroad.sendTransform(st);

		// Send Autolevel state
		if (state != prevPubState) {
            std_msgs::Int32 stateMsg;
            stateMsg.data = state;
            statePub.publish(stateMsg);
            prevPubState = state;
		}
	}
};
typedef Handle<ROSAutoLevel> ROSAutoLevelPtr;

int main(int argc, char** argv) {
    ros::init(argc, argv, "autolevel");
    ros::NodeHandle nh;

    ROSAutoLevelPtr level = new ROSAutoLevel();
    level->connect(nh);


    while (ros::ok()) {
        ros::spin();
    }

    level->stop();

	return 0;
}

