#include <destrum/Graphics/Camera.h>
#include <destrum/Input/InputManager.h>

#include <iostream>
#include <glm/gtc/matrix_inverse.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/string_cast.hpp>

#include "glm/gtx/norm.hpp"


Camera::Camera(const glm::vec3& position, const glm::vec3& up): m_position{position}, m_up{up} {
}

void Camera::Update(float deltaTime) {
    auto& input = InputManager::GetInstance();

    // --- tuning ---
    float moveSpeed = m_movementSpeed;
    if (input.IsKeyDown(SDL_SCANCODE_LSHIFT) || input.IsKeyDown(SDL_SCANCODE_RSHIFT)) {
        moveSpeed *= 2.0f;
    }

    // Controller speed boost (pad0 LB)
    const SDL_JoystickID pad0 = input.GetPadInstanceId(0);
    if (pad0 >= 0 && input.IsPadButtonDown(pad0, SDL_CONTROLLER_BUTTON_LEFTSHOULDER)) {
        moveSpeed *= 3.0f;
    }

    // Clamp pitch like your old code
    m_pitch = glm::clamp(m_pitch, -glm::half_pi<float>() + 0.01f, glm::half_pi<float>() - 0.01f);

    // =========================
    // Movement (Keyboard)
    // =========================
    glm::vec3 move(0.0f);

    if (input.IsKeyDown(SDL_SCANCODE_W)) move += m_forward;
    if (input.IsKeyDown(SDL_SCANCODE_S)) move -= m_forward;
    if (input.IsKeyDown(SDL_SCANCODE_D)) move += m_right;
    if (input.IsKeyDown(SDL_SCANCODE_A)) move -= m_right;

    if (input.IsKeyDown(SDL_SCANCODE_Q)) move += m_up;
    if (input.IsKeyDown(SDL_SCANCODE_E)) move -= m_up;

    if (glm::length2(move) > 0.0f) {
        move = glm::normalize(move);
        m_position += move * (moveSpeed * deltaTime);
    }

    // =========================
    // Movement (Controller)
    // =========================
    if (pad0 >= 0) {
        const float lx = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_LEFTX); // [-1..1]
        const float ly = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_LEFTY); // [-1..1]

        // SDL Y is typically +down, so invert for "forward"
        glm::vec3 padMove(0.0f);
        padMove += m_forward * (-ly);
        padMove += m_right   * ( lx);

        // Triggers for vertical movement (optional)
        // SDL controller triggers are axes too: 0..1-ish after normalization in our helper, but signless.
        // With our NormalizeAxis, triggers will sit near 0 until pressed (depending on mapping).
        // If your NormalizeAxis maps triggers weirdly, swap to raw event value approach.
        const float lt = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_TRIGGERLEFT);
        const float rt = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_TRIGGERRIGHT);
        const float vertical = (rt - lt);
        padMove += m_up * vertical;

        if (glm::length2(padMove) > 0.0001f) {
            // do NOT normalize: preserve analog magnitude for smooth movement
            m_position += padMove * (moveSpeed * deltaTime);
        }
    }

    // =========================
    // Look (Keyboard arrows only)
    // =========================
    // Use radians/sec so framerate-independent
    const float keyLookSpeed = glm::radians(120.0f); // degrees per second

    if (input.IsKeyDown(SDL_SCANCODE_UP))    m_pitch += keyLookSpeed * deltaTime;
    if (input.IsKeyDown(SDL_SCANCODE_DOWN))  m_pitch -= keyLookSpeed * deltaTime;
    if (input.IsKeyDown(SDL_SCANCODE_LEFT))  m_yaw   -= keyLookSpeed * deltaTime;
    if (input.IsKeyDown(SDL_SCANCODE_RIGHT)) m_yaw   += keyLookSpeed * deltaTime;

    // =========================
    // Look (Controller right stick)
    // =========================
    if (pad0 >= 0) {
        const float rx = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_RIGHTX);
        const float ry = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_RIGHTY);

        const float padLookSpeed = 2.2f; // radians/sec at full deflection
        m_yaw   += rx * padLookSpeed * deltaTime;
        m_pitch -= ry * padLookSpeed * deltaTime;
    }

    // Clamp pitch again after modifications
    m_pitch = glm::clamp(m_pitch, -glm::half_pi<float>() + 0.01f, glm::half_pi<float>() - 0.01f);

    // Recompute basis from yaw/pitch (same convention you used)
    const glm::mat4 yawMatrix   = glm::rotate(glm::mat4(1.0f), -m_yaw,   glm::vec3(0, 1, 0));
    const glm::mat4 pitchMatrix = glm::rotate(glm::mat4(1.0f),  m_pitch, glm::vec3(0, 0, 1));
    const glm::mat4 rotation    = yawMatrix * pitchMatrix;

    m_forward = glm::normalize(glm::vec3(rotation * glm::vec4(1, 0, 0, 0))); // +X forward
    m_right   = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
    m_up      = glm::normalize(glm::cross(m_right, m_forward));

    // keep target mode off when manually controlled
    m_useTarget = false;

    CalculateProjectionMatrix();
    CalculateViewMatrix();
}

void Camera::CalculateViewMatrix() {
    if (m_useTarget) {
        m_forward = glm::normalize(m_target - m_position);
        m_right = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
        m_up = glm::normalize(glm::cross(m_right, m_forward));
        m_viewMatrix = glm::lookAt(m_position, m_target, m_up);
    } else {
        m_viewMatrix = glm::lookAt(m_position, m_position + m_forward, m_up);
    }

    m_invMatrix = glm::inverse(m_viewMatrix);
}

void Camera::CalculateProjectionMatrix() {
    m_projectionMatrix = glm::perspectiveRH_ZO(glm::radians(fovAngle), m_aspectRatio, m_zNear, m_zFar);
}

void Camera::ClearTarget() {
    m_useTarget = false;
    m_forward = glm::normalize(m_target - m_position);
    m_right = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
    m_up = glm::normalize(glm::cross(m_right, m_forward));
}

void Camera::SetTarget(const glm::vec3& target) {
    m_target = target;
    m_useTarget = true;
    // m_forward = glm::normalize(m_target - m_position);
    // m_right = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
    // m_up = glm::normalize(glm::cross(m_right, m_forward));
}

void Camera::Target(const glm::vec3& target) {
    glm::vec3 directionToTarget = glm::normalize(target - m_position);

    m_forward = glm::normalize(target - m_position);
    m_right = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
    m_up = glm::normalize(glm::cross(m_right, m_forward));

    m_viewMatrix = glm::lookAt(m_position, m_position + m_forward, m_up);
    m_invMatrix = glm::inverse(m_viewMatrix);
}

void Camera::SetRotation(float yawRadians, float pitchRadians) {
    m_yaw = yawRadians;
    m_pitch = glm::clamp(
        pitchRadians,
        -glm::half_pi<float>() + 0.001f,
         glm::half_pi<float>() - 0.001f
    );

    // Yaw around world Y, pitch around local Z (same convention you used)
    const glm::mat4 yawMatrix   = glm::rotate(glm::mat4(1.0f), -m_yaw,   glm::vec3(0, 1, 0));
    const glm::mat4 pitchMatrix = glm::rotate(glm::mat4(1.0f),  m_pitch, glm::vec3(0, 0, 1));
    const glm::mat4 rotation    = yawMatrix * pitchMatrix;

    // Forward is +X in your camera space
    m_forward = glm::normalize(glm::vec3(rotation * glm::vec4(1, 0, 0, 0)));
    m_right   = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0)));
    m_up      = glm::normalize(glm::cross(m_right, m_forward));

    m_useTarget = false; // rotation overrides target mode
}

void Camera::SetRotation(const glm::vec2& yawPitchRadians) {
    SetRotation(yawPitchRadians.x, yawPitchRadians.y);
}