CtrEditor/Simulacion/Aether.cs

using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;

using System.Windows;
using System.Diagnostics;
using nkast.Aether.Physics2D.Dynamics;
using nkast.Aether.Physics2D.Common;
using nkast.Aether.Physics2D.Collision.Shapes;
using nkast.Aether.Physics2D.Dynamics.Contacts;
using nkast.Aether.Physics2D.Dynamics.Joints;


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

        public void RemoverBody()
        {
            if (Body != null && _world.BodyList.Count>0 && _world.BodyList.Contains(Body))
            {
                _world.Remove(Body);
            }
        }
        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)
        {
            Body.SetTransform(new Vector2(x, y), Body.Rotation);
        }
        public void SetPosition(Vector2 centro)
        {
            try
            {
                Body.SetTransform(centro, Body.Rotation);
            }
            catch { }
        }
    }

    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
        private List<Action> _deferredActions;

        public simCurve(World world, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 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, Vector2 position)
        {
            if (_world == null) return;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            _startAngle = GradosARadianes(startAngle);
            _endAngle = GradosARadianes(endAngle);
            Create(position);
        }

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

            // Crear la geometría del sensor de curva
            List<Vertices> segments = CreateCurveVertices(_innerRadius, _outerRadius, _startAngle, _endAngle);
            Body = _world.CreateBody();
            foreach (var segment in segments)
            {
                var shape = new PolygonShape(segment, 1f);
                var fixture = Body.CreateFixture(shape);
                fixture.IsSensor = true;
            }            
            Body.Position = position;
            Body.BodyType = BodyType.Static;
            Body.Tag = this;
        }

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

        private List<Vertices> CreateCurveVertices(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            List<Vertices> verticesList = new List<Vertices>();
            int segments = 32;
            float angleStep = (endAngle - startAngle) / segments;

            Vertices innerVertices = new Vertices();
            Vertices outerVertices = new Vertices();

            for (int i = 0; i <= segments; i++)
            {
                float angle = startAngle + i * angleStep;
                innerVertices.Add(new Vector2(innerRadius * (float)Math.Cos(angle), innerRadius * (float)Math.Sin(angle)));
                outerVertices.Add(new Vector2(outerRadius * (float)Math.Cos(angle), outerRadius * (float)Math.Sin(angle)));
            }

            outerVertices.Reverse();
            innerVertices.AddRange(outerVertices);
            verticesList.Add(innerVertices);

            return verticesList;
        }

        public void ApplyCurveEffect(Fixture bottle)
        {
            Vector2 centerToBottle = bottle.Body.Position - Body.Position;
            float distanceToCenter = centerToBottle.Length();

            if (distanceToCenter >= _innerRadius && distanceToCenter <= _outerRadius)
            {
                // Calcular la velocidad tangencial
                float speedMetersPerSecond = Speed / 60.0f;
                float angularVelocity = speedMetersPerSecond / distanceToCenter;

                // Vector tangente (perpendicular al radio)
                Vector2 tangent = new Vector2(-centerToBottle.Y, centerToBottle.X);
                tangent.Normalize();

                // Velocidad deseada
                Vector2 desiredVelocity = tangent * angularVelocity * distanceToCenter;
                bottle.Body.LinearVelocity = desiredVelocity;
            }
        }
    }


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

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

        public void SetDiameter(float diameter)
        {
            _radius = diameter / 2;
            Create(Body.Position);  // Recrear el círculo con el nuevo tamaño
        }

        public void Create(Vector2 position)
        {
            RemoverBody();
            Body = _world.CreateCircle(_radius, 1f, position);

            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Tag = this; // Importante para la identificación durante la colisión
        }
    }


    public class simTransporte : simBase
    {
        public float Speed { get; set; } // Velocidad para efectos de cinta transportadora
        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(World world, List<Action> deferredActions, float width, float height, Vector2 position, float angle = 0)
        {
            _world = world;
            _deferredActions = deferredActions;
            Create(width, height, position, angle);
        }

        public float Angle
        {
            get { return RadianesAGrados(Body.Rotation); }
            set { Body.Rotation = GradosARadianes(value); }
        }

        public new void SetPosition(float x, float y)
        {
            Body.Position = new Vector2(x, y);
        }

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

        public void SetDimensions(float width, float height)
        {
            Body.Remove(Body.FixtureList[0]);

            var newShape = new PolygonShape(PolygonTools.CreateRectangle(width / 2, height / 2), 1f);
            Body.CreateFixture(newShape);
            Width = width;
            Height = height;
        }
        public void Create(float width, float height, Vector2 position, float angle = 0)
        {
            RemoverBody();
            Width = width;
            Height = height;
            Friction = 0.1f;
            Body = _world.CreateRectangle( width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = GradosARadianes(angle);
            Body.Tag = this; // Importante para la identificación durante la colisión
        }
    }

    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(World world, List<Action> deferredActions, float width, float height, Vector2 position, float angle = 0, bool detectectNeck = false)
        {
            _world = world;
            _height = height;
            DetectNeck = detectectNeck;
            _deferredActions = deferredActions;
            ListSimBotellaContact = new List<simBotella>();
            Create(width, height, position, angle);
        }

        public float Angle
        {
            get { return RadianesAGrados(Body.Rotation); }
            set { Body.Rotation = GradosARadianes(value); }
        }

        public new void SetPosition(float x, float y)
        {
            Body.Position = new Vector2(x, y);
        }

        public void SetDimensions(float width, float height)
        {
            Body.Remove(Body.FixtureList[0]);

            var newShape = new PolygonShape(PolygonTools.CreateRectangle(width / 2, height / 2), 1f);
            Body.CreateFixture(newShape);
        }

        public void Create(float width, float height, Vector2 position, float angle = 0, bool detectectNeck = false)
        {
            RemoverBody();
            _height = height;
            DetectNeck = detectectNeck;
            Body = _world.CreateRectangle( width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = GradosARadianes(angle);
            Body.Tag = this; // Importante para la identificación durante la colisión
            LuzCortada = 0;
        }

        public void CheckIfNecksIsTouching()
        {
            if (LuzCortada == 0) return;

            foreach (var botella in ListSimBotellaContact)
            {
                // Obtener el centro de la barrera
                Vector2 sensorCenter = Body.Position;

                // Calcular el vector de la línea horizontal centrada de la barrera
                float halfHeight = _height / 2;
                float cos = (float)Math.Cos(Body.Rotation);
                float sin = (float)Math.Sin(Body.Rotation);

                // Calcular los puntos inicial y final de la línea horizontal centrada y rotada
                Vector2 lineStart = sensorCenter + new Vector2(-halfHeight * cos, halfHeight * sin);
                Vector2 lineEnd = sensorCenter + new Vector2(halfHeight * cos, -halfHeight * sin);

                // Proyectar el centro de la botella sobre la línea horizontal
                Vector2 fixtureCenter = botella.Body.Position;
                Vector2 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);

                // Calcular la distancia entre el punto más cercano y el centro del cuello de la botella
                float distance;
                Vector2.Distance(ref closestPoint,ref fixtureCenter, out distance);
                Distancia = distance;
                if (distance <= botella._neckRadius)
                {
                    LuzCortadaNeck = true;
                    return;
                }
            }
            LuzCortadaNeck = false;
        }

        private Vector2 ProjectPointOntoLine(Vector2 point, Vector2 lineStart, Vector2 lineEnd)
        {
            Vector2 lineDirection = lineEnd - lineStart;
            lineDirection.Normalize();

            Vector2 pointToLineStart = point - lineStart;
            float projectionLength;
            Vector2.Dot(ref pointToLineStart, ref lineDirection, out projectionLength);

            return lineStart + projectionLength * lineDirection;
        }



    }

    public class simGuia : simBase
    {
        private List<Action> _deferredActions;
        public simGuia(World world, List<Action> deferredActions, Vector2 start, Vector2 end)
        {
            _world = world;
            _deferredActions = deferredActions;
            Create(start, end);
        }

        public void Create(Vector2 start, Vector2 end)
        {
            RemoverBody();
            Body = _world.CreateEdge( start, end);
            Body.BodyType = BodyType.Static;
            Body.Tag = this; // Importante para la identificación durante la colisión
        }

        public void UpdateVertices(Vector2 newStart, Vector2 newEnd)
        {
            Create(newStart, newEnd); // Recrear la línea con nuevos vértices
        }
    }

    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 Fixture axisRestrictedBy;

        public float OverlapPercentage;

        public float _neckRadius;
        
        PrismaticJoint _activeJoint;
        private List<Action> _deferredActions;

        public simBotella(World world, List<Action> deferredActions, float diameter, Vector2 position, float mass,float neckRadius = 0)
        {
            _world = world;
            ListOnTransports = new List<simBase>();
            _deferredActions = deferredActions;
            Radius = diameter / 2;
            _mass = mass;
            _activeJoint = null;

            if (neckRadius<=0)
                neckRadius = diameter / 4;

            _neckRadius = neckRadius;

            Create(position);
        }

        public float CenterX
        {
            get { return Body.Position.X; }
            set { }
        }

        public float CenterY
        {
            get { return Body.Position.Y; }
            set { }
        }

        public Vector2 Center
        {
            get { return Body.Position; }
        }

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

        private void Create(Vector2 position)
        {
            RemoverBody();
            isOnTransports = 0;
            _activeJoint = null;

            Body = _world.CreateCircle( Radius, 1f, position);
            Body.BodyType = BodyType.Dynamic;

            // Restablecer manejador de eventos de colisión
            Body.OnCollision += HandleCollision;
            Body.OnSeparation += HandleOnSeparation;

            Body.Tag = this; // Importante para la identificación durante la colisión

            // Configurar la fricción          
            Body.SetFriction(0.2f);

            // Configurar amortiguamiento
            Body.LinearDamping = 3f;  // Ajustar para controlar la reducción de la velocidad lineal
            Body.AngularDamping = 1f;  // Ajustar para controlar la reducción de la velocidad angular
            Body.SetRestitution(0f); // Baja restitución para menos rebote            

            Body.SleepingAllowed = false;
            Body.IsBullet = true;
        }

        public void SetDiameter(float diameter)
        {
            Radius = diameter / 2;
            Create(Body.Position);  // Recrear el círculo con el nuevo tamaño
        }

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

        private bool HandleCollision(Fixture fixtureA, Fixture fixtureB, Contact contact)
        {
            if (fixtureB.Body.Tag is simBarrera Sensor)
            {
                Sensor.LuzCortada += 1;
                Sensor.ListSimBotellaContact.Add(this);
                return true;
            }
            else if (fixtureB.Body.Tag is simCurve curve)
            {
                isOnTransports += 1;
                ListOnTransports.Add(curve);
                return true; // No aplicar respuestas físicas
            }
            else if (fixtureB.Body.Tag is simDescarte)
            {
                Descartar = true;
                return true;
            }
            else if (fixtureB.Body.Tag is simTransporte)
            {
                simTransporte conveyor = fixtureB.Body.Tag as simTransporte;

                isOnTransports += 1;
                ListOnTransports.Add(conveyor);

                // Aplicar el efecto del transportador usando el porcentaje calculado
                if (conveyor.TransportWithGuides && conveyor.isBrake)
                {
                    ConveyorRestrictedTo = conveyor;
                    axisRestrictedBy = fixtureB;
                    OriginalMass = Body.Mass;
                    isRestricted = true;
                    isNoMoreRestricted = false;
                }
                return true; // No aplicar respuestas físicas
            }
            return true; // No aplicar respuestas físicas
        }

        private float CalculateOverlapPercentage(simTransporte conveyor)
        {
            CircleShape circleShape = Body.FixtureList[0].Shape as CircleShape;
            PolygonShape polygonShape = conveyor.Body.FixtureList[0].Shape as PolygonShape;

            // Obtener centro y radio del círculo
            Vector2 centroCirculo = Body.Position;
            float radio = circleShape.Radius;

            // Obtener los vértices del polígono (rectángulo)
            Vector2[] vertices = new Vector2[polygonShape.Vertices.Count];
            float cos = (float)Math.Cos(conveyor.Body.Rotation);
            float sin = (float)Math.Sin(conveyor.Body.Rotation);

            for (int i = 0; i < polygonShape.Vertices.Count; i++)
            {
                Vector2 vertex = polygonShape.Vertices[i];
                float rotatedX = vertex.X * cos - vertex.Y * sin + conveyor.Body.Position.X;
                float rotatedY = vertex.X * sin + vertex.Y * cos + conveyor.Body.Position.Y;
                vertices[i] = new Vector2(rotatedX, rotatedY);
            }

            // Calcular el porcentaje de la superficie compartida
            return InterseccionCirculoRectangulo.CalcularSuperficieCompartida(vertices, centroCirculo, radio);
        }

        private void HandleOnSeparation(Fixture sender, Fixture fixtureB, Contact contact)
        {
            if (isOnTransports > 0 && fixtureB.Body.Tag is simTransporte)
            {
                if (fixtureB.Body.Tag is simTransporte transport)
                    ListOnTransports.Remove(transport);
                isOnTransports -= 1;
            }
            if (isOnTransports > 0 && fixtureB.Body.Tag is simCurve)
            {
                if (fixtureB.Body.Tag is simCurve transport)
                    ListOnTransports.Remove(transport);
                isOnTransports -= 1;
            }
            if (isRestricted && fixtureB == axisRestrictedBy)
            {
                isRestricted = false;
                isNoMoreRestricted = true;
            }
            if (fixtureB.Body.Tag is simBarrera Sensor)
            {
                Sensor.LuzCortada -= 1;
                Sensor.ListSimBotellaContact.Remove(this);
            }
        }
        /// <summary>
        /// Aplica la fuerza de traccion a la botellas segun los
        /// transportes sobre los que se encuentra
        /// </summary>
        /// <param name="deltaTime_s"></param>
        public void ApplyConveyorSpeed(float deltaTime_s)
        {
            foreach (var transporte in ListOnTransports)
            {
                if (transporte is simTransporte conveyorRect)
                    if (ApplyConveyorEffect(deltaTime_s, conveyorRect))
                        break;
                if (transporte is simCurve conveyorCurve)
                    conveyorCurve.ApplyCurveEffect(Body.FixtureList[0]);
            }
        }

        private bool ApplyConveyorEffect(float deltaTime_s, simTransporte conveyor)
        {
            // Calcular el porcentaje de superficie sobrepuesta
            float overlapPercentage = CalculateOverlapedArea(this, conveyor);  // CalculateOverlapPercentage(conveyor);
            OverlapPercentage = overlapPercentage;

            // Calcular la velocidad deseada del transporte
            float speedMetersPerSecond = conveyor.Speed / 60.0f;
            Vector2 desiredVelocity = new Vector2((float)Math.Cos(conveyor.Body.Rotation), (float)Math.Sin(conveyor.Body.Rotation)) * speedMetersPerSecond;

            // Obtener la velocidad actual de la botella
            Vector2 currentVelocity = Body.LinearVelocity;

            // Calcular la diferencia de velocidad deseada
            Vector2 velocityDifference = desiredVelocity - currentVelocity;

            // Calcular la fuerza de fricción necesaria - 0 : ya esta en velocidad - 1 : esta detenido
            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;
            }
            // Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta
            Body.LinearVelocity += frictionCoefficient * desiredVelocity;
            //Body.ApplyForce(frictionForce * overlapPercentage);
            return false;
        }

        public float CalculateOverlapedArea(simBotella botella, simTransporte conveyor)
        {
            //float areaBotella = (float) ((botella.Radius * botella.Radius * Math.PI) / (Math.PI / 4)); // Math.PI/4 porque es un cuadrado en vez de un circulo
            //float area = OverlapedArea.CalculateOverlapedArea(botella.Body, conveyor.Body);
            //if (areaBotella == 0) return 0;
            //return area/areaBotella;
            return OverlapedAreaFast.CalculateOverlapedArea(botella, conveyor);
        }

        public static float CalculateProportion(Vector2 currentVelocity, Vector2 desiredVelocity)
        {
            // Calcular la proyección escalar de v2 sobre v1
            float dotProduct;
            Vector2.Dot(ref desiredVelocity, ref currentVelocity, out dotProduct);
            float magnitudeV1Squared = desiredVelocity.LengthSquared();

            // Si la magnitud de v1 es 0, la proporción no está definida
            if (magnitudeV1Squared == 0)
            {
                return 0;
            }

            // Calcular la proporción
            float proportion = dotProduct / magnitudeV1Squared;
            return proportion;
        }

        public void CenterFixtureOnConveyor()
        {
            if (!isRestricted || ConveyorRestrictedTo == null)
                return;

            // Obtener el centro del conveyor
            Vector2 conveyorCenter = ConveyorRestrictedTo.Body.Position;

            // Calcular el vector de la línea horizontal centrada de conveyor
            float halfDistance = ConveyorRestrictedTo.DistanceGuide2Guide / 2;
            float cos = (float)Math.Cos(ConveyorRestrictedTo.Body.Rotation);
            float sin = (float)Math.Sin(ConveyorRestrictedTo.Body.Rotation);

            Vector2 offset = new Vector2(halfDistance * cos, halfDistance * sin);

            // Línea horizontal centrada de conveyor en el espacio del mundo
            Vector2 lineStart = conveyorCenter - offset;
            Vector2 lineEnd = conveyorCenter + offset;

            // Proyectar el centro de fixtureA sobre la línea horizontal
            Vector2 fixtureCenter = Body.Position;
            Vector2 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);

            // Mover fixtureA al punto más cercano en la línea horizontal
            Body.Position = closestPoint;
        }

        private Vector2 ProjectPointOntoLine(Vector2 point, Vector2 lineStart, Vector2 lineEnd)
        {
            Vector2 lineDirection = lineEnd - lineStart;
            lineDirection.Normalize();

            Vector2 pointToLineStart = point - lineStart;
            Vector2.Dot(ref pointToLineStart,ref lineDirection, out float projectionLength);

            return lineStart + projectionLength * lineDirection;
        }
    }

    public class SimulationManagerFP
    {
        private World world;
        private Canvas simulationCanvas;
        public List<simBase> Cuerpos;
        public List<Action> _deferredActions;
        SolverIterations Iterations;

        private Stopwatch stopwatch;
        private double stopwatch_last;

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

        public SimulationManagerFP()
        {
            world = new World(new Vector2(0, 0));  // Vector2.Zero

            SolverIterations Iterations = new SolverIterations();
            Iterations.PositionIterations = 1;
            Iterations.VelocityIterations = 1;

            Cuerpos = new List<simBase>();
            _deferredActions = new List<Action>();
            stopwatch = new Stopwatch();
            stopwatch.Start();
        }

        public void Clear()
        {
            if (world.BodyList.Count > 0)
                world.Clear();
            if (Cuerpos.Count > 0)
                Cuerpos.Clear();
        }
        // ******************************************************************************************************************************************
        // ******************************************************************************************************************************************

        public void Step()
        {
            // Detener el cronómetro y obtener el tiempo transcurrido en milisegundos
            float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
            stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;

            // Pasar el tiempo transcurrido al método Step
            world.Step(elapsedMilliseconds / 1000.0f);
           
            foreach (var cuerpo in Cuerpos)
            {
                if (cuerpo is simBotella botella)
                {
                    botella.ApplyConveyorSpeed(elapsedMilliseconds/1000);

                    if (botella.isRestricted)
                    {
                        botella.CenterFixtureOnConveyor();
                        botella.Body.Inertia = 0;
                        botella.Body.Mass = 100;
                    }
                    else if (botella.isNoMoreRestricted)
                    {
                        botella.isNoMoreRestricted = false;
                        botella.Body.Mass = botella.OriginalMass;
                    }
                }
                else if (cuerpo is simBarrera barrera)
                    barrera.CheckIfNecksIsTouching();
            }

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

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

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

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

        public simTransporte AddRectangle(float width, float height, Vector2 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, Vector2 position, float angle, bool detectarCuello)
        {
            simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle, detectarCuello);
            Cuerpos.Add(rectangle);
            return rectangle;
        }

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

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

        public void Debug_DrawInitialBodies()
        {
            Debug_ClearSimulationShapes();
            world.Step(0.01f);  // Para actualizar la BodyList
            foreach (Body body in world.BodyList)
            {
                foreach (Fixture fixture in body.FixtureList)
                {
                    DrawShape(fixture);
                }
            }
        }

        public void Debug_ClearSimulationShapes()
        {
            var simulationShapes = simulationCanvas.Children.OfType<System.Windows.Shapes.Shape>().Where(s => s.Tag as string == "Simulation").ToList();
            foreach (var shape in simulationShapes)
            {
                simulationCanvas.Children.Remove(shape);
            }
        }

        private void DrawShape(Fixture fixture)
        {
            System.Windows.Shapes.Shape shape;
            switch (fixture.Shape.ShapeType)
            {
                case ShapeType.Circle:
                    shape = DrawCircle(fixture);
                    break;
                case ShapeType.Polygon:
                    shape = DrawPolygon(fixture);
                    break;
                case ShapeType.Edge:
                    shape = DrawEdge(fixture);
                    break;
                default:
                    return;
            }
            shape.Tag = "Simulation"; // Marcar para simulación
            Canvas.SetZIndex(shape, 20);
            simulationCanvas.Children.Add(shape);
        }

        private float p(float x)
        {
            float c = PixelToMeter.Instance.calc.MetersToPixels(x);
            return c;
        }

        private System.Windows.Shapes.Shape DrawEdge(Fixture fixture)
        {
            EdgeShape edge = fixture.Shape as EdgeShape;
            Line line = new Line
            {
                X1 = p(edge.Vertex1.X + fixture.Body.Position.X),  // Aplicar escala y posición
                Y1 = p(edge.Vertex1.Y + fixture.Body.Position.Y),
                X2 = p(edge.Vertex2.X + fixture.Body.Position.X),
                Y2 = p(edge.Vertex2.Y + fixture.Body.Position.Y),
                Stroke = Brushes.Black,
                StrokeThickness = 2
            };
            return line;
        }

        private System.Windows.Shapes.Shape DrawCircle(Fixture fixture)
        {
            CircleShape circle = fixture.Shape as CircleShape;
            Ellipse ellipse = new Ellipse
            {
                Width = p(circle.Radius * 2), // Escalado para visualización
                Height = p(circle.Radius * 2), // Escalado para visualización
                Stroke = Brushes.Black,
                StrokeThickness = 2
            };
            Canvas.SetLeft(ellipse, p(fixture.Body.Position.X - circle.Radius));
            Canvas.SetTop(ellipse, p(fixture.Body.Position.Y - circle.Radius));
            return ellipse;
        }

        private System.Windows.Shapes.Shape DrawPolygon(Fixture fixture)
        {
            Polygon polygon = new Polygon { Stroke = Brushes.Black, StrokeThickness = 2 };
            PolygonShape polyShape = fixture.Shape as PolygonShape;

            float cos = (float)Math.Cos(fixture.Body.Rotation);
            float sin = (float)Math.Sin(fixture.Body.Rotation);

            foreach (Vector2 vertex in polyShape.Vertices)
            {
                float rotatedX = vertex.X * cos - vertex.Y * sin + fixture.Body.Position.X;
                float rotatedY = vertex.X * sin + vertex.Y * cos + fixture.Body.Position.Y;

                polygon.Points.Add(new Point(p(rotatedX), p(rotatedY)));
            }

            return polygon;
        }
    }
}