#include <ros/ros.h>
#include <std_msgs/Int8MultiArray.h>
#include <std_msgs/Float32MultiArray.h>

#include "torcsclient.h"

#include <sstream>

torcs::TorcsClient client;

void commandMsgCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    torcs::Command cmd;
    cmd.accelCmd = msg->data[0];
    cmd.steerCmd = msg->data[1];
    cmd.brakeCmd = msg->data[2];
    ROS_INFO("Received command.");
    ROS_INFO("    accel: %f", cmd.accelCmd);
    ROS_INFO("    steer: %f", cmd.steerCmd);
    ROS_INFO("    break: %f", cmd.brakeCmd);
    
    client.sendCommand(cmd);
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "torcs");

    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/commands", 1000, commandMsgCallback);
    ros::Publisher pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
    ros::Rate loop_rate(100);

    while (ros::ok()) {
        std_msgs::Int8MultiArray msg;

        msg.data.clear();
        msg.data.resize(torcs::IMAGE_SIZE_BYTES);
        const auto imageData = client.getScreenshot();
        memcpy(msg.data.data(), imageData->data(), imageData->size());
        pub.publish(msg);
        loop_rate.sleep();
    }


    return 0;
}