using CtrEditor.ObjetosSim;
using OpenCvSharp;
using System;
using System.Collections.Generic;
using System.Diagnostics.Eventing.Reader;
using System.Numerics;
using System.Windows.Shapes;

public class Circle
{
    private Vector2 position;
    public float Left
    {
        get { return position.X; }
        set { position.X = value; }
    }
    public float Top
    {
        get { return position.Y; }
        set { position.Y = value; }
    }
    public float Diameter { get; set; }
    public float Mass { get; set; }
    public float AngleofMovement { get; set; }  // En grados
    public float Speed { get; set; }
    public float Overlap { get; set; }

    public Circle(float left = 0, float top = 0, float diameter = 10, float mass = 1, float angle = 0, float speed = 0)
    {
        position = new Vector2(left, top);
        Diameter = diameter;
        Mass = mass;
        AngleofMovement = angle;
        Speed = speed;
    }

    public void Move(float timeStep_ms, List<Circle> circles, List<Rectangle> rectangles, List<Line> lines)
    {
        // Convertir timeStep de milisegundos a segundos para la simulación
        float timeStepInSeconds = timeStep_ms / 1000.0f;
        bool isTracted = false; // Indicador para verificar si el círculo está siendo traccionado
        Overlap = 0;

        // Aplicar fuerza desde el rectángulo si está sobre uno
        foreach (var rectangle in rectangles)
        {
            float overlap = CalculateOverlapPercentage(this, rectangle);
            if (overlap > 10)
            {
                Overlap += overlap;
                isTracted = true; // El círculo está siendo traccionado por un rectángulo
                                  // Convertir la velocidad del rectángulo de metros por minuto a metros por segundo
                float rectangleSpeedInMetersPerSecond = rectangle.Speed / 60.0f;

                if (rectangleSpeedInMetersPerSecond < Speed)
                {
                    // Aplicar una fuerza de frenado si la velocidad del rectángulo es menor que la velocidad del círculo
                    float brakingForce = (Speed - rectangleSpeedInMetersPerSecond) * (overlap / 100.0f);
                    Speed -= brakingForce * timeStepInSeconds;
                }
                else
                {
                    // Alinear gradualmente la velocidad del círculo con la del rectángulo si es mayor
                    Speed += (rectangleSpeedInMetersPerSecond - Speed) * (overlap / 100.0f) * timeStepInSeconds;
                }

                AngleofMovement = rectangle.Angle;
            }
        }

        // Si el círculo no está siendo traccionado, aplicar desaceleración
        if (!isTracted)
        {
            float deceleration = (1.0f / Mass) * 10.0f; // Coeficiente de desaceleración inversamente proporcional a la masa
            Speed -= deceleration * timeStepInSeconds;
            if (Speed < 0) Speed = 0; // Evitar que la velocidad sea negativa
        }

        // Calcular nueva posición
        Vector2 direction = new Vector2((float)Math.Cos(AngleofMovement * Math.PI / 180), (float)Math.Sin(AngleofMovement * Math.PI / 180));
        Vector2 velocity = direction * Speed * timeStepInSeconds;
        position += velocity;

        // Ajustar por colisiones con líneas
        foreach (var line in lines)
        {
            Vector2 movementVector = Vector2FromPolar(Speed, AngleofMovement);
            Vector2 circleCenter = GetCircleCenter(this);

            // Calcular la nueva posición tentativa del centro del círculo
            Vector2 newPosition = circleCenter + movementVector * timeStepInSeconds;

            if (LineCircleCollision(newPosition, Diameter / 2, line, out Vector2 collisionPoint))
            {
                // Ajustar la posición del centro del círculo y el vector de movimiento
                AdjustCircleAfterCollision(ref newPosition, ref movementVector, line, collisionPoint, Diameter / 2);
            }

            // Actualizar la posición del círculo basada en el nuevo centro
            Left = newPosition.X - Diameter / 2;
            Top = newPosition.Y - Diameter / 2;
            Speed = movementVector.Length();
            AngleofMovement = PolarAngleFromVector(movementVector);
        }

        // Ajustar por superposición con otros círculos
        foreach (var other in circles)
        {
            if (this != other && IsColliding(this, other))
            {
                AdjustForOverlap(other);
            }
        }

    }

    Vector2 GetCircleCenter(Circle circle)
    {
        return new Vector2(circle.Left + circle.Diameter / 2, circle.Top + circle.Diameter / 2);
    }


    private bool LineCircleCollision(Vector2 circleCenter, float radius, Line line, out Vector2 collisionPoint)
    {
        // Transformar la línea a un vector con el ángulo rotado
        float angleRadians = line.Angle * (float)Math.PI / 180;
        Vector2 lineDirection = new Vector2(
            (float)Math.Cos(angleRadians),
            (float)Math.Sin(angleRadians)
        );

        // Calcular los puntos de inicio y fin de la línea
        Vector2 lineStart = new Vector2(line.Left, line.Top);
        Vector2 lineEnd = lineStart + lineDirection * line.Length;

        // Encontrar el punto más cercano del centro del círculo a la línea
        Vector2 lineToCircle = circleCenter - lineStart;
        float projection = Vector2.Dot(lineToCircle, lineDirection);
        projection = Math.Clamp(projection, 0, line.Length);
        Vector2 closestPointOnLine = lineStart + projection * lineDirection;

        // Calcular la distancia entre el círculo y el punto más cercano
        float distance = Vector2.Distance(circleCenter, closestPointOnLine);
        collisionPoint = closestPointOnLine;

        // Verificar si hay colisión
        return distance <= radius;
    }

    private void AdjustCircleAfterCollision(ref Vector2 circlePosition, ref Vector2 movementVector, Line line, Vector2 collisionPoint, float radius)
    {
        // Calcular el vector normal de la línea para reflejar la dirección de movimiento del círculo
        float angleRadians = line.Angle * (float)Math.PI / 180;
        Vector2 lineNormal = new Vector2(-(float)Math.Sin(angleRadians), (float)Math.Cos(angleRadians));

        // Asegurar que la normal está correctamente orientada hacia fuera de la línea
        if (Vector2.Dot(lineNormal, circlePosition - collisionPoint) < 0)
            lineNormal = -lineNormal;

        // Calcular el desplazamiento necesario para separar el círculo de la línea
        Vector2 displacement = lineNormal * (radius - Vector2.Distance(circlePosition, collisionPoint));
        circlePosition += displacement;

        // Opcionalmente, ajustar la velocidad si el círculo debe "rebotar" de la línea
        movementVector = Vector2.Reflect(movementVector, lineNormal);
    }


    private Vector2 Vector2FromPolar(float speed, float angleDegrees)
    {
        float angleRadians = angleDegrees * (float)Math.PI / 180;
        return new Vector2(
            speed * (float)Math.Cos(angleRadians),
            speed * (float)Math.Sin(angleRadians)
        );
    }

    private float PolarAngleFromVector(Vector2 vector)
    {
        return (float)Math.Atan2(vector.Y, vector.X) * 180 / (float)Math.PI;
    }


    private void AdjustForOverlap(Circle other)
    {
        if (this == other) return; // No auto-interacción

        float distance = Vector2.Distance(this.position, other.position);
        float radiusSum = (this.Diameter / 2) + (other.Diameter / 2);

        if (distance < radiusSum) // Los círculos están solapando
        {
            Vector2 directionToOther = Vector2.Normalize(other.position - this.position);
            float overlapDistance = radiusSum - distance;

            // Decidir qué círculo mover basado en sus velocidades
            if (this.Speed == 0 && other.Speed > 0)
            {
                // Mover este círculo si su velocidad es cero y el otro se está moviendo
                this.position -= directionToOther * overlapDistance;
            }
            else if (other.Speed == 0 && this.Speed > 0)
            {
                // Mover el otro círculo si su velocidad es cero y este se está moviendo
                other.position += directionToOther * overlapDistance;
            }
            else if (this.Speed == 0 && other.Speed == 0)
            {
                // Si ambos tienen velocidad cero, mover ambos a la mitad del solapamiento
                this.position -= directionToOther * (overlapDistance / 2);
                other.position += directionToOther * (overlapDistance / 2);
            }
        }
    }


    private bool IsColliding(Circle circle1, Circle circle2)
    {
        float distance = Vector2.Distance(circle1.position, circle2.position);
        float radiusSum = (circle1.Diameter / 2) + (circle2.Diameter / 2);
        return distance <= radiusSum;
    }

    // Circulos sobre Rectangulos
    public static float CalculateOverlapPercentage(Circle circle, Rectangle rectangle)
    {
        // Convertir el círculo en un cuadrado aproximado.
        float squareSide = circle.Diameter / (float)Math.Sqrt(Math.PI);
        RotatedRect square = new RotatedRect(
            new Point2f(circle.Left + circle.Diameter / 2, circle.Top + circle.Diameter / 2),
            new Size2f(squareSide, squareSide),
            0  // Sin rotación
        );

        // Ajustamos el rectángulo para que se considere rotado desde el centro, pero calculado desde Top-Left
        RotatedRect rotatedRectangle = CreateRotatedRectFromTopLeft(rectangle);

        // Usar OpenCV para encontrar la intersección.
        using (var mat = new Mat())
        {
            var result = Cv2.RotatedRectangleIntersection(square, rotatedRectangle, mat);
            if (result != RectanglesIntersectTypes.None)
            {
                // Calcular el área de la intersección
                float intersectionArea = (float) Cv2.ContourArea(mat);
                float circleArea = (float)(Math.PI * Math.Pow(circle.Diameter / 2, 2));
                return (intersectionArea / circleArea) * 100;
            }
        }

        return 0; // No hay intersección
    }

    public static RotatedRect CreateRotatedRectFromTopLeft(Rectangle rectangle)
    {
        // El punto de pivote es Top-Left, calculamos el centro sin rotar
        float originalCenterX = rectangle.Left + rectangle.Length / 2.0f;
        float originalCenterY = rectangle.Top + rectangle.Width / 2.0f;

        // Convertimos el ángulo a radianes para la rotación
        float angleRadians = rectangle.Angle * (float)Math.PI / 180;

        // Calcular las nuevas coordenadas del centro después de la rotación
        float rotatedCenterX = rectangle.Left + (originalCenterX - rectangle.Left) * (float)Math.Cos(angleRadians) - (originalCenterY - rectangle.Top) * (float)Math.Sin(angleRadians);
        float rotatedCenterY = rectangle.Top + (originalCenterX - rectangle.Left) * (float)Math.Sin(angleRadians) + (originalCenterY - rectangle.Top) * (float)Math.Cos(angleRadians);

        // Crear el RotatedRect con el nuevo centro y el tamaño original
        RotatedRect rotatedRect = new RotatedRect(
            new Point2f(rotatedCenterX, rotatedCenterY),
            new Size2f(rectangle.Length, rectangle.Width),
            rectangle.Angle
        );

        return rotatedRect;
    }



}

public class Rectangle
{
    private Vector2 position;
    public float Left
    {
        get { return position.X; }
        set { position = new Vector2(value, position.Y); }
    }
    public float Top
    {
        get { return position.Y; }
        set { position = new Vector2(position.X, value); }
    }
    public float Length { get; set; }
    public float Width { get; set; }
    public float Angle { get; set; }  // En grados
    public float Speed { get; set; }  // Velocidad del rectángulo

    public Rectangle(float left = 0, float top = 0, float length = 10, float width = 10, float angle = 0, float speed = 0)
    {
        position = new Vector2(left, top);
        Length = length;
        Width = width;
        Angle = angle;
        Speed = speed;
    }

}

public class Line
{
    private Vector2 position;
    public float Left
    {
        get { return position.X; }
        set { position = new Vector2(value, position.Y); }
    }
    public float Top
    {
        get { return position.Y; }
        set { position = new Vector2(position.X, value); }
    }
    public float Length { get; set; }
    public float Width { get; set; }
    public float Angle { get; set; }  // En grados
    public float Grip { get; set; }  // Friccion por contacto

    public Line(float left = 0, float top = 0, float length = 10, float angle = 0, float grip = 0)
    {
        position = new Vector2(left, top);
        Length = length;
        Angle = angle;
        Grip = grip;
    }
}

public class Square
{
    public float Left { get; set; }
    public float Top { get; set; }
    public float Size { get; set; }  // 'Size' es la longitud de un lado del cuadrado

    public Square(float left, float top, float size)
    {
        Left = left;
        Top = top;
        Size = size;
    }
}



// Clase principal que gestiona la simulación
public class SimulationManager
{
    public List<Circle> circles;
    public List<Rectangle> rectangles;
    public List<Line> lines;    


    public SimulationManager()
    {
        circles = new List<Circle>();
        rectangles = new List<Rectangle>();
        lines = new List<Line>();
    }

    public void Step(float timeStep)
    {
        foreach (var circle in circles)
        {
            circle.Move(timeStep, circles, rectangles, lines);
        }
    }
}