// SPDX-FileCopyrightText: Copyright 2024-2026 shadPS4 Emulator Project
// SPDX-License-Identifier: GPL-2.0-or-later

#include <sstream>
#include <unordered_set>
#include <SDL3/SDL.h>
#include <common/elf_info.h>
#include <common/singleton.h>
#include "common/logging/log.h"
#include "controller.h"
#include "core/emulator_settings.h"
#include "core/libraries/kernel/time.h"
#include "core/libraries/pad/pad.h"
#include "core/libraries/system/userservice.h"
#include "core/user_settings.h"
#include "input/controller.h"

namespace Input {

using Libraries::Pad::OrbisPadButtonDataOffset;

void State::OnButton(OrbisPadButtonDataOffset button, bool isPressed) {
    if (isPressed) {
        buttonsState |= button;
    } else {
        buttonsState &= ~button;
    }
}

void State::OnAxis(Axis axis, int value, bool smooth) {
    auto const i = std::to_underlying(axis);
    // forcibly finish the previous smoothing task by jumping to the end
    axes[i] = axis_smoothing_end_values[i];

    axis_smoothing_start_times[i] = time;
    axis_smoothing_start_values[i] = axes[i];
    axis_smoothing_end_values[i] = value;
    axis_smoothing_flags[i] = smooth;
    const auto toggle = [&](const auto button) {
        if (value > 0) {
            buttonsState |= button;
        } else {
            buttonsState &= ~button;
        }
    };
    switch (axis) {
    case Axis::TriggerLeft:
        toggle(OrbisPadButtonDataOffset::L2);
        break;
    case Axis::TriggerRight:
        toggle(OrbisPadButtonDataOffset::R2);
        break;
    default:
        break;
    }
}

void State::OnTouchpad(int touchIndex, bool isDown, float x, float y) {
    touchpad[touchIndex].state = isDown;
    touchpad[touchIndex].x = static_cast<u16>(x * 1920);
    touchpad[touchIndex].y = static_cast<u16>(y * 941);
}

void State::OnGyro(const float gyro[3]) {
    angularVelocity.x = gyro[0];
    angularVelocity.y = gyro[1];
    angularVelocity.z = gyro[2];
}

void State::OnAccel(const float accel[3]) {
    acceleration.x = accel[0];
    acceleration.y = accel[1];
    acceleration.z = accel[2];
}

void State::UpdateAxisSmoothing() {
    for (int i = 0; i < std::to_underlying(Axis::AxisMax); i++) {
        // if it's not to be smoothed or close enough, just jump to the end
        if (!axis_smoothing_flags[i] || std::abs(axes[i] - axis_smoothing_end_values[i]) < 16) {
            if (axes[i] != axis_smoothing_end_values[i]) {
                axes[i] = axis_smoothing_end_values[i];
            }
            continue;
        }
        auto now = Libraries::Kernel::sceKernelGetProcessTime();
        f32 t =
            std::clamp((now - axis_smoothing_start_times[i]) / f32{axis_smoothing_time}, 0.f, 1.f);
        axes[i] = s32(axis_smoothing_start_values[i] * (1 - t) + axis_smoothing_end_values[i] * t);
    }
}

GameController::GameController() : m_states_queue(64) {}

void GameController::ReadState(State* state, bool* isConnected, int* connectedCount) {
    *isConnected = m_connected;
    *connectedCount = m_connected_count;
    *state = m_state;
}

int GameController::ReadStates(State* states, int states_num, bool* isConnected,
                               int* connectedCount) {
    *isConnected = m_connected;
    *connectedCount = m_connected_count;

    int ret_num = 0;
    if (m_connected) {
        std::lock_guard lg(m_states_queue_mutex);
        for (int i = 0; i < states_num; i++) {
            auto o_state = m_states_queue.Pop();
            if (!o_state) {
                break;
            }
            states[ret_num++] = *o_state;
        }
    }
    return ret_num;
}

void GameController::Button(OrbisPadButtonDataOffset button, bool is_pressed) {
    m_state.OnButton(button, is_pressed);
    PushState();
}

void GameController::Axis(Input::Axis axis, int value, bool smooth) {
    m_state.OnAxis(axis, value, smooth);
    PushState();
}

void GameController::Gyro(int id) {
    m_state.OnGyro(gyro_buf);
    PushState();
}

void GameController::Acceleration(int id) {
    m_state.OnAccel(accel_buf);
    PushState();
}

void GameController::UpdateGyro(const float gyro[3]) {
    std::scoped_lock l(m_states_queue_mutex);
    std::memcpy(gyro_buf, gyro, sizeof(gyro_buf));
}

void GameController::UpdateAcceleration(const float acceleration[3]) {
    std::scoped_lock l(m_states_queue_mutex);
    std::memcpy(accel_buf, acceleration, sizeof(accel_buf));
}

void GameController::UpdateAxisSmoothing() {
    m_state.UpdateAxisSmoothing();
}

void GameController::SetLightBarRGB(u8 r, u8 g, u8 b) {
    colour = {r, g, b};
    if (m_sdl_gamepad != nullptr) {
        SDL_SetGamepadLED(m_sdl_gamepad, r, g, b);
    }
}

Colour GameController::GetLightBarRGB() {
    return colour;
}

void GameController::PollLightColour() {
    if (m_sdl_gamepad != nullptr) {
        SDL_SetGamepadLED(m_sdl_gamepad, colour.r, colour.g, colour.b);
    }
}

bool GameController::SetVibration(u8 smallMotor, u8 largeMotor) {
    if (m_sdl_gamepad != nullptr) {
        return SDL_RumbleGamepad(m_sdl_gamepad, (smallMotor / 255.0f) * 0xFFFF,
                                 (largeMotor / 255.0f) * 0xFFFF, -1);
    }
    return true;
}

void GameController::SetTouchpadState(int touchIndex, bool touchDown, float x, float y) {
    if (touchIndex < 2) {
        bool was_pressed = m_state.touchpad[0].state || m_state.touchpad[1].state;
        m_state.OnTouchpad(touchIndex, touchDown, x, y);
        PushState();
        if (!m_state.touchpad[0].state && !m_state.touchpad[1].state && was_pressed) {
            last_touch_down_timestamp = 0;
        } else if ((m_state.touchpad[0].state || m_state.touchpad[1].state) && !was_pressed) {
            last_touch_down_timestamp = m_state.time;
        }
    }
}

std::array<std::optional<Colour>, 4> GameControllers::controller_override_colors{
    std::nullopt, std::nullopt, std::nullopt, std::nullopt};

void GameControllers::CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration,
                                           Libraries::Pad::OrbisFVector3& angularVelocity,
                                           float deltaTime,
                                           Libraries::Pad::OrbisFQuaternion& lastOrientation,
                                           Libraries::Pad::OrbisFQuaternion& orientation) {
    // avoid wildly off values coming from elapsed time between two samples
    // being too high, such as on the first time the controller is polled
    if (deltaTime > 1.0f) {
        orientation = lastOrientation;
        return;
    }
    Libraries::Pad::OrbisFQuaternion q = lastOrientation;
    Libraries::Pad::OrbisFQuaternion ω = {angularVelocity.x, angularVelocity.y, angularVelocity.z,
                                          0.0f};

    Libraries::Pad::OrbisFQuaternion qω = {q.w * ω.x + q.x * ω.w + q.y * ω.z - q.z * ω.y,
                                           q.w * ω.y + q.y * ω.w + q.z * ω.x - q.x * ω.z,
                                           q.w * ω.z + q.z * ω.w + q.x * ω.y - q.y * ω.x,
                                           q.w * ω.w - q.x * ω.x - q.y * ω.y - q.z * ω.z};

    Libraries::Pad::OrbisFQuaternion qDot = {0.5f * qω.x, 0.5f * qω.y, 0.5f * qω.z, 0.5f * qω.w};

    q.x += qDot.x * deltaTime;
    q.y += qDot.y * deltaTime;
    q.z += qDot.z * deltaTime;
    q.w += qDot.w * deltaTime;

    float norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
    q.x /= norm;
    q.y /= norm;
    q.z /= norm;
    q.w /= norm;

    orientation.x = q.x;
    orientation.y = q.y;
    orientation.z = q.z;
    orientation.w = q.w;
    LOG_DEBUG(Lib_Pad, "Calculated orientation: {:.2f} {:.2f} {:.2f} {:.2f}", orientation.x,
              orientation.y, orientation.z, orientation.w);
}

bool is_first_check = true;

void GameControllers::TryOpenSDLControllers() {
    using namespace Libraries::UserService;
    int controller_count;
    s32 move_count = 0;
    SDL_JoystickID* new_joysticks = SDL_GetGamepads(&controller_count);
    LOG_INFO(Input, "{} controllers are currently connected", controller_count);

    std::unordered_set<SDL_JoystickID> assigned_ids;
    std::array<bool, 4> slot_taken{false, false, false, false};

    for (int i = 0; i < 4; i++) {
        SDL_Gamepad* pad = controllers[i]->m_sdl_gamepad;
        if (pad) {
            SDL_JoystickID id = SDL_GetGamepadID(pad);
            bool still_connected = false;
            ControllerType type = ControllerType::Standard;
            for (int j = 0; j < controller_count; j++) {
                if (new_joysticks[j] == id) {
                    still_connected = true;
                    assigned_ids.insert(id);
                    slot_taken[i] = true;
                    break;
                }
            }
            if (!still_connected) {
                auto u = UserManagement.GetUserByID(controllers[i]->user_id);
                UserManagement.LogoutUser(u);
                SDL_CloseGamepad(pad);
                controllers[i]->m_sdl_gamepad = nullptr;
                controllers[i]->user_id = -1;
                slot_taken[i] = false;
            }
        }
    }

    for (int j = 0; j < controller_count; j++) {
        SDL_JoystickID id = new_joysticks[j];
        if (assigned_ids.contains(id))
            continue;

        SDL_Gamepad* pad = SDL_OpenGamepad(id);
        if (!pad) {
            continue;
        }

        for (int i = 0; i < 4; i++) {
            if (!slot_taken[i]) {
                auto u = UserManagement.GetUserByPlayerIndex(i + 1);
                if (!u) {
                    LOG_INFO(Input, "User {} not found", i + 1);
                    continue; // for now, if you don't specify who Player N is in the config,
                              // Player N won't be registered at all
                }
                auto* c = controllers[i];
                c->m_sdl_gamepad = pad;
                LOG_INFO(Input, "Gamepad registered for slot {}! Handle: {}", i,
                         SDL_GetGamepadID(pad));
                c->user_id = u->user_id;
                slot_taken[i] = true;
                UserManagement.LoginUser(u, i + 1);
                if (EmulatorSettings.IsMotionControlsEnabled()) {
                    if (SDL_SetGamepadSensorEnabled(c->m_sdl_gamepad, SDL_SENSOR_GYRO, true)) {
                        c->gyro_poll_rate =
                            SDL_GetGamepadSensorDataRate(c->m_sdl_gamepad, SDL_SENSOR_GYRO);
                        LOG_INFO(Input, "Gyro initialized, poll rate: {}", c->gyro_poll_rate);
                    } else {
                        LOG_ERROR(Input, "Failed to initialize gyro controls for gamepad {}",
                                  c->user_id);
                    }
                    if (SDL_SetGamepadSensorEnabled(c->m_sdl_gamepad, SDL_SENSOR_ACCEL, true)) {
                        c->accel_poll_rate =
                            SDL_GetGamepadSensorDataRate(c->m_sdl_gamepad, SDL_SENSOR_ACCEL);
                        LOG_INFO(Input, "Accel initialized, poll rate: {}", c->accel_poll_rate);
                    } else {
                        LOG_ERROR(Input, "Failed to initialize accel controls for gamepad {}",
                                  c->user_id);
                    }
                }
                break;
            }
        }
    }
    if (is_first_check) [[unlikely]] {
        is_first_check = false;
        if (controller_count - move_count == 0) {
            auto u = UserManagement.GetUserByPlayerIndex(1);
            controllers[0]->user_id = u->user_id;
            UserManagement.LoginUser(u, 1);
        }
    }
    SDL_free(new_joysticks);
}
u8 GameController::GetTouchCount() {
    return m_touch_count;
}

void GameController::SetTouchCount(u8 touchCount) {
    m_touch_count = touchCount;
}

u8 GameController::GetSecondaryTouchCount() {
    return m_secondary_touch_count;
}

void GameController::SetSecondaryTouchCount(u8 touchCount) {
    m_secondary_touch_count = touchCount;
    if (touchCount == 0) {
        m_was_secondary_reset = true;
    }
}

u8 GameController::GetPreviousTouchNum() {
    return m_previous_touchnum;
}

void GameController::SetPreviousTouchNum(u8 touchNum) {
    m_previous_touchnum = touchNum;
}

bool GameController::WasSecondaryTouchReset() {
    return m_was_secondary_reset;
}

void GameController::UnsetSecondaryTouchResetBool() {
    m_was_secondary_reset = false;
}

void GameController::SetLastOrientation(Libraries::Pad::OrbisFQuaternion& orientation) {
    m_orientation = orientation;
}

Libraries::Pad::OrbisFQuaternion GameController::GetLastOrientation() {
    return m_orientation;
}

std::chrono::steady_clock::time_point GameController::GetLastUpdate() {
    return m_last_update;
}

void GameController::SetLastUpdate(std::chrono::steady_clock::time_point lastUpdate) {
    m_last_update = lastUpdate;
}

void GameController::PushState() {
    std::lock_guard lg(m_states_queue_mutex);
    m_state.time = Libraries::Kernel::sceKernelGetProcessTime();
    m_states_queue.Push(m_state);
}

u8 GameControllers::GetGamepadIndexFromJoystickId(SDL_JoystickID id) {
    auto g = SDL_GetGamepadFromID(id);
    ASSERT(g != nullptr);
    for (int i = 0; i < 5; i++) {
        if (controllers[i]->m_sdl_gamepad == g) {
            return i;
        }
    }
    // LOG_TRACE(Input, "Gamepad index: {}", index);
    return -1;
}

std::optional<u8> GameControllers::GetControllerIndexFromUserID(s32 user_id) {
    auto const u = UserManagement.GetUserByID(user_id);
    if (!u) {
        return std::nullopt;
    }
    return u->player_index - 1;
}

std::optional<u8> GameControllers::GetControllerIndexFromControllerID(s32 controller_id) {
    if (controller_id < 1 || controller_id > 5) {
        return std::nullopt;
    }
    return controller_id - 1;
}

} // namespace Input
