#include "joystick-reader.h"
#include <iostream>
#include <thread>
#include <chrono>

JoystickReader::JoystickReader(int frequency)
    : joystick(nullptr), updateIntervalMs(1000 / frequency) {}

JoystickReader::~JoystickReader() {
    if (joystick) {
        SDL_JoystickClose(joystick);
    }
    SDL_Quit();
}

bool JoystickReader::initialize() {
    if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
        std::cerr << "SDL initialization failed: " << SDL_GetError() << std::endl;
        return false;
    }

    int numJoysticks = SDL_NumJoysticks();
    if (numJoysticks < 1) {
        std::cerr << "No joysticks found" << std::endl;
        return false;
    }

    std::cout << "Found " << numJoysticks << " joystick(s)" << std::endl;

    // Открываем первый джойстик
    joystick = SDL_JoystickOpen(0);
    if (!joystick) {
        std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl;
        return false;
    }

    joystickName = SDL_JoystickName(joystick);
    std::cout << "Opened joystick: " << joystickName << std::endl;
    std::cout << "Axes: " << SDL_JoystickNumAxes(joystick)
              << ", Buttons: " << SDL_JoystickNumButtons(joystick)
              << ", Balls: " << SDL_JoystickNumBalls(joystick)
              << ", Hats: " << SDL_JoystickNumHats(joystick) << std::endl;

    return true;
}

bool JoystickReader::readData(std::vector<uint16_t>& data) {
    if (!joystick) return false;
    if (SDL_JoystickGetAttached(joystick) != SDL_TRUE) { return false; }

    SDL_JoystickUpdate();

    data.resize(16, 1500); // Заполняем нейтральным значением

    // Читаем оси (обычно до 6 осей)
    int axes = std::min(SDL_JoystickNumAxes(joystick), 6);
    for (int i = 0; i < axes; ++i) {
        Sint16 axisValue = SDL_JoystickGetAxis(joystick, i);
        // Преобразуем из [-32768, 32767] в [1000, 2000]
        data[i] = static_cast<uint16_t>((axisValue + 32768) * 1000 / 65536 + 1000);
    }

    // Читаем кнопки
    int buttons = SDL_JoystickNumButtons(joystick);
    for (int i = 0; i < buttons && i < data.size() - axes; ++i) {
        Uint8 buttonState = SDL_JoystickGetButton(joystick, i);
        data[axes + i] = buttonState ? 2000 : 1000;
    }

    for (auto& i: data) {
        if (i < 900) i = 900;
        if (i > 1100) i = 1100;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs));
    return true;
}

std::string JoystickReader::getJoystickName() const {
    return joystickName;
}