CtrEditor/Simulacion/Aether.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using CtrEditor.Convertidores;
using System.Windows;
using System.Diagnostics;
using CtrEditor.ObjetosSim;
using System.Windows.Documents;
using nkast.Aether.Physics2D;
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.Collision;
using Transform = nkast.Aether.Physics2D.Common.Transform;
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.Remove(Body);
            }
        }
        public void SetPosition(float x, float y)
        {
            Body.SetTransform(new Vector2(x, y), Body.Rotation);
        }
        public void SetPosition(Vector2 centro)
        {
            Body.SetTransform(centro, Body.Rotation);
        }
    }

    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 = Microsoft.Xna.Framework.MathHelper.ToRadians(startAngle);
            _endAngle = Microsoft.Xna.Framework.MathHelper.ToRadians(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 = Microsoft.Xna.Framework.MathHelper.ToRadians(startAngle);
            _endAngle = Microsoft.Xna.Framework.MathHelper.ToRadians(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 = new Body();
            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 DistanceGuide2Guide { get; set; }
        public bool isBrake { get; set; }
        public bool TransportWithGuides = false;
        private List<Action> _deferredActions;

        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 Microsoft.Xna.Framework.MathHelper.ToDegrees(Body.Rotation); }
            set { Body.Rotation = Microsoft.Xna.Framework.MathHelper.ToRadians(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);
        }
        public void Create(float width, float height, Vector2 position, float angle = 0)
        {
            RemoverBody();
            Body = _world.CreateRectangle( width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = Microsoft.Xna.Framework.MathHelper.ToRadians(angle);
            Body.Tag = this; // Importante para la identificación durante la colisión
        }
    }

    public class simBarrera : simBase
    {
        public int LuzCortada;
        private List<Action> _deferredActions;

        public simBarrera(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 Microsoft.Xna.Framework.MathHelper.ToDegrees(Body.Rotation); }
            set { Body.Rotation = Microsoft.Xna.Framework.MathHelper.ToRadians(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)
        {
            RemoverBody();
            Body = _world.CreateRectangle( width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = Microsoft.Xna.Framework.MathHelper.ToRadians(angle);
            Body.Tag = this; // Importante para la identificación durante la colisión
            LuzCortada = 0;
        }
    }

    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
    {
        private float _radius;
        private float _mass;
        public bool Descartar = false;

        public int isOnTransports;
        public List<simTransporte> ListOnTransports;
        public bool isRestricted;
        public bool isNoMoreRestricted;
        public float OriginalMass;
        public simTransporte ConveyorRestrictedTo;
        public Fixture axisRestrictedBy;
        
        PrismaticJoint _activeJoint;
        private List<Action> _deferredActions;

        public simBotella(World world, List<Action> deferredActions, float diameter, Vector2 position, float mass)
        {
            _world = world;
            ListOnTransports = new List<simTransporte>();
            _deferredActions = deferredActions;
            _radius = diameter / 2;
            _mass = mass;
            _activeJoint = null;
            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.5f);

            // Configurar amortiguamiento
            Body.LinearDamping = 1f;  // 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(0.1f); // 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;
                return true;
            }
            else if (fixtureB.Body.Tag is simCurve curve)
            {
                curve.ApplyCurveEffect(fixtureA);
                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
        }

        public static bool IntersectAABBs(ref AABB aabbA, ref AABB aabbB, out AABB overlap)
        {
            overlap = new AABB();

            float minX = Math.Max(aabbA.LowerBound.X, aabbB.LowerBound.X);
            float minY = Math.Max(aabbA.LowerBound.Y, aabbB.LowerBound.Y);
            float maxX = Math.Min(aabbA.UpperBound.X, aabbB.UpperBound.X);
            float maxY = Math.Min(aabbA.UpperBound.Y, aabbB.UpperBound.Y);

            if (minX < maxX && minY < maxY)
            {
                overlap.LowerBound = new Vector2(minX, minY);
                overlap.UpperBound = new Vector2(maxX, maxY);
                return true;
            }
            return false;
        }

        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 (isRestricted && fixtureB == axisRestrictedBy)
            {
                isRestricted = false;
                isNoMoreRestricted = true;
            }
            if (fixtureB.Body.Tag is simBarrera Sensor)
                Sensor.LuzCortada -= 1;
        }

        public void ApplyConveyorSpeed()
        {
            Body.LinearVelocity = new Vector2(0.0f, 0.0f);
            foreach (var conveyor in ListOnTransports)
            {
                ApplyConveyorEffect(conveyor);
            }
        }

        private void ApplyConveyorEffect(simTransporte conveyor)
        {
            float speedMetersPerSecond = conveyor.Speed / 60.0f;
            Vector2 desiredVelocity = new Vector2((float)Math.Cos(conveyor.Body.Rotation), (float)Math.Sin(conveyor.Body.Rotation)) * speedMetersPerSecond;
            Body.LinearVelocity += desiredVelocity;
        }

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

                    if (botella.isRestricted)
                    {
                        botella.CenterFixtureOnConveyor();                        
                        botella.Body.Inertia = 0;
                        botella.Body.Mass = 20;
                    } else if (botella.isNoMoreRestricted)
                    {
                        botella.isNoMoreRestricted = false;
                        botella.Body.Mass = botella.OriginalMass;
                    }
                }
            }

            // 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)
        {
            simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle);
            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;
        }
    }
}