using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using BulletSharp;
using BulletSharp.Math;

namespace CtrEditor.Simulacion
{
    public class simBase
    {
        public RigidBody Body { get; protected set; }
        public DiscreteDynamicsWorld _world;

        public void RemoverBody()
        {
            if (Body != null && _world != null)
            {
                _world.RemoveRigidBody(Body);
                Body?.Dispose();
                Body = null;
            }
        }

        public static float Min(float Value, float Min = 0.01f)
        {
            return Value < Min ? Min : Value;
        }

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

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

        public void SetPosition(float x, float y)
        {
            if (Body != null)
            {
                var transform = Body.WorldTransform;
                transform.Origin = new Vector3(x, y, 0);
                Body.WorldTransform = transform;
                Body.Activate();
            }
        }

        public void SetPosition(Vector3 centro)
        {
            try
            {
                if (Body != null)
                {
                    var transform = Body.WorldTransform;
                    transform.Origin = centro;
                    Body.WorldTransform = transform;
                    Body.Activate();
                }
            }
            catch
            {
            }
        }

        public Vector3 GetPosition()
        {
            return Body?.WorldTransform.Origin ?? Vector3.Zero;
        }
    }

    public class simCurve : simBase
    {
        private float _innerRadius;
        private float _outerRadius;
        private float _startAngle;
        private float _endAngle;
        public float Speed { get; set; }

        private List<Action> _deferredActions;

        public simCurve(DiscreteDynamicsWorld world, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
        {
            _world = world;
            _deferredActions = deferredActions;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(endAngle);
            Create(position);
        }

        public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
        {
            if (_world == null)
                return;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(endAngle);
            Create(position);
        }

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

            // Crear una forma compuesta para la curva
            var compoundShape = new CompoundShape();
            
            // Crear segmentos de la curva como cajas pequeñas
            int segments = 32;
            float angleStep = (_endAngle - _startAngle) / segments;
            float thickness = 0.1f; // Grosor de la curva
            
            for (int i = 0; i < segments; i++)
            {
                float angle = _startAngle + i * angleStep;
                float midRadius = (_innerRadius + _outerRadius) / 2f;
                float segmentWidth = (_outerRadius - _innerRadius);
                float segmentLength = midRadius * angleStep;
                
                var segmentShape = new BoxShape(segmentLength / 2f, segmentWidth / 2f, thickness / 2f);
                
                var segmentTransform = Matrix.Identity;
                segmentTransform.Origin = new Vector3(
                    midRadius * (float)Math.Cos(angle),
                    midRadius * (float)Math.Sin(angle),
                    0
                );
                segmentTransform = Matrix.RotationZ(angle) * segmentTransform;
                
                compoundShape.AddChildShape(segmentTransform, segmentShape);
            }

            var motionState = new DefaultMotionState(Matrix.Translation(position));
            var rbInfo = new RigidBodyConstructionInfo(0, motionState, compoundShape, Vector3.Zero);
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
            
            _world.AddRigidBody(Body);
        }

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

        public void ApplyCurveEffect(RigidBody bottle)
        {
            Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
            float distanceToCenter = centerToBottle.Length;
            
            if (distanceToCenter >= _innerRadius && distanceToCenter <= _outerRadius)
            {
                float overlapPercentage = CalculateAngleOverlap(bottle);
                
                if (overlapPercentage > 0)
                {
                    float speedMetersPerSecond = Speed / 60.0f;
                    Vector3 tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0);
                    tangent.Normalize();
                    
                    if (speedMetersPerSecond < 0)
                        tangent = -tangent;
                    
                    Vector3 currentVelocity = bottle.LinearVelocity;
                    float currentSpeed = currentVelocity.Length;
                    float conveyorSpeed = Math.Abs(speedMetersPerSecond);
                    float targetSpeed = Math.Max(currentSpeed, conveyorSpeed);
                    
                    Vector3 currentDir = currentVelocity;
                    if (currentDir.LengthSquared > 0)
                        currentDir.Normalize();
                    else
                        currentDir = tangent;
                    
                    Vector3 newDirection = currentDir * (1 - overlapPercentage) + tangent * overlapPercentage;
                    newDirection.Normalize();
                    
                    bottle.LinearVelocity = newDirection * targetSpeed;
                }
            }
        }

        private float CalculateAngleOverlap(RigidBody bottle)
        {
            Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
            float bottleAngle = (float)Math.Atan2(centerToBottle.Y, centerToBottle.X);
            
            float normalizedBottleAngle = bottleAngle < 0 ? bottleAngle + 2 * (float)Math.PI : bottleAngle;
            float normalizedStartAngle = _startAngle < 0 ? _startAngle + 2 * (float)Math.PI : _startAngle;
            float normalizedEndAngle = _endAngle < 0 ? _endAngle + 2 * (float)Math.PI : _endAngle;
            
            if (normalizedStartAngle > normalizedEndAngle)
            {
                if (!(normalizedBottleAngle >= normalizedStartAngle || normalizedBottleAngle <= normalizedEndAngle))
                    return 0;
            }
            else
            {
                if (normalizedBottleAngle < normalizedStartAngle || normalizedBottleAngle > normalizedEndAngle)
                    return 0;
            }
            
            float distanceToCenter = centerToBottle.Length;
            if (distanceToCenter < _innerRadius || distanceToCenter > _outerRadius)
                return 0;
            
            float angleFromStart, angleToEnd, totalAngle;
            
            if (normalizedStartAngle > normalizedEndAngle)
            {
                totalAngle = (2 * (float)Math.PI - normalizedStartAngle) + normalizedEndAngle;
                if (normalizedBottleAngle >= normalizedStartAngle)
                {
                    angleFromStart = normalizedBottleAngle - normalizedStartAngle;
                    angleToEnd = (2 * (float)Math.PI - normalizedBottleAngle) + normalizedEndAngle;
                }
                else
                {
                    angleFromStart = (2 * (float)Math.PI - normalizedStartAngle) + normalizedBottleAngle;
                    angleToEnd = normalizedEndAngle - normalizedBottleAngle;
                }
            }
            else
            {
                angleFromStart = normalizedBottleAngle - normalizedStartAngle;
                angleToEnd = normalizedEndAngle - normalizedBottleAngle;
                totalAngle = normalizedEndAngle - normalizedStartAngle;
            }
            
            float minAngleDistance = Math.Min(angleFromStart, angleToEnd);
            float overlapPercentage = Math.Min(minAngleDistance / (totalAngle * 0.1f), 1.0f);
            return overlapPercentage;
        }
    }

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

        public simDescarte(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position)
        {
            _world = world;
            _deferredActions = deferredActions;
            _radius = diameter / 2;
            Create(position);
        }

        public void SetDiameter(float diameter)
        {
            if (diameter <= 0)
                diameter = 0.01f;
            _radius = diameter / 2;
            Create(Body.WorldTransform.Origin);
        }

        public void Create(Vector3 position)
        {
            RemoverBody();
            
            var shape = new CylinderShape(_radius, _radius, 0.1f); // Cilindro plano para simular círculo 2D
            var motionState = new DefaultMotionState(Matrix.Translation(position));
            var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
            
            _world.AddRigidBody(Body);
        }
    }

    public class simTransporte : simBase
    {
        public float Speed { get; set; }
        public float Friction { get; set; }
        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(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0)
        {
            _world = world;
            _deferredActions = deferredActions;
            Create(width, height, position, angle);
        }

        public float Angle
        {
            get
            {
                var rotation = Body.WorldTransform.GetBasis();
                return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
            }
            set
            {
                var transform = Body.WorldTransform;
                transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
                Body.WorldTransform = transform;
            }
        }

        public new void SetPosition(float x, float y)
        {
            var transform = Body.WorldTransform;
            transform.Origin = new Vector3(x, y, 0);
            Body.WorldTransform = transform;
        }

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

        public void SetDimensions(float width, float height)
        {
            RemoverBody();
            Create(width, height, Body.WorldTransform.Origin, Angle);
        }

        public void Create(float width, float height, Vector3 position, float angle = 0)
        {
            RemoverBody();
            Width = width;
            Height = height;
            Friction = 0.1f;
            
            var shape = new BoxShape(width / 2, height / 2, 0.05f); // Caja plana para simular rectángulo 2D
            var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
            var motionState = new DefaultMotionState(transform);
            var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
            
            _world.AddRigidBody(Body);
        }
    }

    public class simBarrera : simBase
    {
        public float Distancia;
        public int LuzCortada;
        public bool LuzCortadaNeck;
        public bool DetectNeck;
        public List<simBotella> ListSimBotellaContact;
        
        float _height;
        private List<Action> _deferredActions;

        public simBarrera(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
        {
            _world = world;
            _height = Min(height);
            DetectNeck = detectectNeck;
            _deferredActions = deferredActions;
            ListSimBotellaContact = new List<simBotella>();
            Create(Min(width), _height, position, angle);
        }

        public float Angle
        {
            get
            {
                var rotation = Body.WorldTransform.GetBasis();
                return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
            }
            set
            {
                var transform = Body.WorldTransform;
                transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
                Body.WorldTransform = transform;
            }
        }

        public new void SetPosition(float x, float y)
        {
            var transform = Body.WorldTransform;
            transform.Origin = new Vector3(x, y, 0);
            Body.WorldTransform = transform;
        }

        public void SetDimensions(float width, float height)
        {
            RemoverBody();
            Create(Min(width), Min(height), Body.WorldTransform.Origin, Angle);
        }

        public void Create(float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
        {
            RemoverBody();
            _height = Min(height);
            DetectNeck = detectectNeck;
            
            var shape = new BoxShape(Min(width) / 2, _height / 2, 0.05f);
            var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
            var motionState = new DefaultMotionState(transform);
            var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
            
            _world.AddRigidBody(Body);
            LuzCortada = 0;
        }

        public void CheckIfNecksIsTouching()
        {
            if (LuzCortada == 0)
                return;
                
            foreach (var botella in ListSimBotellaContact)
            {
                Vector3 sensorCenter = Body.WorldTransform.Origin;
                float halfHeight = _height / 2;
                var rotation = Body.WorldTransform.GetBasis();
                float cos = rotation.M11;
                float sin = rotation.M21;
                
                Vector3 lineStart = sensorCenter + new Vector3(-halfHeight * cos, halfHeight * sin, 0);
                Vector3 lineEnd = sensorCenter + new Vector3(halfHeight * cos, -halfHeight * sin, 0);
                
                Vector3 fixtureCenter = botella.Body.WorldTransform.Origin;
                Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
                
                float distance = (closestPoint - fixtureCenter).Length;
                Distancia = distance;
                
                if (distance <= botella._neckRadius)
                {
                    LuzCortadaNeck = true;
                    return;
                }
            }
            
            LuzCortadaNeck = false;
        }

        private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
        {
            Vector3 lineDirection = lineEnd - lineStart;
            lineDirection.Normalize();
            Vector3 pointToLineStart = point - lineStart;
            float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
            return lineStart + projectionLength * lineDirection;
        }
    }

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

        public simGuia(DiscreteDynamicsWorld world, List<Action> deferredActions, Vector3 start, Vector3 end)
        {
            _world = world;
            _deferredActions = deferredActions;
            Create(start, end);
        }

        public void Create(Vector3 start, Vector3 end)
        {
            RemoverBody();
            
            Vector3 center = (start + end) / 2;
            Vector3 direction = end - start;
            float length = direction.Length;
            
            var shape = new BoxShape(length / 2, 0.01f, 0.05f); // Línea como caja muy delgada
            
            float angle = (float)Math.Atan2(direction.Y, direction.X);
            var transform = Matrix.Translation(center) * Matrix.RotationZ(angle);
            var motionState = new DefaultMotionState(transform);
            var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            
            _world.AddRigidBody(Body);
        }

        public void UpdateVertices(Vector3 newStart, Vector3 newEnd)
        {
            Create(newStart, newEnd);
        }
    }

    public class simBotella : simBase
    {
        public float Radius;
        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 RigidBody axisRestrictedBy;
        public float OverlapPercentage;
        public float _neckRadius;
        
        private List<Action> _deferredActions;
        private DiscreteDynamicsWorld _worldRef;

        public simBotella(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position, float mass, float neckRadius = 0)
        {
            _world = world;
            _worldRef = world;
            ListOnTransports = new List<simBase>();
            _deferredActions = deferredActions;
            
            diameter = Min(diameter, 0.01f);
            Radius = diameter / 2;
            _mass = mass;
            
            if (neckRadius <= 0)
                neckRadius = diameter / 4;
            _neckRadius = neckRadius;
            Create(position);
        }

        public float CenterX => Body.WorldTransform.Origin.X;
        public float CenterY => Body.WorldTransform.Origin.Y;
        public Vector3 Center => Body.WorldTransform.Origin;

        public float Mass
        {
            get
            {
                if (_mass <= 0)
                    _mass = 1;
                return _mass;
            }
            set
            {
                _mass = value;
            }
        }

        private void Create(Vector3 position)
        {
            RemoverBody();
            isOnTransports = 0;
            
            var shape = new CylinderShape(Radius, Radius, 0.1f); // Cilindro para simular círculo 2D
            var motionState = new DefaultMotionState(Matrix.Translation(position));
            
            Vector3 inertia;
            shape.CalculateLocalInertia(_mass, out inertia);
            var rbInfo = new RigidBodyConstructionInfo(_mass, motionState, shape, inertia);
            
            Body = new RigidBody(rbInfo);
            Body.UserObject = this;
            Body.SetDamping(3f, 1f); // Linear y angular damping
            Body.SetSleepingThresholds(0, 0); // No dormir
            Body.CcdMotionThreshold = 1e-7f;
            Body.CcdSweptSphereRadius = Radius * 0.2f;
            
            _world.AddRigidBody(Body);
        }

        public void SetDiameter(float diameter)
        {
            diameter = Min(diameter, 0.01f);
            Radius = diameter / 2;
            Create(Body.WorldTransform.Origin);
        }

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

        public void ApplyConveyorSpeed(float deltaTime_s)
        {
            foreach (var transporte in ListOnTransports)
            {
                if (transporte is simTransporte conveyorRect)
                    ApplyConveyorEffect(deltaTime_s, conveyorRect);
                if (transporte is simCurve conveyorCurve)
                    conveyorCurve.ApplyCurveEffect(Body);
            }
        }

        private bool ApplyConveyorEffect(float deltaTime_s, simTransporte conveyor)
        {
            float overlapPercentage = CalculateOverlapedArea(this, conveyor);
            OverlapPercentage = overlapPercentage;
            
            float speedMetersPerSecond = conveyor.Speed / 60.0f;
            var rotation = conveyor.Body.WorldTransform.GetBasis();
            Vector3 desiredVelocity = new Vector3(rotation.M11, rotation.M21, 0) * speedMetersPerSecond;
            
            Vector3 currentVelocity = Body.LinearVelocity;
            Vector3 velocityDifference = desiredVelocity - currentVelocity;
            
            float proporcionalVelocityNeeded = 1 - CalculateProportion(currentVelocity, desiredVelocity);
            float frictionCoefficient;
            
            switch (proporcionalVelocityNeeded)
            {
                case > 0.3f:
                    frictionCoefficient = conveyor.Friction * overlapPercentage;
                    break;
                default:
                    frictionCoefficient = proporcionalVelocityNeeded * overlapPercentage;
                    break;
            }

            if (isRestricted && conveyor == ConveyorRestrictedTo && overlapPercentage > 0.5f)
            {
                Body.LinearVelocity = desiredVelocity;
                return true;
            }

            Body.LinearVelocity += frictionCoefficient * desiredVelocity;
            return false;
        }

        public float CalculateOverlapedArea(simBotella botella, simTransporte conveyor)
        {
            // Implementación simplificada del cálculo de área superpuesta
            Vector3 bottlePos = botella.Body.WorldTransform.Origin;
            Vector3 conveyorPos = conveyor.Body.WorldTransform.Origin;
            
            // Transformar posición de botella al espacio local del transportador
            Matrix invTransform = conveyor.Body.WorldTransform.Inverse();
            Vector3 localBottlePos = Vector3.TransformCoordinate(bottlePos, invTransform);
            
            // Verificar si la botella está dentro del rectángulo del transportador
            float halfWidth = conveyor.Width / 2;
            float halfHeight = conveyor.Height / 2;
            
            if (Math.Abs(localBottlePos.X) <= halfWidth + botella.Radius &&
                Math.Abs(localBottlePos.Y) <= halfHeight + botella.Radius)
            {
                // Calcular área superpuesta aproximada
                float overlapX = Math.Max(0, Math.Min(halfWidth, localBottlePos.X + botella.Radius) - Math.Max(-halfWidth, localBottlePos.X - botella.Radius));
                float overlapY = Math.Max(0, Math.Min(halfHeight, localBottlePos.Y + botella.Radius) - Math.Max(-halfHeight, localBottlePos.Y - botella.Radius));
                
                float overlapArea = overlapX * overlapY;
                float bottleArea = (float)(Math.PI * botella.Radius * botella.Radius);
                
                return Math.Min(1.0f, overlapArea / bottleArea);
            }
            
            return 0;
        }

        public static float CalculateProportion(Vector3 currentVelocity, Vector3 desiredVelocity)
        {
            float dotProduct = Vector3.Dot(desiredVelocity, currentVelocity);
            float magnitudeV1Squared = desiredVelocity.LengthSquared;
            
            if (magnitudeV1Squared == 0)
                return 0;
            
            float proportion = dotProduct / magnitudeV1Squared;
            return proportion;
        }

        public void CenterFixtureOnConveyor()
        {
            if (!isRestricted || ConveyorRestrictedTo == null)
                return;
                
            Vector3 conveyorCenter = ConveyorRestrictedTo.Body.WorldTransform.Origin;
            float halfDistance = ConveyorRestrictedTo.DistanceGuide2Guide / 2;
            var rotation = ConveyorRestrictedTo.Body.WorldTransform.GetBasis();
            
            Vector3 offset = new Vector3(halfDistance * rotation.M11, halfDistance * rotation.M21, 0);
            Vector3 lineStart = conveyorCenter - offset;
            Vector3 lineEnd = conveyorCenter + offset;
            
            Vector3 fixtureCenter = Body.WorldTransform.Origin;
            Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
            
            var transform = Body.WorldTransform;
            transform.Origin = closestPoint;
            Body.WorldTransform = transform;
        }

        private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
        {
            Vector3 lineDirection = lineEnd - lineStart;
            lineDirection.Normalize();
            Vector3 pointToLineStart = point - lineStart;
            float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
            return lineStart + projectionLength * lineDirection;
        }

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

    public class SimulationManagerFP
    {
        private DiscreteDynamicsWorld world;
        private Canvas simulationCanvas;
        public List<simBase> Cuerpos;
        public List<Action> _deferredActions;
        private Stopwatch stopwatch;
        private double stopwatch_last;
        
        // Componentes de Bullet Physics
        private DefaultCollisionConfiguration collisionConfiguration;
        private CollisionDispatcher dispatcher;
        private DbvtBroadphase broadphase;
        private SequentialImpulseConstraintSolver solver;

        public Canvas DebugCanvas { get => simulationCanvas; set => simulationCanvas = value; }

        public SimulationManagerFP()
        {
            // Inicializar Bullet Physics
            collisionConfiguration = new DefaultCollisionConfiguration();
            dispatcher = new CollisionDispatcher(collisionConfiguration);
            broadphase = new DbvtBroadphase();
            solver = new SequentialImpulseConstraintSolver();
            
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
            world.Gravity = new Vector3(0, 0, 0); // Sin gravedad para simular 2D
            
            Cuerpos = new List<simBase>();
            _deferredActions = new List<Action>();
            stopwatch = new Stopwatch();
            stopwatch.Start();
        }

        public void Clear()
        {
            if (world.NumCollisionObjects > 0)
            {
                for (int i = world.NumCollisionObjects - 1; i >= 0; i--)
                {
                    var obj = world.CollisionObjectArray[i];
                    var body = RigidBody.Upcast(obj);
                    if (body != null && body.MotionState != null)
                    {
                        body.MotionState.Dispose();
                    }
                    world.RemoveCollisionObject(obj);
                    obj.Dispose();
                }
            }
            
            if (Cuerpos.Count > 0)
                Cuerpos.Clear();
        }

        public void Start()
        {
            stopwatch.Start();
            stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
        }

        public void Step()
        {
            float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
            stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
            
            world.StepSimulation(elapsedMilliseconds / 1000.0f, 10);
            
            // Detectar colisiones manualmente para sensores
            CheckCollisions();
            
            foreach (var cuerpo in Cuerpos)
            {
                if (cuerpo is simBotella botella)
                {
                    botella.ApplyConveyorSpeed(elapsedMilliseconds / 1000);
                    if (botella.isRestricted)
                    {
                        botella.CenterFixtureOnConveyor();
                        botella.Body.SetMassProps(100, Vector3.Zero);
                    }
                    else if (botella.isNoMoreRestricted)
                    {
                        botella.isNoMoreRestricted = false;
                        botella.Body.SetMassProps(botella.OriginalMass, Vector3.Zero);
                    }
                }
                else if (cuerpo is simBarrera barrera)
                    barrera.CheckIfNecksIsTouching();
            }
            
            foreach (var action in _deferredActions)
            {
                action();
            }
            
            _deferredActions.Clear();
        }

        private void CheckCollisions()
        {
            int numManifolds = world.Dispatcher.NumManifolds;
            
            for (int i = 0; i < numManifolds; i++)
            {
                PersistentManifold contactManifold = world.Dispatcher.GetManifoldByIndexInternal(i);
                var obA = contactManifold.Body0;
                var obB = contactManifold.Body1;
                
                var bodyA = RigidBody.Upcast(obA);
                var bodyB = RigidBody.Upcast(obB);
                
                if (bodyA?.UserObject is simBotella botella && bodyB?.UserObject is simBase other)
                {
                    HandleCollision(botella, other, bodyB);
                }
                else if (bodyB?.UserObject is simBotella botella2 && bodyA?.UserObject is simBase other2)
                {
                    HandleCollision(botella2, other2, bodyA);
                }
            }
        }

        private void HandleCollision(simBotella botella, simBase other, RigidBody otherBody)
        {
            switch (other)
            {
                case simBarrera sensor:
                    if (!sensor.ListSimBotellaContact.Contains(botella))
                    {
                        sensor.LuzCortada += 1;
                        sensor.ListSimBotellaContact.Add(botella);
                    }
                    break;
                    
                case simCurve curve:
                    if (!botella.ListOnTransports.Contains(curve))
                    {
                        botella.isOnTransports += 1;
                        botella.ListOnTransports.Add(curve);
                    }
                    break;
                    
                case simDescarte:
                    botella.Descartar = true;
                    break;
                    
                case simTransporte conveyor:
                    if (!botella.ListOnTransports.Contains(conveyor))
                    {
                        botella.isOnTransports += 1;
                        botella.ListOnTransports.Add(conveyor);
                        
                        if (conveyor.TransportWithGuides && conveyor.isBrake)
                        {
                            botella.ConveyorRestrictedTo = conveyor;
                            botella.axisRestrictedBy = otherBody;
                            botella.OriginalMass = botella.Body.Mass;
                            botella.isRestricted = true;
                            botella.isNoMoreRestricted = false;
                        }
                    }
                    break;
            }
        }

        public void Remove(simBase Objeto)
        {
            if (Objeto != null)
            {
                Objeto.RemoverBody();
                Cuerpos.Remove(Objeto);
            }
        }

        public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
        {
            simCurve curva = new simCurve(world, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, position);
            Cuerpos.Add(curva);
            return curva;
        }

        public simBotella AddCircle(float diameter, Vector3 position, float mass)
        {
            simBotella circle = new simBotella(world, _deferredActions, diameter, position, mass);
            Cuerpos.Add(circle);
            return circle;
        }

        public simTransporte AddRectangle(float width, float height, Vector3 position, float angle)
        {
            simTransporte rectangle = new simTransporte(world, _deferredActions, width, height, position, angle);
            Cuerpos.Add(rectangle);
            return rectangle;
        }

        public simBarrera AddBarrera(float width, float height, Vector3 position, float angle, bool detectarCuello)
        {
            simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle, detectarCuello);
            Cuerpos.Add(rectangle);
            return rectangle;
        }

        public simGuia AddLine(Vector3 start, Vector3 end)
        {
            simGuia line = new simGuia(world, _deferredActions, start, end);
            Cuerpos.Add(line);
            return line;
        }

        public simDescarte AddDescarte(float diameter, Vector3 position)
        {
            simDescarte descarte = new simDescarte(world, _deferredActions, diameter, position);
            Cuerpos.Add(descarte);
            return descarte;
        }

        ~SimulationManagerFP()
        {
            Clear();
            world?.Dispose();
            solver?.Dispose();
            broadphase?.Dispose();
            dispatcher?.Dispose();
            collisionConfiguration?.Dispose();
        }
    }
}