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

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

        public static void Run()
        {
            VerifyInactiveControllerStops();
            VerifyStraightCommand();
            VerifyFortyFiveDegreeMotionDirection();
            VerifyNinetyDegreeMotionDirection();
            VerifyInvalidVelocityUsesReferenceSpeed();
            VerifyCompletionStops();
            VerifyExcessiveTrackingErrorFaults();
            VerifyCancelStops();

            Console.WriteLine(
                "FleetController车队中心控制测试通过。共8个场景。");
        }

        private static void VerifyInactiveControllerStops()
        {
            var controller = CreateController();
            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Inactive,
                "未启动控制器");
            AssertStop(command, "未启动控制器");
        }

        private static void VerifyStraightCommand()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.4, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.CommandGenerated,
                "直线控制");
            AssertNear(
                command.ReferencePointInFleet.XMeters,
                0.0,
                "直线控制参考点X");
            AssertNear(
                command.ReferencePointInFleet.YMeters,
                0.0,
                "直线控制参考点Y");
            AssertTwist(
                command.TwistAtReferencePoint,
                0.4,
                0.0,
                0.0,
                "直线控制");
        }

        private static void VerifyInvalidVelocityUsesReferenceSpeed()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, false),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.CommandGenerated,
                "速度尚未初始化");
            AssertTwist(
                command.TwistAtReferencePoint,
                0.4,
                0.0,
                0.0,
                "速度尚未初始化");
        }

        private static void VerifyFortyFiveDegreeMotionDirection()
        {
            VerifyMotionDirection(
                Math.PI / 4.0,
                "45度运动方向");
        }

        private static void VerifyNinetyDegreeMotionDirection()
        {
            VerifyMotionDirection(
                Math.PI / 2.0,
                "90度运动方向");
        }

        private static void VerifyMotionDirection(
            double motionDirectionInFleetRadians,
            string scenario)
        {
            var controller = CreateController(
                motionDirectionInFleetRadians);
            controller.Start(
                CreateStraightTrajectory(
                    motionDirectionInFleetRadians));

            var directionX =
                Math.Cos(motionDirectionInFleetRadians);
            var directionY =
                Math.Sin(motionDirectionInFleetRadians);
            var result = controller.ComputeCommand(
                CreateState(
                    0.5 * directionX,
                    0.5 * directionY,
                    0.4 * directionX,
                    0.4 * directionY,
                    true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.CommandGenerated,
                scenario);
            AssertTwist(
                command.TwistAtReferencePoint,
                0.4 * directionX,
                0.4 * directionY,
                0.0,
                scenario);
        }

        private static void VerifyCompletionStops()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(1.0, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Completed,
                "终点完成");
            AssertStop(command, "终点完成");

            if (controller.IsActive || !controller.IsCompleted)
            {
                throw new InvalidOperationException(
                    "终点完成后控制器状态错误。");
            }
        }

        private static void VerifyExcessiveTrackingErrorFaults()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.5, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Faulted,
                "轨迹偏离保护");
            AssertStop(command, "轨迹偏离保护");

            if (string.IsNullOrWhiteSpace(
                    controller.LastFailureReason))
            {
                throw new InvalidOperationException(
                    "轨迹偏离故障没有保存原因。");
            }
        }

        private static void VerifyCancelStops()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());
            controller.Cancel();

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Inactive,
                "取消控制");
            AssertStop(command, "取消控制");
        }

        private static FleetController CreateController(
            double motionDirectionInFleetRadians = 0.0)
        {
            return new FleetController(
                new StraightLateralController(),
                new ReferenceLongitudinalController(),
                new GcpCommandAllocator(
                    Math.PI / 4.0),
                virtualControlPointRadiusMeters: 0.5,
                motionDirectionInFleetRadians:
                    motionDirectionInFleetRadians);
        }

        private static Trajectory2D CreateStraightTrajectory(
            double motionDirectionInFleetRadians = 0.0)
        {
            var directionX =
                Math.Cos(motionDirectionInFleetRadians);
            var directionY =
                Math.Sin(motionDirectionInFleetRadians);

            return new Trajectory2D(
                new[]
                {
                    new TrajectoryPoint(
                        0.0,
                        Pose2D.Identity,
                        0.0,
                        0.4),
                    new TrajectoryPoint(
                        1.0,
                        new Pose2D(
                            directionX,
                            directionY,
                            0.0),
                        0.0,
                        0.4)
                });
        }

        private static FleetState CreateState(
            double xMeters,
            double yMeters,
            double vxMetersPerSecond,
            bool hasValidVelocityEstimate)
        {
            return CreateState(
                xMeters,
                yMeters,
                vxMetersPerSecond,
                0.0,
                hasValidVelocityEstimate);
        }

        private static FleetState CreateState(
            double xMeters,
            double yMeters,
            double vxMetersPerSecond,
            double vyMetersPerSecond,
            bool hasValidVelocityEstimate)
        {
            return new FleetState(
                sampleTimestampSeconds: 1.0,
                fleetPoseInWorld: new Pose2D(
                    xMeters,
                    yMeters,
                    0.0),
                twistAtFleetOriginInWorld: new Twist2D(
                    vxMetersPerSecond,
                    vyMetersPerSecond,
                    0.0),
                hasValidVelocityEstimate:
                    hasValidVelocityEstimate);
        }

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

        private static void AssertStop(
            FleetMotionCommand command,
            string scenario)
        {
            AssertTwist(
                command.TwistAtReferencePoint,
                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}。");
            }
        }

        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()
            {
            }
        }
    }
}