using System;
using System.Collections.Generic;
using MultiWheelC.Trajectory;
using MyParking.Shared;
// 负责运动中：每周期计算每辆车的速度命令
namespace MultiWheelC.Fleet
{
    // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。
    public enum FleetCoordinationCycleResult
    {
        Inactive = 0,
        WaitingForState = 1,
        CommandGenerated = 2,
        Completed = 3,
        Faulted = 4
    }

    // 保存本周期的状态估计、车队命令和成员基础命令。
    public sealed class FleetCoordinationCycleOutput
    {
        internal FleetCoordinationCycleOutput(
            FleetState? state,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            FleetMotionCommand fleetCommand,
            IReadOnlyList<FleetMemberCommand> baseMemberCommands,
            IReadOnlyList<FleetMemberCommand> memberCommands,
            double speedScale,
            string reason)
        {
            NumericGuard.EnsureFiniteNonNegative(
                speedScale,
                nameof(speedScale));
            if (speedScale > 1.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(speedScale),
                    "车队统一速度比例不能大于1。");
            }

            State = state;
            MemberErrors = memberErrors ??
                throw new ArgumentNullException(
                    nameof(memberErrors));
            FleetCommand = fleetCommand;
            BaseMemberCommands = baseMemberCommands ??
                throw new ArgumentNullException(
                    nameof(baseMemberCommands));
            MemberCommands = memberCommands ??
                throw new ArgumentNullException(
                    nameof(memberCommands));
            SpeedScale = speedScale;
            Reason = reason ?? string.Empty;
        }

        public FleetState? State { get; }

        public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }

        public FleetMotionCommand FleetCommand { get; }

        // 仅由车队刚体命令分解得到，尚未叠加成员相对布局纠偏。
        public IReadOnlyList<FleetMemberCommand> BaseMemberCommands { get; }

        // 已转换到成员当前车体系并叠加小范围相对布局纠偏的最终命令。
        public IReadOnlyList<FleetMemberCommand> MemberCommands { get; }

        // 因成员布局误差施加到Vx、Vy和Omega的统一比例，范围为[0,1]。
        public double SpeedScale { get; }

        public string Reason { get; }
    }

    // 在主车上串联车队状态估计、中心轨迹控制和成员命令分解。
    public sealed class FleetCoordinator
    {
        private static readonly IReadOnlyList<FleetMemberLayoutError>
            EmptyMemberErrors = Array.AsReadOnly(
                Array.Empty<FleetMemberLayoutError>());
        private static readonly IReadOnlyList<FleetMemberCommand>
            EmptyMemberCommands = Array.AsReadOnly(
                Array.Empty<FleetMemberCommand>());

        private readonly FleetStateEstimator _stateEstimator;
        private readonly FleetController _fleetController;
        private readonly FleetMemberCommandCorrector
            _memberCommandCorrector;
        private readonly double _memberPositionErrorWarningMeters;
        private readonly double _maximumMemberPositionErrorMeters;
        private readonly double _memberYawErrorWarningRadians;
        private readonly double _maximumMemberYawErrorRadians;

        private FleetLayout _activeLayout;
        private bool _isCompleted;
        private bool _isFaulted;

        public FleetCoordinator(
            FleetStateEstimator stateEstimator,
            FleetController fleetController,
            FleetMemberCommandCorrector memberCommandCorrector,
            double memberPositionErrorWarningMeters,
            double maximumMemberPositionErrorMeters,
            double memberYawErrorWarningRadians,
            double maximumMemberYawErrorRadians)
        {
            _stateEstimator = stateEstimator ??
                throw new ArgumentNullException(
                    nameof(stateEstimator));
            _fleetController = fleetController ??
                throw new ArgumentNullException(
                    nameof(fleetController));
            _memberCommandCorrector =
                memberCommandCorrector ??
                throw new ArgumentNullException(
                    nameof(memberCommandCorrector));
            NumericGuard.EnsureFiniteNonNegative(
                memberPositionErrorWarningMeters,
                nameof(memberPositionErrorWarningMeters));
            NumericGuard.EnsureFinitePositive(
                maximumMemberPositionErrorMeters,
                nameof(maximumMemberPositionErrorMeters));
            NumericGuard.EnsureFiniteNonNegative(
                memberYawErrorWarningRadians,
                nameof(memberYawErrorWarningRadians));
            NumericGuard.EnsureFinitePositive(
                maximumMemberYawErrorRadians,
                nameof(maximumMemberYawErrorRadians));

            if (memberPositionErrorWarningMeters >=
                maximumMemberPositionErrorMeters)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(memberPositionErrorWarningMeters),
                    "成员位置误差警告阈值必须小于停止阈值。");
            }

            if (memberYawErrorWarningRadians >=
                maximumMemberYawErrorRadians)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(memberYawErrorWarningRadians),
                    "成员航向误差警告阈值必须小于停止阈值。");
            }

            if (maximumMemberYawErrorRadians > Math.PI)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(maximumMemberYawErrorRadians),
                    "成员航向误差上限不能大于π。");
            }

            _memberPositionErrorWarningMeters =
                memberPositionErrorWarningMeters;
            _maximumMemberPositionErrorMeters =
                maximumMemberPositionErrorMeters;
            _memberYawErrorWarningRadians =
                memberYawErrorWarningRadians;
            _maximumMemberYawErrorRadians =
                maximumMemberYawErrorRadians;
            LastFailureReason = string.Empty;
        }

        public FleetLayout ActiveLayout => _activeLayout;

        public bool IsActive =>
            _activeLayout != null &&
            !_isCompleted &&
            !_isFaulted &&
            _fleetController.IsActive;

        public bool IsCompleted => _isCompleted;

        public bool IsFaulted => _isFaulted;

        public string LastFailureReason { get; private set; }

        // 运动前成员β准备必须与车队控制器采用同一个车队运动方向。
        public double MotionDirectionInFleetRadians =>
            _fleetController.MotionDirectionInFleetRadians;

        public void Start(
            FleetLayout layout,
            Trajectory2D trajectory)
        {
            if (layout == null)
            {
                throw new ArgumentNullException(nameof(layout));
            }

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

            _fleetController.Start(trajectory);
            _activeLayout = layout;
            _isCompleted = false;
            _isFaulted = false;
            LastFailureReason = string.Empty;
        }

        public FleetCoordinationCycleResult ExecuteCycle(
            IReadOnlyList<FleetMemberStateSample> memberStates,
            double targetTimestampSeconds,
            double deltaTimeSeconds,
            out FleetCoordinationCycleOutput output)
        {
            if (memberStates == null)
            {
                throw new ArgumentNullException(nameof(memberStates));
            }

            NumericGuard.EnsureFiniteNonNegative(
                targetTimestampSeconds,
                nameof(targetTimestampSeconds));
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (_activeLayout == null)
            {
                output = CreateOutput(
                    null,
                    EmptyMemberErrors,
                    EmptyMemberCommands,
                    "车队布局尚未激活。");
                return FleetCoordinationCycleResult.Inactive;
            }

            if (_isFaulted)
            {
                output = CreateStopOutput(
                    null,
                    EmptyMemberErrors,
                    LastFailureReason);
                return FleetCoordinationCycleResult.Faulted;
            }

            if (_isCompleted)
            {
                output = CreateStopOutput(
                    null,
                    EmptyMemberErrors,
                    string.Empty);
                return FleetCoordinationCycleResult.Completed;
            }

            if (!_fleetController.IsActive)
            {
                output = CreateStopOutput(
                    null,
                    EmptyMemberErrors,
                    "车队中心控制器尚未启动或已经取消。");
                return FleetCoordinationCycleResult.Inactive;
            }

            var estimate = _stateEstimator.Estimate(
                _activeLayout,
                memberStates,
                targetTimestampSeconds);
            if (!estimate.IsAvailable ||
                !estimate.State.HasValue)
            {
                output = CreateStopOutput(
                    null,
                    estimate.MemberErrors,
                    estimate.UnavailableReason);
                return FleetCoordinationCycleResult.WaitingForState;
            }

            var state = estimate.State.Value;
            var speedScale = CalculateLayoutSpeedScale(
                estimate.MemberErrors,
                out var layoutFailureReason,
                out var limitingReason);
            if (layoutFailureReason != null)
            {
                return Fail(
                    state,
                    estimate.MemberErrors,
                    layoutFailureReason,
                    out output);
            }

            var controlResult =
                _fleetController.ComputeCommand(
                    state,
                    deltaTimeSeconds,
                    out var fleetCommand);

            if (controlResult ==
                FleetControlCycleResult.CommandGenerated)
            {
                var scaledFleetCommand = ScaleFleetCommand(
                    fleetCommand,
                    speedScale);
                var baseMemberCommands =
                    FleetKinematics.Decompose(
                        _activeLayout,
                        scaledFleetCommand);
                var memberCommands =
                    _memberCommandCorrector.Correct(
                        _activeLayout,
                        baseMemberCommands,
                        estimate.MemberErrors,
                        applyRelativeCorrection:
                            speedScale >= 1.0);
                output = new FleetCoordinationCycleOutput(
                    state,
                    estimate.MemberErrors,
                    scaledFleetCommand,
                    baseMemberCommands,
                    memberCommands,
                    speedScale,
                    limitingReason);
                return FleetCoordinationCycleResult.CommandGenerated;
            }

            if (controlResult ==
                FleetControlCycleResult.Completed)
            {
                _isCompleted = true;
                output = CreateStopOutput(
                    state,
                    estimate.MemberErrors,
                    string.Empty);
                return FleetCoordinationCycleResult.Completed;
            }

            if (controlResult ==
                FleetControlCycleResult.Faulted)
            {
                var reason = string.IsNullOrWhiteSpace(
                        _fleetController.LastFailureReason)
                    ? "车队中心轨迹控制失败。"
                    : _fleetController.LastFailureReason;
                return Fail(
                    state,
                    estimate.MemberErrors,
                    reason,
                    out output,
                    cancelController: false);
            }

            output = CreateStopOutput(
                state,
                estimate.MemberErrors,
                "车队中心控制器当前未生成命令。");
            return FleetCoordinationCycleResult.Inactive;
        }

        public void Cancel()
        {
            _fleetController.Cancel();
            _isCompleted = false;
            _isFaulted = false;
            LastFailureReason = string.Empty;
        }

        private double CalculateLayoutSpeedScale(
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            out string failureReason,
            out string limitingReason)
        {
            var speedScale = 1.0;
            failureReason = null;
            limitingReason = string.Empty;

            for (var index = 0;
                 index < memberErrors.Count;
                 index++)
            {
                var memberError = memberErrors[index];
                var poseError =
                    memberError.ActualPoseInExpectedVehicleFrame;
                var positionErrorMeters = Math.Sqrt(
                    poseError.XMeters * poseError.XMeters +
                    poseError.YMeters * poseError.YMeters);
                var yawErrorRadians =
                    Math.Abs(poseError.YawRadians);

                if (positionErrorMeters >=
                    _maximumMemberPositionErrorMeters)
                {
                    failureReason =
                        $"车辆{memberError.VehicleId}相对布局位置误差" +
                        $"{positionErrorMeters:F3}m达到停止阈值。";
                    return 0.0;
                }

                if (yawErrorRadians >=
                    _maximumMemberYawErrorRadians)
                {
                    failureReason =
                        $"车辆{memberError.VehicleId}相对布局航向误差" +
                        $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°" +
                        "达到停止阈值。";
                    return 0.0;
                }

                var positionScale = CalculateScale(
                    positionErrorMeters,
                    _memberPositionErrorWarningMeters,
                    _maximumMemberPositionErrorMeters);
                if (positionScale < speedScale)
                {
                    speedScale = positionScale;
                    limitingReason =
                        $"车辆{memberError.VehicleId}相对布局位置误差" +
                        $"{positionErrorMeters:F3}m，车队统一速度比例" +
                        $"降至{speedScale:F3}。";
                }

                var yawScale = CalculateScale(
                    yawErrorRadians,
                    _memberYawErrorWarningRadians,
                    _maximumMemberYawErrorRadians);
                if (yawScale < speedScale)
                {
                    speedScale = yawScale;
                    limitingReason =
                        $"车辆{memberError.VehicleId}相对布局航向误差" +
                        $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°，" +
                        $"车队统一速度比例降至{speedScale:F3}。";
                }
            }

            return speedScale;
        }

        private static double CalculateScale(
            double errorMagnitude,
            double warningThreshold,
            double stopThreshold)
        {
            if (errorMagnitude <= warningThreshold)
            {
                return 1.0;
            }

            return (stopThreshold - errorMagnitude) /
                   (stopThreshold - warningThreshold);
        }

        private static FleetMotionCommand ScaleFleetCommand(
            FleetMotionCommand command,
            double speedScale)
        {
            var twist = command.TwistAtReferencePoint;
            return new FleetMotionCommand(
                command.ReferencePointInFleet,
                new Twist2D(
                    twist.VxMetersPerSecond * speedScale,
                    twist.VyMetersPerSecond * speedScale,
                    twist.OmegaRadiansPerSecond * speedScale));
        }

        private FleetCoordinationCycleResult Fail(
            FleetState state,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            string reason,
            out FleetCoordinationCycleOutput output,
            bool cancelController = true)
        {
            if (cancelController)
            {
                _fleetController.Cancel();
            }

            _isFaulted = true;
            _isCompleted = false;
            LastFailureReason = reason ?? string.Empty;
            output = CreateStopOutput(
                state,
                memberErrors,
                LastFailureReason);
            return FleetCoordinationCycleResult.Faulted;
        }

        private FleetCoordinationCycleOutput CreateStopOutput(
            FleetState? state,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            string reason)
        {
            var stopCommands = _activeLayout == null
                ? EmptyMemberCommands
                : FleetKinematics.Decompose(
                    _activeLayout,
                    FleetMotionCommand.Stop());

            return CreateOutput(
                state,
                memberErrors,
                stopCommands,
                reason);
        }

        private static FleetCoordinationCycleOutput CreateOutput(
            FleetState? state,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            IReadOnlyList<FleetMemberCommand> memberCommands,
            string reason)
        {
            return new FleetCoordinationCycleOutput(
                state,
                memberErrors,
                FleetMotionCommand.Stop(),
                memberCommands,
                memberCommands,
                0.0,
                reason);
        }
    }
}