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

namespace MultiWheelC.Tests
{
    internal static class FleetMemberCommandCorrectorTests
    {
        private const double Tolerance = 1e-9;

        public static void Run()
        {
            VerifyZeroErrorsPreserveBaseCommands();
            VerifyRelativePositionErrorProducesOpposingCorrection();
            VerifyCommonTranslationIsRemoved();
            VerifyCommonRotationIsRemoved();
            VerifyDeadbandSuppressesSmallErrors();
            VerifyCorrectionLimits();

            Console.WriteLine(
                "FleetMemberCommandCorrector测试通过，共6个场景。");
        }

        private static void VerifyZeroErrorsPreserveBaseCommands()
        {
            var layout = CreateLayout();
            var baseCommands = FleetKinematics.Decompose(
                layout,
                new FleetMotionCommand(
                    Point2D.Zero,
                    new Twist2D(0.4, 0.1, 0.05)));
            var corrected = CreateCorrector().Correct(
                layout,
                baseCommands,
                CreateErrors(Pose2D.Identity, Pose2D.Identity));

            AssertCommandsEqual(
                corrected,
                baseCommands,
                "零布局误差");
        }

        private static void
            VerifyRelativePositionErrorProducesOpposingCorrection()
        {
            var layout = CreateLayout();
            var corrected = CreateCorrector().Correct(
                layout,
                CreateStopCommands(layout),
                CreateErrors(
                    new Pose2D(0.02, 0.0, 0.0),
                    new Pose2D(0.02, 0.0, 0.0)));

            AssertTwist(
                FindCommand(corrected, 1).TwistInVehicleBody,
                -0.02,
                0.0,
                0.0,
                "车辆1相对位置纠偏");
            AssertTwist(
                FindCommand(corrected, 2).TwistInVehicleBody,
                -0.02,
                0.0,
                0.0,
                "车辆2相对位置纠偏");
        }

        private static void VerifyCommonTranslationIsRemoved()
        {
            var layout = CreateLayout();
            var corrected = CreateCorrector().Correct(
                layout,
                CreateStopCommands(layout),
                CreateErrors(
                    new Pose2D(0.02, 0.0, 0.0),
                    new Pose2D(-0.02, 0.0, 0.0)));

            AssertAllStopped(
                corrected,
                "共同平移不应成为成员相对纠偏");
        }

        private static void VerifyCommonRotationIsRemoved()
        {
            const double fleetYawErrorRadians = 0.02;
            var layout = CreateLayout();
            var commonRotationError = new Pose2D(
                0.0,
                -fleetYawErrorRadians,
                fleetYawErrorRadians);
            var corrected = CreateCorrector().Correct(
                layout,
                CreateStopCommands(layout),
                CreateErrors(
                    commonRotationError,
                    commonRotationError));

            AssertAllStopped(
                corrected,
                "共同旋转不应成为成员相对纠偏");
        }

        private static void VerifyDeadbandSuppressesSmallErrors()
        {
            var layout = CreateLayout();
            var corrector = new FleetMemberCommandCorrector(
                longitudinalPositionGainPerSecond: 1.0,
                lateralPositionGainPerSecond: 1.0,
                yawGainPerSecond: 1.0,
                positionErrorDeadbandMeters: 0.005,
                yawErrorDeadbandRadians: 0.02,
                maximumLinearCorrectionMetersPerSecond: 1.0,
                maximumAngularCorrectionRadiansPerSecond: 1.0);
            var corrected = corrector.Correct(
                layout,
                CreateStopCommands(layout),
                CreateErrors(
                    new Pose2D(0.004, 0.003, 0.01),
                    new Pose2D(0.004, -0.003, -0.01)));

            AssertAllStopped(corrected, "布局误差死区");
        }

        private static void VerifyCorrectionLimits()
        {
            var layout = CreateLayout();
            var corrector = new FleetMemberCommandCorrector(
                longitudinalPositionGainPerSecond: 1.0,
                lateralPositionGainPerSecond: 1.0,
                yawGainPerSecond: 1.0,
                positionErrorDeadbandMeters: 0.0,
                yawErrorDeadbandRadians: 0.0,
                maximumLinearCorrectionMetersPerSecond: 0.03,
                maximumAngularCorrectionRadiansPerSecond: 0.05);
            var corrected = corrector.Correct(
                layout,
                CreateStopCommands(layout),
                CreateErrors(
                    new Pose2D(0.2, 0.0, 0.2),
                    new Pose2D(0.2, 0.0, -0.2)));

            for (var index = 0; index < corrected.Count; index++)
            {
                var twist = corrected[index].TwistInVehicleBody;
                var linearMagnitude = Math.Sqrt(
                    twist.VxMetersPerSecond *
                    twist.VxMetersPerSecond +
                    twist.VyMetersPerSecond *
                    twist.VyMetersPerSecond);

                AssertNear(
                    linearMagnitude,
                    0.03,
                    "线速度纠偏限幅");
                AssertNear(
                    Math.Abs(twist.OmegaRadiansPerSecond),
                    0.05,
                    "角速度纠偏限幅");
            }
        }

        private static FleetMemberCommandCorrector CreateCorrector()
        {
            return new FleetMemberCommandCorrector(
                longitudinalPositionGainPerSecond: 1.0,
                lateralPositionGainPerSecond: 1.0,
                yawGainPerSecond: 1.0,
                positionErrorDeadbandMeters: 0.0,
                yawErrorDeadbandRadians: 0.0,
                maximumLinearCorrectionMetersPerSecond: 1.0,
                maximumAngularCorrectionRadiansPerSecond: 1.0);
        }

        private static FleetLayout CreateLayout()
        {
            return new FleetLayout(
                new[]
                {
                    new VehicleLayout(
                        1,
                        new Pose2D(-1.0, 0.0, 0.0)),
                    new VehicleLayout(
                        2,
                        new Pose2D(1.0, 0.0, Math.PI))
                });
        }

        private static IReadOnlyList<FleetMemberCommand>
            CreateStopCommands(FleetLayout layout)
        {
            return FleetKinematics.Decompose(
                layout,
                FleetMotionCommand.Stop());
        }

        private static FleetMemberLayoutError[] CreateErrors(
            Pose2D vehicle1Error,
            Pose2D vehicle2Error)
        {
            return new[]
            {
                new FleetMemberLayoutError(1, vehicle1Error),
                new FleetMemberLayoutError(2, vehicle2Error)
            };
        }

        private static FleetMemberCommand FindCommand(
            IReadOnlyList<FleetMemberCommand> commands,
            int vehicleId)
        {
            for (var index = 0; index < commands.Count; index++)
            {
                if (commands[index].VehicleId == vehicleId)
                {
                    return commands[index];
                }
            }

            throw new InvalidOperationException(
                $"没有找到车辆{vehicleId}的成员命令。");
        }

        private static void AssertCommandsEqual(
            IReadOnlyList<FleetMemberCommand> actual,
            IReadOnlyList<FleetMemberCommand> expected,
            string scenario)
        {
            if (actual.Count != expected.Count)
            {
                throw new InvalidOperationException(
                    $"{scenario}的命令数量不一致。");
            }

            for (var index = 0; index < expected.Count; index++)
            {
                var expectedCommand = expected[index];
                var actualCommand = FindCommand(
                    actual,
                    expectedCommand.VehicleId);
                AssertTwist(
                    actualCommand.TwistInVehicleBody,
                    expectedCommand.TwistInVehicleBody
                        .VxMetersPerSecond,
                    expectedCommand.TwistInVehicleBody
                        .VyMetersPerSecond,
                    expectedCommand.TwistInVehicleBody
                        .OmegaRadiansPerSecond,
                    scenario);
            }
        }

        private static void AssertAllStopped(
            IReadOnlyList<FleetMemberCommand> commands,
            string scenario)
        {
            for (var index = 0; index < commands.Count; index++)
            {
                AssertTwist(
                    commands[index].TwistInVehicleBody,
                    0.0,
                    0.0,
                    0.0,
                    scenario);
            }
        }

        private static void AssertTwist(
            Twist2D actual,
            double expectedVx,
            double expectedVy,
            double expectedOmega,
            string scenario)
        {
            AssertNear(
                actual.VxMetersPerSecond,
                expectedVx,
                scenario + " Vx");
            AssertNear(
                actual.VyMetersPerSecond,
                expectedVy,
                scenario + " Vy");
            AssertNear(
                actual.OmegaRadiansPerSecond,
                expectedOmega,
                scenario + " Omega");
        }

        private static void AssertNear(
            double actual,
            double expected,
            string valueName)
        {
            if (Math.Abs(actual - expected) > Tolerance)
            {
                throw new InvalidOperationException(
                    $"{valueName}错误：" +
                    $"actual={actual:F9}, expected={expected:F9}。");
            }
        }
    }
}