#include <cmath>
#include <string>

#include <destrum/ObjectModel/Component.h>
#include <destrum/ObjectModel/GameObject.h>
#include <destrum/Scene/Scene.h>
#include <destrum/Scene/SceneManager.h>
#include <destrum/Physics/JoltPhysicsWorld.h>
#include <destrum/Components/Physics/SphereCollider.h>
#include <destrum/Components/Physics/BoxCollider.h>
#include <destrum/Components/Physics/CapsuleCollider.h>
#include <destrum/Components/Physics/Collider.h>
#include <destrum/Components/Physics/ConvexHullCollider.h>
#include <destrum/Components/Physics/CylinderCollider.h>
#include <destrum/Components/Physics/MeshCollider.h>
#include <destrum/Components/LightComponent.h>
#include <destrum/Components/Physics/Rigidbody.h>

#include "TestUtils.h"

using namespace test;

namespace {
    void testPhysicsWorldUnits()
    {
        JoltPhysicsWorld world{{
            .workerThreadCount = 1,
            .gravity = glm::vec3{0.0f}
        }};
        GameObject object{"scaled-physics"};
        object.GetTransform().SetLocalScale(glm::vec3{0.5f});
        object.AddComponent<SphereCollider>(1.0f);
        Rigidbody* rigidbody = object.AddComponent<Rigidbody>();
        world.RegisterRigidbody(*rigidbody);

        PhysicsRaycastHit hit;
        check(world.Raycast({-2.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, 5.0f, hit),
              "scaled physics bodies must remain raycastable");
        check(close(hit.distance, 1.0f),
              "physics collider dimensions must remain in world units");

        object.GetTransform().SetLocalPosition({10.0f, 0.0f, 0.0f});
        world.RefreshRigidbody(*rigidbody);
        check(world.Raycast({8.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, 5.0f, hit),
              "refreshing a physics body must use the current transform position");
        check(close(hit.distance, 1.0f),
              "refreshed physics bodies must remain aligned with rendered transforms");
    }

    void testJoltSphereCollisions()
    {
        JoltPhysicsWorld world{{
            .workerThreadCount = 1,
            .gravity = {0.0f, -9.81f, 0.0f}
        }};

        GameObject floor{"floor"};
        floor.GetTransform().SetLocalPosition({0.0f, -1.0f, 0.0f});
        floor.AddComponent<BoxCollider>(glm::vec3{10.0f, 0.5f, 10.0f});
        auto* floorBody = floor.AddComponent<Rigidbody>();
        floorBody->SetType(RigidbodyType::Static);
        world.RegisterRigidbody(*floorBody);

        GameObject first{"first"};
        first.AddComponent<SphereCollider>(1.5f);
        auto* firstBody = first.AddComponent<Rigidbody>();
        world.RegisterRigidbody(*firstBody);
        first.GetTransform().SetLocalPosition({0.0f, 100.0f, 0.0f});
        first.GetTransform().SetLocalScale(glm::vec3{0.015f});
        world.RefreshRigidbody(*firstBody);

        GameObject second{"second"};
        second.AddComponent<SphereCollider>(1.5f);
        auto* secondBody = second.AddComponent<Rigidbody>();
        world.RegisterRigidbody(*secondBody);
        second.GetTransform().SetLocalPosition({0.0f, 100.0f, 0.0f});
        second.GetTransform().SetLocalScale(glm::vec3{0.015f});
        world.RefreshRigidbody(*secondBody);

        for (int step = 0; step < 240; ++step) {
            world.Step(1.0f / 60.0f);
            world.SyncDynamicBodiesToTransforms();
        }

        check(first.GetTransform().GetWorldPosition().y > 0.8f &&
              second.GetTransform().GetWorldPosition().y > 0.8f,
              "Jolt spheres must collide with the ground plane");
        check(glm::distance(first.GetTransform().GetWorldPosition(),
                            second.GetTransform().GetWorldPosition()) > 2.5f,
              "Jolt spheres must collide with one another instead of occupying the same point");
    }

    void testSceneJoltSphereCollisions()
    {
        Scene& scene = SceneManager::GetInstance().CreateScene("scene-physics");

        GameObject* floor = scene.CreateGameObject("floor");
        floor->GetTransform().SetLocalPosition({0.0f, -1.0f, 0.0f});
        floor->AddComponent<BoxCollider>(glm::vec3{10.0f, 0.5f, 10.0f});
        auto* floorBody = floor->AddComponent<Rigidbody>();
        floorBody->SetType(RigidbodyType::Static);

        GameObject* first = scene.CreateGameObject("first");
        first->AddComponent<SphereCollider>(1.5f);
        first->AddComponent<Rigidbody>();
        first->GetTransform().SetWorldPosition({0.0f, 100.0f, 0.0f});
        first->GetTransform().SetWorldScale(glm::vec3{0.015f});

        GameObject* second = scene.CreateGameObject("second");
        second->AddComponent<SphereCollider>(1.5f);
        second->AddComponent<Rigidbody>();
        second->GetTransform().SetWorldPosition({0.0f, 100.0f, 0.0f});
        second->GetTransform().SetWorldScale(glm::vec3{0.015f});

        scene.CommitPendingAdditions();
        scene.GetPhysics().RefreshGameObject(*first);
        scene.GetPhysics().RefreshGameObject(*second);

        for (int step = 0; step < 240; ++step) {
            scene.FixedUpdate(1.0f / 60.0f);
        }

        check(first->GetTransform().GetWorldPosition().y > 0.8f &&
              second->GetTransform().GetWorldPosition().y > 0.8f,
              "scene physics spheres must collide with the ground plane");
        check(glm::distance(first->GetTransform().GetWorldPosition(),
                            second->GetTransform().GetWorldPosition()) > 2.5f,
              "scene physics spheres must not overlap after settling");

        SceneManager::GetInstance().Destroy();
    }

    class TriggerCounter final : public Component {
    public:
        explicit TriggerCounter(GameObject& parent)
            : Component(parent, "TriggerCounter") {}

        std::string GetTypeName() const override { return "TriggerCounter"; }
        void OnTriggerEnter(GameObject*) override { ++enters; }
        void OnTriggerExit(GameObject*) override { ++exits; }

        int enters{0};
        int exits{0};
    };

    void testNonPhysicsChangesKeepBodies()
    {
        Scene& scene = SceneManager::GetInstance().CreateScene("physics-refresh");

        GameObject* trigger = scene.CreateGameObject("trigger");
        trigger->AddComponent<BoxCollider>(glm::vec3{3.0f})->SetTrigger(true);
        trigger->AddComponent<Rigidbody>()->SetType(RigidbodyType::Static);
        auto* counter = trigger->AddComponent<TriggerCounter>();

        GameObject* crate = scene.CreateGameObject("crate");
        crate->AddComponent<BoxCollider>(glm::vec3{0.5f});
        auto* crateBody = crate->AddComponent<Rigidbody>();
        crateBody->SetUseGravity(false);
        crateBody->SetAllowSleep(false);

        GameObject* lamp = scene.CreateGameObject("lamp");
        auto* light = lamp->AddComponent<LightComponent>();

        scene.CommitPendingAdditions();
        scene.GetPhysics().RefreshGameObject(*trigger);
        scene.GetPhysics().RefreshGameObject(*crate);

        const auto step = [&scene] {
            for (int i = 0; i < 10; ++i) {
                scene.FixedUpdate(1.0f / 60.0f);
            }
        };
        step();
        check(counter->enters == 1 && counter->exits == 0, "a body inside a trigger must enter it once");

        light->SetEnabled(false);
        light->SetEnabled(true);
        crate->AddComponent<LightComponent>()->SetEnabled(false);
        step();
        check(counter->enters == 1 && counter->exits == 0,
              "non physics components must not rebuild physics bodies");

        crateBody->SetMass(5.0f);
        step();
        check(counter->enters == 2 && counter->exits == 1,
              "rebuilding a body inside a trigger must exit the old body and enter the new one");

        SceneManager::GetInstance().Destroy();
    }

    void testKinematicBodiesMoveWithVelocity()
    {
        constexpr float fixedDt = 1.0f / 60.0f;
        Scene& scene = SceneManager::GetInstance().CreateScene("kinematic-velocity");

        GameObject* platform = scene.CreateGameObject("platform");
        platform->AddComponent<BoxCollider>(glm::vec3{2.0f, 0.25f, 2.0f});
        auto* platformBody = platform->AddComponent<Rigidbody>();
        platformBody->SetType(RigidbodyType::Kinematic);

        GameObject* crate = scene.CreateGameObject("crate");
        crate->GetTransform().SetWorldPosition({0.0f, 0.76f, 0.0f});
        crate->AddComponent<BoxCollider>(glm::vec3{0.5f});
        auto* crateBody = crate->AddComponent<Rigidbody>();
        crateBody->SetFriction(1.0f);
        crateBody->SetAllowSleep(false);

        scene.CommitPendingAdditions();
        scene.GetPhysics().RefreshGameObject(*platform);
        scene.GetPhysics().RefreshGameObject(*crate);

        for (int step = 0; step < 30; ++step) {
            scene.FixedUpdate(fixedDt);
        }
        check(glm::length(platformBody->GetLinearVelocity()) < 0.001f,
              "a kinematic body that is not moved must have no velocity");

        constexpr float platformSpeed = 2.0f;
        for (int step = 0; step < 60; ++step) {
            platform->GetTransform().SetWorldPosition(
                platform->GetTransform().GetWorldPosition() + glm::vec3{platformSpeed * fixedDt, 0.0f, 0.0f});
            scene.FixedUpdate(fixedDt);
        }

        check(close(platformBody->GetLinearVelocity().x, platformSpeed),
              "a kinematic body moved through its transform must get the matching velocity");
        check(crate->GetTransform().GetWorldPosition().x > 1.5f,
              "a dynamic body resting on a moving kinematic platform must be carried along");

        SceneManager::GetInstance().Destroy();
    }

    void testColliderCenterOffset()
    {
        Scene& scene = SceneManager::GetInstance().CreateScene("collider-offset");

        GameObject* box = scene.CreateGameObject("offset box");
        box->AddComponent<BoxCollider>(glm::vec3{0.5f})->SetCenterOffset({0.0f, 5.0f, 0.0f});
        box->AddComponent<Rigidbody>()->SetType(RigidbodyType::Static);
        scene.CommitPendingAdditions();
        scene.GetPhysics().RefreshGameObject(*box);

        PhysicsRaycastHit hit{};
        const bool found = box->GetComponent<Rigidbody>()->GetPhysicsWorld()->Raycast(
            {0.0f, 10.0f, 0.0f}, {0.0f, -1.0f, 0.0f}, 20.0f, hit);
        check(found && std::abs(hit.distance - 4.5f) < 0.01f,
              "a collider center offset must move the shape, not just the center of mass");

        SceneManager::GetInstance().Destroy();
    }

    void testFrozenRotationCapsule()
    {
        Scene& scene = SceneManager::GetInstance().CreateScene("frozen-capsule");

        GameObject* floor = scene.CreateGameObject("floor");
        floor->GetTransform().SetLocalPosition({0.0f, -1.0f, 0.0f});
        floor->AddComponent<BoxCollider>(glm::vec3{10.0f, 0.5f, 10.0f});
        floor->AddComponent<Rigidbody>()->SetType(RigidbodyType::Static);

        GameObject* character = scene.CreateGameObject("character");
        character->AddComponent<CapsuleCollider>(0.4f, 1.8f);
        auto* body = character->AddComponent<Rigidbody>();
        body->SetFriction(1.0f);
        body->SetFreezeRotation(true);
        character->GetTransform().SetWorldPosition({0.0f, 1.0f, 0.0f});

        GameObject* trigger = scene.CreateGameObject("trigger");
        trigger->GetTransform().SetWorldPosition({2.0f, 0.0f, 0.0f});
        trigger->AddComponent<BoxCollider>(glm::vec3{2.0f})->SetTrigger(true);
        trigger->AddComponent<Rigidbody>()->SetType(RigidbodyType::Static);

        scene.CommitPendingAdditions();
        scene.GetPhysics().RefreshGameObject(*floor);
        scene.GetPhysics().RefreshGameObject(*character);
        scene.GetPhysics().RefreshGameObject(*trigger);

        // Sliding with friction would tip a free capsule over
        body->SetLinearVelocity({4.0f, 0.0f, 0.0f});
        for (int step = 0; step < 180; ++step) {
            scene.FixedUpdate(1.0f / 60.0f);
        }

        const glm::vec3 position = character->GetTransform().GetWorldPosition();
        const glm::quat rotation = character->GetTransform().GetWorldRotation();
        check(std::abs(rotation.w) > 0.999f, "a capsule with frozen rotation must stay upright");
        check(std::abs(position.y - (-0.5f + 0.9f)) < 0.05f, "a capsule must rest on top of the floor");

        // Cast from inside the capsule (and the trigger it is standing in), like a ground check does
        PhysicsRaycastHit hit{};
        const bool hitGround = body->GetPhysicsWorld()->Raycast(
            position - glm::vec3{0.0f, 0.7f, 0.0f}, glm::vec3{0.0f, -1.0f, 0.0f}, 0.3f, hit, body->GetBody());
        check(hitGround && hit.object == floor && hit.normal.y > 0.9f,
              "a ground ray must skip the caster and triggers and hit the floor");

        PhysicsRaycastHit selfHit{};
        check(body->GetPhysicsWorld()->Raycast(
                  position - glm::vec3{0.0f, 0.7f, 0.0f}, glm::vec3{0.0f, -1.0f, 0.0f}, 0.3f, selfHit) &&
              selfHit.object == character,
              "a ray that doesn't ignore the caster must hit it");

        SceneManager::GetInstance().Destroy();
    }

    class TriggerTracker final : public Component {
    public:
        explicit TriggerTracker(GameObject& owner)
            : Component(owner, "TriggerTracker") {}

        void Update(float) override {}
        std::string GetTypeName() const override { return "TriggerTracker"; }

        void OnTriggerEnter(GameObject* other) override {
            ++enterCount;
            lastOther = other;
            enterObjectName = other ? other->GetName() : "null";
        }

        void OnTriggerExit(GameObject*) override {
            ++exitCount;
        }

        int enterCount{0};
        int exitCount{0};
        GameObject* lastOther{nullptr};
        std::string enterObjectName;
    };

    void testTriggerZone()
    {
        Scene& scene = SceneManager::GetInstance().CreateScene("trigger-test");

        // Trigger box at origin (Static rigidbody + BoxCollider as trigger).
        GameObject* triggerObj = scene.CreateGameObject("TriggerZone");
        triggerObj->AddComponent<BoxCollider>(glm::vec3{3.0f, 3.0f, 3.0f});
        triggerObj->GetComponent<BoxCollider>()->SetTrigger(true);
        auto* triggerRb = triggerObj->AddComponent<Rigidbody>();
        triggerRb->SetType(RigidbodyType::Static);
        auto* tracker = triggerObj->AddComponent<TriggerTracker>();

        // Falling sphere above the trigger.
        GameObject* sphere = scene.CreateGameObject("FallingSphere");
        sphere->AddComponent<SphereCollider>(0.5f);
        sphere->AddComponent<Rigidbody>();
        sphere->GetTransform().SetWorldPosition({0.0f, 8.0f, 0.0f});

        scene.CommitPendingAdditions();

        // Run physics for enough steps that the sphere falls into the trigger.
        for (int step = 0; step < 120; ++step) {
            scene.FixedUpdate(1.0f / 60.0f);
        }

        check(tracker->enterCount >= 1,
              "falling sphere must trigger OnTriggerEnter at least once");
        check(tracker->exitCount >= 1,
              "falling sphere passing through trigger must exit");
        check(tracker->enterObjectName == "FallingSphere",
              "OnTriggerEnter must report the correct entering object");

        SceneManager::GetInstance().Destroy();
    }

    std::shared_ptr<const PhysicsMeshData> makeCubeMesh(float halfExtent)
    {
        auto mesh = std::make_shared<PhysicsMeshData>();
        for (int corner = 0; corner < 8; ++corner) {
            mesh->points.push_back({
                corner & 1 ? halfExtent : -halfExtent,
                corner & 2 ? halfExtent : -halfExtent,
                corner & 4 ? halfExtent : -halfExtent});
        }
        mesh->points.push_back(glm::vec3{0.0f});
        mesh->indices = {
            0, 2, 1, 1, 2, 3, 4, 5, 6, 5, 7, 6,
            0, 1, 4, 1, 5, 4, 2, 6, 3, 3, 6, 7,
            0, 4, 2, 2, 4, 6, 1, 3, 5, 3, 7, 5};
        return mesh;
    }

    std::shared_ptr<const PhysicsMeshData> makeQuadMesh(float halfExtent)
    {
        auto mesh = std::make_shared<PhysicsMeshData>();
        mesh->points = {
            {-halfExtent, 0.0f, -halfExtent},
            {halfExtent, 0.0f, -halfExtent},
            {-halfExtent, 0.0f, halfExtent},
            {halfExtent, 0.0f, halfExtent}};
        mesh->indices = {0, 2, 1, 1, 2, 3};
        return mesh;
    }

    void testCylinderCollider()
    {
        JoltPhysicsWorld world{{.workerThreadCount = 1, .gravity = glm::vec3{0.0f}}};
        GameObject object{"cylinder"};
        object.AddComponent<CylinderCollider>(1.0f, 4.0f);
        Rigidbody* rigidbody = object.AddComponent<Rigidbody>();
        rigidbody->SetType(RigidbodyType::Static);
        world.RegisterRigidbody(*rigidbody);

        PhysicsRaycastHit hit;
        check(world.Raycast({-3.0f, 1.5f, 0.0f}, {1.0f, 0.0f, 0.0f}, 5.0f, hit) && close(hit.distance, 2.0f),
              "cylinder sides must be at its radius along the whole height");
        check(world.Raycast({0.0f, 5.0f, 0.0f}, {0.0f, -1.0f, 0.0f}, 5.0f, hit) && close(hit.distance, 3.0f),
              "cylinder caps must be flat at half its height");
    }

    void testConvexHullCollider()
    {
        JoltPhysicsWorld world{{.workerThreadCount = 1, .gravity = glm::vec3{0.0f}}};
        GameObject object{"hull"};
        object.GetTransform().SetLocalScale(glm::vec3{2.0f});
        auto* collider = object.AddComponent<ConvexHullCollider>();
        collider->SetCustomMesh(makeCubeMesh(1.0f));
        check(collider->BuildPhysicsShape().type == PhysicsShapeType::ConvexHull,
              "a cube mesh must build a convex hull");
        check(collider->BuildPhysicsShape().mesh->points.size() == 8,
              "interior points must not end up in the hull");

        Rigidbody* rigidbody = object.AddComponent<Rigidbody>();
        rigidbody->SetType(RigidbodyType::Static);
        world.RegisterRigidbody(*rigidbody);

        PhysicsRaycastHit hit;
        check(world.Raycast({-5.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, 10.0f, hit) && close(hit.distance, 3.0f),
              "mesh based colliders must apply the transform scale");

        GameObject flat{"flat-hull"};
        auto* flatCollider = flat.AddComponent<ConvexHullCollider>();
        flatCollider->SetCustomMesh(makeQuadMesh(1.0f));
        check(flatCollider->BuildPhysicsShape().type == PhysicsShapeType::Box,
              "a mesh without volume must fall back to a box");

        GameObject empty{"empty-hull"};
        empty.AddComponent<ConvexHullCollider>();
        Rigidbody* emptyBody = empty.AddComponent<Rigidbody>();
        world.RegisterRigidbody(*emptyBody);
        check(!emptyBody->HasPhysicsBody(), "a mesh collider without a mesh must not create a body");
    }

    void testMeshCollider()
    {
        JoltPhysicsWorld world{{.workerThreadCount = 1, .gravity = {0.0f, -9.81f, 0.0f}}};

        GameObject ground{"mesh-ground"};
        ground.AddComponent<MeshCollider>()->SetCustomMesh(makeQuadMesh(10.0f));
        Rigidbody* groundBody = ground.AddComponent<Rigidbody>();
        groundBody->SetType(RigidbodyType::Static);
        world.RegisterRigidbody(*groundBody);
        check(groundBody->HasPhysicsBody(), "a static mesh collider must create a body");

        GameObject ball{"ball"};
        ball.GetTransform().SetLocalPosition({0.0f, 2.0f, 0.0f});
        ball.AddComponent<SphereCollider>(0.5f);
        Rigidbody* ballBody = ball.AddComponent<Rigidbody>();
        world.RegisterRigidbody(*ballBody);

        for (int step = 0; step < 180; ++step) {
            world.Step(1.0f / 60.0f);
            world.SyncDynamicBodiesToTransforms();
        }
        const float restHeight = ball.GetTransform().GetWorldPosition().y;
        check(restHeight > 0.4f && restHeight < 0.6f, "a sphere must come to rest on a mesh collider");

        GameObject kinematic{"kinematic-mesh"};
        kinematic.GetTransform().SetLocalPosition({50.0f, 0.0f, 0.0f});
        kinematic.AddComponent<MeshCollider>()->SetCustomMesh(makeCubeMesh(1.0f));
        Rigidbody* kinematicBody = kinematic.AddComponent<Rigidbody>();
        kinematicBody->SetType(RigidbodyType::Kinematic);
        world.RegisterRigidbody(*kinematicBody);
        check(kinematicBody->HasPhysicsBody(), "a kinematic mesh collider must create a body");

        GameObject dynamic{"dynamic-mesh"};
        auto* dynamicCollider = dynamic.AddComponent<MeshCollider>();
        dynamicCollider->SetCustomMesh(makeCubeMesh(1.0f));
        dynamic.AddComponent<Rigidbody>();
        check(dynamicCollider->BuildPhysicsShape().type == PhysicsShapeType::ConvexHull,
              "dynamic mesh colliders must fall back to a convex hull");
    }
}

void RunPhysicsTests()
{
    testPhysicsWorldUnits();
    testJoltSphereCollisions();
    testSceneJoltSphereCollisions();
    testNonPhysicsChangesKeepBodies();
    testKinematicBodiesMoveWithVelocity();
    testColliderCenterOffset();
    testFrozenRotationCapsule();
    testCylinderCollider();
    testConvexHullCollider();
    testMeshCollider();
    testTriggerZone();
}