using System;
using System.Collections.Generic;
using MyParking.Shared;

namespace MultiWheelC.Fleet
{
    // 将小范围成员布局误差转换为不改变车队整体刚体运动的相对速度修正。
    public sealed class FleetMemberCommandCorrector
    {
        private const double GeometryTolerance = 1e-12;

        private readonly double _longitudinalPositionGainPerSecond;
        private readonly double _lateralPositionGainPerSecond;
        private readonly double _yawGainPerSecond;
        private readonly double _positionErrorDeadbandMeters;
        private readonly double _yawErrorDeadbandRadians;
        private readonly double _maximumLinearCorrectionMetersPerSecond;
        private readonly double _maximumAngularCorrectionRadiansPerSecond;

        public FleetMemberCommandCorrector(
            double longitudinalPositionGainPerSecond,
            double lateralPositionGainPerSecond,
            double yawGainPerSecond,
            double positionErrorDeadbandMeters,
            double yawErrorDeadbandRadians,
            double maximumLinearCorrectionMetersPerSecond,
            double maximumAngularCorrectionRadiansPerSecond)
        {
            NumericGuard.EnsureFiniteNonNegative(
                longitudinalPositionGainPerSecond,
                nameof(longitudinalPositionGainPerSecond));
            NumericGuard.EnsureFiniteNonNegative(
                lateralPositionGainPerSecond,
                nameof(lateralPositionGainPerSecond));
            NumericGuard.EnsureFiniteNonNegative(
                yawGainPerSecond,
                nameof(yawGainPerSecond));
            NumericGuard.EnsureFiniteNonNegative(
                positionErrorDeadbandMeters,
                nameof(positionErrorDeadbandMeters));
            NumericGuard.EnsureFiniteNonNegative(
                yawErrorDeadbandRadians,
                nameof(yawErrorDeadbandRadians));
            NumericGuard.EnsureFinitePositive(
                maximumLinearCorrectionMetersPerSecond,
                nameof(maximumLinearCorrectionMetersPerSecond));
            NumericGuard.EnsureFinitePositive(
                maximumAngularCorrectionRadiansPerSecond,
                nameof(maximumAngularCorrectionRadiansPerSecond));

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

            _longitudinalPositionGainPerSecond =
                longitudinalPositionGainPerSecond;
            _lateralPositionGainPerSecond =
                lateralPositionGainPerSecond;
            _yawGainPerSecond = yawGainPerSecond;
            _positionErrorDeadbandMeters =
                positionErrorDeadbandMeters;
            _yawErrorDeadbandRadians =
                yawErrorDeadbandRadians;
            _maximumLinearCorrectionMetersPerSecond =
                maximumLinearCorrectionMetersPerSecond;
            _maximumAngularCorrectionRadiansPerSecond =
                maximumAngularCorrectionRadiansPerSecond;
        }

        public IReadOnlyList<FleetMemberCommand> Correct(
            FleetLayout layout,
            IReadOnlyList<FleetMemberCommand> baseCommands,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            bool applyRelativeCorrection = true)
        {
            if (layout == null)
            {
                throw new ArgumentNullException(nameof(layout));
            }

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

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

            if (baseCommands.Count != layout.VehicleCount)
            {
                throw new ArgumentException(
                    "成员基础命令数量必须与车队布局一致。",
                    nameof(baseCommands));
            }

            if (memberErrors.Count != layout.VehicleCount)
            {
                throw new ArgumentException(
                    "成员布局误差数量必须与车队布局一致。",
                    nameof(memberErrors));
            }

            var commandsByVehicleId =
                IndexCommands(baseCommands);
            var errorsByVehicleId =
                IndexErrors(memberErrors);
            var orderedCommands =
                new FleetMemberCommand[layout.VehicleCount];
            var orderedErrors =
                new FleetMemberLayoutError[layout.VehicleCount];
            var rawCorrectionsInFleet =
                new Twist2D[layout.VehicleCount];

            for (var index = 0;
                 index < layout.Vehicles.Count;
                 index++)
            {
                var vehicleLayout = layout.Vehicles[index];
                if (!commandsByVehicleId.TryGetValue(
                        vehicleLayout.VehicleId,
                        out var baseCommand))
                {
                    throw new ArgumentException(
                        $"缺少车辆{vehicleLayout.VehicleId}的基础命令。",
                        nameof(baseCommands));
                }

                if (!errorsByVehicleId.TryGetValue(
                        vehicleLayout.VehicleId,
                        out var memberError))
                {
                    throw new ArgumentException(
                        $"缺少车辆{vehicleLayout.VehicleId}的布局误差。",
                        nameof(memberErrors));
                }

                orderedCommands[index] = baseCommand;
                orderedErrors[index] = memberError;
                rawCorrectionsInFleet[index] =
                    applyRelativeCorrection
                        ? CalculateRawCorrectionInFleet(
                            vehicleLayout,
                            memberError)
                        : Twist2D.Zero;
            }

            var relativeCorrectionsInFleet =
                RemoveCommonRigidMotion(
                    layout,
                    rawCorrectionsInFleet);
            var correctedCommands =
                new FleetMemberCommand[layout.VehicleCount];

            for (var index = 0;
                 index < layout.Vehicles.Count;
                 index++)
            {
                var vehicleLayout = layout.Vehicles[index];
                var baseTwistInFleet =
                    FrameTransform2D.TransformTwistAtSamePoint(
                        vehicleLayout.PoseInFleet,
                        orderedCommands[index].TwistInVehicleBody);
                var correctionInFleet = LimitCorrection(
                    relativeCorrectionsInFleet[index]);
                var correctedTwistInFleet = Add(
                    baseTwistInFleet,
                    correctionInFleet);

                // 使用成员当前相对姿态表达最终命令，避免小航向误差造成坐标表达偏差。
                var actualPoseInFleet =
                    FrameTransform2D.Compose(
                        vehicleLayout.PoseInFleet,
                        orderedErrors[index]
                            .ActualPoseInExpectedVehicleFrame);
                var fleetPoseInActualVehicle =
                    FrameTransform2D.Inverse(
                        actualPoseInFleet);
                var correctedTwistInVehicleBody =
                    FrameTransform2D.TransformTwistAtSamePoint(
                        fleetPoseInActualVehicle,
                        correctedTwistInFleet);

                correctedCommands[index] =
                    new FleetMemberCommand(
                        vehicleLayout.VehicleId,
                        correctedTwistInVehicleBody);
            }

            return Array.AsReadOnly(correctedCommands);
        }

        private Twist2D CalculateRawCorrectionInFleet(
            VehicleLayout vehicleLayout,
            FleetMemberLayoutError memberError)
        {
            var error =
                memberError.ActualPoseInExpectedVehicleFrame;
            var correctionInExpectedVehicle = new Twist2D(
                -_longitudinalPositionGainPerSecond *
                ApplyDeadband(
                    error.XMeters,
                    _positionErrorDeadbandMeters),
                -_lateralPositionGainPerSecond *
                ApplyDeadband(
                    error.YMeters,
                    _positionErrorDeadbandMeters),
                -_yawGainPerSecond *
                ApplyDeadband(
                    error.YawRadians,
                    _yawErrorDeadbandRadians));

            return FrameTransform2D.TransformTwistAtSamePoint(
                vehicleLayout.PoseInFleet,
                correctionInExpectedVehicle);
        }

        private static Twist2D[] RemoveCommonRigidMotion(
            FleetLayout layout,
            IReadOnlyList<Twist2D> rawCorrectionsInFleet)
        {
            var count = layout.VehicleCount;
            var meanX = 0.0;
            var meanY = 0.0;
            var meanVx = 0.0;
            var meanVy = 0.0;
            var meanOmega = 0.0;

            for (var index = 0; index < count; index++)
            {
                var position = layout.Vehicles[index].PoseInFleet;
                var correction = rawCorrectionsInFleet[index];
                meanX += position.XMeters;
                meanY += position.YMeters;
                meanVx += correction.VxMetersPerSecond;
                meanVy += correction.VyMetersPerSecond;
                meanOmega += correction.OmegaRadiansPerSecond;
            }

            meanX /= count;
            meanY /= count;
            meanVx /= count;
            meanVy /= count;
            meanOmega /= count;

            var rotationalNumerator = 0.0;
            var rotationalDenominator = 0.0;
            for (var index = 0; index < count; index++)
            {
                var position = layout.Vehicles[index].PoseInFleet;
                var correction = rawCorrectionsInFleet[index];
                var centeredX = position.XMeters - meanX;
                var centeredY = position.YMeters - meanY;
                var centeredVx =
                    correction.VxMetersPerSecond - meanVx;
                var centeredVy =
                    correction.VyMetersPerSecond - meanVy;

                rotationalNumerator +=
                    -centeredY * centeredVx +
                    centeredX * centeredVy;
                rotationalDenominator +=
                    centeredX * centeredX +
                    centeredY * centeredY;
            }

            var commonOmegaFromTranslation =
                rotationalDenominator <= GeometryTolerance
                    ? 0.0
                    : rotationalNumerator /
                      rotationalDenominator;
            var commonVxAtFleetOrigin =
                meanVx +
                commonOmegaFromTranslation * meanY;
            var commonVyAtFleetOrigin =
                meanVy -
                commonOmegaFromTranslation * meanX;
            var relativeCorrections = new Twist2D[count];

            for (var index = 0; index < count; index++)
            {
                var position = layout.Vehicles[index].PoseInFleet;
                var correction = rawCorrectionsInFleet[index];
                var commonVxAtMember =
                    commonVxAtFleetOrigin -
                    commonOmegaFromTranslation *
                    position.YMeters;
                var commonVyAtMember =
                    commonVyAtFleetOrigin +
                    commonOmegaFromTranslation *
                    position.XMeters;

                relativeCorrections[index] = new Twist2D(
                    correction.VxMetersPerSecond -
                    commonVxAtMember,
                    correction.VyMetersPerSecond -
                    commonVyAtMember,
                    correction.OmegaRadiansPerSecond -
                    meanOmega);
            }

            return relativeCorrections;
        }

        private Twist2D LimitCorrection(Twist2D correction)
        {
            var linearMagnitude = Math.Sqrt(
                correction.VxMetersPerSecond *
                correction.VxMetersPerSecond +
                correction.VyMetersPerSecond *
                correction.VyMetersPerSecond);
            var linearScale =
                linearMagnitude <=
                _maximumLinearCorrectionMetersPerSecond
                    ? 1.0
                    : _maximumLinearCorrectionMetersPerSecond /
                      linearMagnitude;
            var limitedOmega = Math.Max(
                -_maximumAngularCorrectionRadiansPerSecond,
                Math.Min(
                    _maximumAngularCorrectionRadiansPerSecond,
                    correction.OmegaRadiansPerSecond));

            return new Twist2D(
                correction.VxMetersPerSecond * linearScale,
                correction.VyMetersPerSecond * linearScale,
                limitedOmega);
        }

        private static Dictionary<int, FleetMemberCommand>
            IndexCommands(
                IReadOnlyList<FleetMemberCommand> commands)
        {
            var indexed =
                new Dictionary<int, FleetMemberCommand>(
                    commands.Count);

            for (var index = 0; index < commands.Count; index++)
            {
                var command = commands[index];
                if (command.VehicleId <= 0)
                {
                    throw new ArgumentOutOfRangeException(
                        nameof(commands),
                        $"第{index}个成员命令的车号无效。");
                }

                NumericGuard.EnsureFinite(
                    command.TwistInVehicleBody,
                    $"{nameof(commands)}[{index}]." +
                    nameof(FleetMemberCommand.TwistInVehicleBody));

                if (indexed.ContainsKey(command.VehicleId))
                {
                    throw new ArgumentException(
                        $"成员命令包含重复车号{command.VehicleId}。",
                        nameof(commands));
                }

                indexed.Add(command.VehicleId, command);
            }

            return indexed;
        }

        private static Dictionary<int, FleetMemberLayoutError>
            IndexErrors(
                IReadOnlyList<FleetMemberLayoutError> errors)
        {
            var indexed =
                new Dictionary<int, FleetMemberLayoutError>(
                    errors.Count);

            for (var index = 0; index < errors.Count; index++)
            {
                var error = errors[index];
                if (error.VehicleId <= 0)
                {
                    throw new ArgumentOutOfRangeException(
                        nameof(errors),
                        $"第{index}个布局误差的车号无效。");
                }

                NumericGuard.EnsureFinite(
                    error.ActualPoseInExpectedVehicleFrame,
                    $"{nameof(errors)}[{index}]." +
                    nameof(FleetMemberLayoutError
                        .ActualPoseInExpectedVehicleFrame));

                if (indexed.ContainsKey(error.VehicleId))
                {
                    throw new ArgumentException(
                        $"成员布局误差包含重复车号{error.VehicleId}。",
                        nameof(errors));
                }

                indexed.Add(error.VehicleId, error);
            }

            return indexed;
        }

        private static double ApplyDeadband(
            double value,
            double deadband)
        {
            var magnitude = Math.Abs(value);
            if (magnitude <= deadband)
            {
                return 0.0;
            }

            return Math.Sign(value) * (magnitude - deadband);
        }

        private static Twist2D Add(
            Twist2D first,
            Twist2D second)
        {
            return new Twist2D(
                first.VxMetersPerSecond +
                second.VxMetersPerSecond,
                first.VyMetersPerSecond +
                second.VyMetersPerSecond,
                first.OmegaRadiansPerSecond +
                second.OmegaRadiansPerSecond);
        }
    }
}