using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MyParking.Shared;
// 负责运动前：所有车辆舵轮是否准备完成

namespace MultiWheelC.Fleet
{
    /// <summary>表示主车侧车队运动准备的当前阶段。</summary>
    public enum FleetPreparationCoordinatorState
    {
        Idle = 0,
        WaitingForMembers = 1,
        ReadyToActivate = 2,
        ActivationAuthorized = 3,
        Faulted = 4
    }

    /// <summary>保存一次滚动准备中分配给指定成员车的本地β目标。</summary>
    public readonly struct FleetMemberPreparationTarget
    {
        /// <summary>创建一条属于指定任务和成员车的准备目标。</summary>
        public FleetMemberPreparationTarget(
            long planId,
            int vehicleId,
            double motionDirectionInBodyRadians)
        {
            if (planId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(planId),
                    "车队动作任务编号必须大于零。");
            }

            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "成员车号必须大于零。");
            }

            NumericGuard.EnsureFinite(
                motionDirectionInBodyRadians,
                nameof(motionDirectionInBodyRadians));

            PlanId = planId;
            VehicleId = vehicleId;
            MotionDirectionInBodyRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionInBodyRadians);
        }

        public long PlanId { get; }

        public int VehicleId { get; }

        public double MotionDirectionInBodyRadians { get; }
    }

    /// <summary>保存成员车对某次准备任务上报的本地状态。</summary>
    public readonly struct FleetMemberPreparationStatus
    {
        /// <summary>创建一条成员车准备状态报告。</summary>
        public FleetMemberPreparationStatus(
            long planId,
            int vehicleId,
            FleetMemberAgentState state,
            string failureReason = "")
        {
            if (planId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(planId),
                    "车队动作任务编号必须大于零。");
            }

            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "成员车号必须大于零。");
            }

            if (!Enum.IsDefined(
                    typeof(FleetMemberAgentState),
                    state))
            {
                throw new ArgumentOutOfRangeException(
                    nameof(state),
                    "成员车准备状态无效。");
            }

            PlanId = planId;
            VehicleId = vehicleId;
            State = state;
            FailureReason = failureReason ?? string.Empty;
        }

        public long PlanId { get; }

        public int VehicleId { get; }

        public FleetMemberAgentState State { get; }

        public string FailureReason { get; }
    }

    /// <summary>在主车侧分配成员β并管理全队Ready统一激活屏障。</summary>
    public sealed class FleetPreparationCoordinator
    {
        private static readonly IReadOnlyList<
                FleetMemberPreparationTarget>
            EmptyTargets = Array.AsReadOnly(
                Array.Empty<FleetMemberPreparationTarget>());

        private readonly Dictionary<int, FleetMemberAgentState>
            _memberStates =
                new Dictionary<int, FleetMemberAgentState>();

        private IReadOnlyList<FleetMemberPreparationTarget>
            _targets = EmptyTargets;

        /// <summary>创建尚未激活准备任务的主车侧协调器。</summary>
        public FleetPreparationCoordinator()
        {
            State = FleetPreparationCoordinatorState.Idle;
            LastFailureReason = string.Empty;
        }

        public FleetPreparationCoordinatorState State
        {
            get;
            private set;
        }

        public long CurrentPlanId { get; private set; }

        public double MotionDirectionInFleetRadians
        {
            get;
            private set;
        }

        public IReadOnlyList<FleetMemberPreparationTarget>
            Targets => _targets;

        public string LastFailureReason { get; private set; }

        /// <summary>根据车队固定布局为全部成员建立本次滚动β准备目标。</summary>
        public void StartRollingPreparation(
            long planId,
            FleetLayout layout,
            double motionDirectionInFleetRadians)
        {
            if (planId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(planId),
                    "车队动作任务编号必须大于零。");
            }

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

            NumericGuard.EnsureFinite(
                motionDirectionInFleetRadians,
                nameof(motionDirectionInFleetRadians));

            var normalizedFleetDirection =
                AngleMath.NormalizeRadians(
                    motionDirectionInFleetRadians);
            var targets =
                new FleetMemberPreparationTarget[
                    layout.VehicleCount];

            _memberStates.Clear();
            for (var index = 0;
                 index < layout.Vehicles.Count;
                 index++)
            {
                var vehicle = layout.Vehicles[index];
                var rawDirectionInBody =
                    AngleMath.NormalizeRadians(
                        normalizedFleetDirection -
                        vehicle.PoseInFleet.YawRadians);
                var equivalentDirectionInBody =
                    SelectSteeringAxisEquivalent(
                        rawDirectionInBody);

                targets[index] =
                    new FleetMemberPreparationTarget(
                        planId,
                        vehicle.VehicleId,
                        equivalentDirectionInBody);
                _memberStates.Add(
                    vehicle.VehicleId,
                    FleetMemberAgentState.Idle);
            }

            CurrentPlanId = planId;
            MotionDirectionInFleetRadians =
                normalizedFleetDirection;
            _targets = Array.AsReadOnly(targets);
            State =
                FleetPreparationCoordinatorState
                    .WaitingForMembers;
            LastFailureReason = string.Empty;
        }

        /// <summary>接收一辆成员车的状态并重新计算全队Ready状态。</summary>
        public FleetPreparationCoordinatorState
            ReportMemberStatus(
                FleetMemberPreparationStatus status)
        {
            if (State ==
                    FleetPreparationCoordinatorState.Idle ||
                State ==
                    FleetPreparationCoordinatorState.Faulted)
            {
                return State;
            }

            if (status.PlanId != CurrentPlanId)
            {
                return State;
            }

            if (!_memberStates.ContainsKey(status.VehicleId))
            {
                throw new ArgumentException(
                    $"车辆{status.VehicleId}不属于当前车队布局。",
                    nameof(status));
            }

            if (status.State ==
                FleetMemberAgentState.Faulted)
            {
                return Fail(
                    string.IsNullOrWhiteSpace(
                        status.FailureReason)
                        ? $"车辆{status.VehicleId}准备失败。"
                        : $"车辆{status.VehicleId}准备失败：" +
                          status.FailureReason);
            }

            if (State ==
                FleetPreparationCoordinatorState
                    .ActivationAuthorized)
            {
                return State;
            }

            if (status.State ==
                FleetMemberAgentState.Active)
            {
                return Fail(
                    $"车辆{status.VehicleId}在全队统一激活前已经进入Active。");
            }

            _memberStates[status.VehicleId] = status.State;
            State = AreAllMembersReady()
                ? FleetPreparationCoordinatorState
                    .ReadyToActivate
                : FleetPreparationCoordinatorState
                    .WaitingForMembers;
            LastFailureReason = string.Empty;
            return State;
        }

        /// <summary>在全部成员Ready后授权外层向全队广播同一任务的激活命令。</summary>
        public bool TryAuthorizeActivation(long planId)
        {
            if (planId != CurrentPlanId)
            {
                return false;
            }

            if (State ==
                FleetPreparationCoordinatorState
                    .ActivationAuthorized)
            {
                return true;
            }

            if (State !=
                FleetPreparationCoordinatorState
                    .ReadyToActivate)
            {
                return false;
            }

            State = FleetPreparationCoordinatorState
                .ActivationAuthorized;
            LastFailureReason = string.Empty;
            return true;
        }

        /// <summary>查找指定成员车在当前任务中的本地β准备目标。</summary>
        public bool TryGetTarget(
            int vehicleId,
            out FleetMemberPreparationTarget target)
        {
            for (var index = 0;
                 index < _targets.Count;
                 index++)
            {
                if (_targets[index].VehicleId == vehicleId)
                {
                    target = _targets[index];
                    return true;
                }
            }

            target = default;
            return false;
        }

        /// <summary>取消当前准备任务并清除成员状态和β目标。</summary>
        public void Cancel()
        {
            _memberStates.Clear();
            _targets = EmptyTargets;
            CurrentPlanId = 0;
            MotionDirectionInFleetRadians = 0.0;
            State = FleetPreparationCoordinatorState.Idle;
            LastFailureReason = string.Empty;
        }

        /// <summary>判断当前任务中的每辆成员车是否都已报告Ready。</summary>
        private bool AreAllMembersReady()
        {
            foreach (var state in _memberStates.Values)
            {
                if (state != FleetMemberAgentState.Ready)
                {
                    return false;
                }
            }

            return _memberStates.Count > 0;
        }

        /// <summary>将有向β转换为±90°内的等效滚动轴，反向运动由轮速符号表达。</summary>
        private static double SelectSteeringAxisEquivalent(
            double directionRadians)
        {
            var equivalent = AngleMath.NormalizeRadians(
                directionRadians);

            if (equivalent > Math.PI / 2.0)
            {
                equivalent -= Math.PI;
            }
            else if (equivalent < -Math.PI / 2.0)
            {
                equivalent += Math.PI;
            }

            return AngleMath.NormalizeRadians(equivalent);
        }

        /// <summary>锁存准备故障，等待外层停止所有成员并取消任务。</summary>
        private FleetPreparationCoordinatorState Fail(
            string reason)
        {
            State = FleetPreparationCoordinatorState.Faulted;
            LastFailureReason = reason ?? string.Empty;
            return State;
        }
    }
}