ActiveProtect/Models/Missile.cs

using System;
using ActiveProtect.SimulationEnvironment;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 表示仿真中的导弹
    /// </summary>
    public class Missile : SimulationElement
    {
        /// <summary>
        /// 导弹类型
        /// </summary>
        public MissileType Type { get; protected set; }        
        public double Speed { get; protected set; }
        public double MaxSpeed { get; protected set; }
        public string TargetId { get; protected set; }
        public double MaxFlightTime { get; protected set; }
        public double MaxFlightDistance { get; protected set; }
        public double FlightTime { get; protected set; }
        public double FlightDistance { get; protected set; }
        public double DistanceToTarget { get; protected set; }
        public MissileDistanceParams DistanceParams { get; protected set; }
        public FlightStageConfig StageConfig { get; protected set; }
        public FlightStage CurrentStage { get; protected set; } 
        protected Vector3D Velocity;
        private Vector3D LastTargetPosition;
        private const double N = 3; // 比例导引系数

        public double ThrustAcceleration { get; protected set; }
        public double EngineBurnTime { get; protected set; }
        public double MaxEngineBurnTime { get; protected set; }

        public double MaxAcceleration { get; protected set; } = 100; // m/s^2

        public double ProportionalNavigationCoefficient { get; set; }

        public bool HasGuidance { get; protected set; } = false;
        protected double LostGuidanceTime { get; set; } = 0;
        protected Vector3D LastKnownVelocity = Vector3D.Zero;
        public const double LAUNCH_SPEED = 10; // 发射速度，单位：m/s
        public const double LAUNCH_DURATION = 0.5; // 发射阶持续时间，单位：秒

        private IMissileStageStrategy currentStage;
        private Dictionary<FlightStage, IMissileStageStrategy> stageStrategies;

        // 添加质量属性
        public double Mass { get; protected set; } = 100; // 默认质量为100kg，可以根据需要调整

        public Missile(string id, MissileConfig missileConfig, ISimulationManager simulationManager)
            : base(id, missileConfig.InitialPosition, missileConfig.InitialOrientation, simulationManager)
        {
            Speed = missileConfig.InitialSpeed;
            MaxSpeed = missileConfig.MaxSpeed;
            MaxFlightTime = missileConfig.MaxFlightTime;
            MaxFlightDistance = missileConfig.MaxFlightDistance;
            DistanceParams = missileConfig.DistanceParams;
            StageConfig = missileConfig.StageConfig;
            IsActive = true;
            FlightTime = 0;
            FlightDistance = 0;
            SimulationManager = simulationManager;
            ThrustAcceleration = missileConfig.ThrustAcceleration;
            EngineBurnTime = 0;
            MaxEngineBurnTime = missileConfig.MaxEngineBurnTime;
            MaxAcceleration = missileConfig.MaxAcceleration;
            ProportionalNavigationCoefficient = missileConfig.ProportionalNavigationCoefficient;
            TargetId = $"Tank_{missileConfig.TargetIndex + 1}";
            Vector3D horizontalDirection = new Vector3D(Orientation.ToVector().X, 0, Orientation.ToVector().Z).Normalize();
            Velocity = horizontalDirection * missileConfig.InitialSpeed;

            // 初始化策略字典
            stageStrategies = new Dictionary<FlightStage, IMissileStageStrategy>
            {
                { FlightStage.Launch, new LaunchStageStrategy(this) },
                { FlightStage.Acceleration, new AccelerationStageStrategy(this) },
                { FlightStage.Cruise, new CruiseStageStrategy(this) },
                { FlightStage.TerminalGuidance, new TerminalGuidanceStageStrategy(this) },
                { FlightStage.Attack, new AttackStageStrategy(this) }
            };

            // 设置初始阶段
            SetInitialStage();

            currentStage = stageStrategies[CurrentStage];

            LastTargetPosition = Position; // 初始化 LastTargetPosition

            DistanceToTarget = Vector3D.Distance(Position, SimulationManager.GetEntityById(TargetId).Position); 
        }

        private void SetInitialStage()
        {
            if (StageConfig.EnableLaunch) CurrentStage = FlightStage.Launch;
            else if (StageConfig.EnableAcceleration) CurrentStage = FlightStage.Acceleration;
            else if (StageConfig.EnableCruise) CurrentStage = FlightStage.Cruise;
            else if (StageConfig.EnableTerminalGuidance) CurrentStage = FlightStage.TerminalGuidance;
            else if (StageConfig.EnableAttack) CurrentStage = FlightStage.Attack;

            Console.WriteLine($"导弹 {Id} 的初始阶段: {CurrentStage}");
        }

        public override void Update(double deltaTime)
        {
            if (!IsActive) return;

            if (ShouldSelfDestruct())
            {
                SelfDestruct();
                return;
            }

            UpdateEngineBurnTime(deltaTime);
            (Position, Velocity) = UpdatePositionAndVelocityRK4(deltaTime);

            Speed = Velocity.Magnitude();
            Orientation = Orientation.FromVector(Velocity);
            FlightTime += deltaTime;
            FlightDistance += Speed * deltaTime;
            DistanceToTarget = Vector3D.Distance(Position, SimulationManager.GetEntityById(TargetId).Position);

            if (CheckHit())
            {
                Explode();
                return;
            }

            UpdateGuidanceStatus();
            currentStage.Update(deltaTime);
        }

        public void ChangeStage(FlightStage newStage)
        {
            if (stageStrategies.TryGetValue(newStage, out var strategy))
            {
                if (IsStageEnabled(newStage))
                {
                    CurrentStage = newStage;
                    currentStage = strategy;
                    Console.WriteLine($"导弹 {Id} 切换到 {newStage} 阶段");
                }
                else
                {
                    // 如果目标阶段不可用，尝试切换到下一个可用阶段
                    TryChangeToNextAvailableStage(newStage);
                }
            }
            else
            {
                Console.WriteLine($"导弹 {Id} 无法切换到未知阶段 {newStage}");
            }
        }

        private bool IsStageEnabled(FlightStage stage)
        {
            return stage switch
            {
                FlightStage.Launch => StageConfig.EnableLaunch,
                FlightStage.Acceleration => StageConfig.EnableAcceleration,
                FlightStage.Cruise => StageConfig.EnableCruise,
                FlightStage.TerminalGuidance => StageConfig.EnableTerminalGuidance,
                FlightStage.Attack => StageConfig.EnableAttack,
                _ => false
            };
        }

        private void TryChangeToNextAvailableStage(FlightStage startStage)
        {
            FlightStage[] stageOrder = [FlightStage.Launch, FlightStage.Acceleration, FlightStage.Cruise, FlightStage.TerminalGuidance, FlightStage.Attack];
            int startIndex = Array.IndexOf(stageOrder, startStage);

            for (int i = startIndex + 1; i < stageOrder.Length; i++)
            {
                if (IsStageEnabled(stageOrder[i]))
                {
                    ChangeStage(stageOrder[i]);
                    return;
                }
            }

            // 如果没有可用的下一个阶段，导弹自毁
            Console.WriteLine($"导弹 {Id} 没有可用的下一个阶段，准备自毁");
            SelfDestruct();
        }

        private (Vector3D, Vector3D) CalculateDerivatives(Vector3D position, Vector3D velocity, double deltaTime)
        {
            Vector3D guidanceAcceleration = Vector3D.Zero;
            Vector3D thrustAcceleration = Vector3D.Zero;

            switch (CurrentStage)
            {
                case FlightStage.Launch:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                    break;
                case FlightStage.Acceleration:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                    guidanceAcceleration = CalculateProportionalNavigation(position);
                    break;
                case FlightStage.Cruise:
                    thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.1);
                    guidanceAcceleration = CalculateProportionalNavigation(position) * 0.5;
                    break;
                case FlightStage.TerminalGuidance:
                    thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.1);
                    guidanceAcceleration = CalculateProportionalNavigation(position) * 1.5;
                    break;
                case FlightStage.Attack:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                    guidanceAcceleration = CalculateProportionalNavigation(position) * 2;
                    break;
            }

            if (!HasGuidance)
            {
                guidanceAcceleration = Vector3D.Zero;
                if (velocity.Magnitude() > 0)
                {
                    thrustAcceleration = velocity.Normalize() * ThrustAcceleration;
                }
                else
                {
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                }
            }

            // 计算空气阻力的影响
            Vector3D dragAcceleration = velocity.Normalize() * -1 * CalculateDrag(velocity.Magnitude()) / Mass;

            // 计算重力加速度
            Vector3D gravityCompensation = new(0, 9.81, 0);
            Vector3D gravityAcceleration = new(0, -9.81, 0);

            Vector3D totalAcceleration = guidanceAcceleration + thrustAcceleration + gravityCompensation + gravityAcceleration + dragAcceleration;

            if (totalAcceleration.Magnitude() > MaxAcceleration)
            {
                totalAcceleration = totalAcceleration.Normalize() * MaxAcceleration;
            }

            return (velocity, totalAcceleration);
        }

        private static double CalculateDrag(double speed)
        {
            // 修改空气阻力计算
            const double dragCoefficient = 0.1; // 减小阻力系数
            const double airDensity = 1.225; // 海平面空气密度，kg/m^3
            const double referenceArea = 0.01; // 减小导弹的参考面积，m^2

            return 0.5 * dragCoefficient * airDensity * referenceArea * speed * speed;
        }

        // 使用 Runge-Kutta 方法更新位置和速度
        private (Vector3D, Vector3D) UpdatePositionAndVelocityRK4(double deltaTime)
        {
            Vector3D k1, k2, k3, k4;
            Vector3D v1, v2, v3, v4;

            (k1, v1) = CalculateDerivatives(Position, Velocity, deltaTime);
            (k2, v2) = CalculateDerivatives(Position + k1 * (deltaTime / 2), Velocity + v1 * (deltaTime / 2), deltaTime / 2);
            (k3, v3) = CalculateDerivatives(Position + k2 * (deltaTime / 2), Velocity + v2 * (deltaTime / 2), deltaTime / 2);
            (k4, v4) = CalculateDerivatives(Position + k3 * deltaTime, Velocity + v3 * deltaTime, deltaTime);

            Vector3D newPosition = Position + (k1 + k2 * 2 + k3 * 2 + k4) * (deltaTime / 6);
            Vector3D newVelocity = Velocity + (v1 + v2 * 2 + v3 * 2 + v4) * (deltaTime / 6);

            // 限制速度不超过最大速度
            if (newVelocity.Magnitude() > MaxSpeed)
            {
                newVelocity = newVelocity.Normalize() * MaxSpeed;
            }

            return (newPosition, newVelocity);
        }

        // 检查是否命中目标
        private bool CheckHit()
        {
            return DistanceToTarget <= DistanceParams.ExplosionDistance;
        }

        // 检查是否应该自毁
        private bool ShouldSelfDestruct()
        {
            if (FlightTime >= MaxFlightTime)
            {
                Console.WriteLine($"导弹 {Id} 超出最大飞行时间 ({FlightTime:F2}/{MaxFlightTime:F2})，准备自毁");
                return true;
            }
            if (FlightDistance >= MaxFlightDistance)
            {
                Console.WriteLine($"导弹 {Id} 超出最大飞行距离 ({FlightDistance:F2}/{MaxFlightDistance:F2})，准备自毁");
                return true;
            }
            if (Position.Y <= 0)
            {
                Console.WriteLine($"导弹 {Id} 高度小于等于0 ({Position.Y:F2})，准备自毁");
                return true;
            }
            return false;
        }

        // 更新发动机燃烧时间
        private void UpdateEngineBurnTime(double deltaTime)
        {
            if (CurrentStage == FlightStage.Acceleration && EngineBurnTime < MaxEngineBurnTime)
            {
                EngineBurnTime += deltaTime;
            }
        }


        // 计算比例导引加速度
        private Vector3D CalculateProportionalNavigation(Vector3D position)
        {
            Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
            Vector3D LOS = targetPosition - position;
            Vector3D LOSRate = (targetPosition - LastTargetPosition) / FlightTime - Velocity;
            LastTargetPosition = targetPosition;

            return Vector3D.CrossProduct(Vector3D.CrossProduct(LOS, LOSRate), LOS).Normalize() 
                   * ProportionalNavigationCoefficient * Velocity.Magnitude();
        }

        // 爆炸
        public void Explode()
        {
            Deactivate();
            SimulationManager.HandleTargetHit(TargetId, Id);
            Console.WriteLine($"导弹 {Id} 在 {Position} 爆炸，命中目标！");
        }

        // 自毁
        public void SelfDestruct()
        {
            if (IsActive)
            {
                string reason = FlightTime >= MaxFlightTime ? "超出最大飞行时间" :
                                FlightDistance >= MaxFlightDistance ? "超出最大飞行距离" :
                                Position.Y <= 0 ? "高度小于等于0" :
                                !HasGuidance ? "失去引导" : "未知原因";
                
                Console.WriteLine($"导弹 {Id} 自毁。原因: {reason}");
                Deactivate();
            }
        }

        // 设置比例导引系数
        public void SetProportionalNavigationCoefficient(double newCoefficient)
        {
            ProportionalNavigationCoefficient = newCoefficient;
            Console.WriteLine($"导弹 {Id} 的比例导引系数已更新为 {newCoefficient}");
        }

        // 获取导弹状态
        public override string GetStatus()
        {
            return $"导弹 {Id}:\n" +
                   $"  位置: {Position}\n" +
                   $"  速度: {Speed:F2} m/s\n" +
                   $"  当前状态: {GetCurrentStateName()}\n" +
                   $"  飞行时间: {FlightTime:F2}/{MaxFlightTime:F2}\n" +
                   $"  飞行距离: {FlightDistance:F2}/{MaxFlightDistance:F2}\n" +
                   $"  距离目标: {DistanceToTarget:F2}\n" +
                   $"  发动机工作时间: {EngineBurnTime:F2}/{MaxEngineBurnTime:F2}\n" +
                   $"  有引导: {(HasGuidance ? "是" : "否")}\n" +
                   $"  失去引导时间: {LostGuidanceTime:F2}";
        }

        // 添加新的方法来更新引导状态
        protected virtual void UpdateGuidanceStatus()
        {
            // 基类中的默认实现
        }

        public string GetCurrentStateName()
        {
            return currentStage.GetType().Name;
        }

    }

    public interface IMissileStageStrategy
    {
        void Update(double deltaTime);
    }

    public class LaunchStageStrategy : IMissileStageStrategy
    {
        private readonly Missile missile;
        private double launchTime = 0;

        public LaunchStageStrategy(Missile missile)
        {
            this.missile = missile;
        }

        public void Update(double deltaTime)
        {
            launchTime += deltaTime;
            if (launchTime >= Missile.LAUNCH_DURATION || missile.Speed >= missile.MaxSpeed * 0.1)
            {
                missile.ChangeStage(FlightStage.Acceleration);
            }
        }
    }

    public class AccelerationStageStrategy(Missile missile) : IMissileStageStrategy
    {
        private readonly Missile missile = missile;

        public void Update(double deltaTime)
        {
            if (missile.EngineBurnTime >= missile.MaxEngineBurnTime || missile.Speed >= missile.MaxSpeed)
            {
                missile.ChangeStage(FlightStage.Cruise);
            }
        }
    }

    public class CruiseStageStrategy(Missile missile) : IMissileStageStrategy
    {
        private readonly Missile missile = missile;

        public void Update(double deltaTime)
        {
            if (missile.DistanceToTarget <= missile.DistanceParams.TerminalGuidanceDistance)
            {
                missile.ChangeStage(FlightStage.TerminalGuidance);
            }
        }
    }

    public class TerminalGuidanceStageStrategy(Missile missile) : IMissileStageStrategy
    {
        private readonly Missile missile = missile;

        public void Update(double deltaTime)
        {
            if (missile.DistanceToTarget <= missile.DistanceParams.AttackDistance)
            {
                missile.ChangeStage(FlightStage.Attack);
            }
        }
    }

    public class AttackStageStrategy(Missile missile) : IMissileStageStrategy
    {
        private readonly Missile missile = missile;

        public void Update(double deltaTime)
        {
            if (missile.DistanceToTarget <= missile.DistanceParams.ExplosionDistance)
            {
                missile.Explode();
            }
        }
    }

    public class UnguidedStageStrategy(Missile missile) : IMissileStageStrategy
    {
        private readonly Missile missile = missile;

        public void Update(double deltaTime)
        {
            // 检查是否重新获得引导
            if (missile.HasGuidance)
            {
                Console.WriteLine($"导弹 {missile.Id} 重新获得引导，退出无制导阶段");
                if (missile.DistanceToTarget <= missile.DistanceParams.AttackDistance)
                {
                    missile.ChangeStage(FlightStage.Attack);
                }
                else if (missile.DistanceToTarget <= missile.DistanceParams.TerminalGuidanceDistance)
                {
                    missile.ChangeStage(FlightStage.TerminalGuidance);
                }
                else
                {
                    missile.ChangeStage(FlightStage.Cruise);
                }
            }
        }
    }
}