#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <sstream>

void publish_value(ros::Publisher output_pub1, ros::Publisher output_pub2, double * timelast)  {
//    std_msgs::Float64 msg; //create message
//    msg.data = ros::Time::now().toSec(); //get current time in Seconds (float)
//    output_pub.publish (msg); //publish message
    double timenow = ros::Time::now().toSec();
    double timeelapsed = timenow - *timelast;
    std_msgs::Float64 msg1; //create message
    std_msgs::String msg2; //create message
    std::stringstream ss;
    ss << "Now:" << std::fixed << std::setw( 11 ) << std::setprecision( 6 )  << std::setfill( '0' ) << timenow;
    ss << " Last:" << *timelast << " | Elapsed:" << timeelapsed; //get current time in Seconds (float)
    msg1.data = timeelapsed;
    msg2.data = ss.str();
    output_pub1.publish (msg1); //publish message
    output_pub2.publish (msg2); //publish message
    *timelast = timenow;
    return;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "timing_src"); //registers node with ROS master
    ros::NodeHandle n; //get the handle for node
//    ros::NodeHandle n("~");  //get the handle for node
    ros::Rate loop_rate(5000);  //define rate for repeatable operations
    ros::Publisher output_pub1; //define the publisher object with global scope
    ros::Publisher output_pub2; //define the publisher object with global scope
    output_pub1 = n.advertise<std_msgs::Float64>("signal",1); //register with the system that we're publishing to a topic.
    output_pub2 = n.advertise<std_msgs::String>("string",1); //register with the system that we're publishing to a topic.
//   output_pub = n.advertise<std_msgs::Float64>("signal",1); //register with the system that we're publishing to a topic.
    double timelast = 0.;
    while (ros::ok())
    {
        publish_value(output_pub1, output_pub2, &timelast);
	ros::spinOnce();  //process all incoming messages
        loop_rate.sleep();  //wait until defined time passes.
    }
}


