using System;
using System.Collections.Generic;
using CounterDrone.Core.Models;

namespace CounterDrone.Core.Algorithms
{
    /// <summary>航路几何工具——对 waypoint 序列做纯几何运算。
    /// 无状态、纯函数，与 Kinematics/DamageAssessment 同范式。
    /// planner（预测）和 DroneEntity（执行）共用，确保两边航路模型一致。</summary>
    public static class RouteGeometry
    {
        /// <summary>航路总弧长（米）：所有相邻 waypoint 三维距离之和。</summary>
        public static float TotalLength(IReadOnlyList<Waypoint> wps)
        {
            if (wps == null || wps.Count < 2) return 0f;
            float total = 0f;
            for (int i = 0; i < wps.Count - 1; i++)
                total += Kinematics.Distance3D(
                    (float)wps[i].PosX, (float)wps[i].PosY, (float)wps[i].PosZ,
                    (float)wps[i + 1].PosX, (float)wps[i + 1].PosY, (float)wps[i + 1].PosZ);
            return total;
        }

        /// <summary>沿航路弧长 s 处的三维位置（分段线性插值）。
        /// s≤0 返回起点，s≥TotalLength 返回终点。</summary>
        public static (float X, float Y, float Z) PositionAt(IReadOnlyList<Waypoint> wps, float arcLength)
        {
            if (wps == null || wps.Count == 0) return (0f, 0f, 0f);
            if (wps.Count == 1 || arcLength <= 0f)
                return ((float)wps[0].PosX, (float)wps[0].PosY, (float)wps[0].PosZ);

            float remaining = arcLength;
            for (int i = 0; i < wps.Count - 1; i++)
            {
                float segLen = Kinematics.Distance3D(
                    (float)wps[i].PosX, (float)wps[i].PosY, (float)wps[i].PosZ,
                    (float)wps[i + 1].PosX, (float)wps[i + 1].PosY, (float)wps[i + 1].PosZ);
                if (remaining <= segLen)
                {
                    float t = segLen > 0.0001f ? remaining / segLen : 0f;
                    return (
                        (float)(wps[i].PosX + (wps[i + 1].PosX - wps[i].PosX) * t),
                        (float)(wps[i].PosY + (wps[i + 1].PosY - wps[i].PosY) * t),
                        (float)(wps[i].PosZ + (wps[i + 1].PosZ - wps[i].PosZ) * t));
                }
                remaining -= segLen;
            }
            // 超过总弧长，返回终点
            var last = wps[^1];
            return ((float)last.PosX, (float)last.PosY, (float)last.PosZ);
        }

        /// <summary>航路上离目标点 (x,z) 最近的点对应的弧长（水平投影最近）。
        /// 用于 planner 定位"无人机穿越点在航路上的弧长位置"。
        /// 算法：逐段求点到线段的最近点，取全局最近者。</summary>
        public static float ArcLengthNearestTo(IReadOnlyList<Waypoint> wps, float x, float z)
        {
            if (wps == null || wps.Count < 2) return 0f;

            float bestArc = 0f;
            float bestDist = float.MaxValue;
            float accumArc = 0f;

            for (int i = 0; i < wps.Count - 1; i++)
            {
                float ax = (float)wps[i].PosX, az = (float)wps[i].PosZ;
                float bx = (float)wps[i + 1].PosX, bz = (float)wps[i + 1].PosZ;
                float segDx = bx - ax, segDz = bz - az;
                float segLenSq = segDx * segDx + segDz * segDz;

                float t = 0f;
                if (segLenSq > 0.0001f)
                {
                    t = ((x - ax) * segDx + (z - az) * segDz) / segLenSq;
                    t = Math.Max(0f, Math.Min(1f, t));
                }
                float projX = ax + segDx * t;
                float projZ = az + segDz * t;
                float distSq = (projX - x) * (projX - x) + (projZ - z) * (projZ - z);

                if (distSq < bestDist)
                {
                    bestDist = distSq;
                    float segLen = (float)Math.Sqrt(segLenSq);
                    bestArc = accumArc + t * segLen;
                }
                accumArc += (float)Math.Sqrt(segLenSq);
            }
            return bestArc;
        }

        /// <summary>从航路起点匀速运动到弧长 s 处的飞行时间（秒）。
        /// 匀速直线模型，时间 = 弧长 / 速度。内部调用 Kinematics.TravelTime。</summary>
        /// <param name="speedKmh">速度 km/h（与 TypicalSpeed 单位一致），必须 > 0</param>
        public static float TravelTimeTo(IReadOnlyList<Waypoint> wps, float arcLength, float speedKmh)
        {
            if (speedKmh <= 0)
                throw new ArgumentException("速度必须 > 0", nameof(speedKmh));
            // 弧长 = 距离，匀速直线时间 = 距离/速度
            // 用 Kinematics.TravelTime 保持单位换算唯一（km/h → m/s 在 Kinematics 内）
            return arcLength / (speedKmh / 3.6f);
        }

        /// <summary>沿航路方向给定弧长 s 处的水平单位切向量 (DirX, DirZ)。
        /// 用于 planner 沿航路方向布云（offset 沿切向，不再写死 X 轴）。
        /// 弧长超出范围时取末段方向。</summary>
        public static (float DirX, float DirZ) TangentAt(IReadOnlyList<Waypoint> wps, float arcLength)
        {
            if (wps == null || wps.Count < 2) return (1f, 0f);

            if (arcLength < 0) arcLength = 0;
            float remaining = arcLength;
            for (int i = 0; i < wps.Count - 1; i++)
            {
                float dx = (float)wps[i + 1].PosX - (float)wps[i].PosX;
                float dz = (float)wps[i + 1].PosZ - (float)wps[i].PosZ;
                float segLen = (float)Math.Sqrt(dx * dx + dz * dz);
                if (remaining <= segLen || i == wps.Count - 2)
                {
                    if (segLen < 0.0001f) return (1f, 0f);
                    return (dx / segLen, dz / segLen);
                }
                remaining -= segLen;
            }
            // 兜底：末段方向
            float ldx = (float)wps[^1].PosX - (float)wps[^2].PosX;
            float ldz = (float)wps[^1].PosZ - (float)wps[^2].PosZ;
            float llen = (float)Math.Sqrt(ldx * ldx + ldz * ldz);
            return llen < 0.0001f ? (1f, 0f) : (ldx / llen, ldz / llen);
        }
    }
}
