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()
        {
            // Solo intentar remover si realmente hemos creado un cuerpo antes
            if (_bodyCreated && _simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                _simulation.Bodies.Remove(BodyHandle);
                _bodyCreated = false; // Marcar como no creado después de remover
            }
        }

        /// <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;
        
        // ✅ NUEVA REFERENCIA - para limpiar motors
        private SimulationManagerBEPU _simulationManager;

        public simTransporte(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0, SimulationManagerBEPU simulationManager = null)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _simulationManager = simulationManager; // ✅ NUEVA REFERENCIA
            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>
        /// ✅ MODIFICADO: RemoverBody que limpia motors conectados antes de eliminar el body
        /// </summary>
        public new void RemoverBody()
        {
            if (_bodyCreated && _simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                // ✅ CRÍTICO: Limpiar todos los motors conectados a este transporte ANTES de eliminar el body
                RemoveConnectedMotors();
                
                _simulation.Bodies.Remove(BodyHandle);
                _bodyCreated = false;
            }
        }
        
        /// <summary>
        /// ✅ NUEVO: Limpia todos los motors conectados a este transporte
        /// </summary>
        private void RemoveConnectedMotors()
        {
            try
            {
                // ✅ USAR REFERENCIA DIRECTA al SimulationManager
                if (_simulationManager != null)
                {
                    _simulationManager.RemoveMotorsConnectedToBody(BodyHandle);
                }
                
                // Limpiar la lista local de botellas
                BottlesOnTransport.Clear();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simTransporte] Error removing connected motors: {ex.Message}");
            }
        }

        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
        
        // ✅ NUEVAS PROPIEDADES - gestión de motores
        public simTransporte CurrentTransport { get; set; }
        public ConstraintHandle CurrentMotor { get; set; }
        public bool HasActiveMotor => CurrentMotor.Value != 0;
        
        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);
        }

        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
        }

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

        // ✅ NUEVOS MÉTODOS - gestión de estado
        public void AssignToTransport(simTransporte transport, ConstraintHandle motor)
        {
            CurrentTransport = transport;
            CurrentMotor = motor;
            if (transport.isBrake)
            {
                isOnBrakeTransport = true;
                CurrentBrakeTransport = transport;
            }
        }
        
        public void RemoveFromTransport()
        {
            CurrentTransport = null;
            CurrentMotor = default;
                    isOnBrakeTransport = false;
                    CurrentBrakeTransport = null;
                }

    }

    /// <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;
        internal List<BodyHandle> _triangleBodyHandles; // Lista de todos los triángulos que componen la curva
        private List<List<Vector3>> _originalTriangles; // ✅ NUEVO: Triángulos originales para visualización debug

        // ✅ SIMPLIFICADO: Solo necesitamos velocidad en m/s, no sistema complejo de direcciones
        public float SpeedMetersPerSecond { get; private set; }
        public List<simBotella> BottlesOnCurve { get; private set; } = new List<simBotella>();
        
        // ✅ NUEVO EVENTO - para actualización de motores
        public event Action<simCurve> OnSpeedChanged;
        
        // ✅ NUEVA REFERENCIA - para limpiar motors
        private SimulationManagerBEPU _simulationManager;

        // ✅ NUEVO: Direcciones tangenciales precalculadas para cada triángulo
        internal Dictionary<BodyHandle, Vector3> _triangleDirections = new Dictionary<BodyHandle, Vector3>();

        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

        /// <summary>
        /// ✅ NUEVO: Expone los triángulos originales para visualización debug
        /// </summary>
        public List<List<Vector3>> GetOriginalTriangles() => _originalTriangles ?? new List<List<Vector3>>();

        public simCurve(Simulation simulation, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0, SimulationManagerBEPU simulationManager = null)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _simulationManager = simulationManager; // ✅ NUEVA REFERENCIA
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);
            _triangleBodyHandles = new List<BodyHandle>();
            _originalTriangles = new List<List<Vector3>>(); // ✅ NUEVO: Inicializar lista de triángulos

            // Crear la curva con los ángulos que definen el sector
            Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0);
        }

        // ✅ MODIFICAR MÉTODO EXISTENTE - disparar evento
        public void SetSpeed(float speed)
        {
            Speed = -speed;
            SpeedMetersPerSecond = Math.Abs(Speed) / simBase.SPEED_CONVERSION_FACTOR;
            OnSpeedChanged?.Invoke(this);
        }

        public void UpdateCurve(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // Recrear la curva con nuevos parámetros manteniendo posición actual
            var currentPosition = GetPosition();
            Create(currentPosition);
        }

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

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var curveSize = outerRadius * 2f;
            var zPosition = zAltura_Curve / 2f + zPos_Curve;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
            
            Create(bepuCenter);
        }

        /// <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
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
            
            Create(bepuCenter);
        }

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

        public new void RemoverBody()
        {
            // ✅ CRÍTICO: Limpiar todos los motors conectados a esta curva ANTES de eliminar los bodies
            RemoveConnectedMotors();
            
            // ✅ NUEVO: Limpiar cachés de direcciones
            _triangleDirections.Clear();
            
            // Remover todos los triángulos de la curva
            if (_triangleBodyHandles != null && _simulation != null)
            {
                foreach (var triangleHandle in _triangleBodyHandles)
                {
                    if (_simulation.Bodies.BodyExists(triangleHandle))
                    {
                        _simulation.Bodies.Remove(triangleHandle);
                    }
                }
                _triangleBodyHandles.Clear();
            }

            // ✅ NUEVO: Limpiar también los triángulos originales
            if (_originalTriangles != null)
            {
                _originalTriangles.Clear();
            }

            // Remover el cuerpo principal (si existe)
            base.RemoverBody();
        }
        
        /// <summary>
        /// ✅ NUEVO: Limpia todos los motors conectados a esta curva
        /// </summary>
        private void RemoveConnectedMotors()
        {
            try
            {
                // ✅ USAR REFERENCIA DIRECTA al SimulationManager
                if (_simulationManager != null)
                {
                    // Limpiar motors del cuerpo principal
                    _simulationManager.RemoveMotorsConnectedToBody(BodyHandle);
                    
                    // Limpiar motors de todos los triángulos
                    if (_triangleBodyHandles != null)
                    {
                        foreach (var triangleHandle in _triangleBodyHandles)
                        {
                            _simulationManager.RemoveMotorsConnectedToBody(triangleHandle);
                        }
                    }
                }
                
                // Limpiar la lista local de botellas
                BottlesOnCurve.Clear();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simCurve] Error removing connected motors: {ex.Message}");
            }
        }

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

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            // 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)
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

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

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

            // Crear triángulos para la curva - los ángulos ya definen la forma correcta
            var triangles = CreateTriangulatedCurve(_innerRadius, _outerRadius, -_startAngle, -_endAngle);

            // ✅ NUEVO: Almacenar triángulos originales para visualización debug
            _originalTriangles = new List<List<Vector3>>(triangles);

            foreach (var triangle in triangles)
            {
                CreateTriangleBody(triangle, position);
            }

            // Crear un cuerpo principal invisible para referencia de posición
            CreateMainBody(position);
        }

        private void CreateMainBody(Vector3 position)
        {
            // Crear un cuerpo principal muy pequeño e invisible solo para referencia
            var smallBox = new Box(0.01f, 0.01f, 0.01f);
            var shapeIndex = _simulation.Shapes.Add(smallBox);

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(position), // Sin rotación - la forma ya está definida por los ángulos
                new CollidableDescription(shapeIndex, 0f), // Sin speculative margin
                new BodyActivityDescription(0.01f)
            );

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

        private void CreateTriangleBody(List<Vector3> triangle, Vector3 basePosition)
        {
            if (triangle.Count != 3)
                return;

            try
            {
                // Calcular centro y dimensiones del triángulo
                var center = (triangle[0] + triangle[1] + triangle[2]) / 3f;
                var worldCenter = center + basePosition;

                                    // ✅ NUEVO: Calcular dirección tangencial con compensación centrípeta
                    // Usamos la posición del centro del triángulo para calcular la tangente
                    var radiusVector = new Vector2(center.X, center.Y);
                    if (radiusVector.Length() > 0.001f)
                    {
                        // Vector tangente (perpendicular al radio) en el plano XY
                        var tangent = new Vector3(-radiusVector.Y, radiusVector.X, 0);
                        tangent = Vector3.Normalize(tangent);
                        
                        // ✅ COMPENSACIÓN CENTRÍPETA: Inclinar la dirección hacia el centro
                        var radiusVector3D = new Vector3(radiusVector.X, radiusVector.Y, 0);
                        var toCenter = -Vector3.Normalize(radiusVector3D); // Vector hacia el centro
                        
                        // Mezclar tangente con componente centrípeta (10% hacia el centro)
                        const float CENTRIPETAL_FACTOR = 0.1f; // 10% de fuerza hacia el centro
                        var adjustedDirection = Vector3.Normalize(tangent + toCenter * CENTRIPETAL_FACTOR);
                        
                        // Ajustar dirección según la velocidad (signo)
                        if (Speed < 0)
                            adjustedDirection = -adjustedDirection;
                        
                        // Almacenar dirección ajustada antes de crear el body
                        var triangleDirection = adjustedDirection;
                    
                    // Calcular dimensiones aproximadas del triángulo para crear una caja plana
                    float minX = Math.Min(Math.Min(triangle[0].X, triangle[1].X), triangle[2].X);
                    float maxX = Math.Max(Math.Max(triangle[0].X, triangle[1].X), triangle[2].X);
                    float minY = Math.Min(Math.Min(triangle[0].Y, triangle[1].Y), triangle[2].Y);
                    float maxY = Math.Max(Math.Max(triangle[0].Y, triangle[1].Y), triangle[2].Y);

                    float width = Math.Max(maxX - minX, 0.01f);
                    float height = Math.Max(maxY - minY, 0.01f);
                    float thickness = zAltura_Curve; // Altura muy pequeña para triángulos planos

                    // Crear una caja plana que aproxime el triángulo
                    var box = new Box(width, height, thickness);
                    var shapeIndex = _simulation.Shapes.Add(box);

                    // Crear como cuerpo estático sólido (NO sensor)
                    var activityDescription = new BodyActivityDescription(0.01f);

                    var bodyDescription = BodyDescription.CreateKinematic(
                        new RigidPose(worldCenter),
                        new CollidableDescription(shapeIndex, 0.001f), // Speculative margin normal
                        activityDescription
                    );

                    var triangleHandle = _simulation.Bodies.Add(bodyDescription);
                    _triangleBodyHandles.Add(triangleHandle);
                    
                    // ✅ NUEVO: Almacenar la dirección tangencial para este triángulo
                    _triangleDirections[triangleHandle] = triangleDirection;
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simCurve] Error creating triangle: {ex.Message}");
            }
        }

        private const float SegmentationFactor = 32f / 3f;
        private const int MinSegments = 8;
        private const int MaxSegments = 64;

        /// <summary>
        /// Crea triángulos para representar una curva/arco en la simulación BEPU.
        /// IMPORTANTE: Los parámetros startAngle y endAngle ya están convertidos a radianes BEPU internamente,
        /// pero además se invierte la dirección del arco para mantener coherencia visual con WPF.
        /// </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 (ya convertido desde grados WPF)</param>
        /// <param name="endAngle">Ángulo final en radianes BEPU (ya convertido desde grados WPF)</param>
        /// <returns>Lista de triángulos que forman el arco</returns>
        private List<List<Vector3>> CreateTriangulatedCurve(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            var triangles = new List<List<Vector3>>();

            // Calcular número de segmentos basado en el tamaño del arco
            float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f);
            int segments = (int)(arcLength * SegmentationFactor);
            segments = Math.Max(MinSegments, Math.Min(segments, MaxSegments));

            // ✅ CRÍTICO: Inversión de dirección del arco para conversión WPF ↔ BEPU
            // 
            // Los ángulos individuales ya están convertidos (WPF grados → BEPU radianes con signo invertido),
            // pero también necesitamos invertir la DIRECCIÓN del arco:
            //
            // • WPF: startAngle → endAngle (sentido horario, Y+ hacia abajo)
            // • BEPU: -endAngle → -startAngle (sentido antihorario, Y+ hacia arriba)
            //
            // Ejemplo: WPF arco de 0° a 90° → BEPU arco de -90° a 0° (equivalente visual)
            float fromAngle = -endAngle;   // Empezar desde el ángulo final negativo
            float toAngle = -startAngle;   // Terminar en el ángulo inicial negativo
            float angleStep = (toAngle - fromAngle) / segments;

            // Generar vértices para los arcos interior y exterior
            var innerPoints = new Vector3[segments + 1];
            var outerPoints = new Vector3[segments + 1];

            for (int i = 0; i <= segments; i++)
            {
                float angle = fromAngle + i * angleStep;
                float cosAngle = (float)Math.Cos(angle);
                float sinAngle = (float)Math.Sin(angle);

                // Triángulos planos en el plano XY (Z = 0 relativo)
                innerPoints[i] = new Vector3(innerRadius * cosAngle, innerRadius * sinAngle, 0);
                outerPoints[i] = new Vector3(outerRadius * cosAngle, outerRadius * sinAngle, 0);
            }

            // Crear triángulos
            for (int i = 0; i < segments; i++)
            {
                // Primer triángulo del sector
                var upperTriangle = new List<Vector3>
                {
                    innerPoints[i],
                    outerPoints[i],
                    outerPoints[i + 1]
                };
                triangles.Add(upperTriangle);

                // Segundo triángulo del sector
                var lowerTriangle = new List<Vector3>
                {
                    innerPoints[i],
                    outerPoints[i + 1],
                    innerPoints[i + 1]
                };
                triangles.Add(lowerTriangle);
            }

            return triangles;
        }



        /// <summary>
        /// Calcula la dirección tangencial para aplicar fuerzas de curva.
        /// NOTA: Este cálculo es dinámico (basado en posición actual) y no se ve afectado 
        /// por la inversión de dirección del arco en CreateTriangulatedCurve.
        /// </summary>
        public Vector3 GetTangentialDirection(Vector3 bottlePosition)
        {
            try
            {
                var curvePosition = GetPosition();
                var centerToBottle = new Vector2(bottlePosition.X - curvePosition.X, bottlePosition.Y - curvePosition.Y);

                if (centerToBottle.Length() < 0.001f)
                    return Vector3.Zero;

                // Vector tangente (perpendicular al radio) en el plano XY
                var tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0);

                // Normalizar
                if (tangent.Length() > 0.001f)
                {
                    tangent = Vector3.Normalize(tangent);
                }

                // Ajustar dirección según la velocidad (signo)
                if (Speed < 0)
                    tangent = -tangent;

                return tangent;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simCurve] Error calculating tangential direction: {ex.Message}");
                return Vector3.Zero;
            }
        }

        // ✅ MÉTODO MEJORADO - versión optimizada
        public Vector3 GetCachedTangentialDirection(Vector3 bottlePosition)
        {
            var direction = GetTangentialDirection(bottlePosition);
            return Speed < 0 ? -direction : direction;
        }

        /// <summary>
        /// ✅ NUEVO - Obtiene la dirección tangencial fija de un triángulo específico
        /// </summary>
        public Vector3 GetTriangleDirection(BodyHandle triangleHandle)
        {
            return _triangleDirections.GetValueOrDefault(triangleHandle, Vector3.Zero);
        }
    }

    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>
        {
            // ✅ SIMPLIFICADO - configuración básica de materiales físicos
            pairMaterial = new PairMaterialProperties
            {
                FrictionCoefficient = 0.6f,      // Fricción moderada por defecto
                MaximumRecoveryVelocity = 2f,    // Velocidad máxima de recuperación
                SpringSettings = new SpringSettings(60, 4)  // Rigidez y amortiguamiento estándar
            };

            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;

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

                // ✅ CONSERVAR - 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 para descartes
                }

                // ✅ PROCESAR CREACIÓN DE MOTORES para transportes y curvas
                var transportA = GetTransportFromCollidable(pair.A);
                var transportB = GetTransportFromCollidable(pair.B);
                (simCurve curveA, Vector3 directionA) = GetCurveAndDirectionFromCollidable(pair.A);
                (simCurve curveB, Vector3 directionB) = GetCurveAndDirectionFromCollidable(pair.B);

                // ✅ VERIFICAR SI YA SE CREÓ MOTOR PARA EVITAR DUPLICADOS
                BodyHandle? bottleHandle = null;
                BodyHandle? elementHandle = null;
                
                if (botella != null && (transportA != null || transportB != null))
                {
                    var transport = transportA ?? transportB;
                    bottleHandle = botella.BodyHandle;
                    elementHandle = transport.BodyHandle;
                    
                    var pairKey = (bottleHandle.Value, elementHandle.Value);
                    if (!_simulationManager._motorsCreated.Contains(pairKey))
                    {
                        _simulationManager.TryCreateTransportMotor(botella, transport);
                        _simulationManager._motorsCreated.Add(pairKey);
                                                    System.Diagnostics.Debug.WriteLine($"[Manifold] Motor transporte: {bottleHandle} → {elementHandle}");
                    }
                    
                    // Fricción alta para transportes (SIEMPRE aplicar para colisión física)
                    pairMaterial.FrictionCoefficient = 0.9f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // ✅ SIMPLIFICADO: Si hay contacto botella-triángulo de curva, crear motor con dirección específica
                else if (botella != null && (curveA != null || curveB != null))
                {
                    var curve = curveA ?? curveB;
                    var direction = curveA != null ? directionA : directionB;
                    var triangleHandle = curveA != null ? pair.A.BodyHandle : pair.B.BodyHandle;
                    
                    bottleHandle = botella.BodyHandle;
                    elementHandle = triangleHandle;
                    
                                        // ✅ SIMPLIFICADO: Crear motor usando sistema natural de colisiones de BEPU
                    var pairKey = (bottleHandle.Value, elementHandle.Value);
                    if (!_simulationManager._motorsCreated.Contains(pairKey))
                    {
                        _simulationManager.TryCreateCurveMotor(botella, curve, direction, triangleHandle);
                        _simulationManager._motorsCreated.Add(pairKey);
                        System.Diagnostics.Debug.WriteLine($"[Manifold] Motor curva: {bottleHandle} → {triangleHandle}");
                    }
                    
                    // Fricción alta para curvas (SIEMPRE aplicar para colisión física)
                    pairMaterial.FrictionCoefficient = 0.9f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                    
                }
                // Solo ajustes básicos de fricción para otras botellas
                else if (botella != null)
                {
                    // Fricción alta para mayor estabilidad de botellas
                    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;

                    // ✅ NUEVO: Buscar específicamente en los triángulos de las curvas
                    return _simulationManager.GetCurveFromTriangleBodyHandle(bodyHandle);
                }
            }
            return null;
        }
        
        /// <summary>
        /// ✅ NUEVO: Obtiene curva y dirección específica del triángulo
        /// </summary>
        private (simCurve curve, Vector3 direction) GetCurveAndDirectionFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;

                    // Buscar curva que contenga este triángulo
                    var curve = _simulationManager.GetCurveFromTriangleBodyHandle(bodyHandle);
                    if (curve != null)
                    {
                        // Obtener dirección específica del triángulo
                        var direction = curve.GetTriangleDirection(bodyHandle);
                        return (curve, direction);
                    }
                }
            }
            return (null, Vector3.Zero);
        }

        /// <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 MotorManager
    {
        private SimulationManagerBEPU _simulationManager;
        private Dictionary<BodyHandle, ConstraintHandle> _activeMotors;
        private Dictionary<ConstraintHandle, simBotella> _motorToBottle;
        
        public MotorManager(SimulationManagerBEPU simulationManager)
        {
            _simulationManager = simulationManager;
            _activeMotors = new Dictionary<BodyHandle, ConstraintHandle>();
            _motorToBottle = new Dictionary<ConstraintHandle, simBotella>();
        }
        
        public void CreateTransportMotor(simBotella bottle, simTransporte transport)
        {
            try
            {
                // ✅ VALIDACIONES CRÍTICAS
                if (bottle == null || transport == null || _simulationManager?.simulation == null)
                    return;
                    
                // Validar que los BodyHandle existan
                if (!_simulationManager.simulation.Bodies.BodyExists(bottle.BodyHandle) ||
                    !_simulationManager.simulation.Bodies.BodyExists(transport.BodyHandle))
                    return;
                
                // Asegurar que DirectionVector esté inicializado
                transport.UpdateCachedProperties();
                
                // Validar que DirectionVector no sea cero
                if (transport.DirectionVector.Length() < 0.001f)
                    return;
                    
                                // ✅ CORREGIDO - LocalAxis debe estar en coordenadas locales del transporte
                // Para transportes, el eje local de movimiento es siempre UnitX (eje largo)
                var localAxis = Vector3.UnitX; // Siempre UnitX en coordenadas locales del transporte
                
                var motor = new LinearAxisMotor() 
                { 
                    LocalOffsetA = Vector3.Zero,
                    LocalOffsetB = Vector3.Zero,
                    LocalAxis = localAxis, // ✅ Usar eje local, no mundial
                    TargetVelocity = transport.SpeedMetersPerSecond,
                    Settings = new MotorSettings(Math.Max(bottle.Mass * 30f, 10f), 5f)
                };
                
                // 🔍 DEBUG: Comparar eje local vs mundial
                System.Diagnostics.Debug.WriteLine($"[Motor] Transport angle: {transport.GetRotationZ()}°");
                System.Diagnostics.Debug.WriteLine($"[Motor] World DirectionVector: {transport.DirectionVector}");
                System.Diagnostics.Debug.WriteLine($"[Motor] Local Axis: {localAxis}");
                System.Diagnostics.Debug.WriteLine($"[Motor] Target Velocity: {transport.SpeedMetersPerSecond}");
                
                var motorHandle = _simulationManager.simulation.Solver.Add(
                    transport.BodyHandle, 
                    bottle.BodyHandle, 
                    motor
                );
                
                _activeMotors[bottle.BodyHandle] = motorHandle;
                _motorToBottle[motorHandle] = bottle;
                bottle.AssignToTransport(transport, motorHandle);
                transport.BottlesOnTransport.Add(bottle);
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error creating transport motor: {ex.Message}");
            }
        }
        
        public void CreateCurveMotor(simBotella bottle, simCurve curve)
        {
            try
            {
                // ✅ VALIDACIONES CRÍTICAS
                if (bottle == null || curve == null || _simulationManager?.simulation == null)
                    return;
                    
                // Validar que los BodyHandle existan
                if (!_simulationManager.simulation.Bodies.BodyExists(bottle.BodyHandle) ||
                    !_simulationManager.simulation.Bodies.BodyExists(curve.BodyHandle))
                    return;
                
                var direction = curve.GetCachedTangentialDirection(bottle.GetPosition());
                
                // Validar que la dirección no sea cero
                if (direction.Length() < 0.001f)
                    return;
                
                // ✅ CONVERTIR DIRECCIÓN MUNDIAL A COORDENADAS LOCALES DE LA CURVA
                Vector3 localAxisDirection;
                
                if (_simulationManager.simulation.Bodies.BodyExists(curve.BodyHandle))
                {
                    var curveBody = _simulationManager.simulation.Bodies[curve.BodyHandle];
                    var inverseCurveOrientation = Quaternion.Conjugate(curveBody.Pose.Orientation);
                    localAxisDirection = Vector3.Transform(direction, inverseCurveOrientation);
                }
                else
                {
                    // Fallback: usar dirección mundial si no se puede obtener orientación
                    localAxisDirection = direction;
                }
                
                var motor = new LinearAxisMotor() 
                { 
                    LocalOffsetA = Vector3.Zero,
                    LocalOffsetB = Vector3.Zero,
                    LocalAxis = localAxisDirection, // ✅ Usar dirección en coordenadas locales
                    TargetVelocity = curve.SpeedMetersPerSecond,
                    Settings = new MotorSettings(Math.Max(bottle.Mass * 20f, 8f), 4f)
                };
                
                var motorHandle = _simulationManager.simulation.Solver.Add(
                    curve.BodyHandle,  // ✅ USAR CUERPO DE LA CURVA
                    bottle.BodyHandle,
                    motor
                );
                
                _activeMotors[bottle.BodyHandle] = motorHandle;
                _motorToBottle[motorHandle] = bottle;
                bottle.CurrentMotor = motorHandle;
                curve.BottlesOnCurve.Add(bottle);
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error creating curve motor: {ex.Message}");
            }
        }
        
        public void RemoveMotor(simBotella bottle)
        {
            try
            {
                if (bottle == null || _simulationManager?.simulation == null)
                    return;
                    
                if (_activeMotors.TryGetValue(bottle.BodyHandle, out var motorHandle))
                {
                    // Validar que el solver existe antes de remover
                    if (_simulationManager.simulation.Solver != null)
                    {
                        _simulationManager.simulation.Solver.Remove(motorHandle);
                        System.Diagnostics.Debug.WriteLine($"[RemoveMotor] Motor eliminado: {motorHandle}");
                    }

                    _activeMotors.Remove(bottle.BodyHandle);
                    _motorToBottle.Remove(motorHandle);
                    
                    // ✅ NUEVO: Limpiar pares de motores creados para esta botella
                    var bottleHandle = bottle.BodyHandle;
                    var motorsToRemove = _simulationManager._motorsCreated
                        .Where(pair => pair.bottle == bottleHandle)
                        .ToList();
                    
                    foreach (var pair in motorsToRemove)
                    {
                        _simulationManager._motorsCreated.Remove(pair);
                        System.Diagnostics.Debug.WriteLine($"[MotorManager] Motor pair cleaned: {pair.bottle} - {pair.element}");
                    }
                    
                    // Limpiar referencias de forma segura
                    bottle.CurrentTransport?.BottlesOnTransport.Remove(bottle);
                    
                    // ✅ SIMPLIFICADO: Solo limpiar listas de botellas en curvas
                    var curves = _simulationManager.Cuerpos.OfType<simCurve>();
                    foreach (var curve in curves)
                    {
                        curve.BottlesOnCurve.Remove(bottle);
                    }
                    
                    bottle.RemoveFromTransport();
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error removing motor: {ex.Message}");
            }
        }
        
        public void UpdateTransportSpeed(simTransporte transport)
        {
            // TODO: Implementar actualización de velocidad cuando se resuelva la API correcta
            // Por ahora, recreamos los motores como alternativa
            foreach (var bottle in transport.BottlesOnTransport.ToList())
            {
                if (_activeMotors.ContainsKey(bottle.BodyHandle))
                {
                    RemoveMotor(bottle);
                    CreateTransportMotor(bottle, transport);
                }
            }
        }
        
        public void UpdateCurveSpeed(simCurve curve)
        {
            // ✅ Las curvas requieren recrear motores cuando cambia la velocidad
            // Esto es porque la dirección tangencial puede cambiar y es más eficiente recrear
            foreach (var bottle in curve.BottlesOnCurve.ToList())
            {
                if (_activeMotors.ContainsKey(bottle.BodyHandle))
                {
                    RemoveMotor(bottle);
                    CreateCurveMotor(bottle, curve);
                }
            }
        }
        
        /// <summary>
        /// ✅ NUEVO: Actualiza la dirección de un motor de curva existente para un nuevo triángulo
        /// </summary>
        public void UpdateCurveMotorDirection(simBotella bottle, simCurve curve, Vector3 newDirection, BodyHandle newTriangleHandle)
        {
            try
            {
                if (bottle == null || curve == null || _simulationManager?.simulation == null)
                    return;
                    
                if (!_activeMotors.ContainsKey(bottle.BodyHandle))
                    return; // No hay motor activo
                
                // Validar que la dirección sea válida
                if (newDirection.Length() < 0.001f)
                    return;
                
                // ✅ ESTRATEGIA: Recrear motor con nueva dirección (más eficiente que actualizar en BEPU)
                var oldMotorHandle = _activeMotors[bottle.BodyHandle];
                
                // Eliminar motor anterior sin limpiar pares silenciados (los conservamos)
                if (_simulationManager.simulation.Solver != null)
                {
                    _simulationManager.simulation.Solver.Remove(oldMotorHandle);
                                            // Motor anterior eliminado y recreado con nueva dirección
                }
                
                _activeMotors.Remove(bottle.BodyHandle);
                _motorToBottle.Remove(oldMotorHandle);
                
                // Crear nuevo motor con nueva dirección (reutilizar CreateCurveMotorWithDirection)
                CreateCurveMotorWithDirection(bottle, curve, newDirection);
                
                System.Diagnostics.Debug.WriteLine($"[UpdateCurveMotorDirection] Motor actualizado con nueva dirección para triángulo: {newTriangleHandle}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error updating curve motor direction: {ex.Message}");
            }
        }
        
        /// <summary>
        /// ✅ NUEVO - Crea motor de curva con dirección precalculada
        /// </summary>
        public void CreateCurveMotorWithDirection(simBotella bottle, simCurve curve, Vector3 direction)
        {
            try
            {
                // ✅ VALIDACIONES CRÍTICAS
                if (bottle == null || curve == null || _simulationManager?.simulation == null)
                    return;
                    
                // Validar que los BodyHandle existan
                if (!_simulationManager.simulation.Bodies.BodyExists(bottle.BodyHandle) ||
                    !_simulationManager.simulation.Bodies.BodyExists(curve.BodyHandle))
                    return;
                
                // Validar que la dirección no sea cero
                if (direction.Length() < 0.001f)
                    return;
                
                // ✅ CONVERTIR DIRECCIÓN MUNDIAL A COORDENADAS LOCALES DE LA CURVA
                // El cuerpo principal de la curva no tiene rotación, pero aún necesitamos coordenadas locales
                Vector3 localAxisDirection;
                
                if (_simulationManager.simulation.Bodies.BodyExists(curve.BodyHandle))
                {
                    var curveBody = _simulationManager.simulation.Bodies[curve.BodyHandle];
                    var inverseCurveOrientation = Quaternion.Conjugate(curveBody.Pose.Orientation);
                    localAxisDirection = Vector3.Transform(direction, inverseCurveOrientation);
                }
                else
                {
                    // Fallback: usar dirección mundial si no se puede obtener orientación
                    localAxisDirection = direction;
                }
                
                var motor = new LinearAxisMotor() 
                { 
                    LocalOffsetA = Vector3.Zero,
                    LocalOffsetB = Vector3.Zero,
                    LocalAxis = localAxisDirection, // ✅ Usar dirección en coordenadas locales
                    TargetVelocity = curve.SpeedMetersPerSecond,
                    Settings = new MotorSettings(Math.Max(bottle.Mass * 20f, 8f), 4f)
                };
                
                var motorHandle = _simulationManager.simulation.Solver.Add(
                    curve.BodyHandle,
                    bottle.BodyHandle,
                    motor
                );
                // Logging con coordenadas mundiales y locales para depuración
                System.Diagnostics.Debug.WriteLine($"[CreateCurveMotorWithDirection] Bottle: {bottle.BodyHandle} World: ({direction.X:F2}, {direction.Y:F2}) Local: ({localAxisDirection.X:F2}, {localAxisDirection.Y:F2}) Speed: {curve.SpeedMetersPerSecond}");

                _activeMotors[bottle.BodyHandle] = motorHandle;
                _motorToBottle[motorHandle] = bottle;
                bottle.CurrentMotor = motorHandle;
                curve.BottlesOnCurve.Add(bottle);
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error creating curve motor with direction: {ex.Message}");
            }
        }
        
        /// <summary>
        /// ✅ OPTIMIZADO: Elimina todos los motors conectados a un body específico de forma eficiente
        /// </summary>
        public void RemoveMotorsByBodyHandle(BodyHandle bodyHandle)
        {
            try
            {
                if (_simulationManager?.simulation?.Solver == null)
                    return;

                // ✅ OPTIMIZACIÓN: Solo procesar si hay motors activos
                if (_activeMotors.Count == 0)
                {
                    return; // No hay motors, salir inmediatamente
                }

                // ✅ OPTIMIZACIÓN: Identificar rápidamente el tipo de body
                var targetObject = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                if (targetObject == null)
                {
                    // ✅ Podría ser un triángulo de curva, buscar curva que lo contenga
                    var curve = _simulationManager.GetCurveFromTriangleBodyHandle(bodyHandle);
                    if (curve == null)
                    {
                        return; // No es un objeto relevante
                    }
                    targetObject = curve;
                }

                // Encontrar motors que deben eliminarse
                var motorsToRemove = new List<(BodyHandle bottleHandle, ConstraintHandle motorHandle)>();
                
                foreach (var kvp in _activeMotors)
                {
                    var bottleHandle = kvp.Key;
                    var motorHandle = kvp.Value;
                    
                    if (_motorToBottle.TryGetValue(motorHandle, out var bottle))
                    {
                        bool shouldRemove = false;
                        
                        // ✅ OPTIMIZADO: Verificación directa por tipo de objeto
                        switch (targetObject)
                        {
                            case simTransporte transport:
                                shouldRemove = bottle.CurrentTransport?.BodyHandle.Equals(bodyHandle) == true;
                                break;
                                
                            case simCurve curve:
                                // Verificar si la botella está en esta curva específica
                                shouldRemove = curve.BottlesOnCurve.Contains(bottle);
                                break;
                        }
                        
                        if (shouldRemove)
                        {
                            motorsToRemove.Add((bottleHandle, motorHandle));
                        }
                    }
                }
                
                // Eliminar motors encontrados
                foreach (var (bottleHandle, motorHandle) in motorsToRemove)
                {
                    try
                    {
                        _simulationManager.simulation.Solver.Remove(motorHandle);
                        _activeMotors.Remove(bottleHandle);
                        
                        if (_motorToBottle.TryGetValue(motorHandle, out var bottle))
                        {
                            bottle.RemoveFromTransport();
                            _motorToBottle.Remove(motorHandle);
                        }
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[MotorManager] Error removing specific motor for body {bodyHandle}: {ex.Message}");
                    }
                }
                
                // ✅ SOLO LOG SI SE ELIMINARON MOTORS
                if (motorsToRemove.Count > 0)
                {
                    System.Diagnostics.Debug.WriteLine($"[MotorManager] Removed {motorsToRemove.Count} motors connected to body {bodyHandle}");
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error in RemoveMotorsByBodyHandle: {ex.Message}");
            }
        }
        


        public void Clear()
        {
            try
            {
                if (_simulationManager?.simulation?.Solver != null)
                {
                    foreach (var motorHandle in _activeMotors.Values)
                    {
                        try
                        {
                            _simulationManager.simulation.Solver.Remove(motorHandle);
                        }
                        catch (Exception ex)
                        {
                            System.Diagnostics.Debug.WriteLine($"[MotorManager] Error removing motor during clear: {ex.Message}");
                        }
                    }
                }
                _activeMotors.Clear();
                _motorToBottle.Clear();
                
                // ✅ NUEVO: Limpiar cachés de direcciones de todas las curvas y pares silenciados
                if (_simulationManager?.Cuerpos != null)
                {
                    var curves = _simulationManager.Cuerpos.OfType<simCurve>();
                    foreach (var curve in curves)
                    {
                        curve._triangleDirections?.Clear();
                    }
                    
                    // También limpiar pares de motores creados
                    _simulationManager._motorsCreated?.Clear();
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[MotorManager] Error during clear: {ex.Message}");
            }
        }
    }

    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 - sistema de motores
        private MotorManager _motorManager;
        
        // ✅ SEPARADO - tabla para bloquear creación de motores (NO bloquea colisiones físicas)
        internal HashSet<(BodyHandle bottle, BodyHandle element)> _motorsCreated;
        
        // ✅ 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;
        
        // ✅ ELIMINAR COMPLETAMENTE
        // private Dictionary<simBotella, simTransporte> _transportContacts; // NO SE USA MÁS
        // private Dictionary<simBotella, simTransporte> _brakeTransportContacts; // NO SE USA MÁS
        
        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>
        /// ✅ NUEVO: Obtiene una simCurve si el BodyHandle pertenece a uno de sus triángulos
        /// </summary>
        public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle)
        {
            // Buscar en todas las curvas si alguna contiene este BodyHandle en sus triángulos
            var curves = Cuerpos.OfType<simCurve>();

            foreach (var curve in curves)
            {
                if (curve._triangleBodyHandles != null && curve._triangleBodyHandles.Contains(bodyHandle))
                {
                    return curve;
                }
            }

            // Si no se encuentra en triángulos, buscar en cuerpos principales como fallback
            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);
                    // Suscribirse a cambios de velocidad
                    transport.OnSpeedChanged += _motorManager.UpdateTransportSpeed;
                    // Calcular propiedades iniciales
                    transport.UpdateCachedProperties();
                    break;
                case simCurve curve:
                    _curveHandles.Add(curve.BodyHandle);
                    // También añadir todos sus triángulos
                    if (curve._triangleBodyHandles != null)
                    {
                        foreach (var triangleHandle in curve._triangleBodyHandles)
                        {
                            _curveHandles.Add(triangleHandle);
                        }
                    }
                    curve.OnSpeedChanged += _motorManager.UpdateCurveSpeed;
                    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);
                    // Eliminar motor si existe
                    _motorManager.RemoveMotor(bottle);
                    // Eliminar de pares de motores creados
                    RemoveFromMotorsCreated(bottle.BodyHandle);
                    break;
                case simTransporte transport:
                    _transportHandles.Remove(transport.BodyHandle);
                    transport.OnSpeedChanged -= _motorManager.UpdateTransportSpeed;
                    break;
                case simCurve curve:
                    _curveHandles.Remove(curve.BodyHandle);
                    if (curve._triangleBodyHandles != null)
                    {
                        foreach (var triangleHandle in curve._triangleBodyHandles)
                        {
                            _curveHandles.Remove(triangleHandle);
                        }
                    }
                    curve.OnSpeedChanged -= _motorManager.UpdateCurveSpeed;
                    break;
                case simBarrera barrier:
                    _barrierHandles.Remove(barrier.BodyHandle);
                    break;
                case simDescarte discard:
                    _discardHandles.Remove(discard.BodyHandle);
                    break;
            }
        }

        private void RemoveFromMotorsCreated(BodyHandle bottleHandle)
        {
            _motorsCreated.RemoveWhere(pair => pair.bottle == bottleHandle);
        }

        // ✅ 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 motores
            _transportHandles = new HashSet<BodyHandle>();
            _curveHandles = new HashSet<BodyHandle>();
            _barrierHandles = new HashSet<BodyHandle>();
            _discardHandles = new HashSet<BodyHandle>();
            _bottleHandles = new HashSet<BodyHandle>();
            _motorsCreated = new HashSet<(BodyHandle, BodyHandle)>();
            
            // ✅ CONSERVAR - resto del constructor igual
            bufferPool = new BufferPool();
            stopwatch = new Stopwatch();

            var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);

            // Configurar amortiguamiento para comportamiento realista:
            // - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen)
            // - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros)
            // ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks
            var poseIntegratorCallbacks = new PoseIntegratorCallbacks(
                gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z
                linearDamping: 0.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);
            
            // ✅ CREAR MOTOR MANAGER DESPUÉS DE LA SIMULACIÓN
            _motorManager = new MotorManager(this);
        }

        public void Clear()
        {
            try
            {
                // ✅ NUEVO - limpiar motor manager
                _motorManager?.Clear();
                
                // ✅ 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();
                    _motorsCreated.Clear(); // ✅ NUEVO
                }
                
                // ✅ NUEVO - limpiar clasificaciones
                _transportHandles.Clear();
                _curveHandles.Clear();
                _barrierHandles.Clear();
                _discardHandles.Clear();
                _bottleHandles.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); // Esto actualiza automáticamente SpeedMetersPerSecond internamente
                }
                
                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;
                }

                // ✅ ELIMINAR COMPLETAMENTE
                // ApplyTransportForces(deltaTime); // NO SE USA MÁS
                // ProcessBrakeTransportContacts(); // NO SE USA MÁS

                // ✅ NUEVO - verificación periódica de salidas de transporte
                CheckBottleExitsFromTransports();

                // ✅ ELIMINADO - UpdateCurveMotorDirections() ya no es necesario
                // Los triángulos de curva funcionan como mini-transportes usando colisiones naturales

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

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

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

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

        private void CheckBottleExitsFromTransports()
        {
            // ✅ Ejecutar cada 10 frames para eficiencia
            if (_frameCount % 10 != 0) return;
            
            var activeBottles = Cuerpos.OfType<simBotella>()
                .Where(b => b.HasActiveMotor && b.CurrentTransport != null)
                .ToList();
            
            foreach (var bottle in activeBottles)
            {
                if (!IsBottleOnTransport(bottle, bottle.CurrentTransport))
                {
                    ProcessBottleExitsTransport(bottle);
                }
            }
        }

        private bool IsBottleOnTransport(simBotella bottle, simTransporte transport)
        {
            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;
        }

        private void ProcessBottleExitsTransport(simBotella bottle)
        {
            RemoveCurrentMotorPair(bottle);
            _motorManager.RemoveMotor(bottle);
        }

        private void RemoveCurrentMotorPair(simBotella bottle)
        {
            if (bottle.CurrentTransport != null)
            {
                _motorsCreated.Remove((bottle.BodyHandle, bottle.CurrentTransport.BodyHandle));
            }
        }

        // ✅ ELIMINADO: Ya no necesitamos UpdateCurveMotorDirections 
        // Cada triángulo de curva funciona como un mini-transporte usando el sistema de colisiones natural

        public void Remove(simBase Objeto)
        {
            // ✅ SIMPLIFICADO - eliminar lógica de masa especial
            UnregisterObjectHandle(Objeto); // ✅ NUEVO
            
            // ✅ NUEVO - Limpiar dimensiones almacenadas en osBase
            CtrEditor.ObjetosSim.osBase.ClearStoredDimensions(Objeto);
            
            Objeto.RemoverBody();
            Cuerpos.Remove(Objeto);

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

        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, this); // ✅ PASAR REFERENCIA
            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, this); // ✅ PASAR REFERENCIA
            Cuerpos.Add(curve);
            RegisterObjectHandle(curve); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return curve;
        }

        /// <summary>
        /// ✅ NUEVO: Método público para limpiar motors conectados a un body específico
        /// </summary>
        public void RemoveMotorsConnectedToBody(BodyHandle bodyHandle)
        {
            _motorManager?.RemoveMotorsByBodyHandle(bodyHandle);
        }



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





        /// <summary>
        /// ✅ IMPLEMENTADO: Intenta crear un motor de transporte si no existe uno activo
        /// </summary>
        public void TryCreateTransportMotor(simBotella bottle, simTransporte transport)
        {
            try
            {
                // Validaciones básicas
                if (bottle == null || transport == null || _motorManager == null)
                    return;
                    
                // Verificar si ya tiene un motor activo
                if (bottle.HasActiveMotor)
                    return;
                
                // Asegurar que el transporte tenga propiedades inicializadas
                if (Math.Abs(transport.Speed) < 0.001f)
                    return; // No crear motor para transportes detenidos
                
                _motorManager.CreateTransportMotor(bottle, transport);
                
                System.Diagnostics.Debug.WriteLine($"[TryCreateTransportMotor] Motor creado: {bottle.BodyHandle} - {transport.BodyHandle}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in TryCreateTransportMotor: {ex.Message}");
            }
        }



        /// <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))
                        {
                            Remove(botella);
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error removing bottle - continue
                    }
                }

                // Botellas eliminadas: {botellasAEliminar.Count}
            }
            catch (Exception ex)
            {
                // Critical error in RemoveMarkedBottles - continue
            }
        }

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


        /// <summary>
        /// ✅ MODIFICADO: Sistema dinámico de motores de curva - permite actualización entre triángulos
        /// </summary>
        public void TryCreateCurveMotor(simBotella bottle, simCurve curve, Vector3 triangleDirection, BodyHandle triangleHandle)
        {
            try
            {
                // Validaciones básicas
                if (bottle == null || curve == null || _motorManager == null)
                    return;
                
                // Verificar que la curva tenga velocidad
                if (Math.Abs(curve.Speed) < 0.001f)
                    return; // No crear motor para curvas detenidas
                
                // Verificar que la dirección del triángulo sea válida
                if (triangleDirection.Length() < 0.001f)
                    return;
                
                // ✅ NUEVO: Si ya tiene motor de curva, actualizarlo; si no, crear uno nuevo
                if (bottle.HasActiveMotor)
                {
                    // Verificar si el motor actual es de una curva
                    var currentCurves = Cuerpos.OfType<simCurve>().Where(c => c.BottlesOnCurve.Contains(bottle));
                    if (currentCurves.Any())
                    {
                        // Actualizar motor existente con nueva dirección del triángulo
                        _motorManager.UpdateCurveMotorDirection(bottle, curve, triangleDirection, triangleHandle);
                        
                        System.Diagnostics.Debug.WriteLine($"[TryCreateCurveMotor] Motor actualizado: {bottle.BodyHandle} → Triangle {triangleHandle}");
                        return;
                    }
                    else
                    {
                        // Tiene motor de transporte, no interferir
                        return;
                    }
                }
                
                // ✅ Crear nuevo motor de curva
                _motorManager.CreateCurveMotorWithDirection(bottle, curve, triangleDirection);
                
                System.Diagnostics.Debug.WriteLine($"[TryCreateCurveMotor] Nuevo motor: {bottle.BodyHandle} → Triangle {triangleHandle}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in TryCreateCurveMotor with direction: {ex.Message}");
            }
        }
    }
}