using System;
using System.Diagnostics;
using System.Numerics;

namespace ThreatSource.Utils
{
    /// <summary>
    /// 运动算法静态类，提供各种运动计算方法
    /// </summary>
    /// <remarks>
    /// 该类包含了导弹运动相关的各种算法，包括：
    /// - 弹道计算
    /// - 运动学计算
    /// - 制导算法
    /// - 扰动计算
    /// 所有方法都是静态的，可以直接调用。
    /// </remarks>
    public static class MotionAlgorithm
    {
        /// <summary>
        /// 计算抛物线弹道最佳发射方向（选择较小的仰角）
        /// </summary>
        /// <param name="startPos">发射位置坐标</param>
        /// <param name="targetPos">目标位置坐标</param>
        /// <param name="initialSpeed">发射初速度，单位：米/秒</param>
        /// <returns>包含最佳发射方向和初始速度向量的元组，如果无解则返回null</returns>
        /// <remarks>
        /// 该方法使用弹道方程计算两个可能的发射角度，并选择较小的仰角作为最佳发射方向。
        /// 计算考虑了重力影响，但未考虑空气阻力。
        /// </remarks>
        public static (Orientation? orientation, Vector3D? velocity) CalculateBestLaunchOrientation(Vector3D startPos, Vector3D targetPos, double initialSpeed)
        {
            // 计算水平距离
            double dx = targetPos.X - startPos.X;
            double dz = targetPos.Z - startPos.Z;
            double horizontalDistance = Math.Sqrt(dx * dx + dz * dz);
            
            // 计算高度差
            double dy = targetPos.Y - startPos.Y;

            double[]? angles = CalculateLaunchAngles(initialSpeed, horizontalDistance, dy);
            
            if (angles == null)
            {
                Debug.WriteLine("无法计算发射角度");
                return (null, null);
            }

            double bestAngle = Math.Min(angles[0], angles[1]);
            double azimuth = Math.Atan2(dz, dx);           
            // 计算初始速度分量
            double vx = initialSpeed * Math.Cos(bestAngle) * Math.Cos(azimuth);
            double vy = initialSpeed * Math.Sin(bestAngle);
            double vz = initialSpeed * Math.Cos(bestAngle) * Math.Sin(azimuth);

            // 返回方向和速度
            return (new Orientation(bestAngle, azimuth, 0), new Vector3D(vx, vy, vz));
        }

        /// <summary>
        /// 计算抛物线弹道发射角度
        /// </summary>
        /// <param name="v0">初始速度，单位：米/秒</param>
        /// <param name="x">目标水平距离，单位：米</param>
        /// <param name="y">目标高度差，单位：米</param>
        /// <param name="g">重力加速度，默认9.81米/秒²</param>
        /// <returns>两个可能的发射角度（弧度），如果无解则返回null</returns>
        /// <remarks>
        /// 使用标准弹道方程计算发射角度，会返回两个解：
        /// - 一个是低角度解（较小仰角）
        /// - 一个是高角度解（较大仰角）
        /// 如果目标距离超出武器射程，则返回null。
        /// </remarks>
        public static double[]? CalculateLaunchAngles(double v0, double x, double y, double g = 9.81)
        {
            double v0_2 = v0 * v0;
            double v0_4 = v0_2 * v0_2;
                       
            double discriminant = v0_4 - g * (g * x * x + 2 * y * v0_2);
                        
            if (discriminant < 0)
            {
                Debug.WriteLine("无实数解 - 目标不可达");
                return null;
            }

            double angle1 = Math.Atan((v0_2 + Math.Sqrt(discriminant)) / (g * x));
            double angle2 = Math.Atan((v0_2 - Math.Sqrt(discriminant)) / (g * x));

            Debug.WriteLine($"计算得到的两个角度: {angle1 * 180 / Math.PI:F2}° 和 {angle2 * 180 / Math.PI:F2}°");
            
            return new[] { angle1, angle2 };
        }

        /// <summary>
        /// 使用运动学定律计算导弹运动状态
        /// </summary>
        /// <param name="currentPosition">当前位置坐标</param>
        /// <param name="currentVelocity">当前速度向量</param>
        /// <param name="acceleration">加速度向量（包含重力加速度）</param>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <returns>包含新位置和新速度的元组</returns>
        /// <remarks>
        /// 该方法适用于无制导状态下的导弹运动计算，使用标准运动学方程：
        /// - 位置更新：p = p0 + v0*t + 0.5*a*t^2
        /// - 速度更新：v = v0 + a*t
        /// </remarks>
        public static (Vector3D newPosition, Vector3D newVelocity) CalculateBallisticMotion(
            Vector3D currentPosition,
            Vector3D currentVelocity,
            Vector3D acceleration,
            double deltaTime)
        {
            // 使用标准运动学方程
            Vector3D newPosition = new(
                currentPosition.X + currentVelocity.X * deltaTime + 0.5 * acceleration.X * deltaTime * deltaTime,
                currentPosition.Y + currentVelocity.Y * deltaTime + 0.5 * acceleration.Y * deltaTime * deltaTime,
                currentPosition.Z + currentVelocity.Z * deltaTime + 0.5 * acceleration.Z * deltaTime * deltaTime
            );

            Vector3D newVelocity = new(
                currentVelocity.X + acceleration.X * deltaTime,
                currentVelocity.Y + acceleration.Y * deltaTime,
                currentVelocity.Z + acceleration.Z * deltaTime
            );

            return (newPosition, newVelocity);
        }

        /// <summary>
        /// 使用四阶龙格库塔法计算导弹运动状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <param name="position">当前位置坐标</param>
        /// <param name="velocity">当前速度向量</param>
        /// <param name="acceleration">当前加速度向量</param>
        /// <returns>包含新位置和新速度的元组</returns>
        /// <remarks>
        /// 四阶龙格库塔法提供了更高精度的数值解：
        /// - 适用于有制导状态下的导弹运动计算
        /// - 考虑了加速度随时间的变化
        /// - 比简单欧拉法具有更高的精度
        /// </remarks>
        public static (Vector3D newPosition, Vector3D newVelocity) RungeKutta4(double deltaTime, Vector3D position, Vector3D velocity, Vector3D acceleration)
        {
            // 定义一个局部函数来计算加速度
            Vector3D AccelerationFunction(Vector3D pos, Vector3D vel)
            {
                // 这里可以添加更复杂的加速度计算，比如考虑空气阻力等
                return acceleration;
            }

            // 第一步
            Vector3D k1v = AccelerationFunction(position, velocity) * deltaTime;
            Vector3D k1r = velocity * deltaTime;

            // 第二步
            Vector3D k2v = AccelerationFunction(position + k1r * 0.5, velocity + k1v * 0.5) * deltaTime;
            Vector3D k2r = (velocity + k1v * 0.5) * deltaTime;

            // 第三步
            Vector3D k3v = AccelerationFunction(position + k2r * 0.5, velocity + k2v * 0.5) * deltaTime;
            Vector3D k3r = (velocity + k2v * 0.5) * deltaTime;

            // 第四步
            Vector3D k4v = AccelerationFunction(position + k3r, velocity + k3v) * deltaTime;
            Vector3D k4r = (velocity + k3v) * deltaTime;

            // 计算新的位置和速度
            Vector3D newPosition = position + (k1r + k2r * 2 + k3r * 2 + k4r) / 6;
            Vector3D newVelocity = velocity + (k1v + k2v * 2 + k3v * 2 + k4v) / 6;

            return (newPosition, newVelocity);
        }

        /// <summary>
        /// 计算比例导引加速度
        /// </summary>
        /// <param name="proportionalNavigationCoefficient">比例导引系数</param>
        /// <param name="missilePosition">导弹当前位置</param>
        /// <param name="missileVelocity">导弹当前速度</param>
        /// <param name="targetPosition">目标当前位置</param>
        /// <param name="targetVelocity">目标当前速度</param>
        /// <returns>比例导引产生的加速度向量</returns>
        /// <remarks>
        /// 使用比例导引法计算制导加速度：
        /// - 计算动态预测时间
        /// - 预测目标未来位置
        /// - 计算视线角速率
        /// - 根据比例导引公式计算所需加速度
        /// 加速度方向垂直于导弹速度方向。
        /// </remarks>
        public static Vector3D CalculateProportionalNavigation(double proportionalNavigationCoefficient, 
            Vector3D missilePosition, Vector3D missileVelocity, 
            Vector3D targetPosition, Vector3D targetVelocity)
        {
            // 计算导弹到目标的距离
            Vector3D r = targetPosition - missilePosition;
            double distance = r.Magnitude();
            
            // 根据距离和相对速度动态计算预测时间
            Vector3D relativeVelocity = targetVelocity - missileVelocity;
            double closingVelocity = -Vector3D.DotProduct(relativeVelocity, r.Normalize());
            
            // 预测时间计算考虑：
            // 1. 距离因素：距离越远，预测时间适当增加
            // 2. 接近速度因素：接近速度越大，预测时间适当减小
            // 3. 最小和最大限制：确保预测时间在合理范围内
            double predictionTime = Math.Clamp(
                distance / (closingVelocity + 1e-6) * 0.1,  // 基础预测时间（取时程的10%）
                0.05,   // 最小预测时间：50ms
                0.5     // 最大预测时间：500ms
            );
            
            // 预测目标位置
            Vector3D predictedTargetPosition = targetPosition + targetVelocity * predictionTime;
            
            // 计算视线矢量和视线变化率
            Vector3D LOS = (predictedTargetPosition - missilePosition).Normalize();
            Vector3D v = targetVelocity - missileVelocity;
            Vector3D LOSRate = (v - (LOS * Vector3D.DotProduct(v, LOS))) / distance;
            
            // 计算制导加速度
            Vector3D acceleration = Vector3D.CrossProduct(
                Vector3D.CrossProduct(LOS, LOSRate), 
                missileVelocity.Normalize()
            ) * proportionalNavigationCoefficient * missileVelocity.Magnitude();
            
            return acceleration;
        }

        /// <summary>
        /// 将角速度转换为加速度
        /// </summary>
        /// <param name="angularRate">角速度</param>
        /// <param name="yawEffectiveness">偏航舵效</param>
        /// <param name="pitchEffectiveness">俯仰舵效</param>
        /// <returns>加速度</returns>
        public static Vector3D ConvertAngularRateToAcceleration(Vector3D angularRate, double yawEffectiveness, double pitchEffectiveness)
        {
            return new Vector3D(
               0,
               angularRate.Y * yawEffectiveness,
               angularRate.Z * pitchEffectiveness
           );
        }

        /// <summary>
        /// 为向量添加高斯噪声
        /// </summary>
        /// <param name="vector">原始向量</param>
        /// <returns>添加高斯噪声后的向量</returns>
        /// <remarks>
        /// 使用Box-Muller变换生成高斯随机数：
        /// - 为向量的每个分量添加独立的高斯噪声
        /// - 噪声强度由标准差控制（默认0.1）
        /// - 用于模拟传感器误差和环境扰动
        /// </remarks>
        public static Vector3D AddRandomPerturbation(Vector3D vector)
        {
            Random random = new();
            // 添加高斯噪声
            double sigma = 0.1; // 扰动标准差
            // 使用Box-Muller变换来生成高斯随机数
            double u1 = random.NextDouble(); // 生成[0, 1)之间的随机数
            double u2 = random.NextDouble(); // 生成[0, 1)之间的随机数
            double r = Math.Sqrt(-2.0 * Math.Log(u1));
            double theta = 2.0 * Math.PI * u2;
            double gaussianX = r * Math.Cos(theta);
            double gaussianY = r * Math.Sin(theta);
            // 由于我们需要三个高斯随机数，我们将再次生成
            u1 = random.NextDouble(); // 生成[0, 1)之间的随机数
            u2 = random.NextDouble(); // 生成[0, 1)之间的随机数
            r = Math.Sqrt(-2.0 * Math.Log(u1));
            theta = 2.0 * Math.PI * u2;
            double gaussianZ = r * Math.Cos(theta);
            // 将高斯随机数转换为指定标准差的高斯随机数
            gaussianX *= sigma;
            gaussianY *= sigma;
            gaussianZ *= sigma;
            return new Vector3D(
            vector.X + gaussianX,
            vector.Y + gaussianY,
            vector.Z + gaussianZ
            );
        }

        /// <summary>
        /// 根据风速和风向计算风速向量
        /// </summary>
        /// <param name="windSpeed">风速，单位：米/秒</param>
        /// <param name="windDirection">风向，0-360度，0为北方，顺时针方向</param>
        /// <returns>风速向量，单位：米/秒</returns>
        /// <remarks>
        /// 将风速和风向转换为三维风速向量
        /// 风向是0-360度，0为北方，顺时针方向
        /// 在坐标系中，北方对应+Z，东方对应+X
        /// </remarks>
        public static Vector3D CalculateWindVector(double windSpeed, double windDirection)
        {
            // 风向是0-360度，0为北方，顺时针方向
            double windDirectionRad = windDirection * Math.PI / 180.0;
            
            // 在水平面上分解风向
            // 北方对应+Z，东方对应+X
            double windX = windSpeed * Math.Sin(windDirectionRad);  // 东西分量
            double windZ = windSpeed * Math.Cos(windDirectionRad);  // 南北分量
            
            return new Vector3D(windX, 0, windZ);
        }
    }
}