#pragma once

#include <cstdint>
#include <array>
#include <memory>
#include <functional>
#include <std_msgs/Float32MultiArray.h>
#include <atomic>

#define DEBUG_LOG false

constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;

namespace torcs
{

constexpr auto IMAGE_WIDTH = 640u;
constexpr auto IMAGE_HEIGHT = 480u;
constexpr auto IMAGE_CHANNELS = 3u;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;

constexpr auto SEMANTIC_WIDTH = 320;
constexpr auto SEMANTIC_HEIGHT = 660;

struct shared_use_st
{
    int written = 0;  //a label, if 1: available to read, if 0: available to write
    uint8_t data[IMAGE_SIZE_BYTES];  // image data field
    int control = 0;
    int pause = 0;
    double fast = 0.0;

    double dist_L = 0.0;
    double dist_R = 0.0;

    double toMarking_L = 0.0;
    double toMarking_M = 0.0;
    double toMarking_R = 0.0;

    double dist_LL = 0.0;
    double dist_MM = 0.0;
    double dist_RR = 0.0;

    double toMarking_LL = 0.0;
    double toMarking_ML = 0.0;
    double toMarking_MR = 0.0;
    double toMarking_RR = 0.0;

    double toMiddle = 0.0;
    double angle = 0.0;
    double speed = 0.0;

    double steerCmd = 0.0;
    double accelCmd = 0.0;
    double brakeCmd = 0.0;
};

struct Command
{
    Command() = default;
    Command(float accel, float steer, float brake)
        : accelCmd(accel), steerCmd(steer), brakeCmd(brake)
    {
    }

    float accelCmd = 0.f;
    float steerCmd = 0.f;
    float brakeCmd = 0.f;
};

using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;

class Affordance
{
public:
    void populate(const shared_use_st& affordance)
    {
        affordance_[0] = float(affordance.fast);
        affordance_[1] = float(affordance.dist_L);
        affordance_[2] = float(affordance.dist_R);
        affordance_[3] = float(affordance.toMarking_L);
        affordance_[4] = float(affordance.toMarking_M);
        affordance_[5] = float(affordance.toMarking_R);
        affordance_[6] = float(affordance.dist_LL);
        affordance_[7] = float(affordance.dist_MM);
        affordance_[8] = float(affordance.dist_RR);
        affordance_[9] = float(affordance.toMarking_LL);
        affordance_[10] = float(affordance.toMarking_ML);
        affordance_[11] = float(affordance.toMarking_MR);
        affordance_[12] = float(affordance.toMarking_RR);
        affordance_[13] = float(affordance.toMiddle); // not used
        affordance_[14] = float(affordance.angle);
        affordance_[15] = float(affordance.speed);
    }

    void populate(const std_msgs::Float32MultiArray& affordance)
    {
        affordance_ = affordance.data;
    }

    float getFast() const { return affordance_[0]; }
    float getDistL() const { return affordance_[1]; }
    float getDistR() const { return affordance_[2]; }
    float getToMarkingL() const { return affordance_[3]; }
    float getToMarkingM() const { return affordance_[4]; }
    float getToMarkingR() const { return affordance_[5]; }
    float getDistLL() const { return affordance_[6]; }
    float getDistMM() const { return affordance_[7]; }
    float getDistRR() const { return affordance_[8]; }
    float getToMarkingLL() const { return affordance_[9]; }
    float getToMarkingML() const { return affordance_[10]; }
    float getToMarkingMR() const { return affordance_[11]; }
    float getToMarkingRR() const { return affordance_[12]; }
    float getToMiddle() const { return affordance_[13]; }
    float getAngle() const { return affordance_[14]; }
    float getSpeed() const { return affordance_[15]; }

    const std::vector<float>& getAsVector()
    {
        return affordance_;
    }

private:
    std::vector<float> affordance_ = std::vector<float>(16);
};

class TorcsClient
{
public:
    explicit TorcsClient();
    ~TorcsClient();

    void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
    void iterate();
    void sendCommand(const Command& cmd);
    void setLanesCount(int count);
    void visualizePredictions(const Affordance& affordance);

private:

    struct shared_use_st *shared;
    bool connected = false;
    std::atomic<bool> gotCommand;
    Command command;
    std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
    int lanesCount = 3; // Use three lanes configuration by default

    void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
                   float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
                   float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
                   float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
                   float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
                   float dist_L, float true_dist_L, float dist_R, float true_dist_R);
    void initVisualization();

};

}