#include "torcsclient.h"

#include <sstream>
#include <thread>
#include <fstream>

#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt8MultiArray.h>

torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher;
ros::Publisher* affordancePublisher;

void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}

void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
{
    client.setLanesCount(msg->data);
}

void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    torcs::Affordance affordance;
    affordance.populate(*msg);
    client.visualizePredictions(affordance);
}

void publishCallback(std::vector<uint8_t> screenshot, torcs::Affordance affordance)
{

    cv::Mat screenRGB(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
    cv::Mat resizeRGB(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);

    uchar* ptr = screenRGB.data;
    for (int h = 0; h < torcs::IMAGE_HEIGHT; h++) {
      for (int w = 0; w < torcs::IMAGE_WIDTH; w++) {
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+2]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+0];
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+1]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+1];
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+0]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+2];
      }
    }

    //Code for debugging: displays screenshot in a window
    //cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
    //cv::imshow("Screenshot", screenRGB);

    cv::resize(screenRGB, resizeRGB, resizeRGB.size(), 0, 0);

    //Code for debugging: displays screenshot in a window
    cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
    cv::imshow("Screenshot", resizeRGB);

    std::vector<uint8_t> data(ROS_IMAGE_SIZE);
    size_t i = 0;
    for (int c = 0; c < 3; ++c) {
      for (int h = 0; h < ROS_IMAGE_HEIGHT; ++h) {
        for (int w = 0; w < ROS_IMAGE_WIDTH; ++w) {
          data[i] = resizeRGB.data[(h*ROS_IMAGE_WIDTH+w)*3+c];
          i++;
        }
      }
    }


    static_assert(sizeof(float) == sizeof(std_msgs::Float32), "Float is not 32 bit long!");

    std_msgs::UInt8MultiArray::Ptr msg(new std_msgs::UInt8MultiArray);
    msg->data.resize(ROS_IMAGE_SIZE);
    memcpy(msg->data.data(), data.data(), data.size());

    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
    speedMsg->data = affordance.getSpeed();

    std_msgs::Float32MultiArray::Ptr affordanceMsg(new std_msgs::Float32MultiArray);
    affordanceMsg->data = affordance.getAsVector();

    cameraPublisher->publish(msg);
    speedPublisher->publish(speedMsg);
    affordancePublisher->publish(affordanceMsg);
}

void clientRunner()
{
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
    while(true)
    {
        client.iterate();
    }
#pragma clang diagnostic pop
}

int main(int argc, char **argv)
{
//    cvNamedWindow("Semantic Visualization",1);
//    IplImage* background=cvLoadImage("TORCSComponent/assets/semantic_background_3lane.png");
//    IplImage* semanticRGB=cvCreateImage(cvSize(torcs::SEMANTIC_WIDTH,torcs::SEMANTIC_HEIGHT),IPL_DEPTH_8U,3);
//    cvCopy(background,semanticRGB);
//    cvShowImage("Semantic Visualization",semanticRGB);
//    cv::waitKey(0);

    client.setPublishCallback(publishCallback);
    ros::init(argc, argv, "TORCSComponent");
    ros::NodeHandle n;
    ros::Publisher cameraPub = n.advertise<std_msgs::UInt8MultiArray>("/camera", 1);
    cameraPublisher = &cameraPub;
    ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
    speedPublisher = &speedPub;
    ros::Publisher affordancePub = n.advertise<std_msgs::Float32MultiArray>("/affordance", 1);
    affordancePublisher = &affordancePub;

    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
    ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
    ros::Subscriber predictionsSub = n.subscribe("/predictions", 1, predictionsChatterCallback);
    ros::Rate loop_rate(10);
    std::thread clientRunnerThread(clientRunner);

    ros::spin();
    clientRunnerThread.join();
    return 0;
}