/*
 * 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/negotiator/flipperPosition.h"

#include <std_msgs/Float64.h>

#define LOG_START        "NegotiatorGazeboFlipper :: "

namespace negotiator {
namespace gazebo {

FlipperPositionGazebo::FlipperPositionGazebo() :
    lastTime(0),
    leftPosition(0),
    rightPosition(0)
{}

FlipperPositionGazebo::~FlipperPositionGazebo() {
}

void FlipperPositionGazebo::start() {
    // Subscribe
    ros::NodeHandle node;
    jointControlSub = node.subscribe("joint_control", 1, &FlipperPositionGazebo::callbackJointControl, this);
    transmissionLeftPub = node.advertise<std_msgs::Float64>("/negotiator/flipper_left_controller/command", 1);
    transmissionRightPub = node.advertise<std_msgs::Float64>("/negotiator/flipper_right_controller/command", 1);

    lastTime = ros::Time::now();
    leftPosition = 0;
    rightPosition = 0;
}

void FlipperPositionGazebo::stop() {
    // Un-subscribe
    jointControlSub.shutdown();
    transmissionLeftPub.shutdown();
    transmissionRightPub.shutdown();
}

void FlipperPositionGazebo::callbackJointControl(sensor_msgs::JointStateConstPtr joints) {
    bool combinedValue = false;

    float newLeftPosition = leftPosition;
    float newRightPosition = rightPosition;
    float newCombinedPosition = leftPosition;
    ros::Time currentTime = ros::Time::now();

    // Iterate and find flipper control signals
    for (int i = 0; i != joints->name.size(); ++i) {
        if (joints->name[i] == "flipper") {
            newCombinedPosition = joints->position[i];
            combinedValue = true;
        } else if (!combinedValue) {
            if (joints->name[i] == "flipper_left" || joints->name[i] == "flipper_left_joint") {
                newLeftPosition = joints->position[i];
            } else if (joints->name[i] == "flipper_right" || joints->name[i] == "flipper_right_joint") {
                newRightPosition = joints->position[i];
            }
        }
    }

    if (combinedValue) {
        leftPosition = newCombinedPosition;
        rightPosition = newCombinedPosition;
    } else {
        leftPosition = newLeftPosition;
        rightPosition = newRightPosition;
    }

    // Update time
    lastTime = currentTime;

    // Send value
    std_msgs::Float64 leftMsg;
    leftMsg.data = leftPosition;
    std_msgs::Float64 rightMsg;
    rightMsg.data = rightPosition;
    transmissionLeftPub.publish(leftMsg);
    transmissionRightPub.publish(rightMsg);
}

} // namespace gazebo
} // namespace negotiator

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

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

    // Gazebo flipper component
    negotiator::gazebo::FlipperPositionGazebo flippers;
    flippers.start();

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

    flippers.stop();
}
