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

        // ✅ 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);
        }

        // ✅ MODIFICADO: Actualizar la velocidad y luego la del cuerpo cinemático
        public void SetSpeed(float speed)
        {
            Speed = speed;
            ActualizarVelocidadCinematica();
        }

        /// <summary>
        /// ✅ NUEVO: Aplica la velocidad angular al cuerpo cinemático de BEPU.
        /// </summary>
        public void ActualizarVelocidadCinematica()
        {
            if (_simulation != null && this.BodyHandle.Value >= 0 && _simulation.Bodies.BodyExists(this.BodyHandle))
            {
                var body = _simulation.Bodies.GetBodyReference(BodyHandle);

                float effectiveRadius = (_innerRadius + _outerRadius) / 2.0f;
                if (effectiveRadius > 0.001f)
                {
                    // La velocidad tangencial (Speed) se convierte a velocidad angular (ω = v / r)
                    // ✅ CORREGIDO: Convertir velocidad de m/min a m/s para la simulación
                    float angularSpeed = (Speed / SpeedConversionFactor) / effectiveRadius;

                    // La rotación es alrededor del eje Z en el sistema de coordenadas de BEPU
                    body.Velocity.Angular = new Vector3(0, 0, angularSpeed);
                }
                else
                {
                    body.Velocity.Angular = Vector3.Zero;
                }
            }
        }

        /// <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 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)
        {
            // ✅ CORREGIDO: 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
            var curveSize = outerRadius * 2f;
            var zPosition = zPos_Curve;
            _curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);

            Create(_curveCenter);  // Llamar a la sobrecarga privada
        }

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

            var triangles = CreateSimpleArcTriangles(_innerRadius, _outerRadius, _startAngle, _endAngle);
            if (triangles.Length == 0)
            {
                System.Diagnostics.Debug.WriteLine($"[simCurve] No se crearon triángulos, no se creará el cuerpo.");
                return;
            }
             _storedTriangles = triangles;

            // ✅ CORREGIDO: Convertir el array de triángulos a un Buffer<Triangle> de BEPU.
            _simulation.BufferPool.Take(triangles.Length, out BepuUtilities.Memory.Buffer<Triangle> triangleBuffer);
            for (int i = 0; i < triangles.Length; i++)
            {
                triangleBuffer[i] = triangles[i];
            }
            var mesh = new Mesh(triangleBuffer, Vector3.One, _simulation.BufferPool);
            var shapeIndex = _simulation.Shapes.Add(mesh);

            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(position, Quaternion.Identity), // La rotación se maneja con la velocidad angular, no con la pose inicial
                new CollidableDescription(shapeIndex, 0.1f),
                new BodyActivityDescription(-1f) // ✅ CORREGIDO: -1f para que el cuerpo cinemático NUNCA duerma y no se mueva por su cuenta.
            );
            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true;

            // ✅ NUEVO: Establecer la velocidad cinemática inicial al crear el cuerpo.
            ActualizarVelocidadCinematica();
        }

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