CtrEditor/Simulacion/BEPU.cs

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 BepuUtilities;
using BepuUtilities.Memory;
using CtrEditor.FuncionesBase;

namespace CtrEditor.Simulacion
{
    /// <summary>
    /// Clase centralizada para manejar todas las conversiones entre coordenadas WPF y BEPU
    /// WPF: Y hacia abajo, ángulos en sentido horario, Top-Left como referencia
    /// BEPU: Y hacia arriba, ángulos en sentido antihorario, Center como referencia
    /// </summary>
    public static class CoordinateConverter
    {
        /// <summary>
        /// Convierte ángulo de WPF a BEPU (invierte signo) - USO INTERNO
        /// </summary>
        internal static float WpfAngleToBepuAngle(float wpfAngle)
        {
            return -wpfAngle;
        }
        
        /// <summary>
        /// Convierte ángulo de BEPU a WPF (invierte signo) - USO INTERNO
        /// </summary>
        internal static float BepuAngleToWpfAngle(float bepuAngle)
        {
            return -bepuAngle;
        }
        
        /// <summary>
        /// Convierte posición Y de WPF a BEPU (invierte signo) - USO INTERNO
        /// </summary>
        internal static float WpfYToBepuY(float wpfY)
        {
            return -wpfY;
        }
        
        /// <summary>
        /// Convierte posición Y de BEPU a WPF (invierte signo) - RETORNA VALOR WPF
        /// </summary>
        public static float BepuYToWpfY(float bepuY)
        {
            return -bepuY;
        }
        
        /// <summary>
        /// Convierte Vector2 de WPF a BEPU (solo invierte Y) - USO INTERNO
        /// </summary>
        internal static Vector2 WpfToBepuVector2(Vector2 wpfVector)
        {
            return new Vector2(wpfVector.X, -wpfVector.Y);
        }
        
        /// <summary>
        /// Convierte Vector2 de BEPU a WPF (solo invierte Y) - RETORNA VALOR WPF
        /// </summary>
        public static Vector2 BepuToWpfVector2(Vector2 bepuVector)
        {
            return new Vector2(bepuVector.X, -bepuVector.Y);
        }
        
        /// <summary>
        /// Convierte Vector3 de BEPU a Vector2 WPF (proyección XY con Y invertida) - RETORNA VALOR WPF
        /// </summary>
        public static Vector2 BepuVector3ToWpfVector2(Vector3 bepuVector)
        {
            return new Vector2(bepuVector.X, -bepuVector.Y);
        }
        
        /// <summary>
        /// Calcula la posición del centro en coordenadas BEPU desde Top-Left WPF - USO INTERNO
        /// Maneja correctamente la rotación del objeto
        /// </summary>
        internal static Vector3 CalculateBepuCenterFromWpfTopLeft(Vector2 wpfTopLeft, float width, float height, float wpfAngle, float zPosition)
        {
            // Calcular el offset del centro desde Top-Left (sin rotación)
            var offsetX = width / 2f;
            var offsetY = height / 2f;
            
            // Convertir ángulo WPF a radianes para cálculos trigonométricos
            var angleRadians = simBase.GradosARadianes(wpfAngle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);
            
            // Rotar el offset alrededor del Top-Left usando el ángulo WPF
            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;
            
            // Calcular nueva posición del centro manteniendo Top-Left fijo
            var centerX = wpfTopLeft.X + rotatedOffsetX;
            var centerY = wpfTopLeft.Y + rotatedOffsetY;
            
            // Convertir a 3D con Y invertida para BEPU
            return new Vector3(centerX, WpfYToBepuY(centerY), zPosition);
        }
        
        /// <summary>
        /// Calcula la posición Top-Left WPF desde el centro BEPU - RETORNA VALOR WPF
        /// Maneja correctamente la rotación del objeto
        /// </summary>
        public static Vector2 CalculateWpfTopLeftFromBepuCenter(Vector3 bepuCenter, float width, float height, float wpfAngle)
        {
            // Convertir centro BEPU a WPF
            var wpfCenterX = bepuCenter.X;
            var wpfCenterY = BepuYToWpfY(bepuCenter.Y);
            
            // Calcular el offset del centro al Top-Left (sin rotación)
            var offsetX = -width / 2f;
            var offsetY = -height / 2f;
            
            // Convertir ángulo WPF a radianes para cálculos trigonométricos
            var angleRadians = simBase.GradosARadianes(wpfAngle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);
            
            // Rotar el offset usando el ángulo WPF
            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;
            
            // Calcular Top-Left desde el centro
            var topLeftX = wpfCenterX + rotatedOffsetX;
            var topLeftY = wpfCenterY + rotatedOffsetY;
            
            return new Vector2(topLeftX, topLeftY);
        }
        
        /// <summary>
        /// Crea un Quaternion para BEPU desde un ángulo WPF - USO INTERNO
        /// </summary>
        internal static Quaternion CreateBepuQuaternionFromWpfAngle(float wpfAngle)
        {
            var bepuAngle = WpfAngleToBepuAngle(wpfAngle);
            return Quaternion.CreateFromAxisAngle(Vector3.UnitZ, simBase.GradosARadianes(bepuAngle));
        }
        
        /// <summary>
        /// Extrae el ángulo WPF desde un Quaternion BEPU - RETORNA VALOR WPF
        /// </summary>
        public static float ExtractWpfAngleFromBepuQuaternion(Quaternion bepuQuaternion)
        {
            // Extraer ángulo Z del quaternion
            var bepuAngleRadians = (float)Math.Atan2(
                2.0 * (bepuQuaternion.W * bepuQuaternion.Z + bepuQuaternion.X * bepuQuaternion.Y),
                1.0 - 2.0 * (bepuQuaternion.Y * bepuQuaternion.Y + bepuQuaternion.Z * bepuQuaternion.Z)
            );
            
            var bepuAngleDegrees = simBase.RadianesAGrados(bepuAngleRadians);
            return BepuAngleToWpfAngle(bepuAngleDegrees);
        }
        
        /// <summary>
        /// Actualiza la posición de un body BEPU manteniendo su rotación actual - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyPosition(Simulation simulation, BodyHandle bodyHandle, Vector3 newBepuPosition)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Position = newBepuPosition;
            }
        }
        
        /// <summary>
        /// Actualiza la rotación de un body BEPU manteniendo su posición actual - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyRotation(Simulation simulation, BodyHandle bodyHandle, float wpfAngle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Orientation = CreateBepuQuaternionFromWpfAngle(wpfAngle);
            }
        }
        
        /// <summary>
        /// Actualiza posición y rotación de un body BEPU simultáneamente - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyPose(Simulation simulation, BodyHandle bodyHandle, Vector3 newBepuPosition, float wpfAngle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Position = newBepuPosition;
                bodyReference.Pose.Orientation = CreateBepuQuaternionFromWpfAngle(wpfAngle);
            }
        }
        
        /// <summary>
        /// Obtiene la posición del centro en coordenadas BEPU - USO INTERNO
        /// </summary>
        internal static Vector3 GetBepuBodyPosition(Simulation simulation, BodyHandle bodyHandle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                return bodyReference.Pose.Position;
            }
            return Vector3.Zero;
        }
        
        /// <summary>
        /// Obtiene el ángulo WPF desde un body BEPU - RETORNA VALOR WPF
        /// </summary>
        public static float GetWpfAngleFromBepuBody(Simulation simulation, BodyHandle bodyHandle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                return ExtractWpfAngleFromBepuQuaternion(bodyReference.Pose.Orientation);
            }
            return 0f;
        }
        
        /// <summary>
        /// ✅ NUEVO: Convierte directamente de grados WPF a radianes BEPU - USO INTERNO
        /// Maneja tanto la inversión de signo como la conversión a radianes en una sola operación
        /// </summary>
        internal static float WpfDegreesToBepuRadians(float wpfDegrees)
        {
            return simBase.GradosARadianes(WpfAngleToBepuAngle(wpfDegrees));
        }
        
        /// <summary>
        /// ✅ NUEVO: Convierte directamente de radianes BEPU a grados WPF - RETORNA VALOR WPF
        /// Maneja tanto la conversión a grados como la inversión de signo en una sola operación
        /// </summary>
        public static float BepuRadiansToWpfDegrees(float bepuRadians)
        {
                          return BepuAngleToWpfAngle(simBase.RadianesAGrados(bepuRadians));
          }
    }

    public class simBase
    {
        public BodyHandle BodyHandle { get; protected set; }
        public Simulation _simulation;
        protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo

        // ✅ CORREGIDO: Restaurar factor de conversión correcto
        public const float SPEED_CONVERSION_FACTOR = 1/2f; // Factor de conversión de velocidad interna a m/s - Para LinearAxisMotor es 0.5f

        // Constantes para las posiciones Z de los objetos 3D
        public const float zPos_Transporte = 0f; // Z de la parte baja
        public const float zAltura_Transporte = 0.1f; // Altura del transporte sobre zPos

        public const float zPos_Guia = 0.05f; // Z de la parte baja
        public const float zAltura_Guia = 0.20f; // Altura de la guía sobre zPos
        public const float zAltura_Barrera = zAltura_Guia; // Altura de la barrera sobre zPos
        public const float zPos_Barrera = zPos_Guia; // Z de la parte baja
        public const float zPos_Descarte = 0.1f; // Z de la parte baja

        // Constantes para configuración
        public const float zPos_Curve = zPos_Transporte; // Z de la parte baja de la curva
        public const float zAltura_Curve = zAltura_Transporte; // Altura de la curva (triángulos planos)


        public void RemoverBody()
        {
            try
            {
                // Solo intentar remover si realmente hemos creado un cuerpo antes
                if (_bodyCreated && _simulation != null && _simulation.Bodies != null && _simulation.Bodies.BodyExists(BodyHandle))
                {
                    _simulation.Bodies.Remove(BodyHandle);
                    _bodyCreated = false; // Marcar como no creado después de remover
                    System.Diagnostics.Debug.WriteLine($"[simBase.RemoverBody] ✅ Body eliminado: {BodyHandle}");
                }
                else
                {
                    System.Diagnostics.Debug.WriteLine($"[simBase.RemoverBody] ⚠️ Body no existe o no creado: {BodyHandle}");
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simBase.RemoverBody] ❌ ERROR: {ex.Message}");
                _bodyCreated = false; // Marcar como no creado en caso de error
            }
        }

        /// <summary>
        /// ✅ NUEVO: Cambia la forma de un body existente, limpiando la forma anterior para evitar memory leaks
        /// </summary>
        protected void ChangeBodyShape(TypedIndex newShapeIndex)
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                
                // ✅ CRÍTICO: Obtener la forma anterior para limpiarla del pool de shapes
                var oldShapeIndex = bodyReference.Collidable.Shape;
                
                // Cambiar a la nueva forma
                _simulation.Bodies.SetShape(BodyHandle, newShapeIndex);
                
                // ✅ CRÍTICO: Limpiar la forma anterior del pool para evitar memory leaks
                // Nota: Solo limpiar si es diferente (para evitar limpiar la forma que acabamos de asignar)
                if (oldShapeIndex.Packed != newShapeIndex.Packed)
                {
                    try
                    {
                        _simulation.Shapes.RemoveAndDispose(oldShapeIndex, _simulation.BufferPool);
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[simBase] Warning: Could not dispose old shape: {ex.Message}");
                        // Continuar - esto no es crítico para la funcionalidad
                    }
                }
            }
        }

        public static float Min(float Value, float Min = 0.01f)
        {
            return Math.Max(Value, Min);
        }

        public static float GradosARadianes(float grados)
        {
            return grados * (float)Math.PI / 180f;
        }

        public static float RadianesAGrados(float radianes)
        {
            return radianes * 180f / (float)Math.PI;
        }

        public void SetPosition(float x, float y, float z = 0)
        {
            CoordinateConverter.UpdateBepuBodyPosition(_simulation, BodyHandle, new Vector3(x, y, z));
        }

        public void SetPosition(Vector2 wpfPosition)
        {
            // Mantener la coordenada Z actual para preservar la altura del objeto
            var currentBepuPosition = CoordinateConverter.GetBepuBodyPosition(_simulation, BodyHandle);
            var newBepuPosition = new Vector3(wpfPosition.X, CoordinateConverter.WpfYToBepuY(wpfPosition.Y), currentBepuPosition.Z);
            CoordinateConverter.UpdateBepuBodyPosition(_simulation, BodyHandle, newBepuPosition);
        }

        public void SetPosition(Vector3 bepuPosition)
        {
            CoordinateConverter.UpdateBepuBodyPosition(_simulation, BodyHandle, bepuPosition);
        }

        public Vector3 GetPosition()
        {
            return CoordinateConverter.GetBepuBodyPosition(_simulation, BodyHandle);
        }

        public void SetRotation(float wpfAngle)
        {
            CoordinateConverter.UpdateBepuBodyRotation(_simulation, BodyHandle, wpfAngle);
        }

        public float GetRotationZ()
        {
            return CoordinateConverter.GetWpfAngleFromBepuBody(_simulation, BodyHandle);
        }
    }

    public class simTransporte : simBase
    {
        public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)
        public float Friction { get; set; } // Friccion para efectos de cinta transportadora
        public float DistanceGuide2Guide { get; set; }
        public bool isBrake { get; set; }

        public bool TransportWithGuides = false;
        private List<Action> _deferredActions;
        public float Width { get; set; }
        public float Height { get; set; }

        // ✅ NUEVAS PROPIEDADES - cachear cálculos costosos
        public Vector3 DirectionVector { get; private set; }
        public float SpeedMetersPerSecond { get; private set; }
        public List<simBotella> BottlesOnTransport { get; private set; } = new List<simBotella>();
        
        // ✅ NUEVO EVENTO - para actualización de motores
        public event Action<simTransporte> OnSpeedChanged;
        


        public simTransporte(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;

            Width = width;
            Height = height;

            // Usar el nuevo método Create que maneja Top-Left correctamente
            Create(width, height, topLeft, angle);
            
            // ✅ INICIALIZAR PROPIEDADES CRÍTICAS
            UpdateCachedProperties();
        }

        public float Angle
        {
            get
            {
                return GetRotationZ();
            }
            set
            {
                SetRotation(value);
            }
        }

        /// <summary>
        /// ✅ SOBRESCRITO: SetRotation que actualiza automáticamente las propiedades cacheadas
        /// </summary>
        public new void SetRotation(float wpfAngle)
        {
            base.SetRotation(wpfAngle);
            
            // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de rotación
            UpdateCachedProperties();
            
            // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección
            OnSpeedChanged?.Invoke(this);
        }

        public new void SetPosition(float x, float y, float z = 0)
        {
            base.SetPosition(x, y, z);
        }
        
        /// <summary>
        /// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales
        /// </summary>
        internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
        {
            var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, currentWpfAngle, zPosition);
            SetPosition(bepuCenter);
        }
        
        /// <summary>
        /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
        /// </summary>
        internal Vector2 GetWpfTopLeft()
        {
            var bepuCenter = GetPosition();
            var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(bepuCenter, Width, Height, wpfAngle);
        }
        
        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle)
        {
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition);
            CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle);
            
            // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de orientación
            UpdateCachedProperties();
            
            // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección
            OnSpeedChanged?.Invoke(this);
        }

        // ✅ NUEVO MÉTODO - actualizar propiedades cacheadas
        internal void UpdateCachedProperties()
        {
            // ✅ CORREGIDO: La dirección siempre es UnitX rotado por el ángulo del transporte
            // NO depende de las dimensiones (Width >= Height) sino solo de la rotación
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                var bepuQuaternion = bodyReference.Pose.Orientation;
                
                // ✅ SIEMPRE usar UnitX y aplicar la rotación
                DirectionVector = Vector3.Transform(Vector3.UnitX, bepuQuaternion);
                
                // 🔍 DEBUG: Agregar información detallada
                var wpfAngle = GetRotationZ();
                System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
            }
            else
            {
                // ✅ CORREGIDO: Aplicar conversión de coordenadas WPF→BEPU en el vector
                var wpfAngle = GetRotationZ(); // Ángulo WPF en grados
                var wpfAngleRadians = simBase.GradosARadianes(wpfAngle);
                
                // Calcular el vector en coordenadas WPF
                var wpfX = (float)Math.Cos(wpfAngleRadians);
                var wpfY = (float)Math.Sin(wpfAngleRadians);
                
                // ✅ APLICAR CONVERSIÓN Y: En WPF Y+ es abajo, en BEPU Y+ es arriba
                DirectionVector = new Vector3(wpfX, -wpfY, 0); // Invertir Y para conversión WPF→BEPU
                
                // 🔍 DEBUG: Agregar información detallada
                System.Diagnostics.Debug.WriteLine($"[UpdateCached-Fallback] WPF Angle: {wpfAngle}°, WPF Vector: ({wpfX:F3}, {wpfY:F3}), BEPU DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
            }
            
            SpeedMetersPerSecond = Speed / simBase.SPEED_CONVERSION_FACTOR;
        }

        // ✅ MODIFICAR MÉTODO EXISTENTE - disparar evento
        public void SetSpeed(float speed)
        {
            Speed = speed;
            UpdateCachedProperties();
            // Disparar evento para actualizar motores activos
            OnSpeedChanged?.Invoke(this);
        }

        /// <summary>
        /// Configura la velocidad del transporte en metros por segundo
        /// Valores positivos mueven en la dirección del transporte, negativos en dirección opuesta
        /// </summary>
        /// <param name="speedMeterPerSecond">Velocidad en m/s (típicamente entre -5.0 y 5.0)</param>
        public void SetTransportSpeed(float speedMeterPerSecond)
        {
            SetSpeed(speedMeterPerSecond * simBase.SPEED_CONVERSION_FACTOR);
        }

        /// <summary>
        /// Detiene completamente el transporte
        /// </summary>
        public void StopTransport()
        {
            SetSpeed(0f);
        }

        /// <summary>
        /// Invierte la dirección del transporte manteniendo la misma velocidad
        /// </summary>
        public void ReverseTransport()
        {
            SetSpeed(-Speed);
        }

        public void SetDimensions(float width, float height)
        {
            Width = width;
            Height = height;

            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var box = new Box(width, height, zAltura_Transporte);
                var shapeIndex = _simulation.Shapes.Add(box);
                ChangeBodyShape(shapeIndex);
            }
            
            // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de dimensiones
            UpdateCachedProperties();
            
            // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección
            OnSpeedChanged?.Invoke(this);
        }

        public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle = 0)
        {
            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var zPosition = zAltura_Transporte / 2f + zPos_Transporte;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
            
            Create(width, height, bepuCenter, wpfAngle);
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: RemoverBody que solo elimina el body
        /// </summary>
        public new void RemoverBody()
        {
            base.RemoverBody();
        }

        public void Create(float width, float height, Vector3 bepuPosition, float wpfAngle = 0)
        {
            RemoverBody();

            var box = new Box(width, height, zAltura_Transporte);
            var shapeIndex = _simulation.Shapes.Add(box);

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(bepuPosition, CoordinateConverter.CreateBepuQuaternionFromWpfAngle(wpfAngle)),
                new CollidableDescription(shapeIndex, 0.1f),
                new BodyActivityDescription(0.01f)
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true; // Marcar que hemos creado un cuerpo
            
            // ✅ CRÍTICO: Actualizar propiedades cacheadas después de crear el body
            UpdateCachedProperties();
        }
    }

    public class simBarrera : simBase
    {
        public float Distancia;
        public bool LuzCortada;
        public bool LuzCortadaNeck;
        public bool DetectNeck;
        public List<simBotella> ListSimBotellaContact;
        float _height;
        private List<Action> _deferredActions;
        public float Width { get; set; }
        public float Height { get; set; }

        public simBarrera(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            Width = width;
            Height = height;
            _height = height;
            DetectNeck = detectectNeck;
            ListSimBotellaContact = new List<simBotella>();

            // Usar el nuevo método Create que maneja Top-Left correctamente
            Create(width, height, topLeft, angle, detectectNeck);
        }

        public float Angle
        {
            get
            {
                return GetRotationZ();
            }
            set
            {
                SetRotation(value);
            }
        }

        public new void SetPosition(float x, float y, float z = 0)
        {
            base.SetPosition(x, y, z);
        }
        
        /// <summary>
        /// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales
        /// </summary>
        internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
        {
            var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, currentWpfAngle, zPosition);
            SetPosition(bepuCenter);
        }
        
        /// <summary>
        /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
        /// </summary>
        internal Vector2 GetWpfTopLeft()
        {
            var bepuCenter = GetPosition();
            var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(bepuCenter, Width, Height, wpfAngle);
        }
        
        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle)
        {
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition);
            CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle);
        }

        public void SetDimensions(float width, float height)
        {
            _height = height;
            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas
                var shapeIndex = _simulation.Shapes.Add(box);
                ChangeBodyShape(shapeIndex);
            }
        }

        public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle = 0, bool detectectNeck = false)
        {
            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var zPosition = zPos_Barrera + zAltura_Barrera / 2f;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
            
            Create(width, height, bepuCenter, wpfAngle, detectectNeck);
        }

        public void Create(float width, float height, Vector3 bepuPosition, float wpfAngle = 0, bool detectectNeck = false)
        {
            RemoverBody();

            // Crear box con altura de 0.2m (desde z=0 hasta z=0.2) para funcionar como detector
            var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas completas
            var shapeIndex = _simulation.Shapes.Add(box);

            // Crear como SENSOR (Kinematic con speculative margin 0 para detección pura)
            // Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos
            var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(bepuPosition, CoordinateConverter.CreateBepuQuaternionFromWpfAngle(wpfAngle)),
                new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores
                activityDescription
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true; // Marcar que hemos creado un cuerpo
        }
    }

    public class simGuia : simBase
    {
        private List<Action> _deferredActions;

        // Propiedades para acceder a las dimensiones del objeto WPF
        public float GuideThickness { get; set; } // Espesor de la guía



        public simGuia(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            GuideThickness = height;
            Create(width, height, topLeft, angle);
        }

        public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle)
        {
            RemoverBody();

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var zPosition = zAltura_Guia / 2 + zPos_Guia;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);

            // Crear el Box con las dimensiones dadas - más alto para contener botellas
            var box = new Box(width, GuideThickness, zAltura_Guia);
            var shapeIndex = _simulation.Shapes.Add(box);

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(bepuCenter, CoordinateConverter.CreateBepuQuaternionFromWpfAngle(wpfAngle)),
                new CollidableDescription(shapeIndex, 0.1f),
                new BodyActivityDescription(0.01f)
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true;
        }

        // Método para actualizar las propiedades desde el objeto WPF
        public void UpdateProperties(float ancho, float altoGuia, float angulo)
        {
            // Nota: ancho y angulo se ignoran ya que se calculan automáticamente desde start->end
            GuideThickness = altoGuia;
        }

        /// <summary>
        /// ✅ NUEVO - Actualiza solo las dimensiones sin recrear el objeto
        /// </summary>
        public void SetDimensions(float width, float height)
        {
            GuideThickness = height;
            
            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var box = new Box(width, GuideThickness, zAltura_Guia);
                var shapeIndex = _simulation.Shapes.Add(box);
                ChangeBodyShape(shapeIndex);
            }
        }

        /// <summary>
        /// ✅ CORREGIDO - Actualiza solo la posición sin recrear el objeto usando CoordinateConverter
        /// Requiere las dimensiones reales para conversión correcta Top-Left → Center
        /// </summary>
        public void SetPosition(Vector2 wpfTopLeft, float wpfAngle, float actualWidth, float actualHeight)
        {
            // ✅ USAR COORDINATECONVERTER para conversión centralizada con dimensiones reales
            var zPosition = zAltura_Guia / 2 + zPos_Guia;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, actualWidth, actualHeight, wpfAngle, zPosition);

            // Actualizar posición y rotación simultáneamente
            CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF (sobrecarga para compatibilidad)
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float width, float height)
        {
            // Usar el método SetPosition con dimensiones correctas
            SetPosition(wpfTopLeft, wpfAngle, width, height);
            
            // Actualizar propiedades internas
            GuideThickness = height;
        }

        /// <summary>
        /// ✅ LEGACY - Mantener compatibilidad con versión anterior (usar dimensiones almacenadas)
        /// </summary>
        public void SetPosition(Vector2 wpfTopLeft, float wpfAngle = 0)
        {
            // Fallback: usar GuideThickness como aproximación si no se proporcionan dimensiones
            SetPosition(wpfTopLeft, wpfAngle, GuideThickness * 10f, GuideThickness);
        }
    }

    public class simBotella : simBase
    {
        public float Radius;
        public float Height; // Altura para la visualización del cilindro en Helix
        private float _mass;
        public bool Descartar = false;
        public int isOnTransports;
        public List<simBase> ListOnTransports;
        public bool isRestricted;
        public bool isNoMoreRestricted;

        public simTransporte ConveyorRestrictedTo;
        public float OverlapPercentage;
        public float _neckRadius;
        public bool isOnBrakeTransport; // Nueva propiedad para marcar si está en un transporte con freno
        public simTransporte CurrentBrakeTransport { get; set; } // ✅ NUEVA: Referencia al transporte de freno actual
        
        // ✅ NUEVO SISTEMA SIMPLIFICADO: Motor dinámico que se crea solo cuando es necesario
        public ConstraintHandle CurrentMotor { get; private set; } = default;
        public simBase CurrentMotorTarget { get; private set; } = null; // Transporte o curva actual
        public bool _hasMotor = false; // ✅ BANDERA PÚBLICA para acceso desde callbacks
        public bool HasMotor => _hasMotor;
        
        // ✅ NUEVAS PROPIEDADES para el motor dinámico
        public Vector3 CurrentDirection { get; private set; } = Vector3.UnitX; // Dirección por defecto (1,0,0)
        public float CurrentSpeed { get; private set; } = 0f; // Velocidad por defecto
        public bool IsOnElement { get; private set; } = false; // Si está sobre un elemento activo
        
        private List<Action> _deferredActions;

        public simBotella(Simulation simulation, List<Action> deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            Radius = diameter / 2f;
            Height = diameter; // Altura igual al diámetro para mantener proporciones similares
            _mass = mass;
            _neckRadius = neckRadius;
            ListOnTransports = new List<simBase>();

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), Radius + zPos_Transporte + zAltura_Transporte);
            Create(position3D);
            
            // ✅ ELIMINADO: No crear motor permanente - se creará cuando sea necesario
        }

        public float CenterX
        {
            get
            {
                return GetPosition().X;
            }
            set
            {
                var pos = GetPosition();
                SetPosition(value, pos.Y, pos.Z);
            }
        }

        public float CenterY
        {
            get
            {
                // ✅ USAR COORDINATECONVERTER para conversión centralizada
                return CoordinateConverter.BepuYToWpfY(GetPosition().Y);
            }
            set
            {
                var pos = GetPosition();
                // ✅ USAR COORDINATECONVERTER para conversión centralizada
                SetPosition(pos.X, CoordinateConverter.WpfYToBepuY(value), pos.Z);
            }
        }

        public Vector2 Center
        {
            get
            {
                var pos3D = GetPosition();
                // ✅ USAR COORDINATECONVERTER para conversión centralizada
                return CoordinateConverter.BepuVector3ToWpfVector2(pos3D);
            }
            set
            {
                // Mantener la Z actual, solo cambiar X, Y
                var currentPos = GetPosition();
                // ✅ USAR COORDINATECONVERTER para conversión centralizada
                SetPosition(value.X, CoordinateConverter.WpfYToBepuY(value.Y), currentPos.Z);
            }
        }

        public float Mass
        {
            get
            {
                if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
                {
                    var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                    return 1f / bodyReference.LocalInertia.InverseMass;
                }
                return _mass;
            }
            set
            {
                _mass = value;
                if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
                {
                    // Usar esfera simple - sin complejidad de inercia personalizada
                    var sphere = new Sphere(Radius);
                    var inertia = sphere.ComputeInertia(value);
                    _simulation.Bodies.SetLocalInertia(BodyHandle, inertia);
                }
            }
        }

        private void Create(Vector3 position)
        {
            RemoverBody();

            // Usar ESFERA en BEPU para simplicidad matemática y eficiencia
            var sphere = new Sphere(Radius);
            var shapeIndex = _simulation.Shapes.Add(sphere);

            // Inercia estándar de esfera - sin complejidad adicional
            var inertia = sphere.ComputeInertia(_mass);

            // NUNCA DORMIR - Valor negativo significa que las botellas JAMÁS entran en sleep mode
            // Esto es crítico para detección continua de barreras, transportes y descartes
            var activityDescription = new BodyActivityDescription(-1f); // -1 = NUNCA dormir

            var bodyDescription = BodyDescription.CreateDynamic(
                new RigidPose(position),
                new BodyVelocity(),
                inertia, // Inercia estándar de esfera
                new CollidableDescription(shapeIndex, 0.001f),
                activityDescription
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true; // Marcar que hemos creado un cuerpo
        }

        /// <summary>
        /// ✅ NUEVO: Crea o actualiza el motor conectándolo al transporte/curva actual
        /// </summary>
        public void CreateOrUpdateMotor(simBase target, Vector3 direction, float speed)
        {
            try
            {
                // ✅ VALIDAR DIRECCIÓN
                if (direction.Length() < 0.001f)
                {
                    return;
                }

                // ✅ VALIDAR QUE EL TARGET Y LA SIMULACIÓN EXISTAN
                if (target == null || _simulation == null || !_simulation.Bodies.BodyExists(BodyHandle))
                {
                    return;
                }

                // ✅ VERIFICAR SI NECESITAMOS CREAR O ACTUALIZAR EL MOTOR
                bool needsNewMotor = false;
                
                if (!HasMotor)
                {
                    // ✅ PRIMERA VEZ: Crear motor nuevo
                    needsNewMotor = true;
                    System.Diagnostics.Debug.WriteLine($"[CreateOrUpdateMotor] 🆕 Creando motor nuevo para {target?.GetType().Name}");
                }
                else if (CurrentMotorTarget != target)
                {
                    // ✅ CAMBIO DE OBJETO: Eliminar motor anterior y crear uno nuevo
                    RemoveCurrentMotor();
                    needsNewMotor = true;
                    System.Diagnostics.Debug.WriteLine($"[CreateOrUpdateMotor] 🔄 Cambiando motor de {CurrentMotorTarget?.GetType().Name} a {target?.GetType().Name}");
                }
                else
                {
                    // ✅ MISMO OBJETO: Solo actualizar velocidad
                    UpdateMotorSpeed(direction, speed);
                    return;
                }

                // ✅ CREAR NUEVO MOTOR SI ES NECESARIO
                if (needsNewMotor && target != null)
                {
                    CreateMotorForTarget(target, direction, speed);
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CreateOrUpdateMotor] ❌ ERROR: {ex.Message}");
            }
        }

        /// <summary>
        /// ✅ NUEVO: Crea un motor específico para un transporte o curva
        /// </summary>
        private void CreateMotorForTarget(simBase target, Vector3 direction, float speed)
        {
            try
            {
                if (_simulation == null || _simulation.Solver == null || !_simulation.Bodies.BodyExists(BodyHandle) || target == null)
                {
                    System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ❌ Simulación, solver, body o target no disponible");
                    return;
                }

                // ✅ VERIFICAR QUE EL TARGET TENGA UN BODY VÁLIDO
                if (!_simulation.Bodies.BodyExists(target.BodyHandle))
                {
                    System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ❌ Target body no existe: {target.BodyHandle}");
                    return;
                }

                // ✅ VERIFICAR QUE NO TENGA YA UN MOTOR VÁLIDO
                if (HasMotor && CurrentMotor.Value != 0)
                {
                    System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ⚠️ Ya existe un motor válido: {CurrentMotor}");
                    return;
                }

                // ✅ CALCULAR VELOCIDAD EFECTIVA
                var effectiveSpeed = CalculateEffectiveSpeed(direction, speed);

                // ✅ CREAR MOTOR CONECTADO AL TARGET
                var motor = new LinearAxisMotor()
                {
                    LocalOffsetA = Vector3.Zero, // Botella
                    LocalOffsetB = Vector3.Zero, // Target
                    LocalAxis = Vector3.UnitX,   // Dirección fija del motor
                    TargetVelocity = effectiveSpeed,
                    Settings = new MotorSettings(Math.Max(_mass * 20f, 8f), 4f)
                };

                // ✅ CONECTAR BOTELLA CON EL TARGET (transporte o curva)
                CurrentMotor = _simulation.Solver.Add(BodyHandle, BodyHandle, motor);
                
                // ✅ VERIFICAR QUE EL MOTOR SE CREÓ CORRECTAMENTE
                if (CurrentMotor.Value == 0)
                {
                    System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ❌ Error: Motor no se creó correctamente");
                    return;
                }
                
                CurrentMotorTarget = target;
                _hasMotor = true; // ✅ ESTABLECER BANDERA
                
                // ✅ ACTUALIZAR PROPIEDADES INTERNAS
                CurrentDirection = direction;
                CurrentSpeed = speed;
                IsOnElement = Math.Abs(speed) > 0.001f;
                
                System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ✅ Motor creado:");
                System.Diagnostics.Debug.WriteLine($"   Botella: {BodyHandle}");
                System.Diagnostics.Debug.WriteLine($"   Target: {target.BodyHandle} ({target.GetType().Name})");
                System.Diagnostics.Debug.WriteLine($"   Motor Handle: {CurrentMotor} (Value: {CurrentMotor.Value})");
                System.Diagnostics.Debug.WriteLine($"   Dirección: {direction}");
                System.Diagnostics.Debug.WriteLine($"   Velocidad efectiva: {effectiveSpeed:F3}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ❌ ERROR: {ex.Message}");
            }
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza solo la velocidad del motor existente
        /// </summary>
        private void UpdateMotorSpeed(Vector3 direction, float speed)
        {
            try
            {
                if (!HasMotor || _simulation == null)
                {
                    return;
                }

                // ✅ OBTENER LA DESCRIPCIÓN ACTUAL DEL MOTOR
                _simulation.Solver.GetDescription(CurrentMotor, out LinearAxisMotor motor);

                // ✅ CALCULAR VELOCIDAD EFECTIVA
                var effectiveSpeed = CalculateEffectiveSpeed(direction, speed);

                // ✅ ACTUALIZAR SOLO LA VELOCIDAD
                motor.TargetVelocity = effectiveSpeed;

                // ✅ ACTUALIZAR EL MOTOR EN EL SOLVER
                _simulation.Solver.ApplyDescription(CurrentMotor, motor);

                // ✅ ACTUALIZAR PROPIEDADES INTERNAS
                CurrentDirection = direction;
                CurrentSpeed = speed;
                IsOnElement = Math.Abs(speed) > 0.001f;
                
                System.Diagnostics.Debug.WriteLine($"[UpdateMotorSpeed] 🔄 Velocidad actualizada: {effectiveSpeed:F3}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[UpdateMotorSpeed] ❌ ERROR: {ex.Message}");
            }
        }

        /// <summary>
        /// ✅ NUEVO: Elimina el motor actual
        /// </summary>
        public void RemoveCurrentMotor()
        {
            try
            {
                if (HasMotor && _simulation != null && _simulation.Solver != null)
                {
                    // ✅ VERIFICAR QUE EL MOTOR EXISTE ANTES DE ELIMINARLO
                    if (CurrentMotor.Value != 0)
                    {
                        try
                        {
                            _simulation.Solver.Remove(CurrentMotor);
                            System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] 🗑️ Motor eliminado: {CurrentMotor}");
                        }
                        catch (Exception removeEx)
                        {
                            System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] ⚠️ Error eliminando motor {CurrentMotor}: {removeEx.Message}");
                            // Continuar con la limpieza incluso si falla la eliminación
                        }
                    }
                    else
                    {
                        System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] ⚠️ Motor ya eliminado o inválido: {CurrentMotor}");
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] ❌ ERROR: {ex.Message}");
            }
            finally
            {
                // ✅ LIMPIAR REFERENCIAS SIEMPRE
                CurrentMotor = default;
                CurrentMotorTarget = null;
                _hasMotor = false; // ✅ LIMPIAR BANDERA
                CurrentDirection = Vector3.UnitX;
                CurrentSpeed = 0f;
                IsOnElement = false;
            }
        }

        /// <summary>
        /// ✅ NUEVO: Detiene el motor (velocidad 0) pero mantiene la conexión
        /// </summary>
        public void StopMotor()
        {
            UpdateMotorSpeed(CurrentDirection, 0f);
        }

        /// <summary>
        /// ✅ NUEVO MÉTODO: Calcular velocidad efectiva para dirección deseada
        /// </summary>
        private float CalculateEffectiveSpeed(Vector3 desiredDirection, float desiredSpeed)
        {
            // ✅ NORMALIZAR LA DIRECCIÓN DESEADA
            var normalizedDirection = Vector3.Normalize(desiredDirection);
            
            // ✅ CALCULAR LA PROYECCIÓN EN EL EJE X (dirección del motor fijo)
            // Como el motor está fijo en UnitX, solo podemos controlar el movimiento en X
            var projectionX = normalizedDirection.X;
            
            // ✅ CALCULAR VELOCIDAD EFECTIVA
            // Si la dirección es principalmente en X, usar la velocidad completa
            // Si la dirección es perpendicular a X, la velocidad será 0
            var effectiveSpeed = projectionX * desiredSpeed;
            
            System.Diagnostics.Debug.WriteLine($"[CalculateEffectiveSpeed] 📐 Cálculo:");
            System.Diagnostics.Debug.WriteLine($"   Dirección deseada: {desiredDirection} (Normalizada: {normalizedDirection})");
            System.Diagnostics.Debug.WriteLine($"   Proyección en X: {projectionX:F3}");
            System.Diagnostics.Debug.WriteLine($"   Velocidad deseada: {desiredSpeed:F3}");
            System.Diagnostics.Debug.WriteLine($"   Velocidad efectiva: {effectiveSpeed:F3}");
            
            return effectiveSpeed;
        }

        public void SetDiameter(float diameter)
        {
            Radius = diameter / 2f;
            Height = diameter; // Mantener altura igual al diámetro para proporciones consistentes

            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var sphere = new Sphere(Radius);
                var shapeIndex = _simulation.Shapes.Add(sphere);
                ChangeBodyShape(shapeIndex);
            }
        }

        public void SetHeight(float height)
        {
            Height = height;

            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var sphere = new Sphere(Radius);
                var shapeIndex = _simulation.Shapes.Add(sphere);
                ChangeBodyShape(shapeIndex);
            }
        }

        public void SetMass(float mass)
        {
            Mass = mass;
        }

        /// <summary>
        /// Limita la rotación de la botella solo al plano XY (siempre "de pie")
        /// Esto simplifica enormemente la simulación y es más realista para botellas
        /// </summary>
        public void LimitRotationToXYPlane()
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);

                // Extraer solo la rotación en Z (plano XY) y eliminar las rotaciones en X e Y
                var currentOrientation = bodyReference.Pose.Orientation;

                // Convertir a ángulo en Z solamente
                var rotationZ = GetRotationZ();

                // Crear nueva orientación solo con rotación en Z (botella siempre "de pie")
                var correctedOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, rotationZ);

                // Aplicar la orientación corregida
                bodyReference.Pose.Orientation = correctedOrientation;

                // También limitar la velocidad angular a solo rotación en Z
                var angularVelocity = bodyReference.Velocity.Angular;
                bodyReference.Velocity.Angular = new Vector3(0, 0, angularVelocity.Z);
            }
        }

        public void ApplyLinearVelocity(Vector3 velocity)
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                bodyReference.Velocity.Linear = velocity;
            }
        }

        public Vector3 GetLinearVelocity()
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                return bodyReference.Velocity.Linear;
            }
            return Vector3.Zero;
        }

        public void CenterFixtureOnConveyor()
        {
            // Implementar lógica de centrado si es necesario
        }

        public bool IsOnAnyTransport()
        {
            return isOnTransports > 0;
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: RemoverBody que confía en BEPU para limpiar constraints
        /// </summary>
        public new void RemoverBody()
        {
            base.RemoverBody();
        }

    }

    /// <summary>
    /// Representa una curva o arco en la simulación física.
    /// ✅ IMPORTANTE: Los ángulos _startAngle y _endAngle se almacenan INTERNAMENTE en radianes BEPU
    /// (ya convertidos desde grados WPF usando CoordinateConverter.WpfDegreesToBepuRadians)
    /// Las propiedades públicas StartAngle/EndAngle devuelven grados WPF usando CoordinateConverter.BepuRadiansToWpfDegrees
    /// ✅ NUEVO: Cada triángulo actúa como un mini-transporte con su propia dirección tangencial fija
    /// </summary>
    public class simCurve : simBase
    {
        private float _innerRadius;
        private float _outerRadius;
        private float _startAngle;  // ✅ SIEMPRE en radianes BEPU
        private float _endAngle;    // ✅ SIEMPRE en radianes BEPU
        public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)

        private List<Action> _deferredActions;

        // ✅ NUEVO: Almacenar el centro real de la curva
        private Vector3 _curveCenter;

        // ✅ SIMPLIFICADO: Propiedades esenciales únicamente
        public List<simBotella> BottlesOnCurve { get; private set; } = new List<simBotella>();
        
        // ✅ EVENTO para actualización de motores
        public event Action<simCurve> OnSpeedChanged;
        


        // ✅ NUEVO: Almacenar triángulos creados para acceso directo
        private Triangle[] _storedTriangles;

        public float InnerRadius => _innerRadius;
        public float OuterRadius => _outerRadius;
        public float StartAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_startAngle); // Convertir de radianes BEPU internos a grados WPF
        public float EndAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_endAngle); // Convertir de radianes BEPU internos a grados WPF

        // ✅ NUEVO: Propiedad para acceder al centro real de la curva
        public Vector3 CurveCenter => _curveCenter;

        public simCurve(Simulation simulation, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ SIMPLIFICADO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Calcular y almacenar el centro real de la curva
            var curveSize = outerRadius * 2f;
            var zPosition = zAltura_Curve / 2f + zPos_Curve;
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(topLeft, curveSize, curveSize, 0f, zPosition);

            // ✅ SIMPLIFICADO: Crear la curva directamente
            Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0);
        }

        // ✅ SIMPLIFICADO: Configurar velocidad angular para AngularAxisMotor
        public void SetSpeed(float speed)
        {
            Speed = speed; // Velocidad angular directa (sin inversión)
            OnSpeedChanged?.Invoke(this);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            // Actualizar parámetros de la curva
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Actualizar el centro real de la curva
            var curveSize = outerRadius * 2f;
            var zPosition = zAltura_Curve / 2f + zPos_Curve;
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
            
            Create(_curveCenter);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza posición desde Top-Left WPF manteniendo parámetros actuales
        /// </summary>
        internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
        {
            var curveSize = _outerRadius * 2f;
            var zPosition = GetPosition().Z; // Mantener Z actual
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
            
            Create(_curveCenter);
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
        /// </summary>
        internal Vector2 GetWpfTopLeft()
        {
            var curveSize = _outerRadius * 2f;
            return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(_curveCenter, curveSize, curveSize, 0f);
        }

        /// <summary>
        /// ✅ CORREGIDO: Obtiene los triángulos reales creados para la curva
        /// Devuelve los triángulos en coordenadas locales para evitar transformación duplicada
        /// </summary>
        public Triangle[] GetRealBEPUTriangles()
        {
            try
            {
                if (_storedTriangles == null || _storedTriangles.Length == 0)
                {
                    System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] No hay triángulos almacenados");
                    return new Triangle[0];
                }

                // ✅ CORREGIDO: Devolver triángulos en coordenadas locales
                // La visualización 3D aplicará la transformación una sola vez
                System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Devolviendo {_storedTriangles.Length} triángulos en coordenadas locales");
                return _storedTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Error: {ex.Message}");
                return new Triangle[0];
            }
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene los triángulos transformados a coordenadas mundiales (solo para debugging)
        /// </summary>
        public Triangle[] GetWorldBEPUTriangles()
        {
            try
            {
                if (_storedTriangles == null || _storedTriangles.Length == 0)
                {
                    return new Triangle[0];
                }

                var worldTriangles = TransformTrianglesToWorldCoordinates(_storedTriangles);
                System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Devolviendo {worldTriangles.Length} triángulos en coordenadas mundiales");
                return worldTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Error: {ex.Message}");
                return new Triangle[0];
            }
        }

        /// <summary>
        /// ✅ NUEVO: Transforma triángulos de coordenadas locales a mundiales
        /// </summary>
        private Triangle[] TransformTrianglesToWorldCoordinates(Triangle[] localTriangles)
        {
            try
            {
                if (_simulation == null || !_simulation.Bodies.BodyExists(BodyHandle))
                {
                    System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Cuerpo no existe, devolviendo triángulos locales");
                    return localTriangles; // Fallback: devolver triángulos sin transformar
                }

                var body = _simulation.Bodies[BodyHandle];
                var bodyPosition = body.Pose.Position;
                var bodyOrientation = body.Pose.Orientation;

                var transformedTriangles = new Triangle[localTriangles.Length];

                for (int i = 0; i < localTriangles.Length; i++)
                {
                    var localTriangle = localTriangles[i];

                    // Transformar cada vértice del triángulo a coordenadas mundiales
                    var worldA = bodyPosition + Vector3.Transform(localTriangle.A, bodyOrientation);
                    var worldB = bodyPosition + Vector3.Transform(localTriangle.B, bodyOrientation);
                    var worldC = bodyPosition + Vector3.Transform(localTriangle.C, bodyOrientation);

                    transformedTriangles[i] = new Triangle(worldA, worldB, worldC);
                }

                System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Transformados {transformedTriangles.Length} triángulos. Body pos: {bodyPosition}");
                return transformedTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Error: {ex.Message}, devolviendo triángulos locales");
                return localTriangles; // Fallback en caso de error
            }
        }


        public new void RemoverBody()
        {
            // ✅ NUEVO: Limpiar triángulos almacenados
            _storedTriangles = null;
            
            // ✅ SIMPLIFICADO: Solo remover el cuerpo principal (Mesh único)
            base.RemoverBody();
        }

        public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 wpfTopLeft, float unused = 0)
        {
            // ✅ CORRIGIDO: Como Aether - startAngle y endAngle definen directamente el sector
            // No hay rotación separada del objeto

            // Actualizar parámetros internos
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Actualizar el centro real de la curva
            // Para curvas, el "tamaño" es el diámetro del radio exterior
            var curveSize = outerRadius * 2f;
            var zPosition = zAltura_Curve / 2f + zPos_Curve;
            
            // Calcular nueva posición del centro (sin rotación - el sector ya está definido por los ángulos)
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

            Create(_curveCenter);  // Sin rotación adicional
        }

        private void Create(Vector3 position)
        {
            RemoverBody();

            // ✅ SIMPLIFICADO: Crear superficie usando Triangle de BEPU directamente
            var triangles = CreateSimpleArcTriangles(_innerRadius, _outerRadius, _startAngle, _endAngle);

            // ✅ ALMACENAR triángulos para acceso directo
            _storedTriangles = triangles;

            // ✅ SIMPLIFICADO: Crear un solo cuerpo con múltiples Triangle shapes usando Mesh
            if (triangles.Length > 0)
            {
                // ✅ CREAR MESH CON LA API CORRECTA DE BEPU
                var triangleBuffer = new BepuUtilities.Memory.Buffer<Triangle>(triangles.Length, _simulation.BufferPool);
                for (int i = 0; i < triangles.Length; i++)
                {
                    triangleBuffer[i] = triangles[i];
                }
                var mesh = new BepuPhysics.Collidables.Mesh(triangleBuffer, Vector3.One, _simulation.BufferPool);
                var shapeIndex = _simulation.Shapes.Add(mesh);

            var bodyDescription = BodyDescription.CreateKinematic(
                    new RigidPose(position),
                    new CollidableDescription(shapeIndex, 0.001f), // ✅ SpeculativeMargin bajo para detección precisa
                new BodyActivityDescription(0.01f)
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true;

                System.Diagnostics.Debug.WriteLine($"[simCurve] Creado con {triangles.Length} triángulos reales almacenados");
            }
        }

        // ✅ MÉTODOS ELIMINADOS: CreateMainBody y CreateTriangleBody ya no son necesarios
        // La curva ahora se crea como un solo Mesh en el método Create simplificado

        /// <summary>
        /// ✅ SIMPLIFICADO: Crea triángulos usando Triangle de BEPU directamente
        /// Solo superficie superior, eliminando complejidad innecesaria
        /// Los triángulos se crean en coordenadas locales (Z=0) y la posición del cuerpo los ubica correctamente
        /// </summary>
        /// <param name="innerRadius">Radio interno del arco</param>
        /// <param name="outerRadius">Radio externo del arco</param>
        /// <param name="startAngle">Ángulo inicial en radianes BEPU</param>
        /// <param name="endAngle">Ángulo final en radianes BEPU</param>
        /// <returns>Array de triángulos nativos de BEPU en coordenadas locales</returns>
        private Triangle[] CreateSimpleArcTriangles(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            var triangles = new List<Triangle>();

            // ✅ SIMPLIFICADO: Menos segmentos, menos complejidad
            float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f);
            int segments = Math.Max(8, Math.Min((int)(arcLength * 8), 32)); // Menos segmentos
            float angleStep = (endAngle - startAngle) / segments;

            // ✅ SIMPLIFICADO: Sin inversión compleja de ángulos
            for (int i = 0; i < segments; i++)
            {
                float angle1 = startAngle + i * angleStep;
                float angle2 = startAngle + (i + 1) * angleStep;
                
                // ✅ COORDENADAS LOCALES: Triángulos en Z=0, el cuerpo los posiciona correctamente
                var inner1 = new Vector3(innerRadius * (float)Math.Cos(angle1), innerRadius * (float)Math.Sin(angle1), 0);
                var outer1 = new Vector3(outerRadius * (float)Math.Cos(angle1), outerRadius * (float)Math.Sin(angle1), 0);
                var inner2 = new Vector3(innerRadius * (float)Math.Cos(angle2), innerRadius * (float)Math.Sin(angle2), 0);
                var outer2 = new Vector3(outerRadius * (float)Math.Cos(angle2), outerRadius * (float)Math.Sin(angle2), 0);
                
                // ✅ USAR Triangle NATIVO DE BEPU (solo superficie superior)
                triangles.Add(new Triangle(inner1, outer1, outer2));
                triangles.Add(new Triangle(inner1, outer2, inner2));
            }

            System.Diagnostics.Debug.WriteLine($"[CreateSimpleArcTriangles] Creados {triangles.Count} triángulos en coordenadas locales");
            return triangles.ToArray();
        }



        // ✅ MÉTODOS ELIMINADOS: Los cálculos tangenciales complejos ya no son necesarios
        // AngularAxisMotor maneja automáticamente la rotación en curvas
    }

    public class simDescarte : simBase
    {
        private float _radius;
        private List<Action> _deferredActions;
        public List<simBotella> ListSimBotellaContact;

        public simDescarte(Simulation simulation, List<Action> deferredActions, float diameter, Vector2 position)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _radius = diameter / 2f;
            ListSimBotellaContact = new List<simBotella>();

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), zPos_Descarte + _radius);
            Create(position3D);
        }

        public float Radius
        {
            get { return _radius; }
            set
            {
                _radius = Math.Max(value, 0.01f); // Mínimo 1cm
                // Recrear el cuerpo con el nuevo radio
                var currentPos = GetPosition();
                Create(currentPos);
            }
        }

        public void SetDiameter(float diameter)
        {
            Radius = diameter / 2f;
        }

        public float GetDiameter()
        {
            return _radius * 2f;
        }

        public void Create(Vector2 position)
        {
            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), zPos_Descarte + _radius);
            Create(position3D);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza posición usando coordenadas WPF apropiadas
        /// </summary>
        internal void UpdateFromWpfCenter(Vector2 wpfCenter)
        {
            var position3D = new Vector3(wpfCenter.X, CoordinateConverter.WpfYToBepuY(wpfCenter.Y), zPos_Descarte + _radius);
            
            // Actualizar solo posición manteniendo orientación
            CoordinateConverter.UpdateBepuBodyPosition(_simulation, BodyHandle, position3D);
        }

        private void Create(Vector3 position)
        {
            RemoverBody();

            // Crear esfera sensor para detección
            var sphere = new BepuPhysics.Collidables.Sphere(_radius);
            var shapeIndex = _simulation.Shapes.Add(sphere);

            // Crear como SENSOR (Kinematic con speculative margin 0 para detección pura)
            // Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos
            var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(position),
                new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores
                activityDescription
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true; // Marcar que hemos creado un cuerpo
        }
    }

    // Callback handlers para BEPU
    public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
    {
        private SimulationManagerBEPU _simulationManager;

        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);
                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, barreras, descartes)
                var staticA = (transportA != null || curveA != null || barrierA != null || discardA != null);
                var staticB = (transportB != null || curveB != null || barrierB != 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
            pairMaterial = new PairMaterialProperties
            {
                FrictionCoefficient = 0.6f,
                MaximumRecoveryVelocity = 2f,
                SpringSettings = new SpringSettings(60, 4)
            };

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

                // ✅ BARRERAS como sensores puros
                if (barreraA != null || barreraB != null)
                {
                    return false; // NO generar contacto físico
                }

                // ✅ DESCARTES como sensores puros
                if (descarteA != null || descarteB != null)
                {
                    var descarte = descarteA ?? descarteB;
                    if (botella != null)
                    {
                        _simulationManager.RegisterDescarteContact(descarte, botella);
                    }
                    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: Crear o actualizar motor inmediatamente
                if (botella != null && (transportA != null || transportB != null))
                {
                    var transport = transportA ?? transportB;
                    
                    // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE
                    transport.UpdateCachedProperties();
                    var direction = transport.DirectionVector;
                    var speed = transport.SpeedMetersPerSecond;
                    botella.CreateOrUpdateMotor(transport, direction, speed);
                    
                    // Fricción alta para transportes
                    pairMaterial.FrictionCoefficient = 0.9f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // ✅ CONTACTO BOTELLA-CURVA: Crear o actualizar motor inmediatamente
                else if (botella != null && (curveA != null || curveB != null))
                {
                    var curve = curveA ?? curveB;
                    
                    // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE
                    var direction = _simulationManager.CalculateCurveDirectionFromBottlePosition(curve, botella);
                    var speed = curve.Speed;
                    botella.CreateOrUpdateMotor(curve, direction, speed);
                    
                    // Fricción alta para curvas
                    pairMaterial.FrictionCoefficient = 0.9f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // Ajustes básicos para otras botellas
                else if (botella != null)
                {
                    pairMaterial.FrictionCoefficient = 0.9f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
            }

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

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

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





        /// <summary>
        /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold
        /// </summary>


        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)
        {
        }

        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
        {
            // Aplicar gravedad
            var gravityWide = Vector3Wide.Broadcast(Gravity);
            velocity.Linear += gravityWide * dt;

            // ✅ ELIMINADO COMPLETAMENTE - ya no se necesita lógica especial de frenado
            // El sistema LinearAxisMotor maneja automáticamente todas las fuerzas

            // Aplicar amortiguamiento lineal y angular para simular resistencia del aire
            // Esto es crucial para que los cilindros se detengan de forma realista
            var linearDampingWide = Vector<float>.One * LinearDamping;
            var angularDampingWide = Vector<float>.One * AngularDamping;

            velocity.Linear *= linearDampingWide;
            velocity.Angular *= angularDampingWide; // ¡Esto es clave para detener rotaciones infinitas!
        }


    }



    public class SimulationManagerBEPU
    {
        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;
        
        
        

        
        // ✅ 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;
        
        // ✅ NUEVO - sistema de eliminación diferida para evitar problemas de sincronización
        private Queue<simBase> _pendingRemovals;
        private object _removalLock = new object();
        

        
        private object _contactsLock = new object();

        /// <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;
        }

        // ✅ NUEVOS - gestión automática de clasificación
        private void RegisterObjectHandle(simBase 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:
                    _barrierHandles.Add(barrier.BodyHandle);
                    break;
                case simDescarte discard:
                    _discardHandles.Add(discard.BodyHandle);
                    break;
            }
        }

        private void UnregisterObjectHandle(simBase obj)
        {
            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:
                    _barrierHandles.Remove(barrier.BodyHandle);
                    break;
                case simDescarte discard:
                    _discardHandles.Remove(discard.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);
        }

        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>();
            
            // ✅ NUEVO - inicializar sistema de eliminación diferida
            _pendingRemovals = new Queue<simBase>();

            
            // ✅ 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.999f,              // Amortiguamiento lineal suave
                angularDamping: 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);
            

        }

        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();
                
                // ✅ NUEVO - limpiar eliminaciones pendientes
                lock (_removalLock)
                {
                    _pendingRemovals.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();
            }
            catch (Exception ex)
            {
                // Critical error during clear - operation failed
            }
        }

        public void Start()
        {
            try
            {
                // ✅ INICIALIZAR PROPIEDADES CACHEADAS
                foreach (var transport in Cuerpos.OfType<simTransporte>())
                {
                    transport.UpdateCachedProperties();
                }
                
                foreach (var curve in Cuerpos.OfType<simCurve>())
                {
                    // ✅ CORREGIDO: Usar método público para actualizar velocidad
                    curve.SetSpeed(curve.Speed); // ✅ SIMPLIFICADO: Reinicializar velocidad
                }
                
                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++;
                
                // ✅ 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
                    }
                }

                // ✅ NUEVO: Detener motores de botellas que no están en contacto con elementos
                // Esto se ejecuta cada 10 frames para eficiencia
                if (_frameCount % 10 == 0)
                {
                    StopMotorsForBottlesNotInContact();
                }

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

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

                // ✅ 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) return;
            
            try
            {
                // ✅ SIMPLIFICADO - eliminar lógica de masa especial
                UnregisterObjectHandle(Objeto); // ✅ NUEVO
                
                // ✅ NUEVO - Limpiar dimensiones almacenadas en osBase
                CtrEditor.ObjetosSim.osBase.ClearStoredDimensions(Objeto);
                
                // ✅ NUEVO - Agregar a cola de eliminación diferida
                lock (_removalLock)
                {
                    _pendingRemovals.Enqueue(Objeto);
                }
                
                // ✅ REMOVER de la lista inmediatamente para evitar referencias colgantes
                Cuerpos.Remove(Objeto);

                if (Is3DUpdateEnabled)
                {
                    Visualization3DManager?.SynchronizeWorld();
                }
                
                System.Diagnostics.Debug.WriteLine($"[Remove] ✅ Objeto marcado para eliminación diferida: {Objeto.GetType().Name}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[Remove] ❌ Error marcando objeto {Objeto?.GetType().Name}: {ex.Message}");
                
                // ✅ CRÍTICO: Siempre remover de la lista incluso si falló
                Cuerpos.Remove(Objeto);
            }
        }
       

        public simBotella AddCircle(float diameter, Vector2 position, float mass)
        {
            var botella = new simBotella(simulation, _deferredActions, diameter, position, mass);
            Cuerpos.Add(botella);
            RegisterObjectHandle(botella); // ✅ NUEVO

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

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

        public simBarrera AddBarrera(float width, float height, Vector2 position, float angle, bool detectarCuello)
        {
            var barrera = new simBarrera(simulation, _deferredActions, width, height, position, angle, detectarCuello);
            Cuerpos.Add(barrera);
            RegisterObjectHandle(barrera); // ✅ NUEVO
            
            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);
            Cuerpos.Add(curve);
            RegisterObjectHandle(curve); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return curve;
        }





        /// <summary>
        /// ✅ NUEVO: Detiene motores de botellas que no están en contacto con elementos
        /// </summary>
        private void StopMotorsForBottlesNotInContact()
        {
            try
            {
                var allBottles = Cuerpos.OfType<simBotella>().ToList();
                
                foreach (var bottle in allBottles)
                {
                    if (bottle != null && bottle.HasMotor && bottle.IsOnElement)
                    {
                        // ✅ VERIFICAR SI LA BOTELLA ESTÁ REALMENTE EN CONTACTO CON ALGÚN ELEMENTO
                        bool isInContact = false;
                        
                        // Verificar contacto con transportes
                        foreach (var transport in Cuerpos.OfType<simTransporte>())
                        {
                            if (IsBottleInContactWithTransport(bottle, transport))
                            {
                                isInContact = true;
                                break;
                            }
                        }
                        
                        // Verificar contacto con curvas
                        if (!isInContact)
                        {
                            foreach (var curve in Cuerpos.OfType<simCurve>())
                            {
                                if (IsBottleInContactWithCurve(bottle, curve))
                                {
                                    isInContact = true;
                                    break;
                                }
                            }
                        }
                        
                        // ✅ DETENER MOTOR SI NO ESTÁ EN CONTACTO
                        if (!isInContact)
                        {
                            bottle.StopMotor();
                            System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ⏹️ Detenido: {bottle.BodyHandle}");
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ❌ ERROR: {ex.Message}");
            }
        }

        /// <summary>
        /// ✅ NUEVO: Verifica si una botella está en contacto con un transporte
        /// </summary>
        private bool IsBottleInContactWithTransport(simBotella bottle, simTransporte transport)
        {
            try
            {
                var bottlePos = bottle.GetPosition();
                var transportPos = transport.GetPosition();
                var distance = Vector2.Distance(
                    new Vector2(bottlePos.X, bottlePos.Y),
                    new Vector2(transportPos.X, transportPos.Y)
                );
                
                // Verificar si está dentro del área del transporte + tolerancia
                var maxDistance = Math.Max(transport.Width, transport.Height) / 2f + bottle.Radius + 0.1f;
                return distance <= maxDistance;
            }
            catch
            {
                return false;
            }
        }

        /// <summary>
        /// ✅ NUEVO: Verifica si una botella está en contacto con una curva
        /// </summary>
        private bool IsBottleInContactWithCurve(simBotella bottle, simCurve curve)
        {
            try
            {
                var bottlePos = bottle.GetPosition();
                var curveCenter = curve.CurveCenter; // ✅ NUEVO: Usar centro real
                var distance = Vector2.Distance(
                    new Vector2(bottlePos.X, bottlePos.Y),
                    new Vector2(curveCenter.X, curveCenter.Y)
                );
                
                // Verificar si está dentro del área de la curva + tolerancia
                var maxDistance = curve.OuterRadius + bottle.Radius + 0.1f;
                return distance <= maxDistance;
            }
            catch
            {
                return false;
            }
        }


        /// <summary>
        /// ✅ NUEVO: Calcula la dirección tangencial específica basada en la posición de la botella
        /// </summary>
        public Vector3 CalculateCurveDirectionFromBottlePosition(simCurve curve, simBotella bottle)
        {
            try
            {
                // ✅ NUEVO: Usar el centro real almacenado de la curva
                var curveCenter = curve.CurveCenter;
                var bottlePosition = bottle.GetPosition();
                
                // Calcular el vector desde el centro de la curva hasta la botella (en el plano XY)
                var radiusVector = new Vector3(bottlePosition.X - curveCenter.X, bottlePosition.Y - curveCenter.Y, 0f);
                var radius = radiusVector.Length();
                
                if (radius < 0.001f)
                {
                    System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ⚠️ Botella muy cerca del centro de la curva");
                    return Vector3.UnitX; // Fallback
                }
                
                // Normalizar el vector radial
                var normalizedRadius = radiusVector / radius;
                
                // Calcular la dirección tangencial (perpendicular al radio)
                // En coordenadas 2D: si r = (x, y), entonces t = (-y, x) es tangente
                var tangentDirection = new Vector3(-normalizedRadius.Y, normalizedRadius.X, 0f);
                
                // Verificar que la dirección tangencial apunte en el sentido correcto según la velocidad de la curva
                if (curve.Speed < 0)
                {
                    tangentDirection = -tangentDirection;
                }
                
                System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] 📐 Dirección calculada:");
                System.Diagnostics.Debug.WriteLine($"   Centro curva: {curveCenter}");
                System.Diagnostics.Debug.WriteLine($"   Posición botella: {bottlePosition}");
                System.Diagnostics.Debug.WriteLine($"   Radio: {radius:F3}");
                System.Diagnostics.Debug.WriteLine($"   Vector radial: {normalizedRadius}");
                System.Diagnostics.Debug.WriteLine($"   Dirección tangencial: {tangentDirection} (Longitud: {tangentDirection.Length():F3})");
                
                return tangentDirection;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ❌ ERROR: {ex.Message}");
                return Vector3.UnitX; // Fallback
            }
        }





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

        /// <summary>
        /// Registra un contacto entre un descarte y una botella para marcar eliminación
        /// </summary>
        /// <param name="descarte">Descarte que detecta la botella</param>
        /// <param name="botella">Botella que debe ser eliminada</param>
        public void RegisterDescarteContact(simDescarte descarte, simBotella botella)
        {
            if (descarte != null && botella != null)
            {
                lock (_contactsLock)
                {
                    // Marcar la botella para eliminación
                    _botellasParaEliminar.Add(botella);
                    botella.Descartar = true;

                    // También registrar el contacto para la lista del descarte
                    if (!_descarteContacts.ContainsKey(descarte))
                    {
                        _descarteContacts[descarte] = new List<simBotella>();
                    }

                    if (!_descarteContacts[descarte].Contains(botella))
                    {
                        _descarteContacts[descarte].Add(botella);
                    }
                }
            }
        }



        /// <summary>
        /// Procesa todas las barreras usando detección geométrica pura
        /// Ya no depende de contactos físicos de BEPU - usa cálculo geométrico para ambos flags
        /// </summary>
        private void ProcessBarreraContacts()
        {
            try
            {
                // Obtener todas las barreras y botellas
                var barreras = Cuerpos.OfType<simBarrera>().ToList();
                var botellas = Cuerpos.OfType<simBotella>().ToList();


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

                        // Resetear flags
                        barrera.LuzCortada = false;
                        barrera.LuzCortadaNeck = false;
                        barrera.Distancia = float.MaxValue;

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

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

                // Limpiar contactos ya que no los usamos más
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                }
            }
            catch (Exception ex)
            {
                // Critical error in ProcessBarrera - continue
            }
        }

        /// <summary>
        /// Calcula detección geométrica pura para una barrera específica contra todas las botellas
        /// Usa el mismo sistema geométrico para ambos flags: LuzCortada (radio completo) y LuzCortadaNeck (radio/2)
        /// </summary>
        /// <param name="barrera">Barrera que está detectando</param>
        /// <param name="todasLasBotellas">Lista de TODAS las botellas en la simulación</param>
        private void CalculateBarreraDetectionGeometric(simBarrera barrera, List<simBotella> todasLasBotellas)
        {
            try
            {
                // Validaciones de seguridad
                if (barrera == null || todasLasBotellas == null || simulation?.Bodies == null)
                    return;

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

                var barrierBody = simulation.Bodies[barrera.BodyHandle];
                var barrierPosition = barrierBody.Pose.Position;
                var barrierOrientation = barrierBody.Pose.Orientation;

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

                // Obtener las dimensiones de la barrera de forma segura
                var halfWidth = Math.Max(barrera.Width / 2f, 0.01f); // Mínimo 1cm

                float minDistance = float.MaxValue;
                var botellasDetectadas = new List<simBotella>();

                // Procesar TODAS las botellas (no solo las en contacto físico)
                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;
                        }

                        // CLAVE: Crear la línea del haz en el plano XY a la altura del centro de la botella
                        var bottleZ = botellaPosition.Z; // Altura actual del centro de la botella

                        // Puntos de la línea del haz en el plano XY a la altura de la botella
                        var localStart = new Vector3(-halfWidth, 0, 0);
                        var localEnd = new Vector3(halfWidth, 0, 0);

                        // Transformar a coordenadas mundiales pero en el plano Z de la botella
                        var worldStartXY = barrierPosition + Vector3.Transform(localStart, barrierOrientation);
                        var worldEndXY = barrierPosition + Vector3.Transform(localEnd, barrierOrientation);

                        // Ajustar Z para que esté en el plano de la botella
                        worldStartXY = new Vector3(worldStartXY.X, worldStartXY.Y, bottleZ);
                        worldEndXY = new Vector3(worldEndXY.X, worldEndXY.Y, bottleZ);

                        // Calcular distancia desde el centro de la botella a la línea del haz
                        var closestPoint = ProjectPointOntoLine(botellaPosition, worldStartXY, worldEndXY);
                        var distance = Vector3.Distance(closestPoint, botellaPosition);

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

                        // Actualizar distancia mínima
                        if (distance < minDistance)
                        {
                            minDistance = distance;
                        }

                        // NUEVO: Verificar LuzCortada usando radio completo
                        if (distance <= botella.Radius)
                        {
                            barrera.LuzCortada = true;
                            botellasDetectadas.Add(botella);
                        }

                        // Verificar detección de cuello usando radio/2 (como antes)
                        if (barrera.DetectNeck && botella.Radius > 0)
                        {
                            var neckRadius = botella.Radius / 2f;
                            if (distance <= neckRadius)
                            {
                                barrera.LuzCortadaNeck = true;
                                // No hacer break aquí - queremos procesar todas las botellas para LuzCortada
                            }
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error processing bottle - continue
                    }
                }

                // Asignar resultados de forma segura
                barrera.Distancia = minDistance == float.MaxValue ? 0f : minDistance;

                // Actualizar lista de botellas detectadas
                if (barrera.ListSimBotellaContact != null)
                {
                    barrera.ListSimBotellaContact.AddRange(botellasDetectadas);
                }
            }
            catch (Exception ex)
            {
                // En caso de error, asignar valores seguros
                if (barrera != null)
                {
                    barrera.Distancia = float.MaxValue;
                    barrera.LuzCortada = false;
                    barrera.LuzCortadaNeck = false;
                }
            }
        }

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

                // Limpiar contactos ya que no los usamos más
                lock (_contactsLock)
                {
                    _descarteContacts.Clear();
                }
            }
            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)
                {
                    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;
        }




    }
}