#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"
#include "torcsclient.h"

#include <sstream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "camera");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::Float64MultiArray>("/camera", 1000);
    ros::Rate loop_rate(10);
    int count = 0;

    torcs::TorcsClient client;

    while (ros::ok())
    {
        std_msgs::Float64MultiArray::Ptr msg(new std_msgs::Float64MultiArray);
	msg->data.resize(torcs::IMAGE_SIZE_BYTES);
        
        const auto imageData = client.getScreenshot();
        memcpy(msg->data.data(), imageData->data(), imageData->size());
        
        ROS_INFO("%f", msg->data[0]);
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
    return 0;
}