#include <iostream>
#include <WinSock2.h>
#include <WS2tcpip.h>
#include <thread>
#include <atomic>
#include <mutex>

#pragma comment(lib, "Ws2_32.lib")

struct DataIn
{
    float AccX, AccY, AccZ;
    float GyrX, GyrY, GyrZ;
    float PosX, PosY, LaserRange;
} dataIn;

struct DataOut
{
    float MotorUL, MotorUR, MotorDL, MotorDR;
} dataOut;

std::atomic<bool> running(true);  // Флаг работы потоков
std::mutex dataMutex;  // Мьютекс для синхронизации доступа к dataIn и dataOut

SOCKET ConnectToServer(const char* ip, int port)
{
    WSAData wsaData;
    WORD DLLVersion = MAKEWORD(2, 1);

    if (WSAStartup(DLLVersion, &wsaData) != 0)
    {
        std::cout << "Error: WSAStartup failed\n";
        return INVALID_SOCKET;
    }

    SOCKADDR_IN addr;
    inet_pton(AF_INET, ip, &addr.sin_addr);
    addr.sin_port = htons(port);
    addr.sin_family = AF_INET;

    SOCKET Connection = socket(AF_INET, SOCK_STREAM, 0);
    if (Connection == INVALID_SOCKET)
    {
        std::cout << "Error: Failed to create socket\n";
        WSACleanup();
        return INVALID_SOCKET;
    }

    if (connect(Connection, (SOCKADDR*)&addr, sizeof(addr)) != 0)
    {
        std::cout << "Error: Failed to connect to server\n";
        closesocket(Connection);
        WSACleanup();
        return INVALID_SOCKET;
    }

    return Connection;
}

void CloseConnection(SOCKET& socket)
{
    if (socket != INVALID_SOCKET)
    {
        closesocket(socket);
        WSACleanup();
        socket = INVALID_SOCKET;
    }
}

// Функция приема данных
void ReceiveHandler(SOCKET socket)
{
    while (running)
    {
        DataIn tempData;
        int result = recv(socket, (char*)&tempData, sizeof(tempData), 0);

        if (result <= 0)
        {
            std::cout << "Error: Connection lost (recv failed)\n";
            running = false;
            break;
        }

        // Потокобезопасное обновление данных
        std::lock_guard<std::mutex> lock(dataMutex);
        dataIn = tempData;
    }
}

// Функция отправки данных
void SendHandler(SOCKET socket)
{
    while (running)
    {
        {
            std::lock_guard<std::mutex> lock(dataMutex);
            send(socket, (char*)&dataOut, sizeof(dataOut), 0);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));  // Задержка для уменьшения нагрузки
    }
}

int main()
{
    SOCKET Connection = ConnectToServer("127.0.0.1", 1001);
    if (Connection == INVALID_SOCKET)
    {
        return 1;
    }

    dataOut.MotorUL = 10;
    dataOut.MotorUR = 10;
    dataOut.MotorDL = 10;
    dataOut.MotorDR = 10;

    // Создание потоков
    std::thread recvThread(ReceiveHandler, Connection);
    std::thread sendThread(SendHandler, Connection);

    for (int i = 0; i < 1000000; i++)
    {
        {
            std::lock_guard<std::mutex> lock(dataMutex);
            std::cout << "Laser Range: " << dataIn.LaserRange << std::endl;
        }
        std::cout << "-----------------------------------------------------------" << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(500));  // Задержка вывода
    }

    // Завершаем работу потоков
    running = false;
    recvThread.join();
    sendThread.join();

    CloseConnection(Connection);

    system("pause");
    return 0;
}