#include "MetaCorePhysics/MetaCorePhysics.h"

#include "MetaCoreScene/MetaCoreScene.h"
#include "MetaCoreScene/MetaCoreTransformUtils.h"

#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/ConstraintSolver/btFixedConstraint.h>

#include <algorithm>
#include <bit>
#include <chrono>
#include <cmath>
#include <limits>
#include <map>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>

namespace MetaCore {
namespace {

btVector3 Bt(glm::vec3 value) { return {value.x, value.y, value.z}; }
glm::vec3 Glm(const btVector3& value) { return {value.x(), value.y(), value.z()}; }

btTransform BuildTransform(const glm::mat4& matrix) {
    glm::vec3 scale{}, translation{}, skew{}; glm::vec4 perspective{}; glm::quat rotation{};
    glm::decompose(matrix, scale, rotation, translation, skew, perspective);
    btTransform result; result.setOrigin(Bt(translation)); result.setRotation({rotation.x, rotation.y, rotation.z, rotation.w}); return result;
}

glm::mat4 BuildMatrix(const btTransform& transform, glm::vec3 scale) {
    const btQuaternion value = transform.getRotation();
    return glm::translate(glm::mat4(1.0F), Glm(transform.getOrigin())) *
        glm::mat4_cast(glm::quat(value.w(), value.x(), value.y(), value.z())) * glm::scale(glm::mat4(1.0F), scale);
}

glm::mat4 WorldMatrix(const MetaCoreScene& scene, MetaCoreGameObject object) {
    glm::mat4 result = MetaCoreBuildTransformMatrix(object.GetComponent<MetaCoreTransformComponent>());
    MetaCoreId parent = object.GetParentId(); std::size_t guard = 0;
    while (parent != 0 && guard++ < 1024U) {
        const auto parentObject = scene.FindGameObject(parent); if (!parentObject) break;
        result = MetaCoreBuildTransformMatrix(parentObject.GetComponent<MetaCoreTransformComponent>()) * result;
        parent = parentObject.GetParentId();
    }
    return result;
}

struct PairKey {
    MetaCoreId A = 0; MetaCoreId B = 0;
    bool Trigger = false;
    auto operator<=>(const PairKey&) const = default;
};

} // namespace

class MetaCorePhysicsWorld::Impl {
public:
    struct Metadata {
        MetaCoreId Collider = 0;
        MetaCoreId Body = 0;
        std::uint32_t Layer = 0;
        bool Trigger = false;
        float Friction = 0.5F;
        float Restitution = 0.0F;
        MetaCorePhysicsMaterialCombineMode FrictionCombine = MetaCorePhysicsMaterialCombineMode::Average;
        MetaCorePhysicsMaterialCombineMode RestitutionCombine = MetaCorePhysicsMaterialCombineMode::Average;
    };
    struct BodyEntry {
        std::unique_ptr<btCollisionShape> Shape{};
        std::unique_ptr<btTriangleMesh> TriangleMesh{};
        std::unique_ptr<btDefaultMotionState> MotionState{};
        std::unique_ptr<btRigidBody> Body{};
        Metadata Meta{};
        glm::vec3 Scale{1.0F};
        glm::vec3 Center{0.0F};
        MetaCoreRigidBodyType Type = MetaCoreRigidBodyType::Static;
        bool Character = false;
        float CharacterHalfHeight = 0.0F;
        float CharacterSkinWidth = 0.05F;
        float CharacterMaxFallSpeed = 55.0F;
    };
    struct ConstraintEntry { MetaCoreId Owner = 0; MetaCoreId Target = 0; MetaCorePhysicsConstraintType Type = MetaCorePhysicsConstraintType::Fixed; std::unique_ptr<btTypedConstraint> Value{}; bool Broken = false; };
    struct Filter final : btOverlapFilterCallback {
        Impl* Owner = nullptr;
        explicit Filter(Impl* owner) : Owner(owner) {}
        bool needBroadphaseCollision(btBroadphaseProxy* lhs, btBroadphaseProxy* rhs) const override {
            const auto* a = static_cast<const btCollisionObject*>(lhs->m_clientObject);
            const auto* b = static_cast<const btCollisionObject*>(rhs->m_clientObject);
            const auto* ma = a ? static_cast<const Metadata*>(a->getUserPointer()) : nullptr;
            const auto* mb = b ? static_cast<const Metadata*>(b->getUserPointer()) : nullptr;
            if (!ma || !mb || ma->Layer >= 32U || mb->Layer >= 32U) return true;
            return ((Owner->Settings.CollisionMasks[ma->Layer] >> mb->Layer) & 1U) != 0U;
        }
    };

    MetaCoreScene& Scene;
    MetaCorePhysicsSettingsDocument Settings;
    MetaCorePhysicsMeshProvider MeshProvider;
    MetaCorePhysicsMaterialProvider MaterialProvider;
    std::unique_ptr<btDefaultCollisionConfiguration> CollisionConfiguration = std::make_unique<btDefaultCollisionConfiguration>();
    std::unique_ptr<btCollisionDispatcher> Dispatcher = std::make_unique<btCollisionDispatcher>(CollisionConfiguration.get());
    std::unique_ptr<btDbvtBroadphase> Broadphase = std::make_unique<btDbvtBroadphase>();
    std::unique_ptr<btSequentialImpulseConstraintSolver> Solver = std::make_unique<btSequentialImpulseConstraintSolver>();
    std::unique_ptr<btDiscreteDynamicsWorld> World = std::make_unique<btDiscreteDynamicsWorld>(Dispatcher.get(), Broadphase.get(), Solver.get(), CollisionConfiguration.get());
    Filter OverlapFilter{this};
    std::unordered_map<MetaCoreId, std::unique_ptr<BodyEntry>> Bodies{};
    std::vector<ConstraintEntry> Constraints{};
    std::map<PairKey, MetaCorePhysicsContactEvent> PreviousPairs{};
    MetaCorePhysicsEventCallback EventCallback{};
    MetaCorePhysicsStatistics Statistics{};
    std::vector<std::string> Diagnostics{};
    std::uint64_t SceneRevision = std::numeric_limits<std::uint64_t>::max();
    std::uint64_t StructureSignature = 0;
    bool Stopped = false;

    Impl(MetaCoreScene& scene, MetaCorePhysicsSettingsDocument settings, MetaCorePhysicsMeshProvider mesh, MetaCorePhysicsMaterialProvider material)
        : Scene(scene), Settings(std::move(settings)), MeshProvider(std::move(mesh)), MaterialProvider(std::move(material)) {
        (void)MetaCoreValidatePhysicsSettings(Settings, &Diagnostics);
        gContactAddedCallback = &Impl::ContactAdded;
        World->setGravity(Bt(Settings.Gravity));
        World->getPairCache()->setOverlapFilterCallback(&OverlapFilter);
        Rebuild();
    }

    ~Impl() { Clear(); }

    static int CombineRank(MetaCorePhysicsMaterialCombineMode mode) {
        switch (mode) {
        case MetaCorePhysicsMaterialCombineMode::Maximum: return 3;
        case MetaCorePhysicsMaterialCombineMode::Multiply: return 2;
        case MetaCorePhysicsMaterialCombineMode::Minimum: return 1;
        case MetaCorePhysicsMaterialCombineMode::Average: return 0;
        }
        return 0;
    }

    static float Combine(float lhs, float rhs, MetaCorePhysicsMaterialCombineMode lhsMode, MetaCorePhysicsMaterialCombineMode rhsMode) {
        const auto mode = CombineRank(lhsMode) >= CombineRank(rhsMode) ? lhsMode : rhsMode;
        switch (mode) {
        case MetaCorePhysicsMaterialCombineMode::Maximum: return std::max(lhs, rhs);
        case MetaCorePhysicsMaterialCombineMode::Multiply: return lhs * rhs;
        case MetaCorePhysicsMaterialCombineMode::Minimum: return std::min(lhs, rhs);
        case MetaCorePhysicsMaterialCombineMode::Average: return (lhs + rhs) * 0.5F;
        }
        return (lhs + rhs) * 0.5F;
    }

    static bool ContactAdded(btManifoldPoint& point, const btCollisionObjectWrapper* lhs, int, int,
        const btCollisionObjectWrapper* rhs, int, int) {
        const auto* a = lhs ? static_cast<const Metadata*>(lhs->getCollisionObject()->getUserPointer()) : nullptr;
        const auto* b = rhs ? static_cast<const Metadata*>(rhs->getCollisionObject()->getUserPointer()) : nullptr;
        if (!a || !b) return true;
        point.m_combinedFriction = std::clamp(Combine(a->Friction, b->Friction, a->FrictionCombine, b->FrictionCombine), -10.0F, 10.0F);
        point.m_combinedRestitution = std::clamp(Combine(a->Restitution, b->Restitution, a->RestitutionCombine, b->RestitutionCombine), 0.0F, 1.0F);
        return true;
    }

    void FlushExitEvents() {
        if (EventCallback) for (const auto& [key, old] : PreviousPairs) {
            auto event = old;
            event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerExit : MetaCorePhysicsEventType::CollisionExit;
            EventCallback(event);
        }
        PreviousPairs.clear();
    }

    void Clear() {
        for (auto& entry : Constraints) if (entry.Value) World->removeConstraint(entry.Value.get());
        Constraints.clear();
        for (auto& [id, entry] : Bodies) { (void)id; if (entry->Body) World->removeRigidBody(entry->Body.get()); }
        Bodies.clear(); PreviousPairs.clear();
    }

    std::uint64_t CalculateStructureSignature() const {
        std::uint64_t hash = 1469598103934665603ULL;
        const auto add = [&hash](std::uint64_t value) { hash ^= value; hash *= 1099511628211ULL; };
        const auto addFloat = [&add](float value) { add(std::bit_cast<std::uint32_t>(value)); };
        const auto addVec = [&addFloat](glm::vec3 value) { addFloat(value.x); addFloat(value.y); addFloat(value.z); };
        for (const MetaCoreGameObject object : Scene.GetGameObjects()) {
            add(object.GetId()); add(object.GetParentId()); addVec(object.GetComponent<MetaCoreTransformComponent>().Scale);
            add(object.HasComponent<MetaCoreColliderComponent>()); add(object.HasComponent<MetaCoreRigidBodyComponent>());
            add(object.HasComponent<MetaCorePhysicsConstraintComponent>()); add(object.HasComponent<MetaCoreCharacterControllerComponent>());
            if (object.HasComponent<MetaCoreColliderComponent>()) {
                const auto& value = object.GetComponent<MetaCoreColliderComponent>(); add(value.Enabled); add(static_cast<std::uint64_t>(value.Shape));
                addVec(value.Center); addVec(value.Size); addFloat(value.Radius); addFloat(value.Height);
                add(MetaCoreAssetGuidHasher{}(value.MeshAssetGuid)); add(MetaCoreAssetGuidHasher{}(value.MaterialAssetGuid)); add(value.CollisionLayer); add(value.IsTrigger);
            }
            if (object.HasComponent<MetaCoreRigidBodyComponent>()) {
                const auto& value = object.GetComponent<MetaCoreRigidBodyComponent>(); add(static_cast<std::uint64_t>(value.BodyType)); addFloat(value.Mass);
                addFloat(value.LinearDamping); addFloat(value.AngularDamping); addFloat(value.GravityScale); addVec(value.InitialLinearVelocity); addVec(value.InitialAngularVelocity);
                add(value.AllowSleep); add(value.ContinuousCollisionDetection); add(value.LockedAxes);
            }
            if (object.HasComponent<MetaCorePhysicsConstraintComponent>()) {
                const auto& value = object.GetComponent<MetaCorePhysicsConstraintComponent>(); add(value.Enabled); add(static_cast<std::uint64_t>(value.Type)); add(value.TargetObjectId);
                addVec(value.Anchor); addVec(value.TargetAnchor); addVec(value.Axis); addVec(value.LinearLowerLimit); addVec(value.LinearUpperLimit); addVec(value.AngularLowerLimit); addVec(value.AngularUpperLimit);
                add(value.MotorEnabled); addFloat(value.MotorTargetVelocity); addFloat(value.MotorMaxImpulse); addFloat(value.BreakForce); addFloat(value.BreakTorque); add(value.EnableConnectedCollision);
            }
            if (object.HasComponent<MetaCoreCharacterControllerComponent>()) {
                const auto& value = object.GetComponent<MetaCoreCharacterControllerComponent>(); add(value.Enabled); addFloat(value.Radius); addFloat(value.Height); addFloat(value.SkinWidth);
                addFloat(value.MaxSlopeDegrees); addFloat(value.StepHeight); addFloat(value.GravityScale); addFloat(value.MaxFallSpeed); addFloat(value.PushForce); add(value.CollisionLayer);
            }
        }
        return hash;
    }

    std::unique_ptr<btCollisionShape> MakeShape(const MetaCoreColliderComponent& collider, glm::vec3 absoluteScale,
        std::unique_ptr<btTriangleMesh>& triangleStorage, MetaCoreRigidBodyType bodyType, MetaCoreId objectId) {
        const glm::vec3 size = glm::max(glm::abs(collider.Size * absoluteScale), glm::vec3(0.001F));
        if (collider.Shape == MetaCoreColliderShape::Box) return std::make_unique<btBoxShape>(Bt(size * 0.5F));
        if (collider.Shape == MetaCoreColliderShape::Sphere) return std::make_unique<btSphereShape>(std::max(0.001F, collider.Radius * std::max({absoluteScale.x, absoluteScale.y, absoluteScale.z})));
        if (collider.Shape == MetaCoreColliderShape::Capsule) {
            const float radius = std::max(0.001F, collider.Radius * std::max(absoluteScale.x, absoluteScale.y));
            const float cylinderHeight = std::max(0.0F, collider.Height * absoluteScale.z - 2.0F * radius);
            return std::make_unique<btCapsuleShapeZ>(radius, cylinderHeight);
        }
        if (!MeshProvider || !collider.MeshAssetGuid.IsValid()) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 缺少网格碰撞载荷"); return {}; }
        const auto mesh = MeshProvider(collider.MeshAssetGuid);
        if (!mesh || mesh->Positions.empty()) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 的网格碰撞载荷无效"); return {}; }
        if (collider.Shape == MetaCoreColliderShape::ConvexHull) {
            auto shape = std::make_unique<btConvexHullShape>();
            for (glm::vec3 point : mesh->Positions) shape->addPoint(Bt(point * absoluteScale), false);
            shape->recalcLocalAabb(); return shape;
        }
        if (bodyType != MetaCoreRigidBodyType::Static) { Diagnostics.push_back("对象 " + std::to_string(objectId) + " 的动态凹网格已被拒绝"); return {}; }
        triangleStorage = std::make_unique<btTriangleMesh>();
        for (std::size_t index = 0; index + 2U < mesh->Indices.size(); index += 3U) {
            const auto a = mesh->Indices[index], b = mesh->Indices[index + 1U], c = mesh->Indices[index + 2U];
            if (a < mesh->Positions.size() && b < mesh->Positions.size() && c < mesh->Positions.size())
                triangleStorage->addTriangle(Bt(mesh->Positions[a] * absoluteScale), Bt(mesh->Positions[b] * absoluteScale), Bt(mesh->Positions[c] * absoluteScale), true);
        }
        return std::make_unique<btBvhTriangleMeshShape>(triangleStorage.get(), true, true);
    }

    void Rebuild() {
        FlushExitEvents(); Clear(); Diagnostics.clear(); SceneRevision = Scene.GetRevision(); StructureSignature = CalculateStructureSignature();
        for (MetaCoreGameObject object : Scene.GetGameObjects()) {
            const bool character = object.HasComponent<MetaCoreCharacterControllerComponent>() && object.GetComponent<MetaCoreCharacterControllerComponent>().Enabled;
            if (!object.HasComponent<MetaCoreColliderComponent>() && !character) continue;
            MetaCoreColliderComponent automaticCollider;
            if (character) { const auto& controller = object.GetComponent<MetaCoreCharacterControllerComponent>(); automaticCollider.Shape=MetaCoreColliderShape::Capsule; automaticCollider.Radius=controller.Radius; automaticCollider.Height=controller.Height; automaticCollider.CollisionLayer=controller.CollisionLayer; }
            const auto collider = object.HasComponent<MetaCoreColliderComponent>() ? object.GetComponent<MetaCoreColliderComponent>() : automaticCollider; if (!collider.Enabled) continue;
            if (object.HasComponent<MetaCoreCharacterControllerComponent>() && object.HasComponent<MetaCoreRigidBodyComponent>()) {
                Diagnostics.push_back("对象 " + std::to_string(object.GetId()) + " 同时包含 CharacterController 与 RigidBody"); continue;
            }
            auto bodyComponent = object.HasComponent<MetaCoreRigidBodyComponent>() ? object.GetComponent<MetaCoreRigidBodyComponent>() : MetaCoreRigidBodyComponent{};
            if (character) { bodyComponent.BodyType = MetaCoreRigidBodyType::Dynamic; bodyComponent.Mass = 80.0F; bodyComponent.AllowSleep = false; }
            auto entry = std::make_unique<BodyEntry>(); entry->Type = bodyComponent.BodyType; entry->Center = collider.Center; entry->Character = character;
            if (character) {
                const auto& controller = object.GetComponent<MetaCoreCharacterControllerComponent>();
                entry->CharacterHalfHeight = controller.Height * 0.5F;
                entry->CharacterSkinWidth = controller.SkinWidth;
                entry->CharacterMaxFallSpeed = controller.MaxFallSpeed;
            }
            glm::vec3 scale{}, translation{}, skew{}; glm::vec4 perspective{}; glm::quat rotation{};
            const glm::mat4 worldMatrix = WorldMatrix(Scene, object); glm::decompose(worldMatrix, scale, rotation, translation, skew, perspective);
            const bool invalidScale = !std::isfinite(scale.x) || !std::isfinite(scale.y) || !std::isfinite(scale.z) ||
                scale.x <= 1.0e-6F || scale.y <= 1.0e-6F || scale.z <= 1.0e-6F || glm::determinant(glm::mat3(worldMatrix)) <= 0.0F;
            const bool sheared = glm::dot(skew, skew) > 1.0e-8F;
            if (invalidScale || sheared) { Diagnostics.push_back("对象 " + std::to_string(object.GetId()) + " 的零/负缩放或父级非均匀变换无法用于物理"); continue; }
            entry->Scale = scale;
            entry->Shape = MakeShape(collider, scale, entry->TriangleMesh, bodyComponent.BodyType, object.GetId()); if (!entry->Shape) continue;
            btTransform transform = BuildTransform(worldMatrix); transform.setOrigin(transform.getOrigin() + transform.getBasis() * Bt(collider.Center * scale));
            btScalar mass = bodyComponent.BodyType == MetaCoreRigidBodyType::Dynamic ? std::max(0.0001F, bodyComponent.Mass) : 0.0F;
            btVector3 inertia{0.0F, 0.0F, 0.0F}; if (mass > 0.0F) entry->Shape->calculateLocalInertia(mass, inertia);
            entry->MotionState = std::make_unique<btDefaultMotionState>(transform);
            btRigidBody::btRigidBodyConstructionInfo info(mass, entry->MotionState.get(), entry->Shape.get(), inertia);
            info.m_linearDamping = std::clamp(bodyComponent.LinearDamping, 0.0F, 1.0F); info.m_angularDamping = std::clamp(bodyComponent.AngularDamping, 0.0F, 1.0F);
            entry->Body = std::make_unique<btRigidBody>(info);
            entry->Meta.Collider = object.GetId(); entry->Meta.Body = object.GetId();
            entry->Meta.Layer = std::min(collider.CollisionLayer, 31U); entry->Meta.Trigger = collider.IsTrigger;
            entry->Body->setUserPointer(&entry->Meta);
            if (bodyComponent.BodyType == MetaCoreRigidBodyType::Kinematic) {
                entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); entry->Body->setActivationState(DISABLE_DEACTIVATION);
            }
            if (collider.IsTrigger) entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
            if (!bodyComponent.AllowSleep) entry->Body->setActivationState(DISABLE_DEACTIVATION);
            if (bodyComponent.ContinuousCollisionDetection) { entry->Body->setCcdMotionThreshold(0.001F); entry->Body->setCcdSweptSphereRadius(std::max(0.001F, collider.Radius)); }
            entry->Body->setLinearVelocity(Bt(bodyComponent.InitialLinearVelocity)); entry->Body->setAngularVelocity(Bt(bodyComponent.InitialAngularVelocity));
            entry->Body->setGravity(Bt(Settings.Gravity * bodyComponent.GravityScale));
            if (character) entry->Body->setAngularFactor(btVector3(0.0F, 0.0F, 0.0F));
            if (MaterialProvider && collider.MaterialAssetGuid.IsValid()) if (auto materialValue = MaterialProvider(collider.MaterialAssetGuid)) {
                entry->Meta.Friction = std::max(0.0F, materialValue->DynamicFriction);
                entry->Meta.Restitution = std::clamp(materialValue->Restitution, 0.0F, 1.0F);
                entry->Meta.FrictionCombine = materialValue->FrictionCombine;
                entry->Meta.RestitutionCombine = materialValue->RestitutionCombine;
            }
            entry->Body->setFriction(entry->Meta.Friction); entry->Body->setRestitution(entry->Meta.Restitution);
            entry->Body->setCollisionFlags(entry->Body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
            const btVector3 linearFactor{
                (bodyComponent.LockedAxes & (1U << 0U)) ? 0.0F : 1.0F,
                (bodyComponent.LockedAxes & (1U << 1U)) ? 0.0F : 1.0F,
                (bodyComponent.LockedAxes & (1U << 2U)) ? 0.0F : 1.0F};
            const btVector3 angularFactor{
                (bodyComponent.LockedAxes & (1U << 3U)) ? 0.0F : 1.0F,
                (bodyComponent.LockedAxes & (1U << 4U)) ? 0.0F : 1.0F,
                (bodyComponent.LockedAxes & (1U << 5U)) ? 0.0F : 1.0F};
            entry->Body->setLinearFactor(linearFactor); entry->Body->setAngularFactor(character ? btVector3(0.0F, 0.0F, 0.0F) : angularFactor);
            World->addRigidBody(entry->Body.get()); Bodies.emplace(object.GetId(), std::move(entry));
        }
        for (MetaCoreGameObject object : Scene.GetGameObjects()) {
            if (!object.HasComponent<MetaCorePhysicsConstraintComponent>()) continue;
            const auto& source = object.GetComponent<MetaCorePhysicsConstraintComponent>(); if (!source.Enabled) continue;
            const auto owner = Bodies.find(object.GetId()); if (owner == Bodies.end()) { Diagnostics.push_back("约束对象 " + std::to_string(object.GetId()) + " 缺少刚体"); continue; }
            auto target = source.TargetObjectId == 0 ? nullptr : (Bodies.contains(source.TargetObjectId) ? Bodies.at(source.TargetObjectId).get() : nullptr);
            if (source.TargetObjectId != 0 && !target) { Diagnostics.push_back("约束对象 " + std::to_string(object.GetId()) + " 的目标刚体不存在"); continue; }
            btRigidBody& a = *owner->second->Body; btRigidBody& b = target ? *target->Body : btTypedConstraint::getFixedBody();
            btTransform frameA, frameB; frameA.setIdentity(); frameB.setIdentity(); frameA.setOrigin(Bt(source.Anchor)); frameB.setOrigin(Bt(source.TargetAnchor));
            std::unique_ptr<btTypedConstraint> constraint;
            if (source.Type == MetaCorePhysicsConstraintType::Fixed) constraint = std::make_unique<btFixedConstraint>(a, b, frameA, frameB);
            else if (source.Type == MetaCorePhysicsConstraintType::Hinge) {
                auto value = std::make_unique<btHingeConstraint>(a, b, frameA, frameB); value->setLimit(glm::radians(source.AngularLowerLimit.x), glm::radians(source.AngularUpperLimit.x));
                if (source.MotorEnabled) value->enableAngularMotor(true, source.MotorTargetVelocity, source.MotorMaxImpulse); constraint = std::move(value);
            } else if (source.Type == MetaCorePhysicsConstraintType::Slider) {
                auto value = std::make_unique<btSliderConstraint>(a, b, frameA, frameB, true); value->setLowerLinLimit(source.LinearLowerLimit.x); value->setUpperLinLimit(source.LinearUpperLimit.x);
                if (source.MotorEnabled) { value->setPoweredLinMotor(true); value->setTargetLinMotorVelocity(source.MotorTargetVelocity); value->setMaxLinMotorForce(source.MotorMaxImpulse); } constraint = std::move(value);
            } else {
                auto value = std::make_unique<btGeneric6DofConstraint>(a, b, frameA, frameB, true); value->setLinearLowerLimit(Bt(source.LinearLowerLimit)); value->setLinearUpperLimit(Bt(source.LinearUpperLimit));
                value->setAngularLowerLimit(Bt(glm::radians(source.AngularLowerLimit))); value->setAngularUpperLimit(Bt(glm::radians(source.AngularUpperLimit))); constraint = std::move(value);
            }
            if (source.BreakForce > 0.0F) constraint->setBreakingImpulseThreshold(source.BreakForce * static_cast<float>(Settings.FixedTimeStep));
            World->addConstraint(constraint.get(), !source.EnableConnectedCollision); Constraints.push_back({object.GetId(), source.TargetObjectId, source.Type, std::move(constraint), false});
        }
        Statistics.BodyCount = Bodies.size();
    }

    bool Accept(const Metadata* metadata, const MetaCorePhysicsQueryFilter& filter) const {
        return metadata && metadata->Layer < 32U && ((filter.LayerMask >> metadata->Layer) & 1U) != 0U &&
            !(metadata->Trigger && filter.Triggers == MetaCorePhysicsTriggerQuery::Ignore) &&
            !filter.IgnoredObjectIds.contains(metadata->Collider) && !filter.IgnoredBodyIds.contains(metadata->Body);
    }

    MetaCorePhysicsHit Hit(const btCollisionObject* object, const btVector3& position, const btVector3& normal, float distance, float fraction) const {
        const auto* metadata = object ? static_cast<const Metadata*>(object->getUserPointer()) : nullptr;
        return {metadata ? metadata->Collider : 0, metadata ? metadata->Body : 0, Glm(position), Glm(normal), distance, fraction, metadata && metadata->Trigger};
    }

    void EmitContacts() {
        std::map<PairKey, MetaCorePhysicsContactEvent> current;
        for (int index = 0; index < Dispatcher->getNumManifolds(); ++index) {
            const btPersistentManifold* manifold = Dispatcher->getManifoldByIndexInternal(index); if (!manifold || manifold->getNumContacts() == 0) continue;
            const auto* a = static_cast<const Metadata*>(manifold->getBody0()->getUserPointer()); const auto* b = static_cast<const Metadata*>(manifold->getBody1()->getUserPointer()); if (!a || !b) continue;
            const bool trigger = a->Trigger || b->Trigger; const bool swapped = a->Body > b->Body;
            PairKey key{std::min(a->Body, b->Body), std::max(a->Body, b->Body), trigger};
            const btManifoldPoint& point = manifold->getContactPoint(0); if (point.getDistance() > 0.0F) continue;
            MetaCorePhysicsContactEvent event; event.Type = trigger ? MetaCorePhysicsEventType::TriggerStay : MetaCorePhysicsEventType::CollisionStay;
            event.ObjectA = swapped ? b->Body : a->Body; event.ObjectB = swapped ? a->Body : b->Body;
            event.ColliderA = swapped ? b->Collider : a->Collider; event.ColliderB = swapped ? a->Collider : b->Collider;
            event.Point = Glm(point.getPositionWorldOnB()); event.Normal = Glm(swapped ? -point.m_normalWorldOnB : point.m_normalWorldOnB); event.Impulse = point.getAppliedImpulse();
            const auto* bodyA = btRigidBody::upcast(manifold->getBody0()); const auto* bodyB = btRigidBody::upcast(manifold->getBody1());
            const btVector3 relativeVelocity = (bodyB ? bodyB->getLinearVelocity() : btVector3{}) - (bodyA ? bodyA->getLinearVelocity() : btVector3{});
            event.RelativeVelocity = Glm(swapped ? -relativeVelocity : relativeVelocity);
            current.insert_or_assign(key, event);
        }
        for (auto& [key, event] : current) {
            if (!PreviousPairs.contains(key)) event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerEnter : MetaCorePhysicsEventType::CollisionEnter;
            if (EventCallback) EventCallback(event);
        }
        for (const auto& [key, old] : PreviousPairs) if (!current.contains(key) && EventCallback) {
            auto event = old; event.Type = key.Trigger ? MetaCorePhysicsEventType::TriggerExit : MetaCorePhysicsEventType::CollisionExit; EventCallback(event);
        }
        PreviousPairs = std::move(current); Statistics.ContactCount = PreviousPairs.size();
        for (auto& entry : Constraints) if (!entry.Broken && entry.Value && !entry.Value->isEnabled()) {
            entry.Broken = true; if (EventCallback) { MetaCorePhysicsContactEvent event; event.Type=MetaCorePhysicsEventType::ConstraintBroken; event.ObjectA=entry.Owner; event.ObjectB=entry.Target; EventCallback(event); }
        }
    }

    void SyncKinematicAndStatic() {
        for (auto& [id, entry] : Bodies) {
            if (entry->Type == MetaCoreRigidBodyType::Dynamic) continue;
            const auto object = Scene.FindGameObject(id); if (!object) continue;
            btTransform transform = BuildTransform(WorldMatrix(Scene, object));
            transform.setOrigin(transform.getOrigin() + transform.getBasis() * Bt(entry->Center * entry->Scale));
            entry->Body->setWorldTransform(transform); entry->MotionState->setWorldTransform(transform); entry->Body->activate(true);
        }
    }

    void WriteBack() {
        for (auto& [id, entry] : Bodies) {
            if (entry->Type != MetaCoreRigidBodyType::Dynamic) continue;
            auto object = Scene.FindGameObject(id); if (!object) continue;
            btTransform transform; entry->MotionState->getWorldTransform(transform);
            transform.setOrigin(transform.getOrigin() - transform.getBasis() * Bt(entry->Center * entry->Scale));
            glm::mat4 local = BuildMatrix(transform, entry->Scale);
            if (object.GetParentId() != 0) if (const auto parent = Scene.FindGameObject(object.GetParentId())) local = glm::inverse(WorldMatrix(Scene, parent)) * local;
            MetaCoreApplyMatrixToTransform(local, object.GetComponent<MetaCoreTransformComponent>());
        }
    }
};

MetaCorePhysicsWorld::MetaCorePhysicsWorld(MetaCoreScene& scene, MetaCorePhysicsSettingsDocument settings,
    MetaCorePhysicsMeshProvider meshProvider, MetaCorePhysicsMaterialProvider materialProvider)
    : Impl_(std::make_unique<Impl>(scene, std::move(settings), std::move(meshProvider), std::move(materialProvider))) {}
MetaCorePhysicsWorld::~MetaCorePhysicsWorld() = default;
void MetaCorePhysicsWorld::SynchronizeScene() {
    const auto signature = Impl_->CalculateStructureSignature();
    if (signature != Impl_->StructureSignature) Impl_->Rebuild();
    else { Impl_->SceneRevision = Impl_->Scene.GetRevision(); Impl_->SyncKinematicAndStatic(); }
}
void MetaCorePhysicsWorld::SetEventCallback(MetaCorePhysicsEventCallback callback) { Impl_->EventCallback = std::move(callback); }
void MetaCorePhysicsWorld::Step(double fixedDeltaSeconds) {
    if (Impl_->Stopped || !std::isfinite(fixedDeltaSeconds) || fixedDeltaSeconds <= 0.0) return;
    const auto begin = std::chrono::steady_clock::now(); SynchronizeScene();
    for (auto& [id, entry] : Impl_->Bodies) {
        (void)id;
        if (!entry->Character) continue;
        btVector3 velocity = entry->Body->getLinearVelocity();
        velocity.setZ(std::max(velocity.z(), -entry->CharacterMaxFallSpeed));
        entry->Body->setLinearVelocity(velocity);
    }
    Impl_->World->stepSimulation(static_cast<btScalar>(fixedDeltaSeconds), 0);
    Impl_->WriteBack(); Impl_->EmitContacts(); Impl_->Statistics.LastStepMilliseconds = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - begin).count();
}
void MetaCorePhysicsWorld::Stop() {
    if (Impl_->Stopped) return;
    Impl_->FlushExitEvents();
    Impl_->Stopped = true; Impl_->Clear();
}

std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::RaycastAll(glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) {
    ++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result;
    if (!std::isfinite(maxDistance) || maxDistance <= 0.0F || glm::dot(direction, direction) < 1.0e-12F) return result;
    direction = glm::normalize(direction); const btVector3 from = Bt(origin), to = Bt(origin + direction * maxDistance);
    struct Callback final : btCollisionWorld::AllHitsRayResultCallback {
        Impl* Owner; const MetaCorePhysicsQueryFilter& Filter;
        Callback(const btVector3& a, const btVector3& b, Impl* owner, const MetaCorePhysicsQueryFilter& filter) : AllHitsRayResultCallback(a, b), Owner(owner), Filter(filter) {}
        bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
    } callback(from, to, Impl_.get(), filter);
    Impl_->World->rayTest(from, to, callback);
    for (int index = 0; index < callback.m_collisionObjects.size(); ++index) result.push_back(Impl_->Hit(callback.m_collisionObjects[index], callback.m_hitPointWorld[index], callback.m_hitNormalWorld[index], maxDistance * callback.m_hitFractions[index], callback.m_hitFractions[index]));
    std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.Fraction, a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.Fraction, b.ColliderObjectId, b.RigidBodyObjectId); }); return result;
}
std::optional<MetaCorePhysicsHit> MetaCorePhysicsWorld::Raycast(glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) { auto all = RaycastAll(origin, direction, maxDistance, filter); return all.empty() ? std::nullopt : std::optional<MetaCorePhysicsHit>(all.front()); }

static std::unique_ptr<btConvexShape> MetaCoreMakeQueryShape(const MetaCorePhysicsQueryShape& shape) {
    if (shape.Type == MetaCorePhysicsQueryShapeType::Sphere && shape.Radius > 0.0F) return std::make_unique<btSphereShape>(shape.Radius);
    if (shape.Type == MetaCorePhysicsQueryShapeType::Capsule && shape.Radius > 0.0F && shape.Height >= shape.Radius * 2.0F) return std::make_unique<btCapsuleShapeZ>(shape.Radius, shape.Height - shape.Radius * 2.0F);
    if (shape.Type == MetaCorePhysicsQueryShapeType::Box && shape.HalfExtents.x > 0.0F && shape.HalfExtents.y > 0.0F && shape.HalfExtents.z > 0.0F) return std::make_unique<btBoxShape>(Bt(shape.HalfExtents));
    return {};
}

std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::ShapeCastAll(const MetaCorePhysicsQueryShape& shape, glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) {
    ++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result; auto queryShape = MetaCoreMakeQueryShape(shape);
    if (!queryShape || maxDistance <= 0.0F || glm::dot(direction, direction) < 1.0e-12F) return result;
    direction = glm::normalize(direction);
    btTransform from, to; from.setIdentity(); to.setIdentity(); from.setOrigin(Bt(origin)); to.setOrigin(Bt(origin + direction * maxDistance));
    struct Callback final : btCollisionWorld::ClosestConvexResultCallback {
        Impl* Owner; const MetaCorePhysicsQueryFilter& Filter; std::vector<MetaCorePhysicsHit>& Hits; float Distance;
        Callback(const btVector3& a, const btVector3& b, Impl* owner, const MetaCorePhysicsQueryFilter& filter, std::vector<MetaCorePhysicsHit>& hits, float distance)
            : ClosestConvexResultCallback(a, b), Owner(owner), Filter(filter), Hits(hits), Distance(distance) { m_closestHitFraction = 1.0F; }
        bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
        btScalar addSingleResult(btCollisionWorld::LocalConvexResult& value, bool normalInWorldSpace) override {
            const btVector3 normal = normalInWorldSpace ? value.m_hitNormalLocal : value.m_hitCollisionObject->getWorldTransform().getBasis() * value.m_hitNormalLocal;
            Hits.push_back(Owner->Hit(value.m_hitCollisionObject, value.m_hitPointLocal, normal, Distance * value.m_hitFraction, value.m_hitFraction)); return 1.0F;
        }
    } callback(from.getOrigin(), to.getOrigin(), Impl_.get(), filter, result, maxDistance);
    Impl_->World->convexSweepTest(queryShape.get(), from, to, callback);
    std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.Fraction, a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.Fraction, b.ColliderObjectId, b.RigidBodyObjectId); }); return result;
}
std::optional<MetaCorePhysicsHit> MetaCorePhysicsWorld::ShapeCast(const MetaCorePhysicsQueryShape& shape, glm::vec3 origin, glm::vec3 direction, float maxDistance, const MetaCorePhysicsQueryFilter& filter) { auto all = ShapeCastAll(shape, origin, direction, maxDistance, filter); return all.empty() ? std::nullopt : std::optional<MetaCorePhysicsHit>(all.front()); }

std::vector<MetaCorePhysicsHit> MetaCorePhysicsWorld::Overlap(const MetaCorePhysicsQueryShape& shape, glm::vec3 center, const MetaCorePhysicsQueryFilter& filter) {
    ++Impl_->Statistics.QueryCount; std::vector<MetaCorePhysicsHit> result; auto queryShape = MetaCoreMakeQueryShape(shape); if (!queryShape) return result;
    btCollisionObject query; query.setCollisionShape(queryShape.get()); btTransform transform; transform.setIdentity(); transform.setOrigin(Bt(center)); query.setWorldTransform(transform);
    struct Callback final : btCollisionWorld::ContactResultCallback {
        Impl* Owner; const MetaCorePhysicsQueryFilter& Filter; std::vector<MetaCorePhysicsHit>& Hits;
        Callback(Impl* owner, const MetaCorePhysicsQueryFilter& filter, std::vector<MetaCorePhysicsHit>& hits) : Owner(owner), Filter(filter), Hits(hits) {}
        bool needsCollision(btBroadphaseProxy* proxy) const override { const auto* object = static_cast<const btCollisionObject*>(proxy->m_clientObject); return Owner->Accept(object ? static_cast<const Impl::Metadata*>(object->getUserPointer()) : nullptr, Filter); }
        btScalar addSingleResult(btManifoldPoint& point, const btCollisionObjectWrapper*, int, int, const btCollisionObjectWrapper* b, int, int) override {
            const btCollisionObject* object = b->getCollisionObject(); Hits.push_back(Owner->Hit(object, point.getPositionWorldOnB(), point.m_normalWorldOnB, 0.0F, 0.0F)); return 0.0F;
        }
    } callback(Impl_.get(), filter, result);
    Impl_->World->contactTest(&query, callback);
    std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { return std::tie(a.ColliderObjectId, a.RigidBodyObjectId) < std::tie(b.ColliderObjectId, b.RigidBodyObjectId); });
    result.erase(std::unique(result.begin(), result.end(), [](const auto& a, const auto& b) { return a.ColliderObjectId == b.ColliderObjectId && a.RigidBodyObjectId == b.RigidBodyObjectId; }), result.end()); return result;
}

bool MetaCorePhysicsWorld::AddForce(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || it->second->Type != MetaCoreRigidBodyType::Dynamic) return false; it->second->Body->activate(true); it->second->Body->applyCentralForce(Bt(value)); return true; }
bool MetaCorePhysicsWorld::AddImpulse(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || it->second->Type != MetaCoreRigidBodyType::Dynamic) return false; it->second->Body->activate(true); it->second->Body->applyCentralImpulse(Bt(value)); return true; }
bool MetaCorePhysicsWorld::SetLinearVelocity(MetaCoreId id, glm::vec3 value) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end()) return false; it->second->Body->setLinearVelocity(Bt(value)); it->second->Body->activate(true); return true; }
std::optional<glm::vec3> MetaCorePhysicsWorld::GetLinearVelocity(MetaCoreId id) const { const auto it = Impl_->Bodies.find(id); return it == Impl_->Bodies.end() ? std::nullopt : std::optional<glm::vec3>(Glm(it->second->Body->getLinearVelocity())); }
bool MetaCorePhysicsWorld::WakeUp(MetaCoreId id) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end()) return false; it->second->Body->activate(true); return true; }
bool MetaCorePhysicsWorld::CharacterMove(MetaCoreId id, glm::vec3 displacement) { const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || !Impl_->Scene.FindGameObject(id).HasComponent<MetaCoreCharacterControllerComponent>()) return false; auto velocity=it->second->Body->getLinearVelocity(); velocity.setX(displacement.x/static_cast<float>(Impl_->Settings.FixedTimeStep)); velocity.setY(displacement.y/static_cast<float>(Impl_->Settings.FixedTimeStep)); it->second->Body->setLinearVelocity(velocity); it->second->Body->activate(true); return true; }
bool MetaCorePhysicsWorld::CharacterJump(MetaCoreId id, float speed) { if (!IsCharacterGrounded(id)) return false; auto value=GetLinearVelocity(id); if(!value)return false; value->z=speed; return SetLinearVelocity(id,*value); }
bool MetaCorePhysicsWorld::SetConstraintEnabled(MetaCoreId id, bool enabled) {
    const auto iterator = std::find_if(Impl_->Constraints.begin(), Impl_->Constraints.end(), [id](const auto& value) { return value.Owner == id; });
    if (iterator == Impl_->Constraints.end() || !iterator->Value) return false;
    iterator->Value->setEnabled(enabled); if (enabled) iterator->Broken = false; return true;
}
bool MetaCorePhysicsWorld::SetConstraintMotor(MetaCoreId id, bool enabled, float targetVelocity, float maxImpulse) {
    const auto iterator = std::find_if(Impl_->Constraints.begin(), Impl_->Constraints.end(), [id](const auto& value) { return value.Owner == id; });
    if (iterator == Impl_->Constraints.end() || !iterator->Value || !std::isfinite(targetVelocity) || !std::isfinite(maxImpulse) || maxImpulse < 0.0F) return false;
    if (auto* hinge = dynamic_cast<btHingeConstraint*>(iterator->Value.get())) { hinge->enableAngularMotor(enabled, targetVelocity, maxImpulse); return true; }
    if (auto* slider = dynamic_cast<btSliderConstraint*>(iterator->Value.get())) { slider->setPoweredLinMotor(enabled); slider->setTargetLinMotorVelocity(targetVelocity); slider->setMaxLinMotorForce(maxImpulse); return true; }
    if (auto* sixDof = dynamic_cast<btGeneric6DofConstraint*>(iterator->Value.get())) {
        auto* motor = sixDof->getTranslationalLimitMotor(); motor->m_enableMotor[0] = enabled; motor->m_targetVelocity[0] = targetVelocity; motor->m_maxMotorForce[0] = maxImpulse; return true;
    }
    return false;
}
bool MetaCorePhysicsWorld::IsCharacterGrounded(MetaCoreId id) const {
    const auto it = Impl_->Bodies.find(id); if (it == Impl_->Bodies.end() || !it->second->Character) return false;
    const btVector3 from = it->second->Body->getWorldTransform().getOrigin();
    const float distance = it->second->CharacterHalfHeight * it->second->Scale.z + it->second->CharacterSkinWidth + 0.08F;
    struct Callback final : btCollisionWorld::ClosestRayResultCallback {
        const btCollisionObject* Ignored;
        Callback(const btVector3& start, const btVector3& end, const btCollisionObject* ignored)
            : ClosestRayResultCallback(start, end), Ignored(ignored) {}
        bool needsCollision(btBroadphaseProxy* proxy) const override {
            return proxy && proxy->m_clientObject != Ignored && ClosestRayResultCallback::needsCollision(proxy);
        }
    } callback(from, from + btVector3(0.0F, 0.0F, -distance), it->second->Body.get());
    Impl_->World->rayTest(callback.m_rayFromWorld, callback.m_rayToWorld, callback);
    return callback.hasHit() && callback.m_hitNormalWorld.z() >= 0.5F;
}

std::vector<MetaCorePhysicsDebugLine> MetaCorePhysicsWorld::BuildDebugLines() const {
    std::vector<MetaCorePhysicsDebugLine> lines; const auto addBox = [&](glm::vec3 min, glm::vec3 max, glm::vec3 color) {
        const glm::vec3 c[8]{{min.x,min.y,min.z},{max.x,min.y,min.z},{max.x,max.y,min.z},{min.x,max.y,min.z},{min.x,min.y,max.z},{max.x,min.y,max.z},{max.x,max.y,max.z},{min.x,max.y,max.z}};
        constexpr int edges[12][2]{{0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},{0,4},{1,5},{2,6},{3,7}}; for (auto& edge : edges) lines.push_back({c[edge[0]], c[edge[1]], color});
    };
    for (const auto& [id, entry] : Impl_->Bodies) { (void)id; btVector3 min, max; entry->Body->getAabb(min, max); addBox(Glm(min), Glm(max), entry->Meta.Trigger ? glm::vec3(1.0F,0.7F,0.1F) : entry->Body->isActive() ? glm::vec3(0.2F,1.0F,0.3F) : glm::vec3(0.3F,0.5F,1.0F)); } return lines;
}
const MetaCorePhysicsStatistics& MetaCorePhysicsWorld::GetStatistics() const { return Impl_->Statistics; }
const std::vector<std::string>& MetaCorePhysicsWorld::GetDiagnostics() const { return Impl_->Diagnostics; }
const MetaCorePhysicsSettingsDocument& MetaCorePhysicsWorld::GetSettings() const { return Impl_->Settings; }

class MetaCoreBulletPhysicsBackend final : public MetaCoreIPhysicsBackend {
public:
    std::string GetBackendId() const override { return "Bullet3"; }
    std::unique_ptr<MetaCorePhysicsWorld> CreateWorld(MetaCoreScene& scene, const MetaCorePhysicsSettingsDocument& settings,
        MetaCorePhysicsMeshProvider mesh, MetaCorePhysicsMaterialProvider material) override {
        return std::make_unique<MetaCorePhysicsWorld>(scene, settings, std::move(mesh), std::move(material));
    }
};
std::unique_ptr<MetaCoreIPhysicsBackend> MetaCoreCreateBulletPhysicsBackend() { return std::make_unique<MetaCoreBulletPhysicsBackend>(); }

} // namespace MetaCore