using System;
using System.Numerics;


namespace ActiveProtect.Utility
{
    public static class MotionAlgorithm
    {
        /// <summary>
        /// 计算抛物线弹道最佳发射方向（选择较小的仰角）
        /// </summary>
        /// <param name="startPos">发射位置</param>
        /// <param name="targetPos">目标位置</param>
        /// <param name="initialSpeed">初始速度</param>
        /// <returns>最佳发射方向，如果无解则返回null</returns>
        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)
            {
                Console.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">初始速度(m/s)</param>
        /// <param name="x">目标点x坐标(m)</param>
        /// <param name="y">目标点y坐标(m)</param>
        /// <param name="g">重力加速度(m/s²)</param>
        /// <returns>两个可能的发射角度(弧度)，如果无解则返回null</returns>
        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)
            {
                Console.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));

            //Console.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>
        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>
        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>
        public static Vector3D CalculateProportionalNavigation(double proportionalNavigationCoefficient, Vector3D missilePosition, Vector3D missileVelocity, Vector3D targetPosition, Vector3D targetVelocity)
        {
            // 预测时间（预测目标前进方向该时间后到达的位置，可以调整）
            double predictionTime = 0.01;

            // 预测目标位置
            Vector3D predictedTargetPosition = targetPosition + targetVelocity * predictionTime;

            Vector3D r = predictedTargetPosition - missilePosition;
            Vector3D v = targetVelocity - missileVelocity;
            
            Vector3D LOS = r.Normalize();
            Vector3D LOSRate = (v - (LOS * Vector3D.DotProduct(v, LOS))) / r.Magnitude();
            
            Vector3D acceleration = Vector3D.CrossProduct(Vector3D.CrossProduct(LOS, LOSRate), missileVelocity.Normalize()) * proportionalNavigationCoefficient * missileVelocity.Magnitude();
            
            return acceleration;
        }

        /// <summary>
        /// 添加高斯噪声
        /// </summary>
        /// <param name="vector">向量</param>
        /// <returns>添加高斯噪声后的向量</returns>
        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
            );
        }

        
    }
}