/*
 * flipper_plugin.cpp
 *
 *  Gazebo plugin to listen to joint controls and publish to transmission the joint states
 *
 *  Created on: 17 Oct 2014
 *      Author: timothyw
 */

#include "casualty_gazebo_plugins/icp/icp.h"

#define LOG_START        "CasualtyGazeboICP :: "

namespace casualty {
namespace gazebo {

ICPGazebo::ICPGazebo()
{}

ICPGazebo::~ICPGazebo() {
}

void ICPGazebo::start() {
    // Subscribe
    ros::NodeHandle node;
    modelStateSub = node.subscribe("/gazebo/model_states", 1, &ICPGazebo::callbackModelStates, this);
}

void ICPGazebo::stop() {
    // Un-subscribe
	modelStateSub.shutdown();
}

void ICPGazebo::callbackModelStates(gazebo_msgs::ModelStatesPtr modelStates) {
    // Robot state
    geometry_msgs::Pose pose;
    geometry_msgs::Twist twist;

    // Iterate and find negotiator model
    for (int i = 0; i != modelStates->name.size(); ++i) {
        if (modelStates->name[i] == "negotiator") {
            pose = modelStates->pose[i];
            twist = modelStates->twist[i];
        }
    }

    // Broadcast transform
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(pose.position.x, pose.position.y, pose.position.z) );
	tf::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
	transform.setRotation(q);
	tfBroadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "icp_test", "base_footprint"));
}

} // namespace gazebo
} // namespace casualty

/**
 * ROS Node components
 */
int main(int argc, char** argv) {
    // Init ROS
    ros::init(argc, argv, "casualty_gazebo_icp");

    // Create ros nodehandle to initialise time etc...
    ros::NodeHandle nh("~");

    // Gazebo flipper component
    casualty::gazebo::ICPGazebo icp;
    icp.start();

    while (ros::ok()) {
        ROS_INFO("%s Spinning", LOG_START);
        ros::spin();
    }

    icp.stop();
}
