#include "torcsclient.h"

#include <iostream>
#include <chrono>
#include <thread>
#include <cstring>
#include <sys/shm.h>
#include <assert.h>

using namespace torcs;

TorcsClient::TorcsClient()
{
    std::cout << "TORCS Client!" << std::endl;

    void *shm = NULL;
    int shmid;

    shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
    if(shmid < 0)
    {
        std::cerr << "Failed to get shared memory!" << std::endl;
	return;
    }

    shm = shmat(shmid, 0, 0);
    if(shm == (void*)-1)
    {
        std::cerr << "Failed to shmat()!" << std::endl;
        return;
    }
    
    connected = true;

    std::cout << "Started shared memory at " << std::hex << shm << std::endl;

    shared = (struct shared_use_st*)shm;
    shared->written = 0;
    shared->control = 0;
    shared->pause = 0;
    shared->fast = 0.0;

    shared->dist_L = 0.0;
    shared->dist_R = 0.0;

    shared->toMarking_L = 0.0;
    shared->toMarking_M = 0.0;
    shared->toMarking_R = 0.0;

    shared->dist_LL = 0.0;
    shared->dist_MM = 0.0;
    shared->dist_RR = 0.0;

    shared->toMarking_LL = 0.0;
    shared->toMarking_ML = 0.0;
    shared->toMarking_MR = 0.0;
    shared->toMarking_RR = 0.0;

    shared->toMiddle = 0.0;
    shared->angle = 0.0;
    shared->speed = 0.0;

    shared->steerCmd = 0.0;
    shared->accelCmd = 0.0;
    shared->brakeCmd = 0.0;
}

std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
{
    assert(connected);

    while(shared->written == 0)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    auto result = std::make_unique<DataContainer>();
    memcpy(result->data(), shared->data, result->size());
    shared->written = 0;

    return result;
}

void TorcsClient::sendCommand(Command cmd)
{
    assert(connected);

    while (shared->control == 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    shared->control = 0;
    //shared->accelCmd = 10.0;
    //shared->steerCmd = 1.0;
}

/*void writeScreenshot(shared_use_st* shared, unsigned int& num)
{
    std::cout << "Read image." << std::endl;

    FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24);
    if (!newShot)
    {
        std::cerr << "Failed to allocate freeimage file." << std::endl;
        FreeImage_Unload(newShot);
        return;
    }

    BYTE *bits = (BYTE*)FreeImage_GetBits(newShot);

    for (int h = 0; h < IMAGE_HEIGHT; h++) {
        BYTE *pixel = (BYTE*)bits;

        for (int w = 0; w < IMAGE_WIDTH; w++) {
            pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0];
            pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1];
            pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2];
            pixel += 3;
        }
        bits += 3*IMAGE_WIDTH;
    }

    FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str());
    FreeImage_Unload(newShot);

    shared->control = 0;
    shared->written = 0;
    std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl;
}*/