/*
 * 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/flipperVelocity.h"
#include <std_msgs/Float64.h>

#define LOG_START        "NegotiatorGazeboFlipper :: "
#define FREQUENCY        0.01f

#define NSEC2SEC(n) ((n) / 1000000000.0f)
#define SEC2USEC(n) ((n) * 1000000)

namespace negotiator {
namespace gazebo {

void FVGThread::run() {
    control.threadLoop();
}

FlipperVelocityGazebo::FlipperVelocityGazebo() :
    operating(false),
    frequency(FREQUENCY),
    lastTime(0),
    velocity(0),
    leftPosition(0),
    rightPosition(0),
    thread(*this)
{}

FlipperVelocityGazebo::~FlipperVelocityGazebo() {
}

void FlipperVelocityGazebo::start() {
    // Subscribe
    ros::NodeHandle node;
    jointControlSub = node.subscribe("joint_control", 1, &FlipperVelocityGazebo::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;

    // Start publishing thread
    operating = true;
    thread.start();
}

void FlipperVelocityGazebo::stop() {
    // Stop thread
    operating = false;

    // Un-subscribe
    jointControlSub.shutdown();
    transmissionLeftPub.shutdown();
    transmissionRightPub.shutdown();

}

void FlipperVelocityGazebo::callbackJointControl(sensor_msgs::JointStateConstPtr joints) {
    // Iterate and find flipper control signals
    for (int i = 0; i != joints->name.size(); ++i) {
        if (joints->name[i] == "flipper") {
            velocity = joints->velocity[i];
        }
    }
}

void FlipperVelocityGazebo::threadLoop() {
    while(operating) {
        ros::Time currentTime = ros::Time::now();

        float timeDiff = NSEC2SEC(currentTime.nsec - lastTime.nsec);
        //ROS_INFO("%sTimediff: %lf", LOG_START, timeDiff);
        leftPosition += frequency * velocity;
        rightPosition += frequency * velocity;

        // 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);

        usleep(SEC2USEC(frequency));
    }
}

} // namespace gazebo
} // namespace negotiator

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

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

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

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

    flippers.stop();
}
