#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} {
    m_yaw = -glm::half_pi<float>();
    m_pitch = 0.0f;
}

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

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

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

    constexpr float keyLookSpeed = glm::radians(120.0f);
    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;

    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;
        m_yaw   += rx * padLookSpeed * deltaTime;
        m_pitch -= ry * padLookSpeed * deltaTime; // Inverted to match stick convention
    }

    m_pitch = glm::clamp(m_pitch, -glm::half_pi<float>() + 0.01f, glm::half_pi<float>() - 0.01f);

    glm::vec3 front;
    front.x = cos(m_yaw) * cos(m_pitch);
    front.y = sin(m_pitch);
    front.z = sin(m_yaw) * cos(m_pitch);

    m_forward = glm::normalize(front);
    m_right   = glm::normalize(glm::cross(m_forward, glm::vec3(0, 1, 0))); // World Up
    m_up      = glm::normalize(glm::cross(m_right, m_forward));

    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_E)) move += glm::vec3(0, 1, 0); // Absolute Up
    if (input.IsKeyDown(SDL_SCANCODE_Q)) move -= glm::vec3(0, 1, 0); // Absolute Down

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

    // Controller Movement
    if (pad0 >= 0) {
        const float lx = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_LEFTX);
        const float ly = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_LEFTY);
        const float lt = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_TRIGGERRIGHT);
        const float rt = input.GetPadAxis(pad0, SDL_CONTROLLER_AXIS_TRIGGERLEFT);

        glm::vec3 padMove = (m_forward * -ly) + (m_right * lx) + (glm::vec3(0, 1, 0) * (lt - rt));
        if (glm::length2(padMove) > 0.0001f) {
            m_position += padMove * (moveSpeed * deltaTime);
        }
    }

    m_useTarget = false;
    CalculateProjectionMatrix();
    CalculateViewMatrix();
}

void Camera::CalculateViewMatrix() {
    if (m_useTarget) {
        m_viewMatrix = glm::lookAt(m_position, m_target, glm::vec3(0, 1, 0));
    } 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);

    m_projectionMatrix[1][1] *= -1;
}

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);

    glm::vec3 front;
    front.x = cos(m_yaw) * cos(m_pitch);
    front.y = sin(m_pitch);
    front.z = sin(m_yaw) * cos(m_pitch);
    m_forward = glm::normalize(front);
    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::SetRotation(const glm::vec2& yawPitchRadians) {
    SetRotation(yawPitchRadians.x, yawPitchRadians.y);
}

void Camera::SetTarget(const glm::vec3&) {
}

void Camera::ClearTarget() {
}

void Camera::Target(const glm::vec3& target) {
    m_target = target;
    m_useTarget = true;
    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));
}