CtrEditor/Simulacion/simTransporte.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
{

    public class simTransporte : simBase
    {
        public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)
        public float Friction { get; set; } // Friccion para efectos de cinta transportadora
        public float DistanceGuide2Guide { get; set; }
        public bool isBrake { get; set; }

        public bool TransportWithGuides = false;
        private List<Action> _deferredActions;
        public float Width { get; set; }
        public float Height { get; set; }

        // ✅ NUEVAS PROPIEDADES - cachear cálculos costosos
        public Vector3 DirectionVector { get; private set; }
        public List<simBotella> BottlesOnTransport { get; private set; } = new List<simBotella>();

        public simTransporte(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0, SimulationManagerBEPU simulationManager = null)
        {
            _simulation = simulation;
            _deferredActions = deferredActions;
            _simulationManager = simulationManager; // ✅ NUEVO: Almacenar referencia

            Width = width;
            Height = height;

            // Usar el nuevo método Create que maneja Top-Left correctamente
            Create(width, height, topLeft, angle);

            // ✅ INICIALIZAR PROPIEDADES CRÍTICAS
            UpdateCachedProperties();
        }

        public float Angle
        {
            get
            {
                return GetRotationZ();
            }
            set
            {
                SetRotation(value);
            }
        }

        /// <summary>
        /// ✅ SOBRESCRITO: SetRotation que actualiza automáticamente las propiedades cacheadas
        /// </summary>
        public new void SetRotation(float wpfAngle)
        {
            base.SetRotation(wpfAngle);

            // ✅ CRÍTICO: Actualizar propiedades cacheadas y velocidad después del cambio de rotación
            UpdateCachedProperties();
        }

        public new void SetPosition(float x, float y, float z = 0)
        {
            base.SetPosition(x, y, z);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales
        /// </summary>
        internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
        {
            var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, currentWpfAngle, zPosition);
            SetPosition(bepuCenter);
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
        /// </summary>
        internal Vector2 GetWpfTopLeft()
        {
            var bepuCenter = GetPosition();
            var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
            return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(bepuCenter, Width, Height, wpfAngle);
        }

        /// <summary>
        /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
        /// </summary>
        internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle)
        {
            var zPosition = GetPosition().Z; // Mantener Z actual
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition);
            CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle);

            // ✅ CRÍTICO: Actualizar propiedades cacheadas y velocidad después del cambio de orientación
            UpdateCachedProperties();
        }

        // ✅ NUEVO MÉTODO - actualizar propiedades cacheadas y velocidad cinemática
        internal void UpdateCachedProperties()
        {
            // ✅ CORREGIDO: La dirección siempre es UnitX rotado por el ángulo del transporte
            // NO depende de las dimensiones (Width >= Height) sino solo de la rotación
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
                var bepuQuaternion = bodyReference.Pose.Orientation;

                // ✅ SIEMPRE usar UnitX y aplicar la rotación
                DirectionVector = Vector3.Transform(Vector3.UnitX, bepuQuaternion);

                // 🔍 DEBUG: Agregar información detallada
                var wpfAngle = GetRotationZ();
                //System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
            }
            else
            {
                // ✅ CORREGIDO: Aplicar conversión de coordenadas WPF→BEPU en el vector
                var wpfAngle = GetRotationZ(); // Ángulo WPF en grados
                var wpfAngleRadians = simBase.GradosARadianes(wpfAngle);

                // Calcular el vector en coordenadas WPF
                var wpfX = (float)Math.Cos(wpfAngleRadians);
                var wpfY = (float)Math.Sin(wpfAngleRadians);

                // ✅ APLICAR CONVERSIÓN Y: En WPF Y+ es abajo, en BEPU Y+ es arriba
                DirectionVector = new Vector3(wpfX, -wpfY, 0); // Invertir Y para conversión WPF→BEPU

                // 🔍 DEBUG: Agregar información detallada
                System.Diagnostics.Debug.WriteLine($"[UpdateCached-Fallback] WPF Angle: {wpfAngle}°, WPF Vector: ({wpfX:F3}, {wpfY:F3}), BEPU DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
            }
            
            // ✅ NUEVO: Actualizar la velocidad del cuerpo cinemático siempre que cambien las propiedades
            ActualizarVelocidadCinematica();
        }

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

        /// <summary>
        /// ✅ NUEVO: Aplica la velocidad 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);
                // ✅ CORREGIDO: Convertir velocidad de m/min a m/s para la simulación
                body.Velocity.Linear = this.DirectionVector * (this.Speed / SpeedConversionFactor);
            }
        }

        /// <summary>
        /// Detiene completamente el transporte
        /// </summary>
        public void StopTransport()
        {
            SetSpeed(0f);
        }

        /// <summary>
        /// Invierte la dirección del transporte manteniendo la misma velocidad
        /// </summary>
        public void ReverseTransport()
        {
            SetSpeed(-Speed);
        }

        public void SetDimensions(float width, float height)
        {
            Width = width;
            Height = height;

            // ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
            if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
            {
                var box = new Box(width, height, zAltura_Transporte);
                var shapeIndex = _simulation.Shapes.Add(box);
                ChangeBodyShape(shapeIndex);
            }

            // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de dimensiones
            UpdateCachedProperties();
        }

        public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle = 0)
        {
            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var zPosition = zAltura_Transporte / 2f + zPos_Transporte;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);

            Create(width, height, bepuCenter, wpfAngle);
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: RemoverBody ya no necesita limpiar restricciones de botellas.
        /// </summary>
        public new void RemoverBody()
        {
            base.RemoverBody();
        }

        public void Create(float width, float height, Vector3 bepuPosition, float wpfAngle = 0)
        {
            RemoverBody();

            var box = new Box(width, height, zAltura_Transporte);
            var shapeIndex = _simulation.Shapes.Add(box);

            // ✅ USAR COORDINATECONVERTER para conversión centralizada
            var bodyDescription = BodyDescription.CreateKinematic(
                new RigidPose(bepuPosition, CoordinateConverter.CreateBepuQuaternionFromWpfAngle(wpfAngle)),
                new CollidableDescription(shapeIndex, 0.1f),
                new BodyActivityDescription(-1f)
            );

            BodyHandle = _simulation.Bodies.Add(bodyDescription);
            _bodyCreated = true; // Marcar que hemos creado un cuerpo

            // ✅ CRÍTICO: Actualizar propiedades cacheadas después de crear el body
            UpdateCachedProperties();
        }
    }

}