using System;

namespace CounterDrone.Core.Algorithms
{
    /// <summary>轨迹偏好（多解选择）</summary>
    public enum TrajectoryPreference
    {
        High,      // 高抛（较大仰角 / 较晚到达目标高度）
        Low,       // 低抛（较小仰角 / 较早到达目标高度）
        Nearest,   // 最短飞行时间（最小正时间）
        Farthest   // 最长飞行时间（最大正时间）
    }

    /// <summary>抛物线运动学核心模块</summary>
    public class ParabolicMotion
    {
        public readonly float V0, Theta, Y0, X0;
        public readonly float Vx, Vy;
        public const float G = 9.81f;

        public ParabolicMotion(float v0, float theta, float y0 = 0, float x0 = 0)
        {
            V0 = v0; Theta = theta; Y0 = y0; X0 = x0;
            Vx = v0 * (float)Math.Cos(theta);
            Vy = v0 * (float)Math.Sin(theta);
        }

        /// <summary>任意时刻 t 的状态</summary>
        public (float x, float y, float vx, float vy) GetState(float t)
        {
            return (X0 + Vx * t, Y0 + Vy * t - 0.5f * G * t * t, Vx, Vy - G * t);
        }

        /// <summary>到达目标高度 yTarget 的所有正时间解（0/1/2 个）</summary>
        public float[] GetFlightTimes(float yTarget)
        {
            float a = 0.5f * G;
            float b = -Vy;
            float c = yTarget - Y0;
            float d = b * b - 4f * a * c;
            if (d < 0) return Array.Empty<float>();

            float sqrtD = (float)Math.Sqrt(d);
            float t1 = (-b - sqrtD) / (2f * a);
            float t2 = (-b + sqrtD) / (2f * a);

            if (t1 > 0 && t2 > 0)
                return t1 < t2 ? new[] { t1, t2 } : new[] { t2, t1 };
            if (t1 > 0) return new[] { t1 };
            if (t2 > 0) return new[] { t2 };
            return Array.Empty<float>();
        }

        /// <summary>根据偏好选择到达目标高度的飞行时间</summary>
        public float? GetFlightTime(float yTarget, TrajectoryPreference pref)
        {
            var ts = GetFlightTimes(yTarget);
            if (ts.Length == 0) return null;
            if (ts.Length == 1) return ts[0];
            return pref switch
            {
                TrajectoryPreference.Nearest or TrajectoryPreference.Low => ts[0],
                TrajectoryPreference.Farthest or TrajectoryPreference.High => ts[1],
                _ => ts[0]
            };
        }

        /// <summary>根据偏好获取水平和飞行时间</summary>
        public (float range, float timeOfFlight)? ComputeRange(float yTarget, TrajectoryPreference pref)
        {
            float? t = GetFlightTime(yTarget, pref);
            if (!t.HasValue) return null;
            return (Vx * t.Value, t.Value);
        }

        // ═══ 逆问题：求解瞄准角度 ═══

        /// <summary>已知水平距离、高度差、初速，求解两个可能的发射角 (rad)</summary>
        public static (float theta1, float theta2)? SolveAngles(float range, float heightDiff, float v0)
        {
            if (range <= 0 || v0 <= 0) return null;
            float v2 = v0 * v0;
            float A = (G * range * range) / (2f * v2);
            float B = -range;
            float C = heightDiff + A;
            float d = B * B - 4f * A * C;
            if (d < 0 || A == 0) return null;
            float sqrtD = (float)Math.Sqrt(d);
            float p1 = (-B + sqrtD) / (2f * A);
            float p2 = (-B - sqrtD) / (2f * A);
            return ((float)Math.Atan(p1), (float)Math.Atan(p2));
        }

        /// <summary>根据偏好选择一个瞄准角度</summary>
        public static float? SolveAngle(float range, float heightDiff, float v0, TrajectoryPreference pref)
        {
            var result = SolveAngles(range, heightDiff, v0);
            if (!result.HasValue) return null;
            var (a1, a2) = result.Value;
            float high = Math.Max(a1, a2);
            float low = Math.Min(a1, a2);
            return pref switch
            {
                TrajectoryPreference.High => high,
                TrajectoryPreference.Low or TrajectoryPreference.Nearest or _ => low
            };
        }
    }
}
