using System;
using CounterDrone.Core.Models;

namespace CounterDrone.Core.Algorithms
{
    /// <summary>通用运动学 / 几何 / 大气扩散工具</summary>
    public static class Kinematics
    {
        /// <summary>Pasquill 稳定度等级</summary>
        public enum StabilityClass { A, B, C, D, E, F }
        /// <summary>风向 → 单位速度矢量 (X, 0, Z)，右手系 Y-up</summary>
        public static (float X, float Y, float Z) WindToVector(WindDirection dir, float speed)
        {
            var angle = dir switch
            {
                WindDirection.N => 0f,
                WindDirection.NE => 45f,
                WindDirection.E => 90f,
                WindDirection.SE => 135f,
                WindDirection.S => 180f,
                WindDirection.SW => 225f,
                WindDirection.W => 270f,
                WindDirection.NW => 315f,
                _ => 0f,
            };
            var rad = angle * (float)Math.PI / 180f;
            return ((float)Math.Sin(rad) * speed, 0, (float)Math.Cos(rad) * speed);
        }

        /// <summary>计算抛物线炮弹的发射角</summary>
        /// <param name="range">水平距离 (m)</param>
        /// <param name="muzzleVelocity">初速 (m/s)</param>
        /// <param name="releaseAltitude">释放高度 (m)，弹道顶点必须 ≥ 此值</param>
        /// <returns>发射角 (rad)，取低弹道</returns>
        /// <summary>弹道角度：使抛物线在(targetY - startY)高度经下行段精确经过距离 range 处</summary>
        public static float CalculateLaunchAngle(float range, float muzzleVelocity, float releaseAltitude)
        {
            float v2 = muzzleVelocity * muzzleVelocity;
            float g = 9.81f;
            float R = Math.Max(1f, range);
            float H = releaseAltitude; // 目标相对高度（从发射点算）
            float a = 0.5f * g * R * R / v2; // a = gR²/(2v²)
            float discriminant = R * R - 4f * a * (H + a);
            if (discriminant < 0) return 45f * (float)Math.PI / 180f; // 不可达，用 45°
            // 平射解（小角度）
            float tanTheta = (R - (float)Math.Sqrt(discriminant)) / (2f * a);
            return (float)Math.Atan(tanTheta);
        }

        /// <summary>弹道飞行时间（秒）</summary>
        public static float ParabolicTimeOfFlight(float range, float launchAngle, float muzzleVelocity)
        {
            float cosAngle = (float)Math.Cos(launchAngle);
            if (cosAngle < 0.001f) return range / Math.Max(1f, muzzleVelocity * 0.7f);
            return range / (muzzleVelocity * cosAngle);
        }

        /// <summary>炮弹飞行时间：抛物线轨迹到目标点的时间（自动求解发射角，下行段命中）</summary>
        public static float EstimatedShellTime(float horizontalDist, float muzzleVelocity)
        {
            return ParabolicShellTime(horizontalDist, 0f, muzzleVelocity);
        }

        /// <summary>抛物线飞行时间：解 tan(θ) 二次方程求到达 (horizontalDist, heightDiff) 的时间</summary>
        public static float ParabolicShellTime(float horizontalDist, float heightDiff, float muzzleVelocity)
        {
            float R = Math.Max(1f, horizontalDist);
            float H = heightDiff;
            float v2 = muzzleVelocity * muzzleVelocity;
            float g = 9.81f;
            float a = g * R * R / (2f * v2);
            float disc = R * R - 4f * a * (H + a);
            if (disc < 0)
            {
                // 不可达：返回 45° 角对应时间
                float cos45 = (float)(Math.Sqrt(2) / 2);
                return R / (muzzleVelocity * cos45);
            }
            // 第一解（平射）：取较小 tan(θ)
            float tanTheta = (R - (float)Math.Sqrt(disc)) / (2f * a);
            float cosTheta = 1f / (float)Math.Sqrt(1f + tanTheta * tanTheta);
            return R / (muzzleVelocity * cosTheta);
        }

        /// <summary>根据发射参数计算抛物线位置</summary>
        public static (float X, float Y, float Z) ParabolicPosition(
            float startX, float startY, float startZ,
            float launchAngle, float azimuth,
            float muzzleVelocity, float time)
        {
            var cosA = (float)Math.Cos(launchAngle);
            var sinA = (float)Math.Sin(launchAngle);
            var g = 9.81f;

            var horizontalDist = muzzleVelocity * cosA * time;
            var x = startX + horizontalDist * (float)Math.Sin(azimuth);
            var z = startZ + horizontalDist * (float)Math.Cos(azimuth);
            var y = startY + muzzleVelocity * sinA * time - 0.5f * g * time * time;

            return (x, y, z);
        }

        /// <summary>空投弹药位置：继承载机速度 + 重力下落</summary>
        /// <param name="startX,startY,startZ">投放点坐标</param>
        /// <param name="carrierVelX,carrierVelY,carrierVelZ">载机速度矢量 (m/s)</param>
        /// <param name="time">经过时间 (s)</param>
        public static (float X, float Y, float Z) AirDropPosition(
            float startX, float startY, float startZ,
            float carrierVelX, float carrierVelY, float carrierVelZ,
            float time)
        {
            const float g = 9.81f;
            var x = startX + carrierVelX * time;
            var y = startY + carrierVelY * time - 0.5f * g * time * time;
            var z = startZ + carrierVelZ * time;
            return (x, y, z);
        }

        /// <summary>空投降落时间：从释放高度下落到目标高度所需时间</summary>
        /// <param name="releaseAlt">释放高度 (m)</param>
        /// <param name="targetAlt">目标高度 (m)</param>
        /// <param name="carrierVelY">载机垂直速度 (m/s)，正=向上，默认 0（平飞）</param>
        public static float AirDropFallTime(float releaseAlt, float targetAlt, float carrierVelY = 0f)
        {
            const float g = 9.81f;
            var dy = releaseAlt - targetAlt;
            if (dy <= 0) return 0f;
            return ((float)Math.Sqrt(carrierVelY * carrierVelY + 2f * g * dy) - carrierVelY) / g;
        }

        /// <summary>两点间方向的速度矢量：normalize(to - from) × speed</summary>
        public static (float X, float Y, float Z) DirectionVelocity(
            float fromX, float fromY, float fromZ,
            float toX, float toY, float toZ,
            float speed)
        {
            float dx = toX - fromX;
            float dy = toY - fromY;
            float dz = toZ - fromZ;
            float dist = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
            if (dist < 0.01f) return (0, 0, 0);
            float s = speed / dist;
            return (dx * s, dy * s, dz * s);
        }

        /// <summary>云团覆盖间隔：无人机穿越单个云团的时间 = 云团直径 / 无人机速度</summary>
        /// <param name="cloudDiameter">云团有效直径 m</param>
        /// <param name="targetSpeedKmh">目标速度 km/h</param>
        /// <param name="minInterval">硬件最小间隔 s</param>
        public static float CloudCoverInterval(float cloudDiameter, float targetSpeedKmh, float minInterval = 0.1f)
        {
            float speedMs = targetSpeedKmh / 3.6f;
            if (speedMs <= 0.1f) return minInterval;
            return Math.Max(minInterval, cloudDiameter / speedMs);
        }

        /// <summary>点到矩形距离（简化判定）</summary>
        public static float Distance2D(float x1, float z1, float x2, float z2)
        {
            var dx = x2 - x1;
            var dz = z2 - z1;
            return (float)Math.Sqrt(dx * dx + dz * dz);
        }

        /// <summary>点到三维点距离</summary>
        public static float Distance3D(float x1, float y1, float z1, float x2, float y2, float z2)
        {
            var dx = x2 - x1;
            var dy = y2 - y1;
            var dz = z2 - z1;
            return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        /// <summary>根据天气+风速确定 Pasquill 稳定度</summary>
        public static StabilityClass GetStabilityClass(WeatherType weather, float windSpeed)
        {
            return weather switch
            {
                WeatherType.Sunny when windSpeed <= 5 => StabilityClass.A,
                WeatherType.Sunny => StabilityClass.B,
                WeatherType.Overcast => StabilityClass.D,
                WeatherType.Rain => StabilityClass.D,
                WeatherType.Fog => StabilityClass.E,
                WeatherType.Night => StabilityClass.F,
                _ => StabilityClass.D,
            };
        }

        /// <summary>Pasquill-Gifford 水平扩散系数 σy(m)，x 为扩散距离(m)</summary>
        public static float SigmaY(StabilityClass cls, float x)
        {
            var coeff = cls switch
            {
                StabilityClass.A => 0.22f, StabilityClass.B => 0.16f,
                StabilityClass.C => 0.11f, StabilityClass.D => 0.08f,
                StabilityClass.E => 0.06f, StabilityClass.F => 0.04f,
                _ => 0.08f,
            };
            return coeff * x / (float)Math.Sqrt(1f + 0.0001f * x);
        }

        /// <summary>Pasquill-Gifford 垂直扩散系数 σz(m)，x 为扩散距离(m)</summary>
        public static float SigmaZ(StabilityClass cls, float x)
        {
            return cls switch
            {
                StabilityClass.A => 0.20f * x,
                StabilityClass.B => 0.12f * x,
                StabilityClass.C => 0.08f * x / (float)Math.Sqrt(1f + 0.0002f * x),
                StabilityClass.D => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
                StabilityClass.E => 0.03f * x / (float)Math.Sqrt(1f + 0.0003f * x),
                StabilityClass.F => 0.016f * x / (float)Math.Sqrt(1f + 0.0003f * x),
                _ => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
            };
        }

        /// <summary>高斯烟团中心浓度 C_max = Q / [(2π)^(3/2) σy² σz]</summary>
        public static float GaussianPeakConcentration(float sourceStrength, float sigmaY, float sigmaZ)
        {
            var denom = (float)Math.Pow(2f * (float)Math.PI, 1.5f) * sigmaY * sigmaY * sigmaZ;
            return denom > 0.001f ? sourceStrength / denom : 0f;
        }

        /// <summary>高斯烟团某点浓度 C(x,y,z)  — 简化：相对中心的偏移</summary>
        public static float GaussianConcentration(float q, float sx, float sy, float sz, float offsetY, float offsetZ)
        {
            var norm = q / ((float)Math.Pow(2f * (float)Math.PI, 1.5f) * sx * sy * sz);
            var ey = (float)Math.Exp(-0.5f * offsetY * offsetY / (sy * sy));
            var ez = (float)Math.Exp(-0.5f * offsetZ * offsetZ / (sz * sz));
            var ezr = (float)Math.Exp(-0.5f * offsetZ * offsetZ / (sz * sz));
            return norm * ey * (ez + ezr);
        }
        public static bool PointInPolygon(float px, float pz, ReadOnlySpan<(float X, float Z)> vertices)
        {
            if (vertices.Length < 3) return false;
            bool inside = false;
            int j = vertices.Length - 1;
            for (int i = 0; i < vertices.Length; i++)
            {
                var xi = vertices[i].X; var zi = vertices[i].Z;
                var xj = vertices[j].X; var zj = vertices[j].Z;
                if ((zi > pz) != (zj > pz) && px < (xj - xi) * (pz - zi) / (zj - zi) + xi)
                    inside = !inside;
                j = i;
            }
            return inside;
        }
    }
}
