using Newtonsoft.Json;
using SimpleLite;
using SimpleLite.RCS;
using SimpleLite.RCS.CarTypes;
using SimpleLite.CADTools;
using SimpleLite.Props;
using SimpleLite.UI;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using StandardScene.Chained;
using StandardScene.InterLock;
using StandardScene.Utils;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using StandardScene.Model;
using static StandardScene.Chained.ChainedDeliveryMission;

namespace StandardScene.Charge
{
    #region Enums and Status Classes

    /// <summary>
    /// 充电决策枚举
    /// </summary>
    public enum ChargeDecision
    {
        MustCharge,
        AvailableToTask,
        NoCharge,
    }

    /// <summary>
    /// 充电任务状态类
    /// </summary>
    public class AbstractChargeMissionStatus : MissionStatus
    {
        #region 运行时状态

        /// <summary>空闲充电桩数量</summary>
        public int freeChargeCount;

        /// <summary>当前最低电量车辆的SOC</summary>
        public float lowerSocCar;

        /// <summary>空闲车辆比例</summary>
        public double freeCarPercent;

        #endregion

        #region 电量阈值配置

        /// <summary>必须充电电量阈值（低于此值必须充电）</summary>
        public double mustChargeSoc;

        /// <summary>空闲充电电量阈值</summary>
        public double idleChargeSoc;

        /// <summary>任务可用电量阈值（高于此值可执行任务）</summary>
        public double taskAvailableSoc;

        /// <summary>允许打断充电的最低电量</summary>
        public double allowInterruptSoc;

        /// <summary>满电阈值</summary>
        public double fullChargeSoc;

        #endregion

        #region 时间配置

        /// <summary>空闲后触发充电的等待秒数</summary>
        public double idleChargeSeconds;

        /// <summary>必须充电的最短秒数</summary>
        public double mustChargeSeconds;

        /// <summary>空闲后触发回待命的等待秒数</summary>
        public double idleSeconds;

        /// <summary>补电保持分钟数（充到100%后继续保持）</summary>
        public double topUpMinutes = 5;

        #endregion

        #region 策略开关

        /// <summary>最小允许空闲车去充电的任务数</summary>
        public int minAllowFreeCarToChargeTaskCnt;

        /// <summary>允许充电数量</summary>
        public int allowChargeCnt;

        /// <summary>是否允许打断充电去执行任务</summary>
        public bool allowInterruptTask;

        /// <summary>是否使用最低电量优先充电</summary>
        public bool useLowerSocForCharge;

        /// <summary>低电量车辆换充阈值差</summary>
        public int lowPowerCarSwapChangeThresholdDelta = 15;

        /// <summary>是否启用充电异常检测</summary>
        public bool enableErrorChargeDetection = false;

        /// <summary>是否使用充电站过滤器</summary>
        public bool useChargeSiteFilter = false;

        #endregion
    }

    /// <summary>
    /// 单个车辆处理过程中的上下文信息
    /// </summary>
    public class CarChargeContext
    {
        /// <summary>当前处理的车辆</summary>
        public Car Car { get; set; }

        /// <summary>当前电量百分比</summary>
        public double Soc { get; set; }

        /// <summary>当前充电电流</summary>
        public double ElectricCurrent { get; set; }

        /// <summary>是否必须充电</summary>
        public bool MustCharge { get; set; }

        /// <summary>是否应该去充电</summary>
        public bool ShouldGoCharge { get; set; }

        /// <summary>是否应该去待命点</summary>
        public bool ShouldGoStandby { get; set; }

        /// <summary>车辆是否在待命点</summary>
        public bool CarInStandbySite { get; set; }

        /// <summary>车辆是否在充电站</summary>
        public bool CarInChargeSite { get; set; }

        /// <summary>充电后是否可执行任务</summary>
        public bool ChargingTaskAvailable { get; set; }

        /// <summary>等待执行的任务数量</summary>
        public int WaitingMissions { get; set; }

        /// <summary>空闲开始时间</summary>
        public DateTime IdleTime { get; set; }

        /// <summary>目标类型：1充电，2待命</summary>
        public int TargetType { get; set; }

        /// <summary>目标路径规划</summary>
        public SegmentPlan TargetPlan { get; set; }
    }

    #endregion

    /// <summary>
    /// 抽象充电任务基类
    /// 负责管理车辆的自动充电和待命调度
    /// </summary>
    public class AbstractChargeLogiceMission : AbstractInterlockMission
    {
        #region Constants

        /// <summary>主循环间隔（毫秒）</summary>
        protected const int LOOP_INTERVAL_MS = 500;

        /// <summary>必须充电触发延迟（毫秒）</summary>
        protected const int MUST_CHARGE_TRIGGER_DELAY_MS = 5000;

        /// <summary>目标类型：充电</summary>
        protected const int TARGET_TYPE_CHARGE = 1;

        /// <summary>目标类型：待命</summary>
        protected const int TARGET_TYPE_STANDBY = 2;

        /// <summary>最大不可达站点记录数</summary>
        protected const int MAX_UNDELIVERABLE_SITES = 5;

        /// <summary>触发任务打断的最小等待任务数</summary>
        protected const int MAX_WAIT_WAITMISSION = 1;

        /// <summary>补电相关标签名称</summary>
        private static readonly string[] TopUpTags = { "topup", "topupInProgress", "topupStart", "topupEnd", "topupMinutes", "topupRequested" };

        #endregion

        #region Fields and Properties

        public override MissionStatus status { get; set; } = new AbstractChargeMissionStatus();

        [JsonIgnore] private protected Thread myThread;
        [JsonIgnore] public Func<Car, ChargeDecision> chargeStrategy = null;
        [JsonIgnore] public bool started = false;

        [JsonIgnore] private int _interruptingCarId = -1;
        [JsonIgnore] private int _bestInterruptionCandidateId = -1;
        [JsonIgnore] private readonly object _interruptLock = new object();

        /// <summary>获取状态对象的类型转换</summary>
        protected AbstractChargeMissionStatus AcmStatus => (AbstractChargeMissionStatus)status;

        #endregion

        #region Virtual Methods - 扩展点

        /// <summary>
        /// 小车到达充电站或待命点时的动作
        /// </summary>
        public virtual void ArriveAction(Car car, Site site) { }

        /// <summary>
        /// 小车离开充电站或待命点时的动作
        /// </summary>
        public virtual void LeaveAction(Car car, Site site) { }

        /// <summary>
        /// 根据小车获取对应车所允许停靠的休息点类型
        /// </summary>
        public virtual string GetStandbyType(AbstractCar car) => "standby";

        /// <summary>
        /// 根据小车获取对应车所允许停靠的充电点类型
        /// </summary>
        public virtual string GetChargeType(AbstractCar car) => "Charge";

        /// <summary>
        /// 获取小车对应车型待执行的任务数量
        /// </summary>
        public virtual int WaitingMissions(AbstractCar car)
        {
            var waitingDeliveries = SimpleProject.proj.Missions
        .OfType<TransportMission>()
          .First()
           .GetDeliveries()
                .FindAll(p => p.GetStatus() == DeliveryStatus.Waiting);
            return waitingDeliveries.Count;
        }

        /// <summary>
        /// 额外的对充电站的筛选要求（如在线状态）
        /// </summary>
        public virtual bool ChargeSiteFilter(Site site) => true;

        /// <summary>
        /// 获取不在充电的最低电量车辆的SOC值
        /// </summary>
        public virtual float LowerCarSoc(AbstractCar car, List<Site> allChargeSite) => 0;

        /// <summary>
        /// 判断车辆是否在线
        /// </summary>
        public virtual bool IsOnlineCar(Car car) => false;

        /// <summary>
        /// 是否禁止小车回待命点或充电点
        /// </summary>
        public virtual bool ForbidBackToStandbyOrCharge(AbstractCar car) => false;

        /// <summary>
        /// 验证车辆是否可以处理
        /// </summary>
        protected virtual bool ValidateCarForProcessing(Car car)
        {
            if (Commons.GetVehicleStatus(car)!= VehicleStatus.Normal) return false;
            if (car.GetLastSite() == -1) return false;
            if (car.fields.ContainsKey("arm")) return false;
            return true;
        }

        /// <summary>
        /// 判断充电桩是否异常
        /// </summary>
        protected virtual bool IsChargePointError(Car car, int chargeSiteId)
        {
            return car != null && Commons.CarValue(car, "electricCurrent") < 0;
        }

        #endregion

        #region Mission Execution - 主执行逻辑

        [MethodMember(Name = "启动进程", Description = "开始处理充电维护进程")]
        public override void Execute()
        {
            status.status = "已启动";
            if (started) return;
            started = true;

            myThread = new Thread(ChargeLoop) { Name = "AbstractChargeMission" };
            myThread.Start();
        }

        /// <summary>
        /// 充电主循环
        /// </summary>
        private void ChargeLoop()
        {
            while (started)
            {
                Thread.Sleep(LOOP_INTERVAL_MS);

                try
                {
                    UpdateMissionParameters();

                    // 按电量从低到高处理所有车辆
                    var orderedCars = SimpleLib.GetAllCars()
              .OfType<Car>()
                     .OrderBy(cc => Commons.CarValue(cc, "Soc"));

                    foreach (var car in orderedCars)
                    {
                        var allChargeSites = GetChargeSitesForCar(car);
                        var chargingCarCount = CalculateGlobalStats(allChargeSites);
                        ProcessSingleCar(car, allChargeSites, chargingCarCount);
                    }
                }
                catch (Exception e)
                {
                    Diagnosis.Log($"chargeMission error:{ExceptionFormatter.FormatEx(e)}", "error", true);
                }
            }
        }

        /// <summary>
        /// 获取车辆可用的充电站点列表
        /// </summary>
        private List<Site> GetChargeSitesForCar(Car car)
        {
            var chargeType = GetChargeType(car);
            var allChargeSites = SimpleLib.GetAllSites()
                   .Where(p => p.fields.TryGetValue("group", out var group) && chargeType.Contains(group))
           .ToList();

            if (AcmStatus.useChargeSiteFilter)
            {
                allChargeSites = allChargeSites.Where(ChargeSiteFilter).ToList();
            }

            return allChargeSites;
        }

        #endregion

        #region Top-Up (补电) 功能

        [MethodMember(Name = "补电功能", Description = "将指定小车发送去补电（到100%并保持若干分钟）")]
        public void StartTopUp()
        {
            try
            {
                G.pushStatus("选择小车");
                var obj = SimpleMonitor.selected.ToArray()[0];
                if (obj is Car car)
                {
                    if (car == null)
                    {
                        Diagnosis.Post($"StartTopUp failed: car not found", "StartTopUpCharge", true);
                        return;
                    }

                    Commons.AddOrUpdateTag(car.tags, "topup", "requested");
                    Commons.AddOrUpdateTag(car.tags, "shouldCharge", "true");
                    Diagnosis.Log($"StartTopUp: car {car.name}({car.id}) requested top-up", "StartTopUpCharge", true);
                }
            }
            catch (Exception ex)
            {
                Diagnosis.Log($"StartTopUp error: {ExceptionFormatter.FormatEx(ex)}", "error", true);
            }
        }

        /// <summary>
        /// 测试Block功能（内部调试用）
        /// </summary>
        public void TestBlock()
        {
            G.pushStatus("选择小车");
            var obj = SimpleMonitor.selected.ToArray()[0];
            if (obj is Car car && car != null)
            {
                CommandToBlock(car.id);
            }
        }

        /// <summary>
        /// 处理补电生命周期
        /// </summary>
        private void HandleTopUpLifecycle(CarChargeContext context)
        {
            var car = context.Car;

            // 如果没有补电相关标签，直接返回
            if (!IsInTopUpMode(car)) return;

            // 补电请求但未到充电站，确保shouldCharge存在
            if (car.tags.Contains("topup") && !context.CarInChargeSite)
            {
                Commons.AddOrUpdateTag(car.tags, "shouldCharge", "true");
                return;
            }

            // 已到充电站且处于补电模式
            if (car.tags.Contains("topup") && context.CarInChargeSite)
            {
                HandleTopUpAtChargeSite(context);
            }
        }

        /// <summary>
        /// 处理车辆在充电站时的补电逻辑
        /// </summary>
        private void HandleTopUpAtChargeSite(CarChargeContext context)
        {
            var car = context.Car;

            // 电量达到100%且未开始计时，启动补电计时器
            if (context.Soc >= 100 && !car.tags.Contains("topupInProgress"))
            {
                StartTopUpTimer(car);
            }

            // 补电进行中，检查是否完成
            if (car.tags.Contains("topupInProgress"))
            {
                CheckTopUpCompletion(car, context.Soc);
            }
        }

        /// <summary>
        /// 启动补电计时器
        /// </summary>
        private void StartTopUpTimer(Car car)
        {
            double minutes = AcmStatus.topUpMinutes;
            if (car.tags.TryGetValue("topupMinutes", out var mStr) &&
       double.TryParse(mStr, out var mVal) && mVal > 0)
            {
                minutes = mVal;
            }

            var start = DateTime.Now;
            var end = start.AddMinutes(minutes);
            Commons.AddOrUpdateTag(car.tags, "topupStart", start.ToString("o"));
            Commons.AddOrUpdateTag(car.tags, "topupEnd", end.ToString("o"));
            Commons.AddOrUpdateTag(car.tags, "topupInProgress", "true");
            Diagnosis.Log($"TopUp started for car {car.name}({car.id}), until {end}", "Charge", true);
        }

        /// <summary>
        /// 检查补电是否完成
        /// </summary>
        private void CheckTopUpCompletion(Car car, double soc)
        {
            if (!car.tags.TryGetValue("topupEnd", out var endStr) ||
                !DateTime.TryParse(endStr, out var endTime))
            {
                return;
            }

            if (DateTime.Now >= endTime && soc >= 100)
            {
                ClearTopUpTags(car);
                Commons.DeleteTag(car.tags, "shouldCharge");
                Commons.AddOrUpdateTag(car.tags, "mustToStandby", "true");
                Diagnosis.Log($"TopUp completed for car {car.name}({car.id})", "Charge", true);
            }
        }

        /// <summary>
        /// 清理所有补电相关标签
        /// </summary>
        private void ClearTopUpTags(Car car)
        {
            foreach (var tag in TopUpTags)
            {
                Commons.DeleteTag(car.tags, tag);
            }
        }

        /// <summary>
        /// 判断车辆是否处于补电模式
        /// </summary>
        private bool IsInTopUpMode(Car car)
        {
            return car.tags.Contains("topup") || car.tags.Contains("topupInProgress");
        }

        #endregion

        #region Parameter Management - 参数管理

        private void UpdateMissionParameters()
        {
            try
            {
                // 从充电策略配置服务读取参数
                var config = ChargeStationHelper.GetChargeStrategyConfig();
                
                if (config != null)
                {
                    // SOC 相关参数
                    AcmStatus.mustChargeSoc = config.MustChargeSoc;
                    AcmStatus.idleChargeSoc = config.IdleChargeSoc;
                    AcmStatus.taskAvailableSoc = config.TaskAvailableSoc;
                    AcmStatus.fullChargeSoc = config.FullChargeSoc;
                    AcmStatus.allowInterruptSoc = config.AllowInterruptSoc;

                    // 时间相关参数
                    AcmStatus.idleChargeSeconds = config.IdleChargeSeconds;
                    AcmStatus.idleSeconds = config.IdleSeconds;
                    AcmStatus.mustChargeSeconds = config.MustChargeSeconds;
                    AcmStatus.topUpMinutes = config.TopUpMinutes;

                    // 任务相关参数
                    AcmStatus.minAllowFreeCarToChargeTaskCnt = config.MinAllowFreeCarToChargeTaskCnt;

                    // 开关参数
                    AcmStatus.allowInterruptTask = config.AllowInterruptTask;
                    AcmStatus.useLowerSocForCharge = config.UseLowerSocForCharge;
                    AcmStatus.enableErrorChargeDetection = config.EnableErrorChargeDetection;
                    AcmStatus.useChargeSiteFilter = config.UseChargeSiteFilter;

                    //Diagnosis.Log("充电策略参数已从配置文件加载");
                }
                else
                {
                    // 配置为空，使用旧的方式（从 fields 读取）
                    LoadParametersFromFields();
                }
            }
            catch (Exception ex)
            {
                Diagnosis.Log($"加载充电策略参数失败，使用fields或默认值: {ex.Message}");
                // 出错时使用旧的方式（从 fields 读取）
                LoadParametersFromFields();
            }
        }

        /// <summary>
        /// 从 fields 字典加载参数（兼容旧方式）
        /// </summary>
        private void LoadParametersFromFields()
        {
            AcmStatus.mustChargeSoc = ParseDouble("mustChargeSoc", 20);
            AcmStatus.idleChargeSoc = ParseDouble("idleChargeSoc", 90);
            AcmStatus.taskAvailableSoc = ParseDouble("taskAvailableSoc", 60);
            AcmStatus.fullChargeSoc = ParseDouble("fullChargeSoc", 90);
            AcmStatus.allowInterruptSoc = ParseDouble("allowInterruptSoc", 45);

            AcmStatus.idleChargeSeconds = ParseDouble("idleChargeSeconds", 30);
            AcmStatus.idleSeconds = ParseDouble("idleSeconds", 5);
            AcmStatus.mustChargeSeconds = ParseDouble("mustChargeSeconds", 60);
            AcmStatus.topUpMinutes = ParseDouble("topUpMinutes", 5);

            AcmStatus.minAllowFreeCarToChargeTaskCnt = ParseInt("minAllowFreeCarToChargeTaskNub", 0);
            AcmStatus.allowInterruptTask = ParseBool("allowInterruptTask", false);
            AcmStatus.useLowerSocForCharge = ParseBool("useLowerSocForCharge", true);
            AcmStatus.enableErrorChargeDetection = ParseBool("enableErrorChargeDetection", false);
            AcmStatus.useChargeSiteFilter = ParseBool("useChargeSiteFilter", false);
        }

        private double ParseDouble(string key, double defaultValue)
        {
            return fields.TryGetValue(key, out var str) && double.TryParse(str, out var val) ? val : defaultValue;
        }

        private int ParseInt(string key, int defaultValue)
        {
            return fields.TryGetValue(key, out var str) && int.TryParse(str, out var val) ? val : defaultValue;
        }

        private bool ParseBool(string key, bool defaultValue)
        {
            return fields.TryGetValue(key, out var str) && bool.TryParse(str, out var val) ? val : defaultValue;
        }

        #endregion

        #region Global Stats - 全局统计

        private int CalculateGlobalStats(List<Site> allChargeSites)
        {
            var validCars = SimpleLib.GetAllCars().Where(p => p.GetLastSite() != -1).ToList();
            int chargingCarCount = 0;
            int freeCarCount = 0;
            int onlineCarCount = 0;

            foreach (var car in validCars)
            {
                if (IsCarOccupyingChargeSite(car, allChargeSites))
                    chargingCarCount++;

                if (car.tags.Contains("idle") && car.status.pendingLocks.Length == 0)
                    freeCarCount++;

                if (IsOnlineCar((Car)car))
                    onlineCarCount++;
            }

            if (onlineCarCount != 0)
                AcmStatus.freeCarPercent = (float)freeCarCount / onlineCarCount;

            return chargingCarCount;
        }

        private bool IsCarOccupyingChargeSite(AbstractCar car, List<Site> allChargeSites)
        {
            return allChargeSites.Any(site =>
        car.status.holdingLocks.Contains(site.id) ||
   car.status.pendingLocks.Contains(site.id));
        }

        #endregion

        #region Car Processing - 单车处理主流程

        private void ProcessSingleCar(Car car, List<Site> allChargeSites, int chargingCarCount)
        {
            if (!ValidateCarForProcessing(car)) return;

            var context = BuildCarContext(car, allChargeSites, chargingCarCount);

            // 清理待命标签
            if (context.CarInStandbySite && car.tags.Contains("mustToStandby"))
                Commons.DeleteTag(car.tags, "mustToStandby");

            // 任务打断检查
            HandleTaskInterruption(context);

            // 前置条件检查
            if (IsCarBusy(car) || ForbidBackToStandbyOrCharge(car)) return;

            // 必须充电逻辑
            HandleMustChargeLogic(context);

            // 更新空闲和充电状态
            if (!UpdateIdleAndChargeStatus(context)) return;

            // 处理补电生命周期
            HandleTopUpLifecycle(context);

            // 决定充电或回待命
            DecideChargeOrStandby(context);

            // 规划路径并执行
            PlanAndExecuteMovement(context, allChargeSites);
        }

        private CarChargeContext BuildCarContext(Car car, List<Site> allChargeSites, int chargingCarCount)
        {
            var soc = Commons.CarValue(car, "Soc");
            var electricCurrent = Commons.CarValue(car, "ElectricCurrent");
            var carSiteId = car.GetLastSite();

            var context = new CarChargeContext
            {
                Car = car,
                Soc = soc,
                ElectricCurrent = electricCurrent,
                MustCharge = soc < AcmStatus.mustChargeSoc,
                WaitingMissions = WaitingMissions(car),
                CarInStandbySite = SimpleLib.GetAllSites()
                .Any(p => p.fields.ContainsKey("standby") && p.id == carSiteId),
                CarInChargeSite = car.status.holdingLocks.Length == 1 &&
                SimpleLib.GetAllSites().Any(p => p.fields.ContainsKey("Charge") && p.id == car.status.holdingLocks.First())
            };

            // 更新全局状态
            AcmStatus.freeChargeCount = allChargeSites.Count - chargingCarCount;
            AcmStatus.lowerSocCar = LowerCarSoc(car, allChargeSites);

            if (car.tags.TryGetValue("idle", out var strIdleTime))
                context.IdleTime = DateTime.Parse(strIdleTime);

            return context;
        }

        private bool IsCarBusy(Car car)
        {
            return car.tags.Contains("occupied") ||
            car.tags.Contains("blocking") ||
                car.status.pendingLocks.Length != 0;
        }

        #endregion

        #region Charging Logic - 充电逻辑

        /// <summary>
        /// 处理任务打断逻辑
        /// </summary>
        private void HandleTaskInterruption(CarChargeContext context)
        {
            lock (_interruptLock)
            {
                // 清理过时的打断持有者
                CleanupStaleInterruptHolder();

                var occupiedValue = GetOccupiedTagValue(context.Car);
                var isMaintenanceTask = IsMaintenanceTask(occupiedValue);

                // 判断是否可以考虑触发打断
                var canConsiderTrigger = CanConsiderInterrupt(context, isMaintenanceTask);

                // 评选最佳候选车
                if (canConsiderTrigger)
                {
                    SelectBestInterruptCandidate(context);
                }

                // 只有最佳候选车才触发打断
                bool isBestCandidate = context.Car.id == _bestInterruptionCandidateId;

                LadderLogic.TriggerOnce(canConsiderTrigger && isBestCandidate, 5000, () =>
                {
                    ExecuteInterrupt(context, occupiedValue);
                }, context.Car.id);
            }
        }

        /// <summary>
        /// 清理过时的打断持有者
        /// </summary>
        private void CleanupStaleInterruptHolder()
        {
            if (_interruptingCarId == -1) return;

            var holder = SimpleLib.GetAllCars().OfType<Car>().FirstOrDefault(c => c.id == _interruptingCarId);
            if (holder == null)
            {
                _interruptingCarId = -1;
                return;
            }

            var holderOccupied = GetOccupiedTagValue(holder);
            if (!IsMaintenanceTask(holderOccupied))
            {
                _interruptingCarId = -1;
            }
        }

        /// <summary>
        /// 判断是否为维护任务（充电或待命）
        /// </summary>
        private bool IsMaintenanceTask(string occupiedValue)
        {
            return occupiedValue.Contains("toCharge") || occupiedValue.Contains("toStandby");
        }

        /// <summary>
        /// 判断是否可以考虑触发打断
        /// </summary>
        private bool CanConsiderInterrupt(CarChargeContext context, bool isMaintenanceTask)
        {
            return context.WaitingMissions > 0 && context.Soc > AcmStatus.allowInterruptSoc &&
            FreeCarNub() == 0 && AcmStatus.allowInterruptTask &&
                 isMaintenanceTask && _interruptingCarId == -1;
        }

        /// <summary>
        /// 评选最佳打断候选车
        /// </summary>
        private void SelectBestInterruptCandidate(CarChargeContext context)
        {
            // 获取所有满足打断条件的车辆
            var candidates = SimpleLib.GetAllCars().OfType<Car>()
                .Where(c =>
                {
                    var cOccupied = GetOccupiedTagValue(c);
                    var cSoc = Commons.CarValue(c, "Soc");
                    return IsMaintenanceTask(cOccupied) && cSoc > AcmStatus.allowInterruptSoc;
                }).ToList();

            if (!candidates.Any()) return;

            // 获取第一个等待任务
            var firstMission = SimpleProject.proj.Missions.OfType<TransportMission>()
                         .First()
          .GetDeliveries()
                 .FindAll(p => p.GetStatus() == DeliveryStatus.Waiting)
         .OrderBy(p => p.CreateTime)
           .FirstOrDefault();

            Car bestCandidate;

            if (firstMission != null)
            {
                // 优先选择距离任务起点最近的车，其次选择电量最高的车
                bestCandidate = candidates
                     .OrderBy(c => CalculateRouteLength(c, ((TransportDelivery)firstMission).Src))
                 .ThenByDescending(c => Commons.CarValue(c, "Soc"))
                 .FirstOrDefault();
            }
            else
            {
                // 仅按电量排序
                bestCandidate = candidates
             .OrderByDescending(c => Commons.CarValue(c, "Soc")).FirstOrDefault();
            }

            if (bestCandidate != null)
            {
                _bestInterruptionCandidateId = bestCandidate.id;
                Diagnosis.Post($"更新最佳候选车ID:{bestCandidate.id}", "CommandToBlock", true);
            }
        }

        /// <summary>
        /// 计算车辆到目标站点的路径长度
        /// </summary>
        private float CalculateRouteLength(Car car, int targetSiteId)
        {
            var mPlan = new SegmentPlan { usingCar = car };
            try
            {
                var routeLength = mPlan.FindRoute(
                  SimpleLib.GetSite(car.GetLastSite()),
            SimpleLib.GetSite(targetSiteId));
                Diagnosis.Post($"当前尝试打断车{car.id}距离任务起点{mPlan.Destination.id}距离:{routeLength}", "CommandToBlock", true);
                return routeLength;
            }
            catch
            {
                return float.MaxValue;
            }
        }

        /// <summary>
        /// 执行打断操作
        /// </summary>
        private void ExecuteInterrupt(CarChargeContext context, string occupiedValue)
        {
            lock (_interruptLock)
            {
                if (_interruptingCarId == -1)
                {
                    _interruptingCarId = context.Car.id;
                    Diagnosis.Post($"小车:{context.Car.name}，触发打断任务，missions:{context.WaitingMissions}," +
          $"isOccupied:{occupiedValue},soc:{context.Soc},freecar:{FreeCarNub()}",
         "CommandToBlock", true);
                    CommandToBlock(context.Car.id);
                    _bestInterruptionCandidateId = -1;
                }
                else
                {
                    Diagnosis.Post($"跳过打断：已有车辆({_interruptingCarId})正在被打断，当前尝试车:{context.Car.id}",
            "CommandToBlock", true);
                }
            }
        }

        /// <summary>
        /// 处理必须充电逻辑
        /// </summary>
        private void HandleMustChargeLogic(CarChargeContext context)
        {
            LadderLogic.TriggerOnce(
       context.MustCharge && !context.Car.tags.Contains("shouldCharge"),
       MUST_CHARGE_TRIGGER_DELAY_MS,
                () =>
                {
                    Diagnosis.Log($"小车:{context.Car.name}，必须充电，当前电量:{context.Soc}", "Charge", true);
                    Commons.AddOrUpdateTag(context.Car.tags, "shouldCharge", "true");
                },
        context.Car.id);
        }

        /// <summary>
        /// 更新空闲和充电状态
        /// </summary>
        private bool UpdateIdleAndChargeStatus(CarChargeContext context)
        {
            var car = context.Car;

            lock (Commons.PlanSession)
            {
                if (car.tags.Contains("occupied") || car.tags.Contains("blocking")) return false;

                if (!car.tags.Contains("idle"))
                    car.tags.Add("idle", DateTime.Now.ToString());

                // 如果正在补电，即使电量满了也不应自动去待命
                if (context.Soc > AcmStatus.fullChargeSoc && !IsInTopUpMode(car))
                    context.ShouldGoStandby = true;
            }

            if (!car.tags.TryGetValue("idle", out var strIdleTime)) return false;
            context.IdleTime = DateTime.Parse(strIdleTime);

            // 清理充电标记
            if (car.tags.Contains("charging") && !context.CarInChargeSite)
                car.tags.Remove("charging");

            // 判断是否需要充电
            var carFree = context.Soc <= AcmStatus.taskAvailableSoc && AcmStatus.freeChargeCount > 0;
            var idleCharge = (context.Soc < AcmStatus.idleChargeSoc || carFree) &&
                          (DateTime.Now - context.IdleTime).TotalSeconds > AcmStatus.idleChargeSeconds;

            UpdateShouldChargeTag(context, idleCharge);

            return true;
        }

        /// <summary>
        /// 更新shouldCharge标签
        /// </summary>
        private void UpdateShouldChargeTag(CarChargeContext context, bool idleCharge)
        {
            var car = context.Car;

            if ((context.MustCharge || (idleCharge && context.WaitingMissions <= AcmStatus.minAllowFreeCarToChargeTaskCnt)) &&
          !context.ShouldGoStandby)
            {
                bool lowerSocOk = (AcmStatus.useLowerSocForCharge && context.Soc <= AcmStatus.lowerSocCar) || !AcmStatus.useLowerSocForCharge;

                if (((context.MustCharge || idleCharge) && lowerSocOk) &&
                AcmStatus.freeChargeCount > 0 && context.Soc < AcmStatus.fullChargeSoc)
                {
                    Commons.AddOrUpdateTag(car.tags, "shouldCharge", "true");
                    Diagnosis.Log($"小车:{car.name}，需求充电，当前电量:{context.Soc},lowerSocCar:{AcmStatus.lowerSocCar}," +
                 $"mustCharge：{context.MustCharge},idleCharge:{idleCharge}，waitingMissions：{context.WaitingMissions}", "Charge", true);
                }
                else
                {
                    Commons.DeleteTag(car.tags, "shouldCharge");
                }
            }
        }

        #endregion

        #region Decision Making - 决策逻辑

        /// <summary>
        /// 决定充电或回待命
        /// </summary>
        private void DecideChargeOrStandby(CarChargeContext context)
        {
            var car = context.Car;

            lock (Commons.PlanSession)
            {
                var carSiteId = car.GetLastSite();
                if (carSiteId == -1 || car.tags.Contains("occupied") || car.tags.Contains("blocking")) return;

                // 更新充电状态
                UpdateChargingTag(context);

                // 检查充电异常
                ErrorChargeTriggerToStandby(context);

                // 清除shouldCharge标记
                if ((context.ChargingTaskAvailable ||
                    (context.WaitingMissions > 0 && context.Soc > AcmStatus.taskAvailableSoc && !car.tags.Contains("charging"))) &&
                   car.tags.Contains("shouldCharge"))
                {
                    Commons.DeleteTag(car.tags, "shouldCharge");
                }

                // 检查是否可以离开充电站执行任务
                CheckCanLeaveForTask(context);

                // 决定去充电
                if (car.tags.Contains("shouldCharge") && !CarInChargeSite(car, carSiteId))
                {
                    context.ShouldGoCharge = true;
                    Diagnosis.Log($"selected car:{car.name}|{car.id} to charge", "Charge", true);
                }

                // 决定去待命
                if (!context.ShouldGoCharge)
                {
                    CheckShouldGoStandby(context, carSiteId);
                }
            }
        }

        /// <summary>
        /// 更新充电标签
        /// </summary>
        private void UpdateChargingTag(CarChargeContext context)
        {
            var car = context.Car;
            var isTopUping = IsInTopUpMode(car);

            if (context.CarInChargeSite)
            {
                if (context.Soc <= AcmStatus.fullChargeSoc || isTopUping)
                {
                    if (!car.tags.Contains("charging"))
                        car.tags.Add("charging", DateTime.Now.ToString());
                }
                else if (!isTopUping)
                {
                    if (car.tags.Contains("charging"))
                        car.tags.Remove("charging");
                    context.ChargingTaskAvailable = true;
                }
            }
        }

        /// <summary>
        /// 检查是否可以离开充电站执行任务
        /// </summary>
        private void CheckCanLeaveForTask(CarChargeContext context)
        {
            var car = context.Car;

            var canInterruptForTask = context.WaitingMissions > MAX_WAIT_WAITMISSION &&
               context.Soc > AcmStatus.allowInterruptSoc;

            if (context.Soc > AcmStatus.taskAvailableSoc && car.tags.Contains("charging"))
            {
                var onlyOneCarCharge = SimpleLib.GetAllCars().Count(p => p.GetLastSite() != -1) == 1;

                var canLeaveButStillCharge =
             (context.Soc - AcmStatus.lowerSocCar) <= AcmStatus.lowPowerCarSwapChangeThresholdDelta &&
               (context.WaitingMissions <= AcmStatus.minAllowFreeCarToChargeTaskCnt ||
                   context.Soc <= AcmStatus.lowerSocCar ||
                onlyOneCarCharge ||
               (context.WaitingMissions == 0 && context.Soc < AcmStatus.fullChargeSoc));

                // 补电模式不允许离开
                if (IsInTopUpMode(car)) return;

                // 满足任务打断条件或满足原有离开条件
                if (canInterruptForTask ||
             (car.tags.TryGetValue("charging", out var cTime) &&
           DateTime.Parse(cTime).AddSeconds(AcmStatus.mustChargeSeconds) < DateTime.Now &&
        !canLeaveButStillCharge))
                {
                    car.tags.Remove("charging");
                    context.ChargingTaskAvailable = true;
                    Commons.AddOrUpdateTag(car.tags, "mustToStandby", "true");
                    Commons.DeleteTag(car.tags, "shouldCharge");
                    if (canInterruptForTask)
                    {
                        Diagnosis.Log($"小车:{car.name}，因有任务且电量{context.Soc}高于{AcmStatus.allowInterruptSoc}，中断充电去执行任务", "Charge", true);
                    }
                }
            }
        }

        /// <summary>
        /// 检查是否应该去待命点
        /// </summary>
        private void CheckShouldGoStandby(CarChargeContext context, int carSiteId)
        {
            var car = context.Car;
            var standbyType = GetStandbyType(car);

            bool isBlocked = SimpleLib.GetAllCars()
                        .Any(c => c.status.pendingLocks.Length != 0 && c.id != car.id &&
                   c.status.pendingLocks.Contains(car.GetLastSite()));

            if (SimpleLib.GetSite(carSiteId).fields.ContainsKey(standbyType) && !isBlocked) return;
            if ((DateTime.Now - context.IdleTime).TotalSeconds < AcmStatus.idleSeconds) return;
            
            context.ShouldGoStandby = true;
        }

        /// <summary>
        /// 充电异常触发回待命点
        /// </summary>
        private void ErrorChargeTriggerToStandby(CarChargeContext context)
        {
            if (!AcmStatus.enableErrorChargeDetection || !context.CarInChargeSite) return;

            var currentChargeSite = context.Car.status.holdingLocks.First();
            bool isChargeError = IsChargePointError(context.Car, currentChargeSite);

            LadderLogic.TriggerOnce(isChargeError, 10000, () =>
            {
                HandleChargeError(context, currentChargeSite);
            }, context.Car.id);
        }

        /// <summary>
        /// 处理充电异常
        /// </summary>
        private void HandleChargeError(CarChargeContext context, int currentChargeSite)
        {
            Commons.DeleteTag(context.Car.tags, "shouldCharge");

            if (context.Car.tags.Contains("charging"))
                context.Car.tags.Remove("charging");

            if (AcmStatus.freeChargeCount == 0)
            {
                Commons.AddOrUpdateTag(context.Car.tags, "mustToStandby", "true");
                context.ShouldGoStandby = true;
                Diagnosis.Log($"小车:{context.Car.name}，充电桩{currentChargeSite}异常，无空闲充电桩，返回待命点", "ChargeError", true);
            }
            else
            {
                context.ShouldGoCharge = true;
                Commons.AddOrUpdateTag(context.Car.tags, "mustToAnotherChargeSite", $"{currentChargeSite}");
                context.ShouldGoStandby = false;
                Diagnosis.Log($"小车:{context.Car.name}，充电桩{currentChargeSite}异常，切换到其他充电桩", "ChargeError", true);
            }
        }


        /// <summary>
        /// 判断当前站点是否是其他小车pendingLocks的终点，若是则需要避让
        /// </summary>
        private bool CheckPendingEndpointConflict(int siteId, Car currentCar)
        {
            if (siteId == -1) return false;
            foreach (var other in SimpleLib.GetAllCars())
            {
                if (other.id == currentCar.id) continue;
                var pending = other.status.pendingLocks.Contains(siteId);
                var holding = other.status.holdingLocks.Contains(siteId);
                if (holding ||
                    pending)
                {
                    return true;
                }
                if (other.tags.TryGetValue("conflictTask", out string conflictTask))
                {

                    if (conflictTask.Contains(siteId.ToString()))
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        #endregion

        #region Movement Planning - 移动规划

        /// <summary>
        /// 规划路径并执行移动
        /// </summary>
        private void PlanAndExecuteMovement(CarChargeContext context, List<Site> allChargeSites)
        {
            var car = context.Car;
            var carCharging = allChargeSites.Contains(SimpleLib.GetSite(car.GetLastSite()));

            // 规划去充电站
            if (ShouldPlanToCharge(context))
            {
                context.TargetPlan = PlanToChargeSite(context);
                if (context.TargetPlan != null)
                {
                    car.tags.Add("occupied", "toCharge");
                    context.TargetType = TARGET_TYPE_CHARGE;
                }
            }

            // 规划去待命点
            if (context.TargetPlan == null && ShouldPlanToStandby(context, carCharging))
            {
                context.TargetPlan = PlanToStandbySite(context);
                if (context.TargetPlan != null)
                {
                    car.tags.Add("occupied", "toStandby");
                    context.TargetType = TARGET_TYPE_STANDBY;
                }
            }

            if (context.TargetPlan == null) return;

            ExecuteMovement(context);
        }

        private bool ShouldPlanToCharge(CarChargeContext context)
        {
            var car = context.Car;
            return (context.ShouldGoCharge ||
                  (car.tags.ContainsKey("mustToAnotherChargeSite") && car.tags.ContainsKey("shouldCharge"))) &&
                 Commons.SelectCar(car, false, true, true) != -1 &&
                  !car.tags.Contains("mustToStandby");
        }

        private bool ShouldPlanToStandby(CarChargeContext context, bool carCharging)
        {
            var car = context.Car;
            return context.ShouldGoStandby &&
           (car.tags.Contains("mustToStandby") || !carCharging || (carCharging && context.ChargingTaskAvailable)) &&
               Commons.SelectCar(car, checkHoldCar: true) != -1;
        }

        private SegmentPlan PlanToChargeSite(CarChargeContext context)
        {
            var car = context.Car;
            return Commons.GetNearestPlan(car, site =>
            {
                if (IsSiteOccupied(site)) return false;

                bool mustToAnotherChargeSite = car.tags.TryGetValue("mustToAnotherChargeSite", out var chargingSite) &&
            int.Parse(chargingSite) == site.id;

                return site.fields.ContainsKey("group") &&
         site.fields.ContainsKey("Charge") &&
          GetChargeType(car).Contains(site.fields["group"]) &&
              ((!mustToAnotherChargeSite && context.CarInChargeSite) || !context.CarInChargeSite);
            });
        }

        private SegmentPlan PlanToStandbySite(CarChargeContext context)
        {
            var car = context.Car;
            return Commons.SimpleToNearestPlan(car, site =>
            {
                if (IsSiteOccupied(site)) return false;
                if (IsUndeliverableSite(car, site.id)) return false;
                
                var isConflict= CheckPendingEndpointConflict(car.status.holdingLocks.First(),car)
                &&(site.fields.ContainsKey("standby")|| site.fields.ContainsKey("giveWay"));
                return ((site.fields.ContainsKey("standby") || isConflict)) &&
                !site.tags.Contains("unavailable") &&
               !context.CarInStandbySite;
            });
        }

        /// <summary>
        /// 执行移动
        /// </summary>
        private void ExecuteMovement(CarChargeContext context)
        {
            var car = context.Car;
            var targetPlan = context.TargetPlan;
            CarProgram program;

            try
            {
                program = targetPlan.Compile(
             $"{car.name}({car.id}) maintenance. charging?{context.ShouldGoCharge}--{targetPlan.Destination.id}");
            }
            catch (Exception ex)
            {
                HandleCarFailure(car, ex);
                if (!context.ShouldGoCharge)
                    AddUndeliverableSite(car, targetPlan.Destination.id);
                return;
            }

            Task.Run(async () =>
            {
                try
                {
                    if (context.TargetType == TARGET_TYPE_STANDBY && car.tags.Contains("shouldCharge"))
                        LeaveAction(car, targetPlan.Source);

                    car.tags.Add("dest", $"{targetPlan.Destination.id}");
                    await program.Queue();

                    CleanupAfterMovement(car, targetPlan.Destination.id);
                }
                catch (Exception e)
                {
                    Diagnosis.Post($"fail to standby/charge exception:{ExceptionFormatter.FormatEx(e)}");
                    HandleCarFailure(car);
                }
            });
        }

        private void CleanupAfterMovement(Car car, int destinationId)
        {
            Commons.DeleteTag(car.tags, "mustToAnotherChargeSite");
            Commons.DeleteTag(car.tags, "undeliverableSite");
            Commons.DeleteTag(car.tags, "mustToStandby");
            car.siteID = destinationId;
            lock (Commons.PlanSession)
                car.tags.Remove("occupied");
        }

        #endregion

        #region Helper Methods - 辅助方法

        /// <summary>
        /// 获取occupied标签值
        /// </summary>
        public string GetOccupiedTagValue(Car car)
        {
            return car.tags.TryGetValue("occupied", out var occupiedValue) ? occupiedValue : "";
        }

        /// <summary>
        /// 处理车辆失败
        /// </summary>
        private void HandleCarFailure(Car car, Exception ex = null)
        {
            Diagnosis.Post($"car{car.id} failed going standby/charge, {ExceptionFormatter.FormatEx(ex)}");
            lock (Commons.PlanSession)
            {
                Commons.DeleteTag(car.tags, "occupied");
                car.tags.Add("idle", DateTime.Now.ToString());
            }
        }

        /// <summary>
        /// 获取空闲车辆数量
        /// </summary>
        private int FreeCarNub()
        {
            return SimpleLib.GetAllCars()
                   .Count(p => !p.tags.Contains("occupied") &&
                           p.status.holdingLocks.Length == 1 &&
                     p.status.pendingLocks.Length == 0 &&
                    !p.tags.ContainsKey("charging") &&
                      p.tags.Contains("idle"));
        }

        /// <summary>
        /// 判断车辆是否在充电站
        /// </summary>
        private bool CarInChargeSite(Car car, int carSiteId)
        {
            var site = SimpleLib.GetSite(carSiteId);
            if (!site.fields.ContainsKey("group")) return false;
            if (!GetChargeType(car).Contains(site.fields["group"])) return false;
            if (!site.fields.ContainsKey("Charge")) return false;
            return true;
        }

        /// <summary>
        /// 判断站点是否被占用
        /// </summary>
        private bool IsSiteOccupied(Site site)
        {
            return SimpleLib.GetAllCars().Any(o =>
             o.status.pendingLocks.Contains(site.id) ||
            o.status.holdingLocks.Contains(site.id));
        }

        /// <summary>
        /// 判断站点是否不可达
        /// </summary>
        private bool IsUndeliverableSite(Car car, int siteId)
        {
            if (!car.tags.TryGetValue("undeliverableSite", out var sites)) return false;

            var siteList = sites.Split(',');
            if (siteList.Length > MAX_UNDELIVERABLE_SITES)
                Commons.DeleteTag(car.tags, "undeliverableSite");

            foreach (var s in siteList)
            {
                if (int.TryParse(s, out var id) && id == siteId)
                {
                    Diagnosis.Log($"{car.name}休息回滚发去待命点,sites[{sites}]失败，undeliverableSite，更换下个站点",
           "UndeliverableSite", specialFolder: true);
                    return true;
                }
            }
            return false;
        }

        /// <summary>
        /// 添加不可达站点记录
        /// </summary>
        private void AddUndeliverableSite(Car car, int siteId)
        {
            if (car.tags.TryGetValue("undeliverableSite", out var undeliverableSite))
                Commons.AddOrUpdateTag(car.tags, "undeliverableSite", $"{undeliverableSite},{siteId}");
            else
                Commons.AddOrUpdateTag(car.tags, "undeliverableSite", siteId.ToString());
        }

        #endregion
    }
}
