using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using System.Windows;
using System.Diagnostics;
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Linq;
using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints;
using BepuPhysics.Trees;
using BepuUtilities;
using BepuUtilities.Memory;
using CtrEditor.FuncionesBase;
using DocumentFormat.OpenXml.Vml;
using DocumentFormat.OpenXml.Spreadsheet;

namespace CtrEditor.Simulacion
{

    // Callback handlers para BEPU
    public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
    {
        private SimulationManagerBEPU _simulationManager;
        private const float minPressureToLostFriction = 1;

        public NarrowPhaseCallbacks(SimulationManagerBEPU simulationManager)
        {
            _simulationManager = simulationManager;
        }

        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
        {
            // ✅ NUEVO FILTRADO: Evitar colisiones innecesarias entre triángulos de curvas
            if (_simulationManager != null)
            {
                // Obtener información de los objetos involucrados
                var curveA = _simulationManager.GetCurveFromTriangleBodyHandle(a.BodyHandle);
                var curveB = _simulationManager.GetCurveFromTriangleBodyHandle(b.BodyHandle);
                var transportA = GetTransportFromCollidable(a);
                var transportB = GetTransportFromCollidable(b);
                // ✅ ELIMINADO: Las barreras ya no tienen body físico
                // var barrierA = GetBarreraFromCollidable(a);
                // var barrierB = GetBarreraFromCollidable(b);
                var discardA = GetDescarteFromCollidable(a);
                var discardB = GetDescarteFromCollidable(b);
                var botellaA = GetBotellaFromCollidable(a);
                var botellaB = GetBotellaFromCollidable(b);
                
                // ✅ FILTRO 1: No permitir colisiones entre triángulos de curvas
                if (curveA != null && curveB != null)
                {
                    return false; // Los triángulos de curva no deben colisionar entre sí
                }
                
                // ✅ FILTRO 2: No permitir colisiones entre transportes
                if (transportA != null && transportB != null)
                {
                    return false; // Los transportes no deben colisionar entre sí
                }
                
                // ✅ FILTRO 3: No permitir colisiones entre elementos estáticos (transportes, curvas, descartes)
                // ✅ ELIMINADO: barrierA y barrierB ya que las barreras no tienen body físico
                var staticA = (transportA != null || curveA != null || discardA != null);
                var staticB = (transportB != null || curveB != null || discardB != null);
                
                if (staticA && staticB)
                {
                    return false; // Los elementos estáticos no deben colisionar entre sí
                }
                
                // ✅ ELIMINADO: No bloquear colisiones físicas aquí
                // Las colisiones físicas deben permitirse siempre para mantener geometría sólida
                // Solo bloqueamos la creación de motores duplicados en ConfigureContactManifold
            }
            
            return true; // Permitir todas las demás colisiones (botella-estático, botella-botella)
        }

        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
        {
            return true;
        }

        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            // ✅ CONFIGURACIÓN BÁSICA de materiales físicos con valores rígidos pero estables
            pairMaterial = new PairMaterialProperties
            {
                FrictionCoefficient = 0.3f,
                MaximumRecoveryVelocity = 0.8f,  // ✅ AJUSTADO: Velocidad de separación moderada
                SpringSettings = new SpringSettings(120, 8)  // ✅ AJUSTADO: Más rígido pero bien amortiguado
            };

            // ✅ ELIMINADO: Sistema de limitación de fuerzas forceScale - reemplazado por intervención post-solver

            if (_simulationManager != null)
            {
                var botellaA = GetBotellaFromCollidable(pair.A);
                var botellaB = GetBotellaFromCollidable(pair.B);
                var descarteA = GetDescarteFromCollidable(pair.A);
                var descarteB = GetDescarteFromCollidable(pair.B);
                var botella = botellaA ?? botellaB;

                // ✅ DESCARTES como sensores puros
                if (descarteA != null || descarteB != null)
                {
                    if (botella != null)
                    {
                        // ✅ CORREGIDO: Marcar la botella para eliminar directamente, en lugar de registrar el contacto.
                        // Esto es más directo y evita el error de compilación.
                        botella.Descartar = true;
                    }
                    return false; // NO generar contacto físico
                }

                // ✅ NUEVO SISTEMA SIMPLIFICADO: Solo registrar contactos para actualización en Step
                var transportA = GetTransportFromCollidable(pair.A);
                var transportB = GetTransportFromCollidable(pair.B);
                var curveA = GetCurveFromCollidable(pair.A);
                var curveB = GetCurveFromCollidable(pair.B);

                // ✅ CONTACTO BOTELLA-TRANSPORTE: USAR FRICCIÓN Y VELOCIDAD CINEMÁTICA
                if (botella != null && (transportA != null || transportB != null))
                {
                    // ✅ ELIMINADO: botella.forceScale = forceScale;
                    // ✅ NUEVO: Lógica de control de presión/flotación
                    botella.IsTouchingTransport = true;
                    var botellaCollidable = GetBotellaFromCollidable(pair.A) != null ? pair.A : pair.B;
                    if (botellaCollidable.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botellaCollidable.BodyHandle))
                    {
                        var body = _simulationManager.simulation.Bodies.GetBodyReference(botellaCollidable.BodyHandle);
                        botella.LastTransportCollisionZ = body.Pose.Position.Z;
                    }

                    var transport = transportA ?? transportB;
                    if (transport.isBrake)
                    {
                        // ✅ NUEVO: Centrar la botella en el transporte de frenado la primera vez que entra.
                        {
                            if (botella.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botella.BodyHandle) &&
                                transport.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(transport.BodyHandle))
                            {
                                var bottleBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                                var transportBody = _simulationManager.simulation.Bodies.GetBodyReference(transport.BodyHandle);

                                var bottlePose = bottleBody.Pose;
                                var transportPose = transportBody.Pose;

                                // Proyectar la posición de la botella sobre el eje longitudinal del transporte.
                                Vector3 transportForward = transport.DirectionVector;
                                Vector3 vectorToBottle = bottlePose.Position - transportPose.Position;
                                float projectionLength = Vector3.Dot(vectorToBottle, transportForward);
                                Vector3 newPositionOnCenterline = transportPose.Position + transportForward * projectionLength;

                                // Crear la nueva pose manteniendo la Z original de la botella para no hundirla.
                                var newBottlePosition = new Vector3(newPositionOnCenterline.X, newPositionOnCenterline.Y, bottlePose.Position.Z);
                                bottleBody.Pose.Position = newBottlePosition;
                                
                                // Opcional: Anular la velocidad lateral para un acoplamiento más suave.
                                var lateralVelocity = Vector3.Dot(bottleBody.Velocity.Linear, transport.DirectionVector);
                                bottleBody.Velocity.Linear = transport.DirectionVector * lateralVelocity;
                            }
                        }

                        botella.isOnBrakeTransport = true;
                        pairMaterial.FrictionCoefficient = 8f;
                        pairMaterial.MaximumRecoveryVelocity = 1f;  // ✅ REMOVIDO: * forceScale
                        pairMaterial.SpringSettings = new SpringSettings(30, 1f);  // ✅ REMOVIDO: * forceScale
                    }
                    else
                    {
                        botella.isOnBrakeTransport = false;
                        // ✅ NUEVO: Usar fricción dinámica basada en deslizamiento
                        pairMaterial.FrictionCoefficient = CalculateTransportFriction(botella, transport, botellaCollidable);
                        pairMaterial.MaximumRecoveryVelocity = 1f;  // ✅ REMOVIDO: * forceScale
                        pairMaterial.SpringSettings = new SpringSettings(30, 1f);  // ✅ REMOVIDO: * forceScale
                    }

                }
                // ✅ CONTACTO BOTELLA-CURVA: USAR FRICCIÓN Y VELOCIDAD CINEMÁTICA
                else if (botella != null && (curveA != null || curveB != null))
                {
                    // ✅ ELIMINADO: botella.forceScale = forceScale;
                    // ✅ NUEVO: Lógica de control de presión/flotación
                    botella.IsTouchingTransport = true;
                    botella.isOnBrakeTransport = false;
                    var botellaCollidable = GetBotellaFromCollidable(pair.A) != null ? pair.A : pair.B;
                    if (botellaCollidable.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botellaCollidable.BodyHandle))
                    {
                        var body = _simulationManager.simulation.Bodies.GetBodyReference(botellaCollidable.BodyHandle);
                        botella.LastTransportCollisionZ = body.Pose.Position.Z;
                    }

                    // ✅ NUEVO: Usar fricción dinámica basada en deslizamiento
                    var curve = curveA ?? curveB;
                    pairMaterial.FrictionCoefficient = CalculateCurveFriction(botella, curve, botellaCollidable);
                    pairMaterial.MaximumRecoveryVelocity = 1f;  // ✅ REMOVIDO: * forceScale
                    pairMaterial.SpringSettings = new SpringSettings(30, 1f); // ✅ REMOVIDO: * forceScale
                }
                // ✅ CONTACTO BOTELLA-GUÍA: Configuración específica de fricción
                else if (botella != null && (GetGuiaFromCollidable(pair.A) != null || GetGuiaFromCollidable(pair.B) != null))
                {
                    // Fricción baja para las guías, deben deslizar, no arrastrar.
                    pairMaterial.FrictionCoefficient = 0.01f;
                    pairMaterial.MaximumRecoveryVelocity = 0.95f;  // ✅ REMOVIDO: * forceScale
                    pairMaterial.SpringSettings = new SpringSettings(40, 1f); // ✅ REMOVIDO: * forceScale
                }
                // ✅ NUEVO: CONTACTO BOTELLA-BOTELLA: Lógica de presión proactiva basada en la penetración.
                else if (botellaA != null && botellaB != null)
                {
                    // ✅ NUEVO: Usar método especializado para contactos botella-botella
                    CalculateBottlePression(ref manifold, botellaA, botellaB);
                    
                    // ✅ NUEVO: Limitar fuerzas basadas en profundidad de penetración
                    //var maxPenetrationDepth = GetMaxPenetrationDepth(ref manifold);
                    // Fuerzas normales para contactos superficiales - botellas rígidas
                    pairMaterial.FrictionCoefficient = 0.02f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;  // ✅ REMOVIDO: * forceScale
                    pairMaterial.SpringSettings = new SpringSettings(40, 10f);  // ✅ REMOVIDO: * forceScale
                }
                // Ajustes básicos para otras botellas
                else if (botella != null)
                {
                    System.Diagnostics.Debug.WriteLine($"[ConfigureContactManifold] ERROR: contacto no resuelto!");
                    // Fricción moderada para colisiones entre botellas.
                    pairMaterial.FrictionCoefficient = 0f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;  // ✅ REMOVIDO: * forceScale
                    pairMaterial.SpringSettings = new SpringSettings(80, 1);  // ✅ REMOVIDO: * forceScale
                }
            }

            return true;
        }



        private simTransporte GetTransportFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simTransporte;
                }
            }
            return null;
        }

        private simBotella GetBotellaFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Dynamic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simBotella;
                }
            }
            return null;
        }

        // ✅ ELIMINADO: GetBarreraFromCollidable - las barreras ya no tienen body físico
        // private simBarrera GetBarreraFromCollidable(CollidableReference collidable) { ... }

        private simDescarte GetDescarteFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simDescarte;
                }
            }
            return null;
        }

        private simCurve GetCurveFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simCurve;
                }
            }
            return null;
        }

        private simGuia GetGuiaFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simGuia;
                }
            }
            return null;
        }







        /// <summary>
        /// ✅ NUEVO: Calcula la fricción dinámica para contactos botella-transporte basada en el deslizamiento relativo
        /// </summary>
        private float CalculateTransportFriction(simBotella botella, simTransporte transport, CollidableReference botellaCollidable)
        {
            try
            {
                if (botella?.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botella.BodyHandle) &&
                    transport?.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(transport.BodyHandle))
                {
                    var bottleBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                    var transportBody = _simulationManager.simulation.Bodies.GetBodyReference(transport.BodyHandle);

                    var bottleVelocity = bottleBody.Velocity.Linear;
                    var transportVelocity = transport.CalcularVelocidadCinematica(); // La velocidad cinemática se actualiza en OnSubstepStarted

                    var slipVelocity = bottleVelocity - transportVelocity;
                    slipVelocity.Z = 0; // Ignoramos el deslizamiento vertical
                    var slipSpeed = slipVelocity.Length();

                    float slipSpeedThreshold = transportVelocity.Length()*0.85f; // m/s - umbral para cambiar de fricción estática a dinámica
                    const float heatupRate = 1f; // Qué tan rápido sube HighSlippery
                    const float cooldownRate = 1f; // Qué tan rápido baja HighSlippery
                    const float staticFriction = 0.50f;
                    const float dynamicFriction = 0.30f;

                    // Sistema de heatup/cooldown para HighSlippery
                    if (slipSpeed > slipSpeedThreshold) // && botella.ContactPressure > 0
                        botella.HighSlippery = Math.Min(9f, botella.HighSlippery + heatupRate);
                    else
                        botella.HighSlippery = Math.Max(0f, botella.HighSlippery - cooldownRate);

                    // Calcular fricción basada en HighSlippery
                    // HighSlippery > 3 = fricción dinámica, <= 3 = fricción estática
                    if (botella.ContactPressure > 0)
                        return dynamicFriction / 10;
                    else
                    {
                        if (botella.HighSlippery > 1f)
                            return dynamicFriction;
                        else
                            return staticFriction;
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateTransportFriction] Error: {ex.Message}");
            }

            // Valor por defecto si hay algún error
            return 1.0f;
        }

        /// <summary>
        /// ✅ NUEVO: Calcula la fricción dinámica para contactos botella-curva basada en el deslizamiento relativo
        /// </summary>
        private float CalculateCurveFriction(simBotella botella, simCurve curve, CollidableReference botellaCollidable)
        {
            try
            {
                if (botella?.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botella.BodyHandle) &&
                    curve?.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(curve.BodyHandle))
                {
                    var bottleBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                    var curveBody = _simulationManager.simulation.Bodies.GetBodyReference(curve.BodyHandle);

                    // Aproximamos la velocidad de la superficie de la curva en la posición de la botella.
                    // v = ω x r
                    var r = bottleBody.Pose.Position - curveBody.Pose.Position;
                    var curveVelocityAtPoint = Vector3.Cross(curve.CalcularVelocidadCinematica(), r);

                    var slipVelocity = bottleBody.Velocity.Linear - curveVelocityAtPoint;
                    slipVelocity.Z = 0; // Ignoramos el deslizamiento vertical
                    var slipSpeed = slipVelocity.Length();

                    float slipSpeedThreshold = curveVelocityAtPoint.Length() * 0.85f; // m/s
                    const float heatupRate = 1f; // Qué tan rápido sube HighSlippery en curvas
                    const float cooldownRate = 1f; // Qué tan rápido baja HighSlippery en curvas
                    const float staticFriction = 0.50f;
                    const float dynamicFriction = 0.30f;

                    // Sistema de heatup/cooldown para HighSlippery
                    if (slipSpeed > slipSpeedThreshold) // && botella.ContactPressure > 0
                        botella.HighSlippery = Math.Min(9f, botella.HighSlippery + heatupRate);
                    else
                        botella.HighSlippery = Math.Max(0f, botella.HighSlippery - cooldownRate);

                    // Calcular fricción basada en HighSlippery
                    // HighSlippery > 3 = fricción dinámica, <= 3 = fricción estática
                    if (botella.ContactPressure > 0)
                        return dynamicFriction / 10;
                    else
                    {
                        if (botella.HighSlippery > 1f)
                            return dynamicFriction;
                        else
                            return staticFriction;
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateCurveFriction] Error: {ex.Message}");
            }

            // Valor por defecto si hay algún error
            return 0.8f;
        }

        /// <summary>
        /// ✅ NUEVO: Calcula los parámetros de material para contactos botella-botella basado en la profundidad de penetración
        /// </summary>
        private void CalculateBottlePression<TManifold>(ref TManifold manifold, simBotella botellaA, simBotella botellaB) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            var pairMaterial = new PairMaterialProperties();

            try
            {
                const float penetrationDepthThreshold = 0.001f; // Umbral de penetración en metros para detectar alta presión.

                // Iteramos sobre todos los puntos de contacto en el manifold.
                for (int i = 0; i < manifold.Count; ++i)
                {
                    // Si la profundidad de la penetración supera nuestro umbral,
                    // consideramos que este contacto está bajo alta presión.
                    if (manifold.GetDepth(i) > penetrationDepthThreshold)
                    {
                        botellaA.ContactPressure = Math.Max(botellaA.ContactPressure, manifold.GetDepth(i) - penetrationDepthThreshold);
                        botellaB.ContactPressure = Math.Max(botellaB.ContactPressure, manifold.GetDepth(i) - penetrationDepthThreshold);
                        // break; // Un punto de alta presión es suficiente.
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateBottleToBottleMaterial] Error: {ex.Message}");
                // Valores por defecto seguros
            }
        }

        /// <summary>
        /// ✅ NUEVA FUNCIÓN: Obtiene la profundidad máxima de penetración en un manifold
        /// </summary>
        private float GetMaxPenetrationDepth<TManifold>(ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            try
            {
                float maxDepth = 0f;
                for (int i = 0; i < manifold.Count; ++i)
                {
                    var depth = manifold.GetDepth(i);
                    if (depth > maxDepth)
                    {
                        maxDepth = depth;
                    }
                }
                return maxDepth;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetMaxPenetrationDepth] Error: {ex.Message}");
                return 0f;
            }
        }



        public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
        {
            return true;
        }

        public void Initialize(Simulation simulation)
        {
        }

        public void Dispose()
        {
        }
    }

    public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks
    {
        public Vector3 Gravity;
        public float LinearDamping;
        public float AngularDamping;
        private SimulationManagerBEPU _simulationManager; // ✅ NUEVA REFERENCIA

        public PoseIntegratorCallbacks(Vector3 gravity, float linearDamping = 0.999f, float angularDamping = 0.995f, SimulationManagerBEPU simulationManager = null)
        {
            Gravity = gravity;
            LinearDamping = linearDamping;
            AngularDamping = angularDamping;
            _simulationManager = simulationManager; // ✅ NUEVA INICIALIZACIÓN
        }

        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;

        public readonly bool AllowSubstepsForUnconstrainedBodies => false;

        public readonly bool IntegrateVelocityForKinematics => false;

        public void Initialize(Simulation simulation)
        {
        }

        public void PrepareForIntegration(float dt)
        {
        }

        // ✅ SIMPLIFICADO: IntegrateVelocity ahora solo aplica física básica
        // El control de velocidades Z se maneja en OnSubstepEnded (post-solver)
        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
        {
            // 1. APLICAR GRAVEDAD Y AMORTIGUAMIENTO ESTÁNDAR (VECTORIZADO)
            var gravityWide = Vector3Wide.Broadcast(Gravity);
            velocity.Linear.X += gravityWide.X * dt;
            velocity.Linear.Y += gravityWide.Y * dt;
            velocity.Linear.Z += gravityWide.Z * dt;

            var linearDampingFactor = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt[0]));
            var angularDampingFactor = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt[0]));

            velocity.Linear *= linearDampingFactor;
            velocity.Angular *= angularDampingFactor;

            // 2. LÓGICA PERSONALIZADA MÍNIMA PARA BOTELLAS (ESCALAR)
            const float bottleExtraZAngularDamping = 0.8f;
            var bottleZAngularDampingFactor = MathF.Pow(MathHelper.Clamp(1 - bottleExtraZAngularDamping, 0, 1), dt[0]);
            
            // Recorremos cada "carril" del lote de simulación.
            for (int i = 0; i < Vector<int>.Count; ++i)
            {
                if (integrationMask[i] != 0)
                {
                    var bodyIndex = bodyIndices[i];
                    // Asegurarse de que el índice del cuerpo es válido para el conjunto activo.
                    if (bodyIndex < _simulationManager.simulation.Bodies.ActiveSet.Count)
                    {
                        var handle = _simulationManager.simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex];
                        if (_simulationManager.CollidableData.TryGetValue(handle.Value, out simBase baseObj) && baseObj is simBotella)
                        {
                            // EXTRAER VELOCIDADES DEL PAQUETE A ARRAYS
                            var angularX = new float[Vector<float>.Count];
                            var angularY = new float[Vector<float>.Count];
                            var angularZ = new float[Vector<float>.Count];

                            velocity.Angular.X.CopyTo(angularX, 0);
                            velocity.Angular.Y.CopyTo(angularY, 0);
                            velocity.Angular.Z.CopyTo(angularZ, 0);

                            // CONTROL ANGULAR BÁSICO PARA BOTELLAS
                            // Anti-vuelco: Anular rotaciones que tumben la botella
                            angularX[i] = 0;
                            angularY[i] = 0;

                            // Damping angular extra en Z
                            angularZ[i] *= bottleZAngularDampingFactor;

                            // RECONSTRUIR LOS VECTORES 'WIDE' CON LOS VALORES MODIFICADOS
                            velocity.Angular.X = new Vector<float>(angularX);
                            velocity.Angular.Y = new Vector<float>(angularY);
                            velocity.Angular.Z = new Vector<float>(angularZ);
                        }
                    }
                }
            }
        }
    }



    public class SimulationManagerBEPU : IDisposable
    {
        public Simulation simulation;
        public List<simBase> Cuerpos;
        public List<Action> _deferredActions;
        private BufferPool bufferPool;
        private Stopwatch stopwatch;
        private double stopwatch_last;

        // Referencia al manager de visualización 3D
        public BEPUVisualization3DManager Visualization3DManager { get; set; }

        // Propiedad para controlar si la actualización 3D está habilitada
        public bool Is3DUpdateEnabled { get; set; } = true;

        // ✅ NUEVAS - para filtrado eficiente de callbacks
        private HashSet<BodyHandle> _transportHandles;
        private HashSet<BodyHandle> _curveHandles; 
        private HashSet<BodyHandle> _barrierHandles;
        private HashSet<BodyHandle> _discardHandles;
        private HashSet<BodyHandle> _bottleHandles;
        private HashSet<BodyHandle> _guiaHandles;

        
        // ✅ NUEVO - contador de frames para optimizaciones
        private int _frameCount = 0;
        
        // ✅ CONSERVAR - sistemas existentes que funcionan bien
        private Dictionary<simBarrera, List<simBotella>> _barreraContacts;
        private Dictionary<simDescarte, List<simBotella>> _descarteContacts;
        private HashSet<simBotella> _botellasParaEliminar;
        
        // ✅ NUEVAS - propiedades configurables para fricción de guías
        public float GuiaFrictionCoefficient { get; set; } = 0.1f;
        public float GuiaMaxRecoveryVelocity { get; set; } = 0.5f;
        public float GuiaSpringFrequency { get; set; } = 40f;
        public float GuiaSpringDampingRatio { get; set; } = 2f;
        
        private object _contactsLock = new object();

        // ✅ NUEVO: Diccionario para mapear BodyHandle a simBase para acceso O(1).
        public Dictionary<int, simBase> CollidableData = new Dictionary<int, simBase>();
        public float GlobalTime { get; private set; }

        // ✅ ELIMINADO: Sistema de limitación de fuerzas - reemplazado por intervención post-solver

        /// <summary>
        /// Obtiene el objeto simBase correspondiente a un BodyHandle
        /// </summary>
        public simBase GetSimBaseFromBodyHandle(BodyHandle bodyHandle)
        {
            return Cuerpos.FirstOrDefault(c => c.BodyHandle.Equals(bodyHandle));
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: Obtiene una simCurve desde su BodyHandle principal
        /// </summary>
        public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle)
        {
            // ✅ SIMPLIFICADO: Solo buscar en cuerpos principales (ya no hay triángulos separados)
            var simBase = GetSimBaseFromBodyHandle(bodyHandle);
            return simBase as simCurve;
        }

        // ✅ ELIMINADO: GetBottlesConnectedToElement ya no es necesario.
        // La conexión ahora es implícita a través de la física de contacto.

        // ✅ NUEVOS - gestión automática de clasificación
        private void RegisterObjectHandle(simBase obj)
        {
            if (obj != null && obj.BodyHandle.Value >= 0)
            {
                CollidableData[obj.BodyHandle.Value] = obj;
            }
            switch (obj)
            {
                case simBotella bottle:
                    _bottleHandles.Add(bottle.BodyHandle);
                    break;
                case simTransporte transport:
                    _transportHandles.Add(transport.BodyHandle);
                    // Calcular propiedades iniciales
                    transport.UpdateCachedProperties();
                    break;
                case simCurve curve:
                    _curveHandles.Add(curve.BodyHandle);
                    // ✅ SIMPLIFICADO: Ya no hay triángulos separados, solo el cuerpo principal
                    break;
                case simBarrera barrier:
                    // ✅ NUEVO: Las barreras ya no tienen body físico, no registrar handle
                    // _barrierHandles.Add(barrier.BodyHandle); // ELIMINADO
                    break;
                case simDescarte discard:
                    _discardHandles.Add(discard.BodyHandle);
                    break;
                case simGuia guia:
                    _guiaHandles.Add(guia.BodyHandle);
                    break;
            }
        }

        private void UnregisterObjectHandle(simBase obj)
        {
            if (obj != null && obj.BodyHandle.Value >= 0)
            {
                CollidableData.Remove(obj.BodyHandle.Value);
            }
            switch (obj)
            {
                case simBotella bottle:
                    _bottleHandles.Remove(bottle.BodyHandle);

                    break;
                case simTransporte transport:
                    _transportHandles.Remove(transport.BodyHandle);
                    break;
                case simCurve curve:
                    _curveHandles.Remove(curve.BodyHandle);
                    // ✅ SIMPLIFICADO: Ya no hay triángulos separados
                    break;
                case simBarrera barrier:
                    // ✅ NUEVO: Las barreras ya no tienen body físico, no desregistrar handle
                    // _barrierHandles.Remove(barrier.BodyHandle); // ELIMINADO
                    break;
                case simDescarte discard:
                    _discardHandles.Remove(discard.BodyHandle);
                    break;
                case simGuia guia:
                    _guiaHandles.Remove(guia.BodyHandle);
                    break;
            }
        }



        // ✅ NUEVOS MÉTODOS - obtener objetos por handle
        private simBotella GetBottleByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simBotella>().FirstOrDefault(b => b.BodyHandle.Equals(handle));
        }

        private simTransporte GetTransportByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simTransporte>().FirstOrDefault(t => t.BodyHandle.Equals(handle));
        }

        private simCurve GetCurveByHandle(BodyHandle handle)
        {
            // Primero buscar en cuerpos principales
            var curve = Cuerpos.OfType<simCurve>().FirstOrDefault(c => c.BodyHandle.Equals(handle));
            if (curve != null) return curve;
            
            // Luego buscar en triángulos
            return GetCurveFromTriangleBodyHandle(handle);
        }

        private simGuia GetGuiaByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simGuia>().FirstOrDefault(g => g.BodyHandle.Equals(handle));
        }

        public SimulationManagerBEPU()
        {
            // ✅ EXISTENTES - conservar
            Cuerpos = new List<simBase>();
            _deferredActions = new List<Action>();
            _barreraContacts = new Dictionary<simBarrera, List<simBotella>>();
            _descarteContacts = new Dictionary<simDescarte, List<simBotella>>();
            _botellasParaEliminar = new HashSet<simBotella>();
            
                    // ✅ NUEVOS - sistemas de filtrado y contactos
        _transportHandles = new HashSet<BodyHandle>();
        _curveHandles = new HashSet<BodyHandle>();
        _barrierHandles = new HashSet<BodyHandle>();
        _discardHandles = new HashSet<BodyHandle>();
        _bottleHandles = new HashSet<BodyHandle>();
        _guiaHandles = new HashSet<BodyHandle>();
            
            
            // ✅ CONSERVAR - resto del constructor igual
            bufferPool = new BufferPool();
            stopwatch = new Stopwatch();

            var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);

            // Configurar amortiguamiento para comportamiento realista:
            // - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen)
            // - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros)
            // ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks
            var poseIntegratorCallbacks = new PoseIntegratorCallbacks(
                gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z
                linearDamping: 0.03f, // 0.999f,              // Amortiguamiento lineal suave
                angularDamping: 0.03f, // 0.995f,             // Amortiguamiento angular más fuerte para detener rotaciones
                simulationManager: this             // ✅ NUEVA REFERENCIA
            );

            var solveDescription = new SolveDescription(8, 1);

            simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription);
            
            // ✅ NUEVO: Suscribirse a los eventos del solver para controlar la velocidad de los cuerpos cinemáticos.
            simulation.Solver.SubstepStarted += OnSubstepStarted;
            simulation.Solver.SubstepEnded += OnSubstepEnded;
        }

        /// <summary>
        /// ✅ NUEVO: Se ejecuta al inicio de cada sub-paso de la simulación.
        /// Restaura las velocidades de los cuerpos cinemáticos para que el solver pueda usarlas para calcular la fricción.
        /// </summary>
        private void OnSubstepStarted(int substepIndex)
        {
            foreach (var transport in Cuerpos.OfType<simTransporte>())
            {
                transport.ActualizarVelocidadCinematica();
            }
            foreach (var curve in Cuerpos.OfType<simCurve>())
            {
                curve.ActualizarVelocidadCinematica();
            }
        }

        /// <summary>
        /// ✅ NUEVO: Se ejecuta al final de cada sub-paso de la simulación, pero antes de la integración de la pose.
        /// Actúa como "fusible de energía" para eliminar velocidades Z excesivas y mantener objetos cinemáticos estáticos.
        /// </summary>
        private void OnSubstepEnded(int substepIndex)
        {
            // 1. INTERVENCIÓN POST-SOLVER: Control de velocidades Z para botellas
            // Esto actúa como un "fusible de energía" que elimina velocidades Z excesivas
            // después de que el solver haya hecho su trabajo
            foreach (var botella in Cuerpos.OfType<simBotella>())
            {
                try
                {
                    if (botella?.BodyHandle.Value >= 0 && simulation?.Bodies?.BodyExists(botella.BodyHandle) == true)
                    {
                        var body = simulation.Bodies.GetBodyReference(botella.BodyHandle);
                        var velocity = body.Velocity;
                        var pose = body.Pose;

                        // CONFIGURACIÓN DE LÍMITES
                        const float maxUpwardVelocity = 0f;    // Velocidad Z máxima hacia arriba (m/s)
                        const float maxXYVelocity = 1.0f;      // Límite de velocidad en el plano XY (m/s)
                        
                        // LIMITAR VELOCIDADES Z EXCESIVAS
                        if (velocity.Linear.Z > maxUpwardVelocity)
                        {
                            // Cancelar velocidades hacia arriba excesivas (anti-elevación)
                            velocity.Linear.Z = 0;
                            pose.Position.Z = botella.Radius + simBase.zPos_Transporte + simBase.zAltura_Transporte;
                            body.Pose = pose; // Se actualiza la pose completa
                        }

                        // LIMITAR VELOCIDAD MÁXIMA EN PLANO XY
                        var velocityXY = new Vector2(velocity.Linear.X, velocity.Linear.Y);
                        if (velocityXY.LengthSquared() > maxXYVelocity * maxXYVelocity)
                        {
                            var speedXY = velocityXY.Length();
                            var scale = maxXYVelocity / speedXY;
                            velocity.Linear.X *= scale;
                            velocity.Linear.Y *= scale;
                        }
                        
                        // Aplicar siempre la velocidad, ya que puede haber sido modificada en Z o XY
                        body.Velocity = velocity;
                                              
                        //// CONTROL DE POSICIÓN EXTREMA
                        //// Si la botella está muy por encima del nivel normal, aplicar corrección
                        //var maxAllowedZ = simBase.zPos_Transporte + simBase.zAltura_Transporte + botella.Radius * 2;
                        
                        //if (position.Z > maxAllowedZ)
                        //{
                        //    // Aplicar velocidad correctiva hacia abajo
                        //    var excessHeight = position.Z - maxAllowedZ;
                        //    var correctionVelocity = -Math.Min(excessHeight * 5f, 0.5f); // Máximo 0.5 m/s hacia abajo
                        //    velocity.Linear.Z = correctionVelocity;
                        //    body.Velocity = velocity;
                        //}
                    }
                }
                catch (Exception ex)
                {
                    System.Diagnostics.Debug.WriteLine($"[OnSubstepEnded] Error procesando botella {botella?.BodyHandle}: {ex.Message}");
                }
            }
            
            // 2. LIMPIAR VELOCIDADES DE OBJETOS CINEMÁTICOS
            // Mantener transportes y curvas estáticos después del solver
            foreach (var transport in Cuerpos.OfType<simTransporte>())
            {
                if (transport.BodyHandle.Value >= 0 && simulation.Bodies.BodyExists(transport.BodyHandle))
                {
                    var body = simulation.Bodies.GetBodyReference(transport.BodyHandle);
                    if (body.Kinematic)
                    {
                        body.Velocity.Linear = Vector3.Zero;
                    }
                }
            }
            foreach (var curve in Cuerpos.OfType<simCurve>())
            {
                if (curve.BodyHandle.Value >= 0 && simulation.Bodies.BodyExists(curve.BodyHandle))
                {
                    var body = simulation.Bodies.GetBodyReference(curve.BodyHandle);
                    if (body.Kinematic)
                    {
                        body.Velocity.Angular = Vector3.Zero;
                    }
                }
            }
        }

        public void Clear()
        {
            try
            {
                
                
                // ✅ SIMPLIFICAR - eliminar lógica de masa especial
                // foreach (var cuerpo in Cuerpos.OfType<simBotella>())
                // {
                //     cuerpo.RestoreOriginalMassIfNeeded(); // NO SE USA MÁS
                // }
                
                // ✅ CONSERVAR - limpiar contactos
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                    _botellasParaEliminar.Clear();

                }
                
                        // ✅ NUEVO - limpiar clasificaciones
        _transportHandles.Clear();
        _curveHandles.Clear();
        _barrierHandles.Clear();
        _discardHandles.Clear();
        _bottleHandles.Clear();
        _guiaHandles.Clear();

                // ✅ CONSERVAR - resto del método igual
                var cuerposToRemove = new List<simBase>(Cuerpos);
                foreach (var cuerpo in cuerposToRemove)
                {
                    try
                    {
                        if (cuerpo != null)
                        {
                            cuerpo.RemoverBody();
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error removing body - continue with cleanup
                    }
                }

                Cuerpos.Clear();
                _deferredActions.Clear();
                
                // ✅ NUEVO - Limpiar todas las dimensiones almacenadas
                CtrEditor.ObjetosSim.osBase.ClearAllStoredDimensions();

                // Limpiar la simulación completamente si existe
                if (simulation != null)
                {
                    try
                    {
                        // Forzar un timestep pequeño para limpiar contactos pendientes
                        simulation.Timestep(1f / 1000f); // 1ms
                    }
                    catch (Exception ex)
                    {
                        // Warning during cleanup timestep - continue
                    }
                }

                // Limpiar visualización 3D
                Visualization3DManager?.Clear();

                // ✅ NUEVO: Limpiar también el nuevo diccionario.
                CollidableData.Clear();
                GlobalTime = 0f;
            }
            catch (Exception ex)
            {
                // Critical error during clear - operation failed
            }
        }

        public void Start()
        {
            try
            {
                // ✅ INICIALIZAR PROPIEDADES CACHEADAS Y VELOCIDADES CINEMÁTICAS
                foreach (var transport in Cuerpos.OfType<simTransporte>())
                {
                    transport.UpdateCachedProperties();
                }
                
                foreach (var curve in Cuerpos.OfType<simCurve>())
                {
                    curve.SetSpeed(curve.Speed);
                }
                
                stopwatch.Start();
                stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in Start: {ex.Message}");
                stopwatch.Start();
                stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
            }
        }

        public void Step()
        {
            try
            {
                _frameCount++;

                // ✅ NUEVO: Verificación temprana para cuando no hay objetos simulables
                // Esto evita cálculos de tiempo irregulares cuando no hay trabajo que hacer
                if (Cuerpos == null || Cuerpos.Count == 0)
                {
                    // Mantener el cronómetro actualizado pero usar deltaTime fijo
                    var currentTimeEmpty = stopwatch.Elapsed.TotalMilliseconds;
                    stopwatch_last = currentTimeEmpty;
                    GlobalTime += 1f / 60f; // Incremento fijo de 60 FPS
                    return;
                }

                // ✅ NUEVO: Resetear el estado de contacto para el sistema de presión
                foreach (var botella in Cuerpos.OfType<simBotella>())
                {
                    botella.IsTouchingTransport = false;
                    botella.ContactPressure = 0f;
                }

                // ✅ ELIMINADO: Reset del mapa de densidad de contactos ya no necesario

                // ✅ ELIMINADO: ApplyZForceLimitation - ahora usamos intervención post-solver en OnSubstepEnded

                // ✅ CONSERVAR - validación de deltaTime
                var currentTime = stopwatch.Elapsed.TotalMilliseconds;
                var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0);
                stopwatch_last = currentTime;

                if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
                {
                    deltaTime = 1f / 60f;
                }

                const float maxDeltaTime = 0.1f;
                if (deltaTime > maxDeltaTime)
                {
                    deltaTime = 1f / 60f;
                }

                // ✅ CONSERVAR - procesar acciones diferidas
                foreach (var action in _deferredActions)
                {
                    action();
                }
                _deferredActions.Clear();
                
                // ✅ CONSERVAR - validaciones de simulación
                if (simulation?.Bodies == null)
                {
                    return;
                }

                var invalidBodies = Cuerpos.Where(c => c != null && !simulation.Bodies.BodyExists(c.BodyHandle)).ToList();
                if (invalidBodies.Count > 0)
                {
                    foreach (var invalidBody in invalidBodies)
                    {
                        Cuerpos.Remove(invalidBody);
                    }
                }

                // ✅ CONSERVAR - timestep
                var timestepValue = Math.Max(deltaTime, 1f / 120f);

                try
                {
                    simulation.Timestep(timestepValue);
                }
                catch (AccessViolationException ex)
                {
                    lock (_contactsLock)
                    {
                        _barreraContacts.Clear();
                    }
                    throw;
                }



                //// ✅ CONSERVAR - limitación de rotación y mantener botellas despiertas
                //foreach (var cuerpo in Cuerpos.OfType<simBotella>().ToList())
                //{
                //    try
                //    {
                //        if (simulation.Bodies.BodyExists(cuerpo.BodyHandle))
                //        {
                //           // cuerpo.LimitRotationToXYPlane();
                //           // simulation.Awakener.AwakenBody(cuerpo.BodyHandle);
                //        }
                //    }
                //    catch (Exception ex)
                //    {
                //        // Error limiting rotation for bottle - continue
                //    }
                //}

                // ✅ CONSERVAR - sistemas que funcionan bien
                ProcessBarreraContacts();
                ProcessDescarteContacts();
                ProcessCleanupSystem();

                // ✅ SIMPLIFICAR - limpiar solo contactos que usamos
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                }

                // ✅ NUEVO: Actualizar el tiempo global de simulación
                GlobalTime += deltaTime;

                // ✅ CONSERVAR - sincronización 3D
                if (Is3DUpdateEnabled)
                {
                    Visualization3DManager?.SynchronizeWorld();
                }
            }
            catch (Exception ex)
            {
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                }
            }
        }





        /// <summary>
        /// ✅ NUEVO: Marca un objeto para eliminación diferida (más seguro)
        /// </summary>
        public void Remove(simBase Objeto)
        {
            if (Objeto != null)
            {
                // Aplazar la eliminación para evitar problemas de concurrencia
                _deferredActions.Add(() =>
                {
                    UnregisterObjectHandle(Objeto);
                    Objeto.RemoverBody();
                    Cuerpos.Remove(Objeto);
                });
            }
        }
       

        public simBotella AddBotella(float diameter, Vector2 position, float mass)
        {
            var botella = new simBotella(simulation, _deferredActions, diameter, position, mass);
            Cuerpos.Add(botella);
            RegisterObjectHandle(botella);
            return botella;
        }

        public simTransporte AddRectangle(float width, float height, Vector2 position, float angle)
        {
            var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle, this);
            Cuerpos.Add(transporte);
            RegisterObjectHandle(transporte); // ✅ NUEVO - incluye UpdateCachedProperties()
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return transporte;
        }

        internal simBarrera AddBarrera(float width, float height, Vector2 wpfTopLeft, float wpfAngle, bool detectarCuello)
        {
            // ✅ CONVERTIR coordenadas WPF a BEPU internas
            var zPosition = simBase.zPos_Barrera;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
            var bepuRadians = CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle);
            
            var barrera = new simBarrera(simulation, width, height, bepuCenter, bepuRadians, detectarCuello, this);
            Cuerpos.Add(barrera);
            RegisterObjectHandle(barrera);
                       
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return barrera;
        }



        public simGuia AddLine(float width, float height, Vector2 topLeft, float angle)
        {
            var guia = new simGuia(simulation, _deferredActions, width, height, topLeft, angle);
            Cuerpos.Add(guia);
            // ✅ NOTA: simGuia no requiere registro especial
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return guia;
        }

        public simDescarte AddDescarte(float diameter, Vector2 position)
        {
            var descarte = new simDescarte(simulation, _deferredActions, diameter, position);
            Cuerpos.Add(descarte);
            RegisterObjectHandle(descarte); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return descarte;
        }

        public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
        {
            var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused, this);
            Cuerpos.Add(curve);
            RegisterObjectHandle(curve); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return curve;
        }

        // ✅ NUEVA INTERFAZ PARA BARRERAS - Solo accesible desde SimulationManagerBEPU
        
        /// <summary>
        /// Crea una nueva barrera y devuelve un handle opaco para WPF
        /// </summary>
        /// <param name="width">Ancho en metros</param>
        /// <param name="height">Alto en metros</param>
        /// <param name="bepuCenter">Posición central en coordenadas BEPU</param>
        /// <param name="bepuRadians">Ángulo en radianes BEPU</param>
        /// <param name="detectarCuello">Si debe detectar el cuello de las botellas</param>
        /// <returns>Handle opaco para referenciar la barrera desde WPF</returns>
        public simBarrera CreateBarrera(float width, float height, Vector2 TopLeft, float wpfAngle, bool detectarCuello)
        {
            var barrera = new simBarrera(simulation, width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width,  height,  wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle), detectarCuello, this);
            Cuerpos.Add(barrera);
            RegisterObjectHandle(barrera);
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            
            return barrera;
        }

        public void UpdateBarrera(simBarrera barrera, float width, float height, Vector2 TopLeft, float wpfAngle)
        {
            barrera.Update(width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle));

            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
        }


        /// <summary>
        /// Elimina una barrera usando su handle
        /// </summary>
        /// <param name="handle">Handle de la barrera a eliminar</param>
        public void RemoveBarrera(simBarrera barrera)
        {
            if (barrera != null)
            {
                Remove(barrera);
            }
        }

        /// <summary>
        /// Configura si la barrera debe detectar el cuello de las botellas
        /// </summary>
        /// <param name="handle">Handle de la barrera</param>
        /// <param name="detectarCuello">True para detectar cuello, false para detectar botella completa</param>
        public void SetBarreraDetectNeck(simBarrera barrera, bool detectarCuello)
        {
            if (barrera != null)
            {
                barrera.DetectNeck = detectarCuello;
            }
        }

        /// <summary>
        /// Obtiene los datos de estado de la barrera
        /// </summary>
        /// <param name="handle">Handle de la barrera</param>
        /// <returns>Estructura con los datos de luz cortada</returns>
        public BarreraData GetBarreraData(simBarrera barrera)
        {
            if (barrera != null)
            {
                return new BarreraData(barrera.LuzCortada, barrera.LuzCortadaNeck);
            }
            return new BarreraData(false, false);
        }

        /// <summary>
        /// ✅ NUEVO: Procesa todas las barreras usando RayCast de BEPU
        /// Elimina la dependencia de contactos físicos y la necesidad de mantener botellas despiertas
        /// </summary>
        private void ProcessBarreraContacts()
        {
            try
            {
                // Obtener todas las barreras
                var barreras = Cuerpos.OfType<simBarrera>().ToList();

                foreach (var barrera in barreras)
                {
                    try
                    {
                        // Verificar que la barrera aún existe (solo verificar que no sea null)
                        if (barrera == null)
                            continue;

                        // ✅ USAR RAYCAST NATIVO DE BEPU
                        barrera.PerformRaycast();

                        // System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Barrera procesada: LuzCortada={barrera.LuzCortada}, LuzCortadaNeck={barrera.LuzCortadaNeck}, Distancia={barrera.Distancia:F3}");
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error procesando barrera: {ex.Message}");
                    }
                }

                // Limpiar contactos ya que no los usamos más
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error crítico: {ex.Message}");
            }
        }

        /// <summary>
        /// Procesa contactos de descarte para eliminar botellas marcadas usando detección geométrica
        /// </summary>
        private void ProcessDescarteContacts()
        {
            try
            {
                // Obtener todos los descartes y botellas
                var descartes = Cuerpos.OfType<simDescarte>().ToList();
                var botellas = Cuerpos.OfType<simBotella>().ToList();

                foreach (var descarte in descartes)
                {
                    try
                    {
                        // Verificar que el descarte aún existe en la simulación
                        if (descarte == null || !simulation?.Bodies?.BodyExists(descarte.BodyHandle) == true)
                            continue;

                        // Limpiar la lista de forma segura
                        if (descarte.ListSimBotellaContact != null)
                        {
                            descarte.ListSimBotellaContact.Clear();
                        }

                        // Calcular detección usando geometría pura para TODAS las botellas
                        CalculateDescarteDetectionGeometric(descarte, botellas);
                    }
                    catch (Exception ex)
                    {
                        // Error processing descarte - continue
                    }
                }

                // Eliminar botellas marcadas para eliminación (después de procesamiento)
                RemoveMarkedBottles();
            }
            catch (Exception ex)
            {
                // Critical error in ProcessDescarte - continue
            }
        }

        /// <summary>
        /// Calcula detección geométrica pura para un descarte específico contra todas las botellas
        /// Usa detección de esfera contra esfera para determinar si hay contacto
        /// </summary>
        private void CalculateDescarteDetectionGeometric(simDescarte descarte, List<simBotella> todasLasBotellas)
        {
            try
            {
                // Validaciones de seguridad
                if (descarte == null || todasLasBotellas == null || simulation?.Bodies == null)
                    return;

                if (!simulation.Bodies.BodyExists(descarte.BodyHandle))
                    return;

                var descarteBody = simulation.Bodies[descarte.BodyHandle];
                var descartePosition = descarteBody.Pose.Position;

                // Validar valores de posición
                if (float.IsNaN(descartePosition.X) || float.IsNaN(descartePosition.Y) || float.IsNaN(descartePosition.Z))
                {
                    return;
                }

                var botellasDetectadas = new List<simBotella>();

                // Procesar TODAS las botellas para detección geométrica
                foreach (var botella in todasLasBotellas)
                {
                    try
                    {
                        if (botella == null || !simulation.Bodies.BodyExists(botella.BodyHandle))
                            continue;

                        var botellaBody = simulation.Bodies[botella.BodyHandle];
                        var botellaPosition = botellaBody.Pose.Position;

                        // Validar posición de la botella
                        if (float.IsNaN(botellaPosition.X) || float.IsNaN(botellaPosition.Y) || float.IsNaN(botellaPosition.Z))
                        {
                            continue;
                        }

                        // Calcular distancia entre centros (detección esfera contra esfera)
                        var distance = Vector3.Distance(descartePosition, botellaPosition);

                        // Validar distancia calculada
                        if (float.IsNaN(distance) || float.IsInfinity(distance))
                        {
                            continue;
                        }

                        // Verificar si las esferas se superponen
                        var totalRadius = descarte.Radius + botella.Radius;
                        if (distance <= totalRadius)
                        {
                            // Marcar botella para eliminación
                            botella.Descartar = true;
                            _botellasParaEliminar.Add(botella);
                            botellasDetectadas.Add(botella);
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error processing bottle - continue
                    }
                }

                // Actualizar lista de botellas detectadas
                if (descarte.ListSimBotellaContact != null)
                {
                    descarte.ListSimBotellaContact.AddRange(botellasDetectadas);
                }
            }
            catch (Exception ex)
            {
                // Critical error in CalculateDescarteGeometric - continue
            }
        }

        /// <summary>
        /// Elimina las botellas marcadas para eliminación de forma segura
        /// </summary>
        private void RemoveMarkedBottles()
        {
            try
            {
                List<simBotella> botellasAEliminar;
                lock (_contactsLock)
                {
                    // ✅ NUEVO: Añadir botellas marcadas como "Descartar" desde los callbacks
                    foreach (var botella in Cuerpos.OfType<simBotella>())
                    {
                        if (botella.Descartar)
                        {
                            _botellasParaEliminar.Add(botella);
                        }
                    }

                    botellasAEliminar = new List<simBotella>(_botellasParaEliminar);
                    _botellasParaEliminar.Clear();
                }

                foreach (var botella in botellasAEliminar)
                {
                    try
                    {
                        if (botella != null && Cuerpos.Contains(botella))
                        {
                            //System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] 🗑️ Marcando botella para eliminación: {botella.BodyHandle}");
                            
                            // ✅ USAR ELIMINACIÓN DIFERIDA (más seguro)
                            Remove(botella);
                        }
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ Error marcando botella {botella?.BodyHandle}: {ex.Message}");
                        // ✅ INTENTAR remover de la lista incluso si Remove falló
                        if (botella != null)
                        {
                            Cuerpos.Remove(botella);
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ ERROR CRÍTICO: {ex.Message}");
            }
        }

        /// <summary>
        /// Sistema de limpieza que elimina botellas que estén debajo de la altura de los transportes
        /// Cualquier botella con Z menor al nivel superior de los transportes será eliminada
        /// </summary>
        private void ProcessCleanupSystem()
        {
            try
            {
                // Altura máxima de los transportes (nivel superior)
                var maxTransportHeight = simBase.zPos_Transporte + simBase.zAltura_Transporte;

                // Obtener todas las botellas
                var botellas = Cuerpos.OfType<simBotella>().ToList();

                foreach (var botella in botellas)
                {
                    try
                    {
                        // Validar que la botella aún existe en la simulación
                        if (botella == null || !simulation?.Bodies?.BodyExists(botella.BodyHandle) == true)
                            continue;

                        var posicion = botella.GetPosition();

                        // Validar posición
                        if (float.IsNaN(posicion.Z) || float.IsInfinity(posicion.Z))
                            continue;

                        // Si la botella está debajo del nivel de los transportes, marcarla para eliminación
                        if (posicion.Z < (maxTransportHeight - 2 * botella.Radius) || posicion.Z > (maxTransportHeight + 2 * botella.Radius))
                        {
                            lock (_contactsLock)
                            {
                                botella.Descartar = true;
                                _botellasParaEliminar.Add(botella);
                            }
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error processing bottle in cleanup - continue
                    }
                }
            }
            catch (Exception ex)
            {
                // Critical error in ProcessCleanupSystem - continue
            }
        }

        /// <summary>
        /// Proyecta un punto sobre una línea definida por dos puntos
        /// </summary>
        private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
        {
            var lineDirection = Vector3.Normalize(lineEnd - lineStart);
            var pointToStart = point - lineStart;
            var projectionLength = Vector3.Dot(pointToStart, lineDirection);

            // Restringir la proyección a los límites de la línea
            var lineLength = Vector3.Distance(lineStart, lineEnd);
            projectionLength = Math.Max(0, Math.Min(projectionLength, lineLength));

            return lineStart + lineDirection * projectionLength;
        }

        // ✅ ELIMINADO: Funciones del sistema de limitación de fuerzas forceScale
        // GetContactRegion, RegisterContactAndGetForceScale, GetContactPosition

        // ✅ ELIMINADO: ApplyZForceLimitation - reemplazado por intervención post-solver en OnSubstepEnded

        public void Dispose()
        {
            Clear();
            simulation?.Dispose();
            bufferPool?.Clear();
        }

        /// <summary>
        /// Registra un contacto entre una barrera y una botella para detección de paso
        /// </summary>
        /// <param name="barrera">Barrera que detecta el paso</param>
        /// <param name="botella">Botella que está pasando</param>
        internal void RegisterBarreraContact(simBarrera barrera, simBotella botella)
        {
            if (barrera != null && botella != null)
            {
                lock (_contactsLock)
                {
                    if (!_barreraContacts.ContainsKey(barrera))
                    {
                        _barreraContacts[barrera] = new List<simBotella>();
                    }

                    if (!_barreraContacts[barrera].Contains(botella))
                    {
                        _barreraContacts[barrera].Add(botella);
                    }
                }
            }
        }
    }
}