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
{
    public class simBase
    {
        public BodyHandle BodyHandle { get; protected set; }
        public Simulation _simulation;
        protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo

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

        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)
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                bodyReference.Pose.Position = new Vector3(x, y, z);
            }
        }

        public void SetPosition(Vector2 position)
        {
            // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
            // Mantener la coordenada Z actual para preservar la altura del objeto
            var currentPosition = GetPosition();
            SetPosition(position.X, -position.Y, currentPosition.Z);
        }

        public void SetPosition(Vector3 position)
        {
            SetPosition(position.X, position.Y, position.Z);
        }

        public Vector3 GetPosition()
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                return bodyReference.Pose.Position;
            }
            return Vector3.Zero;
        }

        public void SetRotation(float angleZ)
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                // ✅ Convertir de grados a radianes para Quaternion.CreateFromAxisAngle
                bodyReference.Pose.Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angleZ));
            }
        }

        public float GetRotationZ()
        {
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                // Extraer el ángulo Z del quaternion
                var q = bodyReference.Pose.Orientation;
                return (float)Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
            }
            return 0f;
        }
    }

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

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

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

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

        public void SetSpeed(float speed)
        {
            Speed = speed;
        }

        /// <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)
        {
            Speed = speedMeterPerSecond;
        }

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

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

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

            // Actualizar la forma del cuerpo existente
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var box = new Box(width, height, zAltura_Transporte);
                var shapeIndex = _simulation.Shapes.Add(box);
                _simulation.Bodies.SetShape(BodyHandle, shapeIndex);
            }
        }

        public void Create(float width, float height, Vector2 topLeft, float angle = 0)
        {
            // Calcular el offset del centro desde Top-Left (sin rotación)
            var offsetX = width / 2f;
            var offsetY = height / 2f;

            // Rotar el offset alrededor del Top-Left usando el ángulo
            // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
            var angleRadians = GradosARadianes(angle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);

            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;

            // Calcular nueva posición del centro manteniendo Top-Left fijo
            var centerX = topLeft.X + rotatedOffsetX;
            var centerY = topLeft.Y + rotatedOffsetY;

            // Convertir a 3D e invertir Y - Transportes en Z = Depth/2 para estar sobre el suelo
            var center3D = new Vector3(centerX, -centerY, zAltura_Transporte / 2f + zPos_Transporte);

            Create(width, height, center3D, -angle);  // ✅ Invertir ángulo cuando se invierte Y
        }

        public void Create(float width, float height, Vector3 position, float angle = 0)
        {
            RemoverBody();

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

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))),
                new CollidableDescription(shapeIndex, 0.1f),
                new BodyActivityDescription(0.01f)
            );

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

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

        public void SetDimensions(float width, float height)
        {
            _height = height;
            // Actualizar la forma del cuerpo existente
            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);
                _simulation.Bodies.SetShape(BodyHandle, shapeIndex);
            }
        }

        public void Create(float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false)
        {
            // Calcular el offset del centro desde Top-Left (sin rotación)
            var offsetX = width / 2f;
            var offsetY = height / 2f;

            // Rotar el offset alrededor del Top-Left usando el ángulo
            // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
            var angleRadians = GradosARadianes(angle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);

            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;

            // Calcular nueva posición del centro manteniendo Top-Left fijo
            var centerX = topLeft.X + rotatedOffsetX;
            var centerY = topLeft.Y + rotatedOffsetY;

            // Convertir a 3D e invertir Y - Sensores centrados en z=0.1 (rango 0-0.2) para detectar botellas
            var center3D = new Vector3(centerX, -centerY, zPos_Barrera + zAltura_Barrera / 2f);
            Create(width, height, center3D, -angle, detectectNeck);  // ✅ Invertir ángulo cuando se invierte Y
        }

        public void Create(float width, float height, Vector3 position, float angle = 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

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))),
                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 topLeft, float angle)
        {
            RemoverBody();

            // Calcular el offset del centro desde Top-Left (sin rotación)
            var offsetX = width / 2f;
            var offsetY = height / 2f;

            // Rotar el offset alrededor del Top-Left usando el ángulo
            // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
            var angleRadians = GradosARadianes(angle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);

            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;

            // Calcular nueva posición del centro manteniendo Top-Left fijo
            var centerX = topLeft.X + rotatedOffsetX;
            var centerY = topLeft.Y + rotatedOffsetY;

            // Convertir a 3D e invertir Y - Guías más altas que transportes
            var center3D = new Vector3(centerX, -centerY, zAltura_Guia / 2 + zPos_Guia);

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

            // ✅ Invertir ángulo cuando se invierte Y para coherencia de rotación
            var orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(-angle));

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(center3D, orientation),
                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;
        }
    }

    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 float OriginalMass;
        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
        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;
            OriginalMass = mass;
            _neckRadius = neckRadius;
            ListOnTransports = new List<simBase>();

            // Convertir Vector2 a Vector3 con Z=0.2 (altura estándar para botellas)
            // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
            var position3D = new Vector3(position.X, -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
            {
                // Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo)
                return -GetPosition().Y;
            }
            set
            {
                var pos = GetPosition();
                // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
                SetPosition(pos.X, -value, pos.Z);
            }
        }

        public Vector2 Center
        {
            get
            {
                var pos3D = GetPosition();
                // Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo)
                return new Vector2(pos3D.X, -pos3D.Y);
            }
            set
            {
                // Mantener la Z actual, solo cambiar X, Y
                var currentPos = GetPosition();
                // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
                SetPosition(value.X, -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.01f),
                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

            // Actualizar la forma del cuerpo existente
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var sphere = new Sphere(Radius);
                var shapeIndex = _simulation.Shapes.Add(sphere);
                _simulation.Bodies.SetShape(BodyHandle, shapeIndex);
            }
        }

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

            // Actualizar la forma del cuerpo existente
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var sphere = new Sphere(Radius);
                var shapeIndex = _simulation.Shapes.Add(sphere);
                _simulation.Bodies.SetShape(BodyHandle, shapeIndex);
            }
        }

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

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

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

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

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

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

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

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

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

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

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

        /// <summary>
        /// ✅ NUEVO: Método de seguridad para restaurar la masa original si la botella tiene masa aumentada
        /// </summary>
        public void RestoreOriginalMassIfNeeded()
        {
            if (OriginalMass > 0) // Solo si hay masa original guardada
            {
                try
                {
                    SetMass(OriginalMass);
                    OriginalMass = 0; // Limpiar para evitar confusión
                    isOnBrakeTransport = false;
                    CurrentBrakeTransport = null;
                }
                catch (Exception ex)
                {
                    // Error restoring original mass - continue
                }
            }
        }
    }

    public class simCurve : simBase
    {
        private float _innerRadius;
        private float _outerRadius;
        private float _startAngle;
        private float _endAngle;
        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

        public float InnerRadius => _innerRadius;
        public float OuterRadius => _outerRadius;
        public float StartAngle => _startAngle;
        public float EndAngle => _endAngle;

        /// <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)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ SENTIDO HORARIO: Convertir ángulos para que vayan en sentido horario
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(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);
        }

        public void SetSpeed(float speed)
        {
            Speed = speed;
        }

        /// <summary>
        /// Configura la velocidad de la curva en metros por segundo
        /// Valores positivos mueven en sentido horario, negativos en sentido antihorario
        /// </summary>
        /// <param name="speedMeterPerSecond">Velocidad en m/s (típicamente entre -5.0 y 5.0)</param>
        public void SetCurveSpeed(float speedMeterPerSecond)
        {
            Speed = speedMeterPerSecond;
        }

        /// <summary>
        /// Detiene completamente la curva
        /// </summary>
        public void StopCurve()
        {
            Speed = 0f;
        }

        /// <summary>
        /// Invierte la dirección de la curva manteniendo la misma velocidad
        /// </summary>
        public void ReverseCurve()
        {
            Speed = -Speed;
        }

        public void UpdateCurve(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(endAngle);

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

        public new void RemoverBody()
        {
            // 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();
        }

        public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, 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;
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(endAngle);

            // Calcular el offset del centro desde Top-Left 
            // Para curvas, el "tamaño" es el diámetro del radio exterior
            var curveSize = outerRadius * 2f;
            var offsetX = curveSize / 2f;
            var offsetY = curveSize / 2f;

            // Calcular nueva posición del centro (sin rotación - el sector ya está definido por los ángulos)
            var centerX = topLeft.X + offsetX;
            var centerY = topLeft.Y + offsetY;

            // Convertir a 3D e invertir Y - Curvas en el mismo nivel que transportes
            var center3D = new Vector3(centerX, -centerY, zAltura_Curve / 2f + zPos_Curve);

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

                // 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.1f), // Speculative margin normal
                    activityDescription
                );

                var triangleHandle = _simulation.Bodies.Add(bodyDescription);
                _triangleBodyHandles.Add(triangleHandle);
            }
            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;

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

            // ✅ SENTIDO HORARIO: Intercambiar ángulos para que vaya en sentido horario
            float fromAngle = endAngle;   // Empezar desde el ángulo final
            float toAngle = startAngle;   // Terminar en el ángulo inicial
            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
        /// </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;
            }
        }
    }

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

            // Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas)
            // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
            var position3D = new Vector3(position.X, -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)
        {
            // Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas)
            // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
            var position3D = new Vector3(position.X, -position.Y, zPos_Descarte + _radius);
            Create(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)
        {
            return true;
        }

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

        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            // Configuración de materiales físicos por defecto
            pairMaterial = new PairMaterialProperties
            {
                FrictionCoefficient = 0.6f,      // Fricción moderada
                MaximumRecoveryVelocity = 2f,    // Velocidad máxima de recuperación
                SpringSettings = new SpringSettings(60, 4)  // Rigidez y amortiguamiento para colisiones sólidas
            };

            // Detectar contactos que involucren botellas para aplicar propiedades especiales
            if (_simulationManager != null)
            {
                var botellaA = GetBotellaFromCollidable(pair.A);
                var botellaB = GetBotellaFromCollidable(pair.B);
                var transportA = GetTransportFromCollidable(pair.A);
                var transportB = GetTransportFromCollidable(pair.B);
                var barreraA = GetBarreraFromCollidable(pair.A);
                var barreraB = GetBarreraFromCollidable(pair.B);
                var curveA = GetCurveFromCollidable(pair.A);
                var curveB = GetCurveFromCollidable(pair.B);
                var botella = botellaA ?? botellaB;

                // Detectar contactos con descarte
                var descarteA = GetDescarteFromCollidable(pair.A);
                var descarteB = GetDescarteFromCollidable(pair.B);

                // IMPORTANTE: Si hay una barrera involucrada, NO generar contacto físico
                if (barreraA != null || barreraB != null)
                {
                    // Ya no necesitamos registrar contactos - usamos detección geométrica pura
                    // Las barreras son sensores completamente virtuales

                    // NO generar contacto físico para barreras - son sensores puros
                    return false;
                }

                // Si hay un descarte involucrado, NO generar contacto físico pero registrar para eliminación
                if (descarteA != null || descarteB != null)
                {
                    var descarte = descarteA ?? descarteB;

                    // Registrar la botella para eliminación si hay contacto
                    if (botella != null)
                    {
                        _simulationManager.RegisterDescarteContact(descarte, botella);
                    }

                    // NO generar contacto físico para descartes - son sensores puros
                    return false;
                }

                // Si hay una botella en contacto (pero no con barrera), aplicar mayor fricción para estabilidad
                if (botella != null)
                {
                    // Aumentar mucho la fricción para botellas (más estables)
                    pairMaterial.FrictionCoefficient = 0.9f;

                    // Reducir la velocidad de recuperación para evitar rebotes excesivos
                    pairMaterial.MaximumRecoveryVelocity = 1f;

                    // Hacer las colisiones más suaves y estables
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }

                // Si hay un transporte y una botella en contacto
                if ((transportA != null || transportB != null) && botella != null)
                {
                    var transporte = transportA ?? transportB;

                    // ✅ NUEVA LÓGICA: Detectar contactos con transporte de freno PRIMERO
                    if (transporte.isBrake)
                    {
                        // FRICCIÓN EXTREMA para transporte con freno - simula frenos mecánicos
                        pairMaterial.FrictionCoefficient = 5.0f; // Fricción extremadamente alta

                        // Reducir velocidad de recuperación para evitar rebotes
                        pairMaterial.MaximumRecoveryVelocity = 0.1f;

                        // Hacer el contacto más rígido para mejor control
                        pairMaterial.SpringSettings = new SpringSettings(200, 20);

                        // ✅ APLICAR FUERZAS DE FRENADO DIRECTAMENTE EN EL MANIFOLD
                        ApplyBrakeForces(ref manifold, botella, transporte);

                        // Registrar contacto con transporte de freno
                        _simulationManager.RegisterBrakeTransportContact(botella, transporte);
                    }
                    else
                    {
                        // ✅ NUEVA LÓGICA: Aplicar fuerzas de transporte DIRECTAMENTE en el manifold
                        if (Math.Abs(transporte.Speed) > 0.001f) // Solo si hay velocidad significativa
                        {
                            pairMaterial.FrictionCoefficient = 1.2f; // Fricción muy alta

                            // ✅ APLICAR FUERZAS DE TRANSPORTE DIRECTAMENTE EN EL MANIFOLD
                            ApplyTransportForces(ref manifold, botella, transporte);
                        }
                    }
                }

                // Si hay una curva y una botella en contacto
                if ((curveA != null || curveB != null) && botella != null)
                {
                    var curve = curveA ?? curveB;

                    // Solo aplicar si hay velocidad significativa en la curva
                    if (Math.Abs(curve.Speed) > 0.001f)
                    {
                        pairMaterial.FrictionCoefficient = 1.0f; // Fricción alta para curvas

                        // Aplicar fuerzas de curva directamente en el manifold
                        ApplyCurveForces(ref manifold, botella, curve);
                    }
                }
            }

            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 MÉTODO: Aplicar fuerzas de frenado directamente en el manifold
        /// </summary>
        private void ApplyBrakeForces<TManifold>(ref TManifold manifold, simBotella botella, simTransporte transporte)
            where TManifold : unmanaged, IContactManifold<TManifold>
        {
            try
            {
                if (_simulationManager?.simulation == null) return;

                var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle);

                // Calcular velocidad relativa
                var relativeVelocity = botellaBody.Velocity.Linear - transporteBody.Velocity.Linear;

                // Si la botella se mueve más rápido que el transporte, aplicar fuerza de frenado
                var transportDirection = GetTransportDirection(transporte);
                var velocityInTransportDirection = Vector3.Dot(relativeVelocity, transportDirection);

                //                if (Math.Abs(velocityInTransportDirection) > Math.Abs(transporte.Speed / 18.5f))
                //              {
                // Calcular fuerza de frenado proporcional a la diferencia de velocidad
                var targetSpeed = transporte.Speed / 18.5f;
                var currentSpeed = Vector3.Dot(botellaBody.Velocity.Linear, transportDirection);
                var speedDifference = currentSpeed - targetSpeed;

                // Aplicar impulso de frenado - esto se hace DURANTE la resolución de colisiones
                var brakeImpulse = -transportDirection * speedDifference * botella.Mass * 0.5f;
                botellaBody.ApplyLinearImpulse(brakeImpulse);
                //                }
            }
            catch (Exception ex)
            {
                // Error applying brake forces during collision resolution
            }
        }

        /// <summary>
        /// ✅ NUEVO MÉTODO: Obtener dirección del transporte
        /// </summary>
        private Vector3 GetTransportDirection(simTransporte transporte)
        {
            var angle = transporte.GetRotationZ();
            return new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0);
        }

        /// <summary>
        /// ✅ NUEVO MÉTODO: Aplicar fuerzas de transporte normal directamente en el manifold
        /// </summary>
        private void ApplyTransportForces<TManifold>(ref TManifold manifold, simBotella botella, simTransporte transporte)
            where TManifold : unmanaged, IContactManifold<TManifold>
        {
            try
            {
                if (_simulationManager?.simulation == null) return;

                var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle);

                // Calcular la dirección del transporte
                var transportDirection = GetTransportDirection(transporte);

                // Calcular la fuerza proporcional a la velocidad deseada
                var targetVelocity = transportDirection * transporte.Speed / 18.5f;
                var currentVelocity = botellaBody.Velocity.Linear;

                // Solo aplicar fuerza en el plano horizontal (X-Y)
                var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);

                // Para transportes con freno (si aplica), limitar la velocidad máxima de la botella
                bool isOnBrakeTransport = botella.isOnBrakeTransport;
                if (isOnBrakeTransport)
                {
                    var transportSpeed = Math.Abs(transporte.Speed / 18.5f);
                    var currentSpeed = horizontalVelocity.Length();

                    // Si la botella va más rápido que el transporte, limitar su velocidad
                    if (currentSpeed > transportSpeed * 1.1f) // 10% de tolerancia
                    {
                        var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed;
                        botellaBody.Velocity.Linear = new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z);
                        horizontalVelocity = limitedVelocity;
                    }
                }

                var velocityDiff = targetVelocity - horizontalVelocity;

                // Aplicar una fuerza proporcional a la diferencia de velocidad
                var forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías
                var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base
                var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador

                if (forceMagnitude > 0.01f)
                {
                    var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado
                    var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);

                    // Aplicar impulso directamente durante la resolución de colisiones
                    botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico
                }
            }
            catch (Exception ex)
            {
                // Error applying transport forces during collision resolution
            }
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: Aplicar fuerzas de curva directamente en el manifold
        /// Si hay colisión física, la botella está sobre la curva - no necesita cálculos adicionales
        /// </summary>
        private void ApplyCurveForces<TManifold>(ref TManifold manifold, simBotella botella, simCurve curve)
            where TManifold : unmanaged, IContactManifold<TManifold>
        {
            try
            {
                if (_simulationManager?.simulation == null) return;

                var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
                var botellaPosition = botellaBody.Pose.Position;

                // ✅ SIMPLIFICADO: Si hay colisión física, aplicar fuerzas directamente
                // Obtener la dirección tangencial
                var tangentialDirection = curve.GetTangentialDirection(botellaPosition);

                if (tangentialDirection.Length() > 0.001f)
                {
                    // Calcular velocidad objetivo
                    var speedMetersPerSecond = Math.Abs(curve.Speed) / 18.5f; // Convertir a m/s
                    var targetVelocity = tangentialDirection * speedMetersPerSecond;

                    // Obtener velocidad actual
                    var currentVelocity = botellaBody.Velocity.Linear;
                    var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);

                    // Usar directamente la dirección tangencial (sin interpolación compleja)
                    var newDirection = tangentialDirection;

                    // Usar la velocidad mayor entre la actual y la de la curva para continuidad
                    var currentSpeed = horizontalVelocity.Length();
                    var targetSpeed = Math.Max(currentSpeed, speedMetersPerSecond);

                    // Calcular nueva velocidad
                    var newVelocity = newDirection * targetSpeed;
                    var velocityDiff = newVelocity - horizontalVelocity;

                    // Aplicar impulso directamente (sin factor de overlap - la colisión es suficiente)
                    var forceMagnitude = velocityDiff.Length() * botella.Mass * 8f;
                    if (forceMagnitude > 0.01f)
                    {
                        var maxForce = botella.Mass * 40f;
                        var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);
                        botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[simCurve] Error applying curve forces: {ex.Message}");
            }
        }

        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;

            // ✅ NUEVA FUNCIONALIDAD: Aplicar fuerzas de frenado durante la integración
            if (_simulationManager != null)
            {
                ApplyBrakeForcesInIntegration(bodyIndices, ref velocity, dt, workerIndex);
            }

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

        /// <summary>
        /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado durante la integración de velocidades
        /// ✅ SIMPLIFICADO: Solo aplicar amortiguamiento extra para botellas en transportes con freno
        /// </summary>
        private void ApplyBrakeForcesInIntegration(Vector<int> bodyIndices, ref BodyVelocityWide velocity, Vector<float> dt, int workerIndex)
        {
            try
            {
                // ✅ SIMPLIFICADO: Aplicar amortiguamiento extra para efectos de frenado
                // Esto es más compatible con la arquitectura vectorizada de BEPU

                // Amortiguamiento adicional para simular efecto de frenado
                var brakeDampingWide = Vector<float>.One * 0.95f; // Más agresivo que el damping normal

                // Aplicar solo si tenemos referencia al simulation manager
                if (_simulationManager != null)
                {
                    // El amortiguamiento extra se aplica globalmente, 
                    // las fuerzas específicas se manejan en ApplyBrakeForces del NarrowPhaseCallbacks
                    velocity.Linear *= brakeDampingWide;
                }
            }
            catch (Exception ex)
            {
                // Error during brake force integration
            }
        }
    }

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

        // Sistema de contactos de transporte para aplicar fuerzas
        private Dictionary<simBotella, simTransporte> _transportContacts;
        // Sistema de contactos de barrera para detección de paso
        private Dictionary<simBarrera, List<simBotella>> _barreraContacts;
        // Sistema de contactos de descarte para marcar botellas para eliminación
        private Dictionary<simDescarte, List<simBotella>> _descarteContacts;
        // Lista de botellas marcadas para eliminación
        private HashSet<simBotella> _botellasParaEliminar;
        // Sistema de contactos de transporte con freno para marcar botellas
        private Dictionary<simBotella, simTransporte> _brakeTransportContacts;
        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;
        }

        public SimulationManagerBEPU()
        {
            Cuerpos = new List<simBase>();
            _deferredActions = new List<Action>();
            _transportContacts = new Dictionary<simBotella, simTransporte>();
            _barreraContacts = new Dictionary<simBarrera, List<simBotella>>();
            _descarteContacts = new Dictionary<simDescarte, List<simBotella>>();
            _botellasParaEliminar = new HashSet<simBotella>();
            _brakeTransportContacts = new Dictionary<simBotella, simTransporte>();
            bufferPool = new BufferPool();
            stopwatch = new Stopwatch();

            var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);

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

            var solveDescription = new SolveDescription(8, 1);

            simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription);
        }

        public void Clear()
        {
            try
            {
                // ✅ NUEVO: Restaurar masas originales de todas las botellas antes de limpiar
                foreach (var cuerpo in Cuerpos.OfType<simBotella>())
                {
                    try
                    {
                        cuerpo.RestoreOriginalMassIfNeeded();
                    }
                    catch (Exception ex)
                    {
                        // Error restoring mass during clear - continue
                    }
                }

                // Limpiar contactos primero para evitar referencias colgantes
                lock (_contactsLock)
                {
                    _transportContacts.Clear();
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                    _botellasParaEliminar.Clear();
                    _brakeTransportContacts.Clear();
                }

                // Remover cuerpos de forma segura
                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();

                // 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()
        {
            stopwatch.Start();
            stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
        }

        public void Step()
        {
            try
            {
                var currentTime = stopwatch.Elapsed.TotalMilliseconds;
                var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0);
                stopwatch_last = currentTime;

                // Validar deltaTime para evitar valores problemáticos
                if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
                {
                    deltaTime = 1f / 60f; // Fallback a 60 FPS
                }

                // Limitar deltaTime a 100ms máximo para evitar saltos tras pausas del debugger
                const float maxDeltaTime = 0.1f; // 100ms
                if (deltaTime > maxDeltaTime)
                {
                    deltaTime = 1f / 60f; // Resetear a 60 FPS estándar si excede el límite
                }

                // Procesar acciones diferidas
                foreach (var action in _deferredActions)
                {
                    action();
                }
                _deferredActions.Clear();

                // Validar que la simulación esté en buen estado antes del timestep
                if (simulation?.Bodies == null)
                {
                    return;
                }

                // Validar que no hay cuerpos con handles inválidos
                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);
                    }
                }

                // Paso de simulación (genera las colisiones/contactos)
                var timestepValue = Math.Max(deltaTime, 1f / 120f); // Mínimo 120 FPS para mejor estabilidad

                try
                {
                    simulation.Timestep(timestepValue);
                }
                catch (AccessViolationException ex)
                {
                    // Limpiar contactos que podrían estar corruptos
                    lock (_contactsLock)
                    {
                        // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
                        _barreraContacts.Clear();
                    }
                    throw; // Re-lanzar para debug
                }

                // ✅ ELIMINADO: ApplyTransportForces - Ahora se aplica directamente en ConfigureContactManifold
                // ApplyTransportForces(deltaTime); // ELIMINADO - Ahora en ConfigureContactManifold

                // Procesar contactos de transporte con freno para marcar/desmarcar botellas
                ProcessBrakeTransportContacts();

                // Limitar rotaciones de botellas solo al plano XY (siempre "de pie")
                // Y GARANTIZAR que nunca entren en sleep mode
                foreach (var cuerpo in Cuerpos.OfType<simBotella>().ToList()) // ToList para evitar modificación durante iteración
                {
                    try
                    {
                        if (simulation.Bodies.BodyExists(cuerpo.BodyHandle))
                        {
                            cuerpo.LimitRotationToXYPlane();

                            // FORZAR que la botella esté siempre despierta - doble seguridad
                            simulation.Awakener.AwakenBody(cuerpo.BodyHandle);
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error limiting rotation for bottle - continue
                    }
                }

                // Procesar contactos de barrera para detección de paso
                ProcessBarreraContacts();

                // Procesar contactos de descarte para eliminación de botellas
                ProcessDescarteContacts();

                // Procesar sistema de limpieza para eliminar botellas debajo de transportes
                ProcessCleanupSystem();

                // Limpiar contactos DESPUÉS de usarlos
                lock (_contactsLock)
                {
                    // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                    _brakeTransportContacts.Clear();
                }

                // Sincronizar con la visualización 3D
                Visualization3DManager?.SynchronizeWorld();
            }
            catch (Exception ex)
            {
                // En caso de error crítico, limpiar contactos para evitar estado corrupto
                lock (_contactsLock)
                {
                    // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
                    _barreraContacts.Clear();
                    _brakeTransportContacts.Clear();
                }
            }
        }

        public void Remove(simBase Objeto)
        {
            // ✅ NUEVO: Si es una botella con masa aumentada, restaurar masa original antes de eliminar
            if (Objeto is simBotella botella && botella.OriginalMass > 0)
            {
                try
                {
                    botella.SetMass(botella.OriginalMass);
                    botella.OriginalMass = 0;
                    botella.isOnBrakeTransport = false;
                    botella.CurrentBrakeTransport = null;
                }
                catch (Exception ex)
                {
                    // Error restoring mass during removal - continue with removal
                }
            }

            Objeto.RemoverBody();
            Cuerpos.Remove(Objeto);

            // Actualizar visualización 3D tras eliminar objeto
            Visualization3DManager?.SynchronizeWorld();
        }

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

            // Sincronizar con la visualización 3D
            Visualization3DManager?.SynchronizeWorld();

            return botella;
        }

        public simTransporte AddRectangle(float width, float height, Vector2 position, float angle)
        {
            var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle);
            Cuerpos.Add(transporte);
            // Sincronizar con la visualización 3D
            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);
            // Sincronizar con la visualización 3D
            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);
            // Sincronizar con la visualización 3D
            Visualization3DManager?.SynchronizeWorld();

            return guia;
        }

        public simDescarte AddDescarte(float diameter, Vector2 position)
        {
            var descarte = new simDescarte(simulation, _deferredActions, diameter, position);
            Cuerpos.Add(descarte);
            // Sincronizar con la visualización 3D
            Visualization3DManager?.SynchronizeWorld();

            return descarte;
        }

        public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
        {
            var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused);
            Cuerpos.Add(curve);
            // Sincronizar con la visualización 3D
            Visualization3DManager?.SynchronizeWorld();

            return curve;
        }

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

        /// <summary>
        /// Registra un contacto entre una botella y un transporte para aplicar fuerzas
        /// </summary>
        /// <param name="botella">Botella en contacto</param>
        /// <param name="transporte">Transporte en contacto</param>
        public void RegisterTransportContact(simBotella botella, simTransporte transporte)
        {
            if (botella != null && transporte != null)
            {
                lock (_contactsLock)
                {
                    _transportContacts[botella] = transporte;
                }
            }
        }

        /// <summary>
        /// Registra un contacto entre una botella y un transporte con freno para marcar la botella
        /// ✅ NUEVO: También aumenta la masa de la botella 10x para simular conexión mecánica
        /// </summary>
        /// <param name="botella">Botella en contacto</param>
        /// <param name="transporte">Transporte con freno en contacto</param>
        public void RegisterBrakeTransportContact(simBotella botella, simTransporte transporte)
        {
            if (botella != null && transporte != null && transporte.isBrake)
            {
                lock (_contactsLock)
                {
                    _brakeTransportContacts[botella] = transporte;
                    // ✅ NUEVO: Mantener referencia directa en la botella
                    botella.CurrentBrakeTransport = transporte;
                    botella.ConveyorRestrictedTo = transporte;

                    // ✅ NUEVO: Aumentar masa 10x para simular conexión mecánica con el transporte
                    if (!botella.isOnBrakeTransport) // Solo si no estaba ya en un transporte con freno
                    {
                        // Guardar masa original si no está guardada ya
                        if (botella.OriginalMass <= 0)
                        {
                            botella.OriginalMass = botella.Mass;
                        }

                        // Aumentar masa 10 veces para simular "enganche" mecánico
                        botella.SetMass(botella.OriginalMass * 100f);
                    }
                }
            }
        }

        /// <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>
        /// Aplica fuerzas de transporte a las botellas en contacto
        /// </summary>
        /// <param name="deltaTime">Tiempo transcurrido desde el último paso</param>
        private void ApplyTransportForces(float deltaTime)
        {
            try
            {
                // Validar deltaTime
                if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
                    return;

                lock (_contactsLock)
                {
                    // Crear copia de contactos para evitar modificación durante iteración
                    var contactsCopy = new Dictionary<simBotella, simTransporte>(_transportContacts);

                    foreach (var contact in contactsCopy)
                    {
                        try
                        {
                            var botella = contact.Key;
                            var transporte = contact.Value;

                            // Validar objetos
                            if (botella == null || transporte == null || simulation?.Bodies == null)
                                continue;

                            // Verificar que ambos objetos aún existen
                            if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle))
                                continue;

                            // Solo aplicar si hay velocidad significativa
                            if (Math.Abs(transporte.Speed) <= 0.001f)
                                continue;

                            // Determinar si es una botella en transporte con freno (guías laterales)
                            bool isOnBrakeTransport = botella.isOnBrakeTransport;
                            float forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías

                            // Aplicar fuerzas de movimiento (longitudinales) con control de velocidad
                            ApplyLongitudinalForces(botella, transporte, deltaTime, forceMultiplier, isOnBrakeTransport);

                        }
                        catch (Exception ex)
                        {
                            // Error processing contact - continue
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                // Critical error in ApplyTransport - continue
            }
        }

        /// <summary>
        /// Aplica fuerzas longitudinales para el movimiento del transporte con control de velocidad
        /// </summary>
        private void ApplyLongitudinalForces(simBotella botella, simTransporte transporte, float deltaTime, float forceMultiplier, bool isOnBrakeTransport)
        {
            try
            {
                // Calcular la dirección del transporte de forma segura
                var angle = transporte.GetRotationZ();

                // Validar ángulo
                if (float.IsNaN(angle) || float.IsInfinity(angle))
                    return;

                var cos = (float)Math.Cos(angle);
                var sin = (float)Math.Sin(angle);

                // Validar valores trigonométricos
                if (float.IsNaN(cos) || float.IsNaN(sin))
                    return;

                // Vector de dirección del transporte (en el plano X-Y)
                var transportDirection = new Vector3(cos, sin, 0);

                // Calcular la fuerza proporcional a la velocidad deseada
                var targetVelocity = transportDirection * transporte.Speed / 18.5f;
                var currentVelocity = botella.GetLinearVelocity();

                // Validar velocidades
                if (float.IsNaN(currentVelocity.X) || float.IsNaN(currentVelocity.Y) || float.IsNaN(currentVelocity.Z))
                    return;

                // Solo aplicar fuerza en el plano horizontal (X-Y)
                var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);

                // Para transportes con freno, limitar la velocidad máxima de la botella
                if (isOnBrakeTransport)
                {
                    var transportSpeed = Math.Abs(transporte.Speed / 18.5f);
                    var currentSpeed = horizontalVelocity.Length();

                    // Si la botella va más rápido que el transporte, limitar su velocidad
                    if (currentSpeed > transportSpeed * 1f) // 10% de tolerancia
                    {
                        var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed;
                        botella.ApplyLinearVelocity(new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z));
                        horizontalVelocity = limitedVelocity;
                    }
                }

                var velocityDiff = targetVelocity - horizontalVelocity;

                // Validar diferencia de velocidad
                if (float.IsNaN(velocityDiff.X) || float.IsNaN(velocityDiff.Y) || float.IsNaN(velocityDiff.Z))
                    return;

                // Aplicar una fuerza proporcional a la diferencia de velocidad
                var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base
                var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador

                // Validar magnitud de fuerza
                if (float.IsNaN(forceMagnitude) || float.IsInfinity(forceMagnitude))
                    return;

                if (forceMagnitude > 0.01f)
                {
                    var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado
                    var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);

                    // Validar fuerza final
                    if (float.IsNaN(force.X) || float.IsNaN(force.Y) || float.IsNaN(force.Z))
                        return;

                    var bodyReference = simulation.Bodies.GetBodyReference(botella.BodyHandle);
                    bodyReference.ApplyLinearImpulse(force * deltaTime);
                }
            }
            catch (Exception ex)
            {
                // Error in ApplyLongitudinalForces - continue
            }
        }


        /// <summary>
        /// Procesa contactos de transporte con freno para marcar/desmarcar botellas
        /// ✅ NUEVO: También gestiona el cambio de masa - restaura masa original al salir del transporte
        /// </summary>
        private void ProcessBrakeTransportContacts()
        {
            try
            {
                // Primero, recopilar botellas que estaban en transporte con freno para detectar cambios
                var todasLasBotellas = Cuerpos.OfType<simBotella>().ToList();
                var botellasQueEstabanEnBrakeTransport = new HashSet<simBotella>();

                foreach (var botella in todasLasBotellas)
                {
                    if (botella != null && botella.isOnBrakeTransport)
                    {
                        botellasQueEstabanEnBrakeTransport.Add(botella);
                        botella.isOnBrakeTransport = false; // Asumir que no están en contacto
                    }
                }

                // Luego, marcar las botellas que SÍ están en contacto con transportes de freno
                lock (_contactsLock)
                {
                    var contactsCopy = new Dictionary<simBotella, simTransporte>(_brakeTransportContacts);

                    foreach (var contact in contactsCopy)
                    {
                        try
                        {
                            var botella = contact.Key;
                            var transporte = contact.Value;

                            // Validar objetos
                            if (botella == null || transporte == null || simulation?.Bodies == null)
                                continue;

                            // Verificar que ambos objetos aún existen
                            if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle))
                                continue;

                            // Verificar que el transporte sigue teniendo freno activado
                            if (transporte.isBrake)
                            {
                                botella.isOnBrakeTransport = true;
                                // Quitar de la lista de botellas que estaban en brake transport
                                botellasQueEstabanEnBrakeTransport.Remove(botella);
                            }
                        }
                        catch (Exception ex)
                        {
                            // Error processing brake transport contact - continue
                        }
                    }
                }

                // ✅ NUEVO: Restaurar masa original de botellas que salieron del transporte con freno
                foreach (var botella in botellasQueEstabanEnBrakeTransport)
                {
                    try
                    {
                        if (botella.OriginalMass > 0) // Solo si tenemos masa original guardada
                        {
                            botella.SetMass(botella.OriginalMass);
                            botella.OriginalMass = 0; // Limpiar para evitar confusión
                            botella.CurrentBrakeTransport = null; // Limpiar referencia
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error restoring original mass - continue
                    }
                }
            }
            catch (Exception ex)
            {
                // Critical error in ProcessBrakeTransportContacts - continue
            }
        }

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

    }
}