using System;
using System.Collections.Generic;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Fleet
{
    /// <summary>表示本机车队任务运行入口的生命周期阶段。</summary>
    public enum FleetRuntimeState
    {
        Idle = 0,
        Preparing = 1,
        Ready = 2,
        Active = 3,
        Completed = 4,
        Faulted = 5
    }

    /// <summary>在主车或从车上串联固定布局滚动车队任务。</summary>
    public sealed class FleetRuntime
    {
        private const int RuntimeFailureCode = 1;
        private const int FaultStopReasonCode = 1;

        private readonly IFleetTransport _transport;
        private readonly FleetMemberAgent _localAgent;
        private readonly IVehicleStateProvider _localStateProvider;
        private readonly FleetPreparationCoordinator _preparationCoordinator;
        private readonly FleetCoordinator _fleetCoordinator;
        private readonly FleetSafetySupervisor _safetySupervisor;
        private readonly double _commandValidForSeconds;
        private readonly double _preparationTimeoutSeconds;
        private readonly Dictionary<int, AcceptedReport> _reports =
            new Dictionary<int, AcceptedReport>();
        private readonly Dictionary<int, long> _lastReportSequences =
            new Dictionary<int, long>();

        private FleetLayout _activeLayout;
        private VehicleState? _lastLocalState;
        private double? _planStartTimeSeconds;
        private long _nextCommandSequence;
        private long _nextReportSequence;
        private long _lastReceivedCommandSequence;
        private long _lastAppliedCommandSequence;

        /// <summary>创建本车运行入口；主车必须额外提供三个主车侧组件。</summary>
        public FleetRuntime(
            int selfVehicleId,
            int leaderVehicleId,
            IFleetTransport transport,
            FleetMemberAgent localAgent,
            IVehicleStateProvider localStateProvider,
            FleetPreparationCoordinator preparationCoordinator = null,
            FleetCoordinator fleetCoordinator = null,
            FleetSafetySupervisor safetySupervisor = null,
            double commandValidForSeconds = 0.5,
            double preparationTimeoutSeconds = 10.0)
        {
            if (selfVehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(nameof(selfVehicleId));
            }

            if (leaderVehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(nameof(leaderVehicleId));
            }

            _transport = transport ??
                throw new ArgumentNullException(nameof(transport));
            _localAgent = localAgent ??
                throw new ArgumentNullException(nameof(localAgent));
            _localStateProvider = localStateProvider ??
                throw new ArgumentNullException(nameof(localStateProvider));
            NumericGuard.EnsureFinitePositive(
                commandValidForSeconds,
                nameof(commandValidForSeconds));
            NumericGuard.EnsureFinitePositive(
                preparationTimeoutSeconds,
                nameof(preparationTimeoutSeconds));

            if (_localAgent.VehicleId != selfVehicleId)
            {
                throw new ArgumentException(
                    "本车执行器车号与运行入口车号不一致。",
                    nameof(localAgent));
            }

            SelfVehicleId = selfVehicleId;
            LeaderVehicleId = leaderVehicleId;
            _preparationCoordinator = preparationCoordinator;
            _fleetCoordinator = fleetCoordinator;
            _safetySupervisor = safetySupervisor;
            _commandValidForSeconds = commandValidForSeconds;
            _preparationTimeoutSeconds = preparationTimeoutSeconds;

            if (IsLeader &&
                (_preparationCoordinator == null ||
                 _fleetCoordinator == null ||
                 _safetySupervisor == null))
            {
                throw new ArgumentException(
                    "主车必须提供准备协调器、车队协调器和安全监督器。");
            }

            State = FleetRuntimeState.Idle;
            LastFailureReason = string.Empty;
        }

        public int SelfVehicleId { get; }

        public int LeaderVehicleId { get; }

        public bool IsLeader => SelfVehicleId == LeaderVehicleId;

        public FleetRuntimeState State { get; private set; }

        public long CurrentPlanId { get; private set; }

        public long LastAppliedCommandSequence =>
            _lastAppliedCommandSequence;

        public string LastFailureReason { get; private set; }

        public FleetCoordinationCycleOutput LastCoordinationOutput
        {
            get;
            private set;
        }

        /// <summary>由主车启动一次使用当前控制器固定β的滚动轨迹任务。</summary>
        public bool StartRollingPlan(
            long planId,
            FleetLayout layout,
            Trajectory2D trajectory)
        {
            EnsureLeader();
            if (planId <= 0)
            {
                throw new ArgumentOutOfRangeException(nameof(planId));
            }

            if (layout == null)
            {
                throw new ArgumentNullException(nameof(layout));
            }

            if (trajectory == null)
            {
                throw new ArgumentNullException(nameof(trajectory));
            }

            if (!layout.TryGetVehicle(SelfVehicleId, out _))
            {
                throw new ArgumentException(
                    "主车不在当前车队布局中。",
                    nameof(layout));
            }

            if (State == FleetRuntimeState.Preparing ||
                State == FleetRuntimeState.Active)
            {
                LastFailureReason = "当前车队任务尚未结束。";
                return false;
            }

            ResetLeaderForNewPlan();
            CurrentPlanId = planId;
            _activeLayout = layout;
            State = FleetRuntimeState.Preparing;

            try
            {
                _fleetCoordinator.Start(layout, trajectory);
                _preparationCoordinator.StartRollingPreparation(
                    planId,
                    layout,
                    _fleetCoordinator.MotionDirectionInFleetRadians);
                _safetySupervisor.Start(planId);

                foreach (var target in _preparationCoordinator.Targets)
                {
                    var command = NewCommand(
                        target.VehicleId,
                        FleetCommandKind.PrepareRolling,
                        target.MotionDirectionInBodyRadians,
                        Twist2D.Zero);

                    if (target.VehicleId == SelfVehicleId)
                    {
                        if (!_localAgent.BeginRollingPreparation(
                                planId,
                                target.MotionDirectionInBodyRadians))
                        {
                            return FaultLeader(
                                LocalFailure("主车舵轮准备失败。"));
                        }

                        AcceptLocalCommand(command.SequenceNumber);
                    }
                    else
                    {
                        _transport.SendCommand(command);
                    }
                }

                return true;
            }
            catch (Exception exception)
            {
                return FaultLeader(
                    "启动车队任务失败：" + exception.Message);
            }
        }

        /// <summary>执行一个本机调度周期，并返回更新后的运行阶段。</summary>
        public FleetRuntimeState Update(
            double currentTimeSeconds,
            double deltaTimeSeconds)
        {
            NumericGuard.EnsureFiniteNonNegative(
                currentTimeSeconds,
                nameof(currentTimeSeconds));
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            return IsLeader
                ? UpdateLeader(currentTimeSeconds, deltaTimeSeconds)
                : UpdateMember(currentTimeSeconds, deltaTimeSeconds);
        }

        /// <summary>正常取消当前任务；主车同时向其余成员广播停止。</summary>
        public void Stop(string reason = "")
        {
            if (IsLeader && CurrentPlanId > 0)
            {
                BroadcastStop(reasonCode: 0);
                _preparationCoordinator.Cancel();
                _fleetCoordinator.Cancel();
                _safetySupervisor.Reset();
            }

            _localAgent.Stop();
            _activeLayout = null;
            CurrentPlanId = FleetProtocol.NoActivePlanId;
            State = FleetRuntimeState.Idle;
            _planStartTimeSeconds = null;
            LastCoordinationOutput = null;
            LastFailureReason = reason ?? string.Empty;
        }

        private FleetRuntimeState UpdateLeader(
            double currentTimeSeconds,
            double deltaTimeSeconds)
        {
            if (State != FleetRuntimeState.Preparing &&
                State != FleetRuntimeState.Active)
            {
                return State;
            }

            _planStartTimeSeconds ??= currentTimeSeconds;
            if (!ReceiveReports(currentTimeSeconds))
            {
                return State;
            }

            if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds))
            {
                FaultLeader(LocalFailure("主车本地执行失败。"));
                return State;
            }

            if (!TryReadLocalState(out var localState))
            {
                FaultLeader("主车本地状态不可用。" );
                return State;
            }

            StoreLocalReport(localState.Value, currentTimeSeconds);

            if (State == FleetRuntimeState.Preparing)
            {
                UpdatePreparationBarrier();
                if (_preparationCoordinator.State ==
                    FleetPreparationCoordinatorState.Faulted)
                {
                    FaultLeader(_preparationCoordinator.LastFailureReason);
                    return State;
                }

                if (currentTimeSeconds - _planStartTimeSeconds.Value >
                    _preparationTimeoutSeconds)
                {
                    FaultLeader("车队舵轮准备超时，未能全员Ready。" );
                    return State;
                }
            }

            if (!CheckLeaderSafety(currentTimeSeconds))
            {
                return State;
            }

            if (State == FleetRuntimeState.Preparing &&
                !ActivateFleet(currentTimeSeconds))
            {
                return State;
            }

            if (State == FleetRuntimeState.Active)
            {
                RunCoordination(currentTimeSeconds, deltaTimeSeconds);
            }

            return State;
        }

        private FleetRuntimeState UpdateMember(
            double currentTimeSeconds,
            double deltaTimeSeconds)
        {
            ReceiveCommands(currentTimeSeconds);
            if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds))
            {
                if (State != FleetRuntimeState.Faulted)
                {
                    FaultMember(LocalFailure("成员车本地执行失败。"));
                }
            }

            var hasState = TryReadLocalState(out var localState);
            if (!hasState && CurrentPlanId > 0 &&
                State != FleetRuntimeState.Faulted)
            {
                FaultMember("成员车本地状态不可用。" );
            }

            SendLocalReport(localState, hasState, currentTimeSeconds);
            return State;
        }

        private bool TickLocalAgent(
            double currentTimeSeconds,
            double deltaTimeSeconds)
        {
            if (State == FleetRuntimeState.Faulted)
            {
                return false;
            }

            if (_localAgent.State == FleetMemberAgentState.Preparing)
            {
                _localAgent.UpdatePreparation(deltaTimeSeconds);
            }

            if (_localAgent.State == FleetMemberAgentState.Active &&
                !_localAgent.UpdateCommandWatchdog(currentTimeSeconds))
            {
                return false;
            }

            if (_localAgent.State == FleetMemberAgentState.Faulted)
            {
                return false;
            }

            if (!IsLeader)
            {
                State = ToRuntimeState(_localAgent.State);
            }

            return true;
        }

        private bool TryReadLocalState(out VehicleState? state)
        {
            try
            {
                if (_localStateProvider.TryGetState(out var value))
                {
                    _lastLocalState = value;
                    state = value;
                    return true;
                }
            }
            catch (Exception exception)
            {
                LastFailureReason =
                    "读取本车状态失败：" + exception.Message;
            }

            state = null;
            return false;
        }

        private bool ReceiveReports(double currentTimeSeconds)
        {
            try
            {
                while (_transport.TryReceiveReport(out var report))
                {
                    if (report.VehicleId == SelfVehicleId ||
                        _activeLayout == null ||
                        !_activeLayout.TryGetVehicle(report.VehicleId, out _) ||
                        report.PlanId != CurrentPlanId)
                    {
                        continue;
                    }

                    ValidateReport(report);
                    if (_lastReportSequences.TryGetValue(
                            report.VehicleId,
                            out var sequence) &&
                        report.SequenceNumber <= sequence)
                    {
                        continue;
                    }

                    _lastReportSequences[report.VehicleId] =
                        report.SequenceNumber;
                    _reports[report.VehicleId] =
                        new AcceptedReport(report, currentTimeSeconds);
                }

                return true;
            }
            catch (Exception exception)
            {
                return FaultLeader(
                    "接收成员报告失败：" + exception.Message);
            }
        }

        private void StoreLocalReport(
            VehicleState state,
            double currentTimeSeconds)
        {
            _reports[SelfVehicleId] = new AcceptedReport(
                CreateReport(state, isStateAvailable: true),
                currentTimeSeconds);
        }

        private void UpdatePreparationBarrier()
        {
            foreach (var accepted in _reports.Values)
            {
                var report = accepted.Report;
                _preparationCoordinator.ReportMemberStatus(
                    new FleetMemberPreparationStatus(
                        CurrentPlanId,
                        report.VehicleId,
                        ToAgentState(report.State),
                        report.State == FleetMemberState.Faulted
                            ? $"成员车故障码{report.FailureCode}。"
                            : string.Empty));
            }
        }

        private bool CheckLeaderSafety(double currentTimeSeconds)
        {
            var statuses = new List<FleetMemberSafetyStatus>();
            foreach (var accepted in _reports.Values)
            {
                var report = accepted.Report;
                statuses.Add(new FleetMemberSafetyStatus(
                    report.VehicleId,
                    report.PlanId,
                    report.IsStateAvailable,
                    report.State == FleetMemberState.Faulted,
                    report.FailureCode,
                    accepted.ReceivedTimeSeconds));
            }

            var graceExpired =
                currentTimeSeconds - _planStartTimeSeconds.Value >
                _safetySupervisor.CommunicationTimeoutSeconds;
            if (statuses.Count != _activeLayout.VehicleCount &&
                !graceExpired)
            {
                return true;
            }

            var decision = _safetySupervisor.Evaluate(
                _activeLayout,
                statuses,
                currentTimeSeconds);
            return !decision.ShouldStop ||
                   FaultLeader(decision.Reason);
        }

        private bool ActivateFleet(double currentTimeSeconds)
        {
            if (_preparationCoordinator.State !=
                FleetPreparationCoordinatorState.ReadyToActivate)
            {
                return false;
            }

            if (!_preparationCoordinator.TryAuthorizeActivation(
                    CurrentPlanId))
            {
                return FaultLeader("无法授权全队激活。" );
            }

            var command = NewCommand(
                FleetProtocol.BroadcastVehicleId,
                FleetCommandKind.Activate,
                0.0,
                Twist2D.Zero);
            if (!_localAgent.Activate(
                    CurrentPlanId,
                    currentTimeSeconds,
                    command.ValidForSeconds))
            {
                return FaultLeader(LocalFailure("主车激活失败。"));
            }

            AcceptLocalCommand(command.SequenceNumber);
            try
            {
                _transport.SendCommand(command);
                State = FleetRuntimeState.Active;
                return true;
            }
            catch (Exception exception)
            {
                return FaultLeader(
                    "广播激活命令失败：" + exception.Message);
            }
        }

        private void RunCoordination(
            double currentTimeSeconds,
            double deltaTimeSeconds)
        {
            var result = _fleetCoordinator.ExecuteCycle(
                BuildMemberStates(),
                currentTimeSeconds,
                deltaTimeSeconds,
                out var output);
            LastCoordinationOutput = output;

            if (result == FleetCoordinationCycleResult.CommandGenerated ||
                result == FleetCoordinationCycleResult.WaitingForState)
            {
                Dispatch(output.MemberCommands, currentTimeSeconds);
                return;
            }

            if (result == FleetCoordinationCycleResult.Completed)
            {
                BroadcastStop(reasonCode: 0);
                _localAgent.Stop();
                _preparationCoordinator.Cancel();
                _safetySupervisor.Reset();
                State = FleetRuntimeState.Completed;
                return;
            }

            var reason = string.IsNullOrWhiteSpace(output.Reason)
                ? _fleetCoordinator.LastFailureReason
                : output.Reason;
            FaultLeader(string.IsNullOrWhiteSpace(reason)
                ? "车队协调器未生成可执行命令。"
                : reason);
        }

        private bool Dispatch(
            IReadOnlyList<FleetMemberCommand> commands,
            double currentTimeSeconds)
        {
            if (commands == null ||
                commands.Count != _activeLayout.VehicleCount)
            {
                return FaultLeader("成员命令数量与布局不一致。" );
            }

            try
            {
                foreach (var memberCommand in commands)
                {
                    var command = NewCommand(
                        memberCommand.VehicleId,
                        FleetCommandKind.Motion,
                        0.0,
                        memberCommand.TwistInVehicleBody);
                    if (memberCommand.VehicleId == SelfVehicleId)
                    {
                        if (!_localAgent.Execute(
                                CurrentPlanId,
                                memberCommand,
                                currentTimeSeconds,
                                command.ValidForSeconds))
                        {
                            return FaultLeader(
                                LocalFailure("主车执行速度命令失败。"));
                        }

                        AcceptLocalCommand(command.SequenceNumber);
                    }
                    else
                    {
                        _transport.SendCommand(command);
                    }
                }

                return true;
            }
            catch (Exception exception)
            {
                return FaultLeader(
                    "分发成员命令失败：" + exception.Message);
            }
        }

        private void ReceiveCommands(double currentTimeSeconds)
        {
            try
            {
                while (_transport.TryReceiveCommand(out var command))
                {
                    ApplyCommand(command, currentTimeSeconds);
                }
            }
            catch (Exception exception)
            {
                FaultMember(
                    "接收或执行主车命令失败：" + exception.Message);
            }
        }

        private void ApplyCommand(
            FleetCommand command,
            double currentTimeSeconds)
        {
            if (command.TargetVehicleId !=
                    FleetProtocol.BroadcastVehicleId &&
                command.TargetVehicleId != SelfVehicleId)
            {
                return;
            }

            ValidateCommand(command);
            var isPreparation =
                command.Kind == FleetCommandKind.PrepareRolling ||
                command.Kind == FleetCommandKind.PrepareSpin;

            if (isPreparation && command.PlanId != CurrentPlanId)
            {
                if (State == FleetRuntimeState.Preparing ||
                    State == FleetRuntimeState.Ready ||
                    State == FleetRuntimeState.Active)
                {
                    return;
                }

                CurrentPlanId = command.PlanId;
                _lastReceivedCommandSequence = 0;
                _lastAppliedCommandSequence = 0;
                LastFailureReason = string.Empty;
            }

            if (command.PlanId != CurrentPlanId ||
                command.SequenceNumber <= _lastReceivedCommandSequence)
            {
                return;
            }

            _lastReceivedCommandSequence = command.SequenceNumber;
            bool accepted;
            switch (command.Kind)
            {
                case FleetCommandKind.PrepareRolling:
                    accepted = _localAgent.BeginRollingPreparation(
                        command.PlanId,
                        command.MotionDirectionInBodyRadians);
                    break;
                case FleetCommandKind.PrepareSpin:
                    accepted =
                        _localAgent.BeginSpinPreparation(command.PlanId);
                    break;
                case FleetCommandKind.Activate:
                    accepted = _localAgent.Activate(
                        command.PlanId,
                        currentTimeSeconds,
                        command.ValidForSeconds);
                    break;
                case FleetCommandKind.Motion:
                    accepted = _localAgent.Execute(
                        command.PlanId,
                        new FleetMemberCommand(
                            SelfVehicleId,
                            command.TwistInVehicleBody),
                        currentTimeSeconds,
                        command.ValidForSeconds);
                    break;
                case FleetCommandKind.Stop:
                    _localAgent.Stop();
                    _lastAppliedCommandSequence = command.SequenceNumber;
                    CurrentPlanId = FleetProtocol.NoActivePlanId;
                    State = FleetRuntimeState.Idle;
                    LastFailureReason = string.Empty;
                    return;
                default:
                    accepted = false;
                    break;
            }

            if (!accepted)
            {
                FaultMember(LocalFailure(
                    $"成员车拒绝{command.Kind}命令。"));
                return;
            }

            _lastAppliedCommandSequence = command.SequenceNumber;
            State = ToRuntimeState(_localAgent.State);
        }

        private void SendLocalReport(
            VehicleState? state,
            bool isStateAvailable,
            double currentTimeSeconds)
        {
            try
            {
                var value = state ?? _lastLocalState ??
                    new VehicleState(
                        currentTimeSeconds,
                        Pose2D.Identity,
                        Twist2D.Zero,
                        hasValidVelocityEstimate: false);
                _transport.SendReport(
                    CreateReport(value, isStateAvailable));
            }
            catch (Exception exception)
            {
                FaultMember(
                    "发送成员报告失败：" + exception.Message);
            }
        }

        private FleetMemberReport CreateReport(
            VehicleState state,
            bool isStateAvailable)
        {
            return new FleetMemberReport(
                SelfVehicleId,
                CurrentPlanId,
                NextReportSequence(),
                state.SampleTimestampSeconds,
                state.PoseInWorld,
                state.TwistInWorld,
                isStateAvailable,
                isStateAvailable && state.HasValidVelocityEstimate,
                ToProtocolState(),
                _lastAppliedCommandSequence,
                State == FleetRuntimeState.Faulted
                    ? RuntimeFailureCode
                    : 0);
        }

        private List<FleetMemberStateSample> BuildMemberStates()
        {
            var states = new List<FleetMemberStateSample>(
                _activeLayout.VehicleCount);
            foreach (var vehicle in _activeLayout.Vehicles)
            {
                var report = _reports[vehicle.VehicleId].Report;
                states.Add(new FleetMemberStateSample(
                    report.VehicleId,
                    report.SampleTimestampSeconds,
                    report.PoseInCommonWorld,
                    report.TwistAtVehicleOriginInCommonWorld,
                    report.IsStateAvailable,
                    report.HasValidVelocityEstimate));
            }

            return states;
        }

        private FleetCommand NewCommand(
            int targetVehicleId,
            FleetCommandKind kind,
            double motionDirectionInBodyRadians,
            Twist2D twist,
            int reasonCode = 0)
        {
            return new FleetCommand(
                CurrentPlanId,
                NextCommandSequence(),
                targetVehicleId,
                kind,
                motionDirectionInBodyRadians,
                twist,
                _commandValidForSeconds,
                reasonCode);
        }

        private bool FaultLeader(string reason)
        {
            var preservedReason = string.IsNullOrWhiteSpace(reason)
                ? "车队运行入口发生未说明故障。"
                : reason;
            if (CurrentPlanId > 0)
            {
                BroadcastStop(FaultStopReasonCode);
            }

            _localAgent.Stop();
            State = FleetRuntimeState.Faulted;
            LastFailureReason = preservedReason;
            return false;
        }

        private void FaultMember(string reason)
        {
            var preservedReason = string.IsNullOrWhiteSpace(reason)
                ? "成员车运行入口发生未说明故障。"
                : reason;
            _localAgent.Stop();
            State = FleetRuntimeState.Faulted;
            LastFailureReason = preservedReason;
        }

        private void BroadcastStop(int reasonCode)
        {
            try
            {
                _transport.SendCommand(NewCommand(
                    FleetProtocol.BroadcastVehicleId,
                    FleetCommandKind.Stop,
                    0.0,
                    Twist2D.Zero,
                    reasonCode));
            }
            catch
            {
                // 本车仍立即停车，失联成员由各自命令看门狗兜底。
            }
        }

        private void ResetLeaderForNewPlan()
        {
            _localAgent.Stop();
            _preparationCoordinator.Cancel();
            _fleetCoordinator.Cancel();
            _safetySupervisor.Reset();
            _reports.Clear();
            _lastReportSequences.Clear();
            _activeLayout = null;
            _lastLocalState = null;
            _planStartTimeSeconds = null;
            _lastReceivedCommandSequence = 0;
            _lastAppliedCommandSequence = 0;
            LastCoordinationOutput = null;
            LastFailureReason = string.Empty;
        }

        private void AcceptLocalCommand(long sequenceNumber)
        {
            _lastReceivedCommandSequence = sequenceNumber;
            _lastAppliedCommandSequence = sequenceNumber;
        }

        private long NextCommandSequence()
        {
            if (++_nextCommandSequence <= 0)
            {
                throw new InvalidOperationException("主车命令序号耗尽。" );
            }

            return _nextCommandSequence;
        }

        private long NextReportSequence()
        {
            if (++_nextReportSequence <= 0)
            {
                throw new InvalidOperationException("成员报告序号耗尽。" );
            }

            return _nextReportSequence;
        }

        private string LocalFailure(string fallback)
        {
            return string.IsNullOrWhiteSpace(_localAgent.LastFailureReason)
                ? fallback
                : _localAgent.LastFailureReason;
        }

        private void EnsureLeader()
        {
            if (!IsLeader)
            {
                throw new InvalidOperationException(
                    "只有主车可以启动完整车队任务。");
            }
        }

        private FleetMemberState ToProtocolState()
        {
            if (State == FleetRuntimeState.Faulted)
            {
                return FleetMemberState.Faulted;
            }

            return _localAgent.State switch
            {
                FleetMemberAgentState.Idle => FleetMemberState.Idle,
                FleetMemberAgentState.Preparing => FleetMemberState.Preparing,
                FleetMemberAgentState.Ready => FleetMemberState.Ready,
                FleetMemberAgentState.Active => FleetMemberState.Active,
                FleetMemberAgentState.Faulted => FleetMemberState.Faulted,
                _ => throw new InvalidOperationException("本车状态无效。")
            };
        }

        private static FleetRuntimeState ToRuntimeState(
            FleetMemberAgentState state)
        {
            return state switch
            {
                FleetMemberAgentState.Idle => FleetRuntimeState.Idle,
                FleetMemberAgentState.Preparing => FleetRuntimeState.Preparing,
                FleetMemberAgentState.Ready => FleetRuntimeState.Ready,
                FleetMemberAgentState.Active => FleetRuntimeState.Active,
                FleetMemberAgentState.Faulted => FleetRuntimeState.Faulted,
                _ => throw new ArgumentOutOfRangeException(nameof(state))
            };
        }

        private static FleetMemberAgentState ToAgentState(
            FleetMemberState state)
        {
            return state switch
            {
                FleetMemberState.Idle => FleetMemberAgentState.Idle,
                FleetMemberState.Preparing => FleetMemberAgentState.Preparing,
                FleetMemberState.Ready => FleetMemberAgentState.Ready,
                FleetMemberState.Active => FleetMemberAgentState.Active,
                FleetMemberState.Faulted => FleetMemberAgentState.Faulted,
                _ => throw new ArgumentOutOfRangeException(nameof(state))
            };
        }

        private static void ValidateCommand(FleetCommand command)
        {
            if (command.PlanId <= 0 || command.SequenceNumber <= 0)
            {
                throw new ArgumentException("车队命令任务号或序号无效。" );
            }

            if (!Enum.IsDefined(typeof(FleetCommandKind), command.Kind))
            {
                throw new ArgumentException("车队命令类型无效。" );
            }

            NumericGuard.EnsureFinite(
                command.MotionDirectionInBodyRadians,
                nameof(command));
            NumericGuard.EnsureFinite(
                command.TwistInVehicleBody,
                nameof(command));
            NumericGuard.EnsureFinitePositive(
                command.ValidForSeconds,
                nameof(command));
        }

        private static void ValidateReport(FleetMemberReport report)
        {
            if (report.VehicleId <= 0 || report.PlanId <= 0 ||
                report.SequenceNumber <= 0 ||
                report.LastAppliedCommandSequence < 0 ||
                !Enum.IsDefined(typeof(FleetMemberState), report.State))
            {
                throw new ArgumentException("成员状态报告字段无效。" );
            }

            NumericGuard.EnsureFiniteNonNegative(
                report.SampleTimestampSeconds,
                nameof(report));
            NumericGuard.EnsureFinite(
                report.PoseInCommonWorld,
                nameof(report));
            NumericGuard.EnsureFinite(
                report.TwistAtVehicleOriginInCommonWorld,
                nameof(report));
        }

        private readonly struct AcceptedReport
        {
            public AcceptedReport(
                FleetMemberReport report,
                double receivedTimeSeconds)
            {
                Report = report;
                ReceivedTimeSeconds = receivedTimeSeconds;
            }

            public FleetMemberReport Report { get; }

            public double ReceivedTimeSeconds { get; }
        }
    }
}