CounterDroneBackend/src/CounterDrone.Core/Simulation/DroneEntity.cs

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

namespace CounterDrone.Core.Simulation
{
    public class DroneEntity
    {
        public string Id { get; }
        public string WaveId { get; }
        public DroneType DroneType { get; }
        public PowerType PowerType { get; }
        public float Wingspan { get; }
        public float CruiseSpeed { get; }
        public string? ModelId { get; }
        public List<Waypoint> Route { get; }
        public float PosX { get; private set; }
        public float PosY { get; private set; }
        public float PosZ { get; private set; }
        public float Hp { get; private set; } = 1.0f;
        public DroneStatus Status { get; private set; } = DroneStatus.Flying;
        public float ExposureTime { get; set; }
        /// <summary>上一帧位置（路径积分用）</summary>
        public float PrevX { get; private set; }
        public float PrevY { get; private set; }
        public float PrevZ { get; private set; }

        public float TraveledArc => _traveledArc;
        public float TotalArc => _totalArc;
        public float Progress => _totalArc > 0 ? _traveledArc / _totalArc : 0f;
        /// <summary>沿航路已飞行弧长（米），由 RouteGeometry 驱动</summary>
        private float _traveledArc;
        /// <summary>航路总弧长（米），构造时一次性算好（航路静态，无需每帧重算）</summary>
        private readonly float _totalArc;
        /// <summary>每段段长（米），_segLen[i] = Route[i]→Route[i+1]</summary>
        private readonly float[] _segLen;
        /// <summary>每段终点累积弧长（米），_cumArc[i] = sum(_segLen[0..i])。
        /// 查找弧长 s 所在段：最大的 i 使 _cumArc[i] >= s。</summary>
        private readonly float[] _cumArc;

        public DroneEntity(string id, string waveId, DroneSpec spec, List<Waypoint> route,
            int formationIndex, float lateralSpacing, int longitudinalIndex, float longitudinalSpacing,
            FormationMode mode, int lateralAxis = 2, int longitudinalAxis = 0)
        {
            Id = id;
            WaveId = waveId;
            DroneType = (DroneType)spec.DroneType;
            PowerType = (PowerType)spec.PowerType;
            Wingspan = (float)spec.Wingspan;
            ModelId = string.IsNullOrEmpty(spec.ModelId) ? null : spec.ModelId;
            float spd = (float)route[0].Speed;
            if (spd <= 0) throw new InvalidOperationException($"无人机 {id}: 航路点速度必须 > 0");
            CruiseSpeed = spd;
            Route = route;

            // 编队偏移：按 LateralAxis/LongitudinalAxis 展开（0=X, 1=Y, 2=Z）
            float latOffset = 0, vertOffset = 0, longOffset = 0;
            if (mode == FormationMode.Formation)
            {
                float lat = formationIndex * lateralSpacing;
                float lon = -longitudinalIndex * longitudinalSpacing;
                if (lateralAxis == 0) latOffset = lat; else if (lateralAxis == 1) vertOffset = lat; else latOffset = lat;
                if (longitudinalAxis == 0) longOffset = lon; else if (longitudinalAxis == 1) vertOffset += lon; else longOffset = lon;
            }
            else if (mode == FormationMode.Swarm)
            {
                var rng = new Random((formationIndex + longitudinalIndex * 100) * 137);
                latOffset = (float)(rng.NextDouble() - 0.5) * lateralSpacing * 3;
                longOffset = (float)(rng.NextDouble() - 0.5) * lateralSpacing * 3;
            }

            var offsetRoute = new List<Waypoint>();
            foreach (var wp in route)
                offsetRoute.Add(new Waypoint
                {
                    PosX = wp.PosX + longOffset,
                    PosY = wp.PosY + vertOffset,
                    PosZ = wp.PosZ + latOffset,
                    Altitude = wp.Altitude,
                    Speed = wp.Speed,
                });
            Route = offsetRoute;

            if (offsetRoute.Count > 0)
            {
                PosX = (float)offsetRoute[0].PosX;
                PosY = (float)offsetRoute[0].PosY;
                PosZ = (float)offsetRoute[0].PosZ;
            }

            // 预计算航路几何缓存：航路在构造后静态不变，避免每帧重算总弧长与各段段长
            int segCount = Math.Max(0, offsetRoute.Count - 1);
            _segLen = new float[segCount];
            _cumArc = new float[segCount];
            float accum = 0f;
            for (int i = 0; i < segCount; i++)
            {
                float sl = Kinematics.Distance3D(
                    (float)offsetRoute[i].PosX, (float)offsetRoute[i].PosY, (float)offsetRoute[i].PosZ,
                    (float)offsetRoute[i + 1].PosX, (float)offsetRoute[i + 1].PosY, (float)offsetRoute[i + 1].PosZ);
                _segLen[i] = sl;
                accum += sl;
                _cumArc[i] = accum;
            }
            _totalArc = accum;
        }

        public void SavePreviousPosition()
        {
            PrevX = PosX; PrevY = PosY; PrevZ = PosZ;
        }

        public void Update(float deltaTime)
        {
            if (Status != DroneStatus.Flying) return;
            if (Route.Count == 0) return;

            // 运动模型统一在 RouteGeometry：弧长推进 + 位置查询。
            // 无人机严格按航路匀速飞行。无人机不受风偏影响（真实无人机有飞控修正航向），
            // 但云团受风偏影响（见 SimulationEngine 毁伤判定的云团参考系修正）。
            // 航路几何在构造时已缓存为 _totalArc / _segLen / _cumArc，此处不再重算。
            if (_totalArc <= 0f)
            {
                Status = DroneStatus.ReachedTarget;
                return;
            }

            float speedMs = CruiseSpeed / 3.6f;
            _traveledArc += speedMs * deltaTime;

            if (_traveledArc >= _totalArc)
            {
                var end = Route[^1];
                PosX = (float)end.PosX;
                PosY = (float)end.PosY;
                PosZ = (float)end.PosZ;
                Status = DroneStatus.ReachedTarget;
                return;
            }

            // 在缓存的累积弧长数组中定位所在段（线性扫描：航点数通常 ≤10）
            int segIdx = 0;
            while (segIdx < _cumArc.Length - 1 && _traveledArc > _cumArc[segIdx])
                segIdx++;

            float segStartArc = segIdx > 0 ? _cumArc[segIdx - 1] : 0f;
            float segLen = _segLen[segIdx];
            float t = segLen > 0.0001f ? (_traveledArc - segStartArc) / segLen : 0f;
            var a = Route[segIdx];
            var b = Route[segIdx + 1];
            PosX = (float)(a.PosX + (b.PosX - a.PosX) * t);
            PosY = (float)(a.PosY + (b.PosY - a.PosY) * t);
            PosZ = (float)(a.PosZ + (b.PosZ - a.PosZ) * t);
        }

        public void ApplyDamage(float damage)
        {
            if (Status != DroneStatus.Flying) return;
            Hp -= damage;
            if (Hp <= 0)
            {
                Hp = 0;
                Status = DroneStatus.Destroyed;
            }
        }

        public void MarkZoneIntruded()
        {
            Status = DroneStatus.ZoneIntruded;
        }
    }
}