CtrEditor/Simulacion/simCurve.cs

using BepuPhysics.Collidables;
using BepuPhysics;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;

namespace CtrEditor.Simulacion
{
    /// <summary>
    /// Representa una curva o arco en la simulación física.
    /// ✅ IMPORTANTE: Los ángulos _startAngle y _endAngle se almacenan INTERNAMENTE en radianes BEPU
    /// (ya convertidos desde grados WPF usando CoordinateConverter.WpfDegreesToBepuRadians)
    /// Las propiedades públicas StartAngle/EndAngle devuelven grados WPF usando CoordinateConverter.BepuRadiansToWpfDegrees
    /// ✅ NUEVO: Cada triángulo actúa como un mini-transporte con su propia dirección tangencial fija
    /// </summary>
    public class simCurve : simBase
    {
        private float _innerRadius;
        private float _outerRadius;
        private float _startAngle;  // ✅ SIEMPRE en radianes BEPU
        private float _endAngle;    // ✅ SIEMPRE en radianes BEPU
        public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)

        private List<Action> _deferredActions;

        // ✅ NUEVO: Almacenar el centro real de la curva
        private Vector3 _curveCenter;

        // ✅ SIMPLIFICADO: Propiedades esenciales únicamente
        public List<simBotella> BottlesOnCurve { get; private set; } = new List<simBotella>();

        // ✅ EVENTO para actualización de motores
        public event Action<simCurve> OnSpeedChanged;

        // ✅ NUEVO: Almacenar triángulos creados para acceso directo
        private Triangle[] _storedTriangles;

        public float InnerRadius => _innerRadius;
        public float OuterRadius => _outerRadius;
        public float StartAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_startAngle); // Convertir de radianes BEPU internos a grados WPF
        public float EndAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_endAngle); // Convertir de radianes BEPU internos a grados WPF

        // ✅ NUEVO: Propiedad para acceder al centro real de la curva
        public Vector3 CurveCenter => _curveCenter;

        public simCurve(Simulation simulation, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0, SimulationManagerBEPU simulationManager = null)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _simulationManager = simulationManager; // ✅ NUEVO: Almacenar referencia
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ SIMPLIFICADO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Calcular y almacenar el centro real de la curva
            var curveSize = outerRadius * 2f;
            var zPosition = zPos_Curve;
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(topLeft, curveSize, curveSize, 0f, zPosition);

            // ✅ SIMPLIFICADO: Crear la curva directamente
            Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0);
        }

        // ✅ SIMPLIFICADO: Configurar velocidad angular para AngularAxisMotor
        public void SetSpeed(float speed)
        {
            Speed = speed; // Velocidad angular directa (sin inversión)
            OnSpeedChanged?.Invoke(this);
        }

        /// <summary>
        /// ✅ NUEVO: Detiene completamente la curva
        /// </summary>
        public void StopCurve()
        {
            SetSpeed(0f);
        }

        /// <summary>
        /// ✅ NUEVO: Invierte la dirección de la curva manteniendo la misma velocidad
        /// </summary>
        public void ReverseCurve()
        {
            SetSpeed(-Speed);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            // Actualizar parámetros de la curva
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Actualizar el centro real de la curva
            var curveSize = outerRadius * 2f;
            var zPosition = zPos_Curve;
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

            Create(_curveCenter);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza posición desde Top-Left WPF manteniendo parámetros actuales
        /// </summary>
        internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
        {
            var curveSize = _outerRadius * 2f;
            var zPosition = GetPosition().Z; // Mantener Z actual
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

            Create(_curveCenter);
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
        /// </summary>
        internal Vector2 GetWpfTopLeft()
        {
            var curveSize = _outerRadius * 2f;
            return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(_curveCenter, curveSize, curveSize, 0f);
        }

        /// <summary>
        /// ✅ CORREGIDO: Obtiene los triángulos reales creados para la curva
        /// Devuelve los triángulos en coordenadas locales para evitar transformación duplicada
        /// </summary>
        public Triangle[] GetRealBEPUTriangles()
        {
            try
            {
                if (_storedTriangles == null || _storedTriangles.Length == 0)
                {
                    System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] No hay triángulos almacenados");
                    return new Triangle[0];
                }

                // ✅ CORREGIDO: Devolver triángulos en coordenadas locales
                // La visualización 3D aplicará la transformación una sola vez
                System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Devolviendo {_storedTriangles.Length} triángulos en coordenadas locales");
                return _storedTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Error: {ex.Message}");
                return new Triangle[0];
            }
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene los triángulos transformados a coordenadas mundiales (solo para debugging)
        /// </summary>
        public Triangle[] GetWorldBEPUTriangles()
        {
            try
            {
                if (_storedTriangles == null || _storedTriangles.Length == 0)
                {
                    return new Triangle[0];
                }

                var worldTriangles = TransformTrianglesToWorldCoordinates(_storedTriangles);
                System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Devolviendo {worldTriangles.Length} triángulos en coordenadas mundiales");
                return worldTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Error: {ex.Message}");
                return new Triangle[0];
            }
        }

        /// <summary>
        /// ✅ NUEVO: Transforma triángulos de coordenadas locales a mundiales
        /// </summary>
        private Triangle[] TransformTrianglesToWorldCoordinates(Triangle[] localTriangles)
        {
            try
            {
                if (_simulation == null || !_simulation.Bodies.BodyExists(BodyHandle))
                {
                    System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Cuerpo no existe, devolviendo triángulos locales");
                    return localTriangles; // Fallback: devolver triángulos sin transformar
                }

                var body = _simulation.Bodies[BodyHandle];
                var bodyPosition = body.Pose.Position;
                var bodyOrientation = body.Pose.Orientation;

                var transformedTriangles = new Triangle[localTriangles.Length];

                for (int i = 0; i < localTriangles.Length; i++)
                {
                    var localTriangle = localTriangles[i];

                    // Transformar cada vértice del triángulo a coordenadas mundiales
                    var worldA = bodyPosition + Vector3.Transform(localTriangle.A, bodyOrientation);
                    var worldB = bodyPosition + Vector3.Transform(localTriangle.B, bodyOrientation);
                    var worldC = bodyPosition + Vector3.Transform(localTriangle.C, bodyOrientation);

                    transformedTriangles[i] = new Triangle(worldA, worldB, worldC);
                }

                System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Transformados {transformedTriangles.Length} triángulos. Body pos: {bodyPosition}");
                return transformedTriangles;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Error: {ex.Message}, devolviendo triángulos locales");
                return localTriangles; // Fallback en caso de error
            }
        }


        public new void RemoverBody()
        {
            // ✅ NUEVO: Limpiar restricciones de botellas conectadas antes de eliminar el cuerpo
            if (_simulationManager != null)
            {
                var connectedBottles = _simulationManager.GetBottlesConnectedToElement(this);
                foreach (var bottle in connectedBottles)
                {
                    bottle.CleanRestrictions(this);
                }
            }

            // ✅ NUEVO: Limpiar triángulos almacenados
            _storedTriangles = null;

            // ✅ SIMPLIFICADO: Solo remover el cuerpo principal (Mesh único)
            base.RemoverBody();
        }

        public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 wpfTopLeft, float unused = 0)
        {
            // ✅ CORRIGIDO: Como Aether - startAngle y endAngle definen directamente el sector
            // No hay rotación separada del objeto

            // Actualizar parámetros internos
            _innerRadius = innerRadius;
            _outerRadius = outerRadius;
            // ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
            _startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
            _endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);

            // ✅ NUEVO: Actualizar el centro real de la curva
            // Para curvas, el "tamaño" es el diámetro del radio exterior
            var curveSize = outerRadius * 2f;
            var zPosition = zPos_Curve;

            // Calcular nueva posición del centro (sin rotación - el sector ya está definido por los ángulos)
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

            Create(_curveCenter);  // Sin rotación adicional
        }

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

            // ✅ SIMPLIFICADO: Crear superficie usando Triangle de BEPU directamente
            var triangles = CreateSimpleArcTriangles(_innerRadius, _outerRadius, _startAngle, _endAngle);

            // ✅ ALMACENAR triángulos para acceso directo
            _storedTriangles = triangles;

            // ✅ SIMPLIFICADO: Crear un solo cuerpo con múltiples Triangle shapes usando Mesh
            if (triangles.Length > 0)
            {
                // ✅ CREAR MESH CON LA API CORRECTA DE BEPU
                var triangleBuffer = new BepuUtilities.Memory.Buffer<Triangle>(triangles.Length, _simulation.BufferPool);
                for (int i = 0; i < triangles.Length; i++)
                {
                    triangleBuffer[i] = triangles[i];
                }
                var mesh = new BepuPhysics.Collidables.Mesh(triangleBuffer, Vector3.One, _simulation.BufferPool);
                var shapeIndex = _simulation.Shapes.Add(mesh);

                var bodyDescription = BodyDescription.CreateKinematic(
                        new RigidPose(position),
                        new CollidableDescription(shapeIndex, 0.001f), // ✅ SpeculativeMargin bajo para detección precisa
                    new BodyActivityDescription(0.01f)
                );

                BodyHandle = _simulation.Bodies.Add(bodyDescription);
                _bodyCreated = true;

                System.Diagnostics.Debug.WriteLine($"[simCurve] Creado con {triangles.Length} triángulos reales almacenados");
            }
        }

        // ✅ MÉTODOS ELIMINADOS: CreateMainBody y CreateTriangleBody ya no son necesarios
        // La curva ahora se crea como un solo Mesh en el método Create simplificado

        /// <summary>
        /// ✅ SIMPLIFICADO: Crea triángulos usando Triangle de BEPU directamente
        /// Solo superficie superior, eliminando complejidad innecesaria
        /// Los triángulos se crean en coordenadas locales (Z=0) y la posición del cuerpo los ubica correctamente
        /// </summary>
        /// <param name="innerRadius">Radio interno del arco</param>
        /// <param name="outerRadius">Radio externo del arco</param>
        /// <param name="startAngle">Ángulo inicial en radianes BEPU</param>
        /// <param name="endAngle">Ángulo final en radianes BEPU</param>
        /// <returns>Array de triángulos nativos de BEPU en coordenadas locales</returns>
        private Triangle[] CreateSimpleArcTriangles(float innerRadius, float outerRadius, float startAngle, float endAngle)
        {
            var triangles = new List<Triangle>();

            // ✅ SIMPLIFICADO: Menos segmentos, menos complejidad
            float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f);
            int segments = Math.Max(8, Math.Min((int)(arcLength * 8), 32)); // Menos segmentos
            float angleStep = (endAngle - startAngle) / segments;

            // ✅ SIMPLIFICADO: Sin inversión compleja de ángulos
            for (int i = 0; i < segments; i++)
            {
                float angle1 = startAngle + i * angleStep;
                float angle2 = startAngle + (i + 1) * angleStep;

                // ✅ COORDENADAS LOCALES: Triángulos en Z=0, el cuerpo los posiciona correctamente
                var inner1 = new Vector3(innerRadius * (float)Math.Cos(angle1), innerRadius * (float)Math.Sin(angle1), 0);
                var outer1 = new Vector3(outerRadius * (float)Math.Cos(angle1), outerRadius * (float)Math.Sin(angle1), 0);
                var inner2 = new Vector3(innerRadius * (float)Math.Cos(angle2), innerRadius * (float)Math.Sin(angle2), 0);
                var outer2 = new Vector3(outerRadius * (float)Math.Cos(angle2), outerRadius * (float)Math.Sin(angle2), 0);

                // ✅ USAR Triangle NATIVO DE BEPU (solo superficie superior)
                triangles.Add(new Triangle(inner1, outer1, outer2));
                triangles.Add(new Triangle(inner1, outer2, inner2));
            }

            System.Diagnostics.Debug.WriteLine($"[CreateSimpleArcTriangles] Creados {triangles.Count} triángulos en coordenadas locales");
            return triangles.ToArray();
        }



        // ✅ MÉTODOS ELIMINADOS: Los cálculos tangenciales complejos ya no son necesarios
        // AngularAxisMotor maneja automáticamente la rotación en curvas
    }

}