#include <ros/ros.h>
    #include "std_msgs/String.h"
    #include <iostream>
    #include <sstream>
    #include <visualization_msgs/Marker.h>

    float old_posx = 0;
    unsigned int value ;

    //TEST RECEPTION LISTENER
    void chatterCallback(const std_msgs::String::ConstPtr& msg)
    {
      std::istringstream myStream (msg->data.c_str());
      myStream >> value;
      std::cout << "The value received is " << value << std::endl;
    }

    int main( int argc, char** argv )
    {
      std_msgs::String msg;
      std::stringstream ss;
      ros::init(argc, argv, "calculator");
      ros::NodeHandle n;
      ros::Subscriber sub = n.subscribe("speed_sensed", 1000, chatterCallback);
      ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
      ros::Publisher response_pub = n.advertise<std_msgs::String>("response", 1);
      ros::Rate r(1);

      // Set our initial shape type to be a cube
      uint32_t shape = visualization_msgs::Marker::ARROW;

      while (ros::ok())
      {
        ss << value;
        msg.data = ss.str();
        ss.str("");
        visualization_msgs::Marker marker;
        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = "/my_frame";
        marker.header.stamp = ros::Time::now();

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "basic_shapes";
        marker.id = 0;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = shape;

        // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
        marker.action = visualization_msgs::Marker::ADD;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        marker.pose.position.x = old_posx;
        marker.pose.position.y = 0;
        marker.pose.position.z = 0;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;


        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = 1.0;
        marker.scale.y = 1.0;
        marker.scale.z = 1.0;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;
        marker.color.a = 1.0;

        marker.lifetime = ros::Duration();
 
        // Publish the marker
        while (marker_pub.getNumSubscribers() < 1)
        {
          if (!ros::ok())
          {
            return 0;
          }
          ROS_WARN_ONCE("Please create a subscriber to the marker");
          sleep(1);
        }
        marker_pub.publish(marker);
        
        while (response_pub.getNumSubscribers() < 1)
        {
          if (!ros::ok())
          {
            return 0;
          }
          ROS_WARN_ONCE("Please create a subscriber for response");
          sleep(1);
        }
        response_pub.publish(msg);
        ros::spinOnce();

        //MON CODE 
        old_posx=value/10.0;
        r.sleep();
      }
    }