CtrEditor/Simulacion/FPhysics.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using FarseerPhysics.Dynamics;
using FarseerPhysics.Factories;
using FarseerPhysics.Collision.Shapes;
using nkast.Aether.Physics2D.Common;

using FarseerPhysics.Common;
using System.Windows;
using System.Diagnostics;

using FarseerPhysics.Dynamics.Joints;
using CtrEditor.ObjetosSim;
using System.Windows.Documents;

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

        public void RemoverBody()
        {
            if (Body != null)
            {
                _world.RemoveBody(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

        public simCurve(World world, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 position)
        {
            _world = world;
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            _startAngle = MathHelper.ToRadians(startAngle);
            _endAngle = 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 = MathHelper.ToRadians(startAngle);
            _endAngle = 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(_world);
            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.UserData = 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;

        public simDescarte(World world, float diameter, Vector2 position)
        {
            _world = world;
            _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 = BodyFactory.CreateCircle(_world, _radius, 1f, position);
            
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.UserData = 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 TransportWithGuides = false;

        public simTransporte(World world, float width, float height, Vector2 position, float angle = 0)
        {
            _world = world;
            Create(width, height, position, angle);
        }

        public float Angle
        {
            get { return MathHelper.ToDegrees(Body.Rotation); }
            set { Body.Rotation = 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.DestroyFixture(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 = BodyFactory.CreateRectangle(_world, width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = MathHelper.ToRadians(angle);
            Body.UserData = this; // Importante para la identificación durante la colisión
        }
    }

    public class simBarrera : simBase
    {
        public bool LuzCortada = false;

        public simBarrera(World world, float width, float height, Vector2 position, float angle = 0)
        {
            _world = world;
            Create(width, height, position, angle);
        }

        public float Angle
        {
            get { return MathHelper.ToDegrees(Body.Rotation); }
            set { Body.Rotation = 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.DestroyFixture(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 = BodyFactory.CreateRectangle(_world, width, height, 1f, position);
            Body.FixtureList[0].IsSensor = true;
            Body.BodyType = BodyType.Static;
            Body.Rotation = MathHelper.ToRadians(angle);
            Body.UserData = this; // Importante para la identificación durante la colisión
            LuzCortada = false;
        }
    }

    public class simGuia : simBase
    {
        public simGuia(World world, Vector2 start, Vector2 end)
        {
            _world = world;
            Create(start, end);
        }

        public void Create(Vector2 start, Vector2 end)
        {
            RemoverBody();
            Body = BodyFactory.CreateEdge(_world, start, end);
            Body.BodyType = BodyType.Static;
            Body.UserData = 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 simBotella(World world, float diameter, Vector2 position, float mass)
        {
            _world = world;
            _radius = diameter / 2;
            _mass = mass;
            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();
            Body = BodyFactory.CreateCircle(_world, _radius, 0.2f, position);
            Body.BodyType = BodyType.Dynamic;

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

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

            // Configurar la fricción
            Body.Friction = 0.3f;  // Ajustar según sea necesario para tu simulación
            
            // Configurar amortiguamiento
            Body.LinearDamping = 0.4f;  // Ajustar para controlar la reducción de la velocidad lineal
            Body.AngularDamping = 0.4f;  // Ajustar para controlar la reducción de la velocidad angular
            Body.Restitution = 0.2f;  // Baja restitución para menos rebote
            // 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, FarseerPhysics.Dynamics.Contacts.Contact contact)
        {
            if (fixtureB.Body.UserData is simBarrera Sensor)
            {
                Sensor.LuzCortada = true;
                return true;
            }
            else if (fixtureB.Body.UserData is simCurve curve)
            {
                curve.ApplyCurveEffect(fixtureA);
                return true; // No aplicar respuestas físicas
            }
            else if (fixtureB.Body.UserData is simDescarte)
            {
                Descartar = true;
                return true;
            } else if (fixtureB.Body.UserData is simTransporte)
            {
                simTransporte conveyor = fixtureB.Body.UserData as simTransporte;

                if ( conveyor.Speed != 0 ) {

                    CircleShape circleShape = fixtureA.Shape as CircleShape;
                    PolygonShape polygonShape = fixtureB.Shape as PolygonShape;

                    // Obtener centro y radio del círculo
                    Vector2 centroCirculo = fixtureA.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(fixtureB.Body.Rotation);
                    float sin = (float)Math.Sin(fixtureB.Body.Rotation);

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

                    // Calcular el porcentaje de la superficie compartida
                    float porcentajeCompartido = InterseccionCirculoRectangulo.CalcularSuperficieCompartida(vertices, centroCirculo, radio);

                    // Aplicar el efecto del transportador usando el porcentaje calculado
                    if (conveyor.TransportWithGuides)
                        if (conveyor.DistanceGuide2Guide <= radio * 2)
                            CenterFixtureOnConveyor(fixtureA, conveyor);
                    ApplyConveyorEffect(conveyor, fixtureA, porcentajeCompartido);

                }
                return true; // No aplicar respuestas físicas
            }
            return true; // No aplicar respuestas físicas
        }

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

        private void CenterFixtureOnConveyor(Fixture fixtureA, simTransporte conveyor)
        {
            // Obtener el centro del conveyor
            Vector2 conveyorCenter = conveyor.Body.Position;

            // Calcular el vector de la línea horizontal centrada de conveyor
            float halfDistance = conveyor.DistanceGuide2Guide / 2;
            float cos = (float)Math.Cos(conveyor.Body.Rotation);
            float sin = (float)Math.Sin(conveyor.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 = fixtureA.Body.Position;
            Vector2 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);

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

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

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

            return lineStart + projectionLength * lineDirection;
        }
    }

    public class SimulationManagerFP
    {
        private World world;
        private Canvas simulationCanvas;
        public List<simBase> Cuerpos;

        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
            Cuerpos = new List<simBase>();
            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);
        }

        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,  innerRadius,  outerRadius,  startAngle,  endAngle,  position);
            Cuerpos.Add(curva);
            return curva;
        }           

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

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

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

        public simDescarte AddDescarte(float diameter, Vector2 position)
        {
            simDescarte descarte = new simDescarte(world, 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.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;
        }
    }
}