using System;
using System.Collections.Generic;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Fleet;
using MultiWheelC.Trajectory;
using MyParking.Shared;

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

        public static void Run()
        {
            VerifyCommandCycle();
            VerifySmallLayoutErrorIsCorrected();
            VerifyWarningRangeScalesAllCommands();
            VerifyUnavailableStateStopsAndRecovers();
            VerifyUnsafeLayoutFaultsAndLatches();
            VerifyCompletionStops();
            VerifyTrackingFaultStops();
            VerifyCancelReturnsInactive();

            Console.WriteLine(
                "FleetCoordinator车队协调测试通过。共8个场景。");
        }

        private static void VerifyCommandCycle()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());

            var result = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(0.5, 0.0, 0.0),
                    new Twist2D(0.4, 0.0, 0.0),
                    1.0),
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.CommandGenerated,
                "正常协调周期");
            if (!output.State.HasValue)
            {
                throw new InvalidOperationException(
                    "正常协调周期没有返回车队状态。");
            }

            AssertNear(
                output.State.Value.FleetPoseInWorld.XMeters,
                0.5,
                "正常协调周期中心X");
            AssertNear(
                output.SpeedScale,
                1.0,
                "正常协调周期速度比例");
            AssertTwist(
                output.FleetCommand.TwistAtReferencePoint,
                0.4,
                0.0,
                0.0,
                "正常协调周期车队命令");
            AssertTwist(
                FindCommand(output.MemberCommands, 1)
                    .TwistInVehicleBody,
                0.4,
                0.0,
                0.0,
                "正常协调周期车辆1");
            AssertTwist(
                FindCommand(output.MemberCommands, 2)
                    .TwistInVehicleBody,
                -0.4,
                0.0,
                0.0,
                "正常协调周期车辆2");
        }

        private static void VerifySmallLayoutErrorIsCorrected()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());
            var result = coordinator.ExecuteCycle(
                new[]
                {
                    CreateMemberState(
                        1,
                        new Pose2D(-0.99, 0.0, 0.0),
                        1.0),
                    CreateMemberState(
                        2,
                        new Pose2D(0.99, 0.0, Math.PI),
                        1.0)
                },
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.CommandGenerated,
                "小范围布局误差纠偏");
            AssertNear(
                output.SpeedScale,
                1.0,
                "小范围布局误差速度比例");
            AssertTwist(
                FindCommand(output.BaseMemberCommands, 1)
                    .TwistInVehicleBody,
                0.4,
                0.0,
                0.0,
                "车辆1基础命令");
            AssertTwist(
                FindCommand(output.BaseMemberCommands, 2)
                    .TwistInVehicleBody,
                -0.4,
                0.0,
                0.0,
                "车辆2基础命令");
            AssertTwist(
                FindCommand(output.MemberCommands, 1)
                    .TwistInVehicleBody,
                0.395,
                0.0,
                0.0,
                "车辆1纠偏后命令");
            AssertTwist(
                FindCommand(output.MemberCommands, 2)
                    .TwistInVehicleBody,
                -0.405,
                0.0,
                0.0,
                "车辆2纠偏后命令");
        }

        private static void VerifyWarningRangeScalesAllCommands()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());
            var result = coordinator.ExecuteCycle(
                new[]
                {
                    CreateMemberState(
                        1,
                        new Pose2D(-0.965, 0.0, 0.0),
                        1.0),
                    CreateMemberState(
                        2,
                        new Pose2D(0.965, 0.0, Math.PI),
                        1.0)
                },
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.CommandGenerated,
                "警告区间统一缩放");
            AssertNear(
                output.SpeedScale,
                0.5,
                "警告区间速度比例");
            AssertTwist(
                output.FleetCommand.TwistAtReferencePoint,
                0.2,
                0.0,
                0.0,
                "警告区间车队命令");
            AssertTwist(
                FindCommand(output.MemberCommands, 1)
                    .TwistInVehicleBody,
                0.2,
                0.0,
                0.0,
                "警告区间车辆1");
            AssertTwist(
                FindCommand(output.MemberCommands, 2)
                    .TwistInVehicleBody,
                -0.2,
                0.0,
                0.0,
                "警告区间车辆2");

            if (string.IsNullOrWhiteSpace(output.Reason))
            {
                throw new InvalidOperationException(
                    "警告区间缩放没有返回限制原因。");
            }
        }

        private static void VerifyUnavailableStateStopsAndRecovers()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());
            var unavailableStates = CreateRigidMemberStates(
                layout,
                new Pose2D(0.5, 0.0, 0.0),
                new Twist2D(0.4, 0.0, 0.0),
                1.0);
            unavailableStates[1] = new FleetMemberStateSample(
                unavailableStates[1].VehicleId,
                unavailableStates[1].SampleTimestampSeconds,
                unavailableStates[1].PoseInWorld,
                unavailableStates[1].TwistAtVehicleOriginInWorld,
                isStateAvailable: false,
                hasValidVelocityEstimate: true);

            var waitingResult = coordinator.ExecuteCycle(
                unavailableStates,
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var waitingOutput);

            AssertResult(
                waitingResult,
                FleetCoordinationCycleResult.WaitingForState,
                "状态暂时不可用");
            AssertStop(waitingOutput, layout.VehicleCount);

            if (!coordinator.IsActive)
            {
                throw new InvalidOperationException(
                    "状态暂时不可用不应取消车队控制器。");
            }

            var recoveredResult = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(0.5, 0.0, 0.0),
                    new Twist2D(0.4, 0.0, 0.0),
                    1.02),
                targetTimestampSeconds: 1.02,
                deltaTimeSeconds: 0.02,
                out _);

            AssertResult(
                recoveredResult,
                FleetCoordinationCycleResult.CommandGenerated,
                "状态恢复");
        }

        private static void VerifyUnsafeLayoutFaultsAndLatches()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());
            var deformedStates = new[]
            {
                CreateMemberState(
                    1,
                    new Pose2D(-0.9, 0.0, 0.0),
                    1.0),
                CreateMemberState(
                    2,
                    new Pose2D(0.9, 0.0, Math.PI),
                    1.0)
            };

            var result = coordinator.ExecuteCycle(
                deformedStates,
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.Faulted,
                "布局误差超限");
            AssertStop(output, layout.VehicleCount);

            if (!coordinator.IsFaulted ||
                string.IsNullOrWhiteSpace(
                    coordinator.LastFailureReason))
            {
                throw new InvalidOperationException(
                    "布局误差故障没有被锁存。");
            }

            var latchedResult = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(0.5, 0.0, 0.0),
                    new Twist2D(0.4, 0.0, 0.0),
                    1.02),
                targetTimestampSeconds: 1.02,
                deltaTimeSeconds: 0.02,
                out var latchedOutput);

            AssertResult(
                latchedResult,
                FleetCoordinationCycleResult.Faulted,
                "布局误差故障锁存");
            AssertStop(latchedOutput, layout.VehicleCount);
        }

        private static void VerifyCompletionStops()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());

            var result = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(1.0, 0.0, 0.0),
                    Twist2D.Zero,
                    1.0),
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.Completed,
                "车队轨迹完成");
            AssertStop(output, layout.VehicleCount);

            if (!coordinator.IsCompleted)
            {
                throw new InvalidOperationException(
                    "车队轨迹完成状态没有被保存。");
            }
        }

        private static void VerifyTrackingFaultStops()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());

            var result = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(0.5, 0.5, 0.0),
                    Twist2D.Zero,
                    1.0),
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.Faulted,
                "车队中心跟踪故障");
            AssertStop(output, layout.VehicleCount);

            if (!coordinator.IsFaulted)
            {
                throw new InvalidOperationException(
                    "车队中心跟踪故障没有传递到协调器。");
            }
        }

        private static void VerifyCancelReturnsInactive()
        {
            var layout = CreateLayout();
            var coordinator = CreateCoordinator();
            coordinator.Start(layout, CreateTrajectory());
            coordinator.Cancel();

            var result = coordinator.ExecuteCycle(
                CreateRigidMemberStates(
                    layout,
                    new Pose2D(0.5, 0.0, 0.0),
                    Twist2D.Zero,
                    1.0),
                targetTimestampSeconds: 1.0,
                deltaTimeSeconds: 0.02,
                out var output);

            AssertResult(
                result,
                FleetCoordinationCycleResult.Inactive,
                "取消车队协调");
            AssertStop(output, layout.VehicleCount);
        }

        private static FleetCoordinator CreateCoordinator()
        {
            var estimator = new FleetStateEstimator(
                maximumMemberStateAgeSeconds: 0.25,
                maximumPositionDisagreementMeters: 0.5,
                maximumYawDisagreementRadians:
                    AngleMath.DegreesToRadians(10.0));
            var controller = new FleetController(
                new StraightLateralController(),
                new ReferenceLongitudinalController(),
                new GcpCommandAllocator(Math.PI / 4.0),
                virtualControlPointRadiusMeters: 0.5);
            var commandCorrector =
                new FleetMemberCommandCorrector(
                    longitudinalPositionGainPerSecond: 1.0,
                    lateralPositionGainPerSecond: 1.0,
                    yawGainPerSecond: 1.0,
                    positionErrorDeadbandMeters: 0.005,
                    yawErrorDeadbandRadians:
                        AngleMath.DegreesToRadians(0.5),
                    maximumLinearCorrectionMetersPerSecond:
                        0.03,
                    maximumAngularCorrectionRadiansPerSecond:
                        AngleMath.DegreesToRadians(2.0));

            return new FleetCoordinator(
                estimator,
                controller,
                commandCorrector,
                memberPositionErrorWarningMeters: 0.02,
                maximumMemberPositionErrorMeters: 0.05,
                memberYawErrorWarningRadians:
                    AngleMath.DegreesToRadians(1.0),
                maximumMemberYawErrorRadians:
                    AngleMath.DegreesToRadians(3.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 Trajectory2D CreateTrajectory()
        {
            return new Trajectory2D(
                new[]
                {
                    new TrajectoryPoint(
                        0.0,
                        Pose2D.Identity,
                        0.0,
                        0.4),
                    new TrajectoryPoint(
                        1.0,
                        new Pose2D(1.0, 0.0, 0.0),
                        0.0,
                        0.4)
                });
        }

        private static FleetMemberStateSample[]
            CreateRigidMemberStates(
                FleetLayout layout,
                Pose2D fleetPoseInWorld,
                Twist2D twistAtFleetOriginInWorld,
                double timestampSeconds)
        {
            var states =
                new FleetMemberStateSample[layout.VehicleCount];

            for (var index = 0;
                 index < layout.Vehicles.Count;
                 index++)
            {
                var vehicle = layout.Vehicles[index];
                var poseInWorld = FrameTransform2D.Compose(
                    fleetPoseInWorld,
                    vehicle.PoseInFleet);
                var offsetX =
                    poseInWorld.XMeters -
                    fleetPoseInWorld.XMeters;
                var offsetY =
                    poseInWorld.YMeters -
                    fleetPoseInWorld.YMeters;
                var twistInWorld = new Twist2D(
                    twistAtFleetOriginInWorld.VxMetersPerSecond -
                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
                    offsetY,
                    twistAtFleetOriginInWorld.VyMetersPerSecond +
                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
                    offsetX,
                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond);

                states[index] = new FleetMemberStateSample(
                    vehicle.VehicleId,
                    timestampSeconds,
                    poseInWorld,
                    twistInWorld,
                    isStateAvailable: true,
                    hasValidVelocityEstimate: true);
            }

            return states;
        }

        private static FleetMemberStateSample CreateMemberState(
            int vehicleId,
            Pose2D poseInWorld,
            double timestampSeconds)
        {
            return new FleetMemberStateSample(
                vehicleId,
                timestampSeconds,
                poseInWorld,
                Twist2D.Zero,
                isStateAvailable: true,
                hasValidVelocityEstimate: true);
        }

        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 AssertStop(
            FleetCoordinationCycleOutput output,
            int expectedMemberCount)
        {
            AssertTwist(
                output.FleetCommand.TwistAtReferencePoint,
                0.0,
                0.0,
                0.0,
                "车队停止命令");

            if (output.MemberCommands.Count != expectedMemberCount)
            {
                throw new InvalidOperationException(
                    "停止输出的成员命令数量错误。");
            }

            if (output.BaseMemberCommands.Count != expectedMemberCount)
            {
                throw new InvalidOperationException(
                    "停止输出的成员基础命令数量错误。");
            }

            for (var index = 0;
                 index < output.MemberCommands.Count;
                 index++)
            {
                AssertTwist(
                    output.BaseMemberCommands[index]
                        .TwistInVehicleBody,
                    0.0,
                    0.0,
                    0.0,
                    "成员基础停止命令");
                AssertTwist(
                    output.MemberCommands[index].TwistInVehicleBody,
                    0.0,
                    0.0,
                    0.0,
                    "成员停止命令");
            }
        }

        private static void AssertResult(
            FleetCoordinationCycleResult actual,
            FleetCoordinationCycleResult expected,
            string scenario)
        {
            if (actual != expected)
            {
                throw new InvalidOperationException(
                    $"{scenario}结果错误：" +
                    $"actual={actual}, expected={expected}。");
            }
        }

        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}。");
            }
        }

        private sealed class StraightLateralController :
            ILateralController
        {
            public LateralControlCommand Compute(
                PathTrackingContext context)
            {
                return LateralControlCommand.Straight;
            }

            public void Reset()
            {
            }
        }

        private sealed class ReferenceLongitudinalController :
            ILongitudinalController
        {
            public double ComputeSpeedMetersPerSecond(
                PathTrackingContext context)
            {
                return context.ControlReferenceSpeedMetersPerSecond;
            }

            public void Reset()
            {
            }
        }
    }
}