#include <components/FirstPersonController.h>
#include <components/Interactable.h>

#include <glm/glm.hpp>
#include <glm/gtx/norm.hpp>

#include <destrum/Components/Physics/CapsuleCollider.h>
#include <destrum/Components/Physics/Rigidbody.h>
#include <destrum/Input/InputManager.h>
#include <destrum/ObjectModel/GameObject.h>
#include <destrum/ObjectModel/Transform.h>
#include <destrum/Physics/PhysicsWorld.h>
#include <destrum/Util/DeltaTime.h>

#include <array>
#include <cmath>

#include <imgui.h>

namespace {
    glm::vec3 MoveTowards(const glm::vec3& current, const glm::vec3& target, float maxDelta)
    {
        const glm::vec3 delta = target - current;
        const float distance = glm::length(delta);
        if (distance <= maxDelta || distance < 1e-6f) {
            return target;
        }
        return current + delta / distance * maxDelta;
    }

    glm::vec3 ProjectOntoSlope(const glm::vec3& vector, const glm::vec3& slopeNormal)
    {
        const glm::vec3 projected = vector - slopeNormal * glm::dot(vector, slopeNormal);
        const float projectedLength = glm::length(projected);
        if (projectedLength < 1e-6f) {
            return glm::vec3{0.0f};
        }
        return projected * (glm::length(vector) / projectedLength);
    }
}

FirstPersonController::FirstPersonController(GameObject& parent)
    : Component(parent, "FirstPersonController")
{
}

void FirstPersonController::SetPossessed(bool possessed)
{
    m_Possessed = possessed;
    m_JumpHeld = false;
    m_JumpBufferTimer = 0.0f;
    InputManager::GetInstance().SetMouseCaptured(possessed);
}

void FirstPersonController::Start()
{
    auto* obj = GetGameObject();

    m_Capsule = obj->GetComponent<CapsuleCollider>();
    if (m_Capsule == nullptr) {
        m_Capsule = obj->AddComponent<CapsuleCollider>(0.4f, 1.8f);
    }

    m_Rigidbody = obj->GetComponent<Rigidbody>();
    if (m_Rigidbody == nullptr) {
        m_Rigidbody = obj->AddComponent<Rigidbody>();
    }

    // The controller owns gravity and friction, the physics body only resolves collisions
    m_Rigidbody->SetMass(80.0f);
    m_Rigidbody->SetFriction(0.0f);
    m_Rigidbody->SetAllowSleep(false);
    m_Rigidbody->SetUseGravity(false);
    m_Rigidbody->SetFreezeRotation(true);
}

void FirstPersonController::Update(float dt)
{
    const auto& input = InputManager::GetInstance();

    m_FocusedInteractable = nullptr;

    if (!m_Possessed)
    {
        return;
    }

    if (input.WasKeyPressed(SDL_SCANCODE_ESCAPE))
    {
        SetPossessed(false);
        return;
    }

    m_JumpBufferTimer = std::max(m_JumpBufferTimer - dt, 0.0f);
    if (input.WasKeyPressed(SDL_SCANCODE_SPACE))
    {
        m_JumpBufferTimer = m_JumpBufferTime;
    }
    m_JumpHeld = input.IsKeyDown(SDL_SCANCODE_SPACE);

    m_Yaw += static_cast<float>(input.MouseDeltaX()) * m_MouseSensitivity;
    m_Pitch -= static_cast<float>(input.MouseDeltaY()) * m_MouseSensitivity;
    m_Pitch = glm::clamp(m_Pitch, glm::radians(-89.0f), glm::radians(89.0f));

    m_FocusedInteractable = FindFocusedInteractable();
    if (m_FocusedInteractable != nullptr && input.WasKeyPressed(SDL_SCANCODE_E))
    {
        m_FocusedInteractable->Interact(*GetGameObject());
    }
}

glm::vec3 FirstPersonController::GetLookDirection() const
{
    return glm::vec3{
        std::cos(m_Yaw) * std::cos(m_Pitch),
        std::sin(m_Pitch),
        std::sin(m_Yaw) * std::cos(m_Pitch),
    };
}

Interactable* FirstPersonController::FindFocusedInteractable() const
{
    if (m_Rigidbody == nullptr || m_Rigidbody->GetPhysicsWorld() == nullptr)
        return nullptr;

    PhysicsRaycastHit hit{};
    if (!m_Rigidbody->GetPhysicsWorld()->Raycast(
            GetEyePosition(), GetLookDirection(), m_InteractDistance, hit, m_Rigidbody->GetBody()))
    {
        return nullptr;
    }
    return Interactable::FindOnObjectOrParents(hit.object);
}

void FirstPersonController::FixedUpdate(float fixedDt)
{
    if (m_Rigidbody == nullptr || !m_Rigidbody->HasPhysicsBody())
        return;

    m_PreviousPosition = GetTransform().GetWorldPosition();
    m_HasPreviousPosition = true;

    const std::optional<GroundContact> ground = FindGround();
    const glm::vec3 groundVelocity = ground ? ground->velocity : glm::vec3{0.0f};

    // Movement happens relative to whatever is underneath, so moving platforms carry the player
    glm::vec3 velocity = m_Rigidbody->GetLinearVelocity() - groundVelocity;

    m_Grounded = ground.has_value() && glm::dot(velocity, ground->normal) <= 0.1f;
    m_GroundNormal = m_Grounded ? ground->normal : glm::vec3{0.0f, 1.0f, 0.0f};
    m_CoyoteTimer = m_Grounded ? m_CoyoteTime : std::max(m_CoyoteTimer - fixedDt, 0.0f);

    const auto& input = InputManager::GetInstance();
    const bool sprinting = m_Possessed && input.IsKeyDown(SDL_SCANCODE_LSHIFT);
    const glm::vec3 wishVelocity = GetWishDirection() * (sprinting ? m_SprintSpeed : m_WalkSpeed);

    if (m_Grounded)
    {
        const glm::vec3 velocityAlongSlope = velocity - m_GroundNormal * glm::dot(velocity, m_GroundNormal);
        velocity = MoveTowards(
            velocityAlongSlope,
            ProjectOntoSlope(wishVelocity, m_GroundNormal),
            m_GroundAcceleration * fixedDt);
        velocity -= m_GroundNormal * m_GroundStickSpeed;
    }
    else
    {
        const glm::vec3 horizontal = MoveTowards(
            glm::vec3{velocity.x, 0.0f, velocity.z},
            wishVelocity,
            m_AirAcceleration * fixedDt);
        velocity.x = horizontal.x;
        velocity.z = horizontal.z;
    }

    if (m_JumpBufferTimer > 0.0f && m_CoyoteTimer > 0.0f)
    {
        velocity.y = std::sqrt(2.0f * m_Gravity * m_JumpHeight);
        m_JumpBufferTimer = 0.0f;
        m_CoyoteTimer = 0.0f;
        m_Grounded = false;
    }

    if (!m_Grounded)
    {
        // Falling faster than rising, and letting go of jump early cuts the jump short
        float gravity = m_Gravity;
        if (velocity.y < 0.0f || !m_JumpHeld)
        {
            gravity *= m_FallGravityMultiplier;
        }
        velocity.y -= gravity * fixedDt;
    }

    m_Rigidbody->SetLinearVelocity(velocity + groundVelocity);
}

std::optional<FirstPersonController::GroundContact> FirstPersonController::FindGround() const
{
    const auto* world = m_Rigidbody->GetPhysicsWorld();
    if (world == nullptr || m_Capsule == nullptr)
        return std::nullopt;

    const glm::vec3 center = GetTransform().GetWorldPosition() + m_Capsule->GetCenterOffset();
    const float radius = m_Capsule->GetRadius();
    const float bottom = center.y - m_Capsule->GetHeight() * 0.5f;

    const float spread = radius * 0.7f;
    const std::array<glm::vec2, 5> offsets{
        glm::vec2{0.0f, 0.0f},
        glm::vec2{spread, 0.0f},
        glm::vec2{-spread, 0.0f},
        glm::vec2{0.0f, spread},
        glm::vec2{0.0f, -spread},
    };

    constexpr float startHeight = 0.2f;
    constexpr float probeDistance = 0.1f;
    const float minWalkableNormalY = std::cos(m_MaxSlopeAngle);
    for (const glm::vec2& offset : offsets)
    {
        const glm::vec3 origin{center.x + offset.x, bottom + startHeight, center.z + offset.y};

        PhysicsRaycastHit hit{};
        if (world->Raycast(origin, glm::vec3{0.0f, -1.0f, 0.0f}, startHeight + probeDistance, hit,
                           m_Rigidbody->GetBody()) &&
            hit.normal.y >= minWalkableNormalY)
        {
            return GroundContact{
                .normal = glm::normalize(hit.normal),
                .velocity = hit.body.IsValid() ? world->GetLinearVelocity(hit.body) : glm::vec3{0.0f},
            };
        }
    }
    return std::nullopt;
}

glm::vec3 FirstPersonController::GetWishDirection() const
{
    if (!m_Possessed)
        return glm::vec3{0.0f};

    const auto& input = InputManager::GetInstance();

    const glm::vec3 forward{std::cos(m_Yaw), 0.0f, std::sin(m_Yaw)};
    const glm::vec3 right{-std::sin(m_Yaw), 0.0f, std::cos(m_Yaw)};

    glm::vec3 direction(0.0f);
    if (input.IsKeyDown(SDL_SCANCODE_W)) direction += forward;
    if (input.IsKeyDown(SDL_SCANCODE_S)) direction -= forward;
    if (input.IsKeyDown(SDL_SCANCODE_D)) direction += right;
    if (input.IsKeyDown(SDL_SCANCODE_A)) direction -= right;

    return glm::length2(direction) > 0.0f ? glm::normalize(direction) : direction;
}

glm::vec3 FirstPersonController::GetEyePosition() const
{
    const glm::vec3 current = GetTransform().GetWorldPosition();
    const glm::vec3 position = m_HasPreviousPosition
        ? glm::mix(m_PreviousPosition, current, Time::GetInstance().FixedAlpha())
        : current;
    return position + glm::vec3{0.0f, m_EyeHeight, 0.0f};
}

void FirstPersonController::ImGuiInspector()
{
    Component::ImGuiInspector();
    ImGui::DragFloat("Walk Speed", &m_WalkSpeed, 0.1f, 0.0f, 100.0f);
    ImGui::DragFloat("Sprint Speed", &m_SprintSpeed, 0.1f, 0.0f, 100.0f);
    ImGui::DragFloat("Ground Acceleration", &m_GroundAcceleration, 0.5f, 0.0f, 500.0f);
    ImGui::DragFloat("Air Acceleration", &m_AirAcceleration, 0.5f, 0.0f, 500.0f);
    ImGui::DragFloat("Jump Height", &m_JumpHeight, 0.01f, 0.0f, 10.0f);
    ImGui::DragFloat("Gravity", &m_Gravity, 0.1f, 0.0f, 100.0f);
    ImGui::DragFloat("Fall Gravity Multiplier", &m_FallGravityMultiplier, 0.01f, 1.0f, 5.0f);
    ImGui::DragFloat("Coyote Time", &m_CoyoteTime, 0.005f, 0.0f, 1.0f);
    ImGui::DragFloat("Jump Buffer Time", &m_JumpBufferTime, 0.005f, 0.0f, 1.0f);
    ImGui::DragFloat("Interact Distance", &m_InteractDistance, 0.05f, 0.0f, 20.0f);
    ImGui::SliderAngle("Max Slope Angle", &m_MaxSlopeAngle, 0.0f, 89.0f);
    ImGui::DragFloat("Ground Stick Speed", &m_GroundStickSpeed, 0.05f, 0.0f, 20.0f);
    ImGui::DragFloat("Mouse Sensitivity", &m_MouseSensitivity, 0.0001f, 0.0f, 0.1f);
    ImGui::DragFloat("Eye Height", &m_EyeHeight, 0.01f, -2.0f, 2.0f);
    ImGui::Text("Grounded: %s", m_Grounded ? "yes" : "no");
    ImGui::Text("Ground slope: %.1f deg", glm::degrees(std::acos(glm::clamp(m_GroundNormal.y, -1.0f, 1.0f))));
    ImGui::Text("Yaw: %.1f deg", glm::degrees(m_Yaw));
    ImGui::Text("Pitch: %.1f deg", glm::degrees(m_Pitch));
    if (bool possessed = m_Possessed; ImGui::Checkbox("Possessed", &possessed)) {
        SetPossessed(possessed);
    }
}

nlohmann::json FirstPersonController::Serialize() const
{
    return {
        {"walkSpeed", m_WalkSpeed},
        {"sprintSpeed", m_SprintSpeed},
        {"groundAcceleration", m_GroundAcceleration},
        {"airAcceleration", m_AirAcceleration},
        {"jumpHeight", m_JumpHeight},
        {"gravity", m_Gravity},
        {"fallGravityMultiplier", m_FallGravityMultiplier},
        {"coyoteTime", m_CoyoteTime},
        {"jumpBufferTime", m_JumpBufferTime},
        {"interactDistance", m_InteractDistance},
        {"maxSlopeAngle", m_MaxSlopeAngle},
        {"groundStickSpeed", m_GroundStickSpeed},
        {"mouseSensitivity", m_MouseSensitivity},
        {"eyeHeight", m_EyeHeight},
        {"yaw", m_Yaw},
        {"pitch", m_Pitch},
    };
}

void FirstPersonController::Deserialize(const nlohmann::json& data)
{
    m_WalkSpeed = data.value("walkSpeed", data.value("movementSpeed", m_WalkSpeed));
    m_SprintSpeed = data.value("sprintSpeed", m_SprintSpeed);
    m_GroundAcceleration = data.value("groundAcceleration", m_GroundAcceleration);
    m_AirAcceleration = data.value("airAcceleration", m_AirAcceleration);
    m_JumpHeight = data.value("jumpHeight", m_JumpHeight);
    m_Gravity = data.value("gravity", m_Gravity);
    m_FallGravityMultiplier = data.value("fallGravityMultiplier", m_FallGravityMultiplier);
    m_CoyoteTime = data.value("coyoteTime", m_CoyoteTime);
    m_JumpBufferTime = data.value("jumpBufferTime", m_JumpBufferTime);
    m_InteractDistance = data.value("interactDistance", m_InteractDistance);
    m_MaxSlopeAngle = data.value("maxSlopeAngle", m_MaxSlopeAngle);
    m_GroundStickSpeed = data.value("groundStickSpeed", m_GroundStickSpeed);
    m_MouseSensitivity = data.value("mouseSensitivity", m_MouseSensitivity);
    m_EyeHeight = data.value("eyeHeight", m_EyeHeight);
    m_Yaw = data.value("yaw", m_Yaw);
    m_Pitch = data.value("pitch", m_Pitch);
}