using System;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Lateral;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Tests
{
    /// <summary>
    /// 验证Stanley控制器在前进和倒车时的横向误差符号及差动转角方向。
    /// </summary>
    internal static class Program
    {
        private const double SpeedMagnitudeMetersPerSecond = 0.4;
        private const double TestLateralErrorMeters = 0.1;
        private const double TestHeadingErrorRadians = 0.1;
        private const double TestCurvaturePerMeter = 0.2;

        /// <summary>
        /// 运行不依赖宿主、Detour或实车底盘的控制器数学测试。
        /// </summary>
        private static void Main()
        {
            foreach (var travelDirection in new[] { 1.0, -1.0 })
            {
                VerifyCrossTrackConvergence(
                    travelDirection,
                    TestLateralErrorMeters);
                VerifyCrossTrackConvergence(
                    travelDirection,
                    -TestLateralErrorMeters);
                VerifyHeadingDirection(travelDirection);
                VerifyCurvatureDirection(travelDirection);
            }

            Console.WriteLine(
                "Stanley前进/倒车横向符号测试通过。共8个场景。");
            FleetKinematicsTests.Run();
            FleetControllerTests.Run();
        }

        /// <summary>
        /// 验证共同转角产生的横向速度始终使轨迹点序横向误差绝对值减小。
        /// </summary>
        private static void VerifyCrossTrackConvergence(
            double travelDirection,
            double lateralErrorMeters)
        {
            var signedSpeedMetersPerSecond =
                travelDirection *
                SpeedMagnitudeMetersPerSecond;
            var controller = CreateController();
            var command = controller.Compute(
                CreateContext(
                    signedSpeedMetersPerSecond,
                    lateralErrorMeters,
                    headingErrorRadians: 0.0,
                    feedforwardCurvaturePerMeter: 0.0));

            var bodyLateralSpeedMetersPerSecond =
                signedSpeedMetersPerSecond *
                Math.Sin(command.CommonAngleRadians);

            // 轨迹执行方向在倒车时与车体X轴相反，因此需要先把车体
            // 横向速度换算到轨迹点序坐标系，再计算参考轨迹相对车辆的误差变化率。
            var lateralErrorDerivativeMetersPerSecond =
                -travelDirection *
                bodyLateralSpeedMetersPerSecond;

            AssertTrue(
                lateralErrorMeters *
                lateralErrorDerivativeMetersPerSecond < 0.0,
                $"横向误差没有收敛：direction={travelDirection}，" +
                $"error={lateralErrorMeters:F3}m，" +
                $"common={command.CommonAngleRadians:F6}rad，" +
                $"errorDerivative={lateralErrorDerivativeMetersPerSecond:F6}m/s。");
        }

        /// <summary>
        /// 验证航向误差差动转角在倒车时仍按行驶方向反号。
        /// </summary>
        private static void VerifyHeadingDirection(
            double travelDirection)
        {
            var command = CreateController().Compute(
                CreateContext(
                    travelDirection *
                    SpeedMagnitudeMetersPerSecond,
                    lateralErrorMeters: 0.0,
                    headingErrorRadians:
                        TestHeadingErrorRadians,
                    feedforwardCurvaturePerMeter: 0.0));

            AssertSameSign(
                command.DifferentialAngleRadians,
                travelDirection,
                "航向误差差动转角");
        }

        /// <summary>
        /// 验证正曲率前馈差动转角在倒车时仍按行驶方向反号。
        /// </summary>
        private static void VerifyCurvatureDirection(
            double travelDirection)
        {
            var command = CreateController().Compute(
                CreateContext(
                    travelDirection *
                    SpeedMagnitudeMetersPerSecond,
                    lateralErrorMeters: 0.0,
                    headingErrorRadians: 0.0,
                    feedforwardCurvaturePerMeter:
                        TestCurvaturePerMeter));

            AssertSameSign(
                command.DifferentialAngleRadians,
                travelDirection,
                "曲率前馈差动转角");
        }

        /// <summary>
        /// 创建使用固定参数的无状态Stanley控制器。
        /// </summary>
        private static StanleyLateralController CreateController()
        {
            return new StanleyLateralController(
                controlPointRadiusMeters: 0.5,
                crossTrackGainPerSecond: 1.0,
                headingErrorGain: 1.0,
                minimumSpeedMetersPerSecond: 0.05,
                useActualSpeedForGain: true);
        }

        /// <summary>
        /// 创建只包含本次符号测试所需字段的轨迹跟踪上下文。
        /// </summary>
        private static PathTrackingContext CreateContext(
            double signedSpeedMetersPerSecond,
            double lateralErrorMeters,
            double headingErrorRadians,
            double feedforwardCurvaturePerMeter)
        {
            var actualTwistInBody = new Twist2D(
                signedSpeedMetersPerSecond,
                0.0,
                0.0);
            var referencePoint = new TrajectoryPoint(
                arcLengthMeters: 0.0,
                poseInWorld: Pose2D.Identity,
                curvaturePerMeter: 0.0,
                referenceSpeedMetersPerSecond:
                    signedSpeedMetersPerSecond);
            var projection = new TrajectoryProjection(
                segmentStartIndex: 0,
                referencePoint: referencePoint,
                lateralErrorMeters: lateralErrorMeters,
                headingErrorRadians: headingErrorRadians,
                distanceToTrajectoryMeters:
                    Math.Abs(lateralErrorMeters),
                remainingDistanceMeters: 1.0);

            return new PathTrackingContext(
                actualTwistInBody,
                true,
                projection,
                signedSpeedMetersPerSecond,
                feedforwardCurvaturePerMeter,
                deltaTimeSeconds: 0.02);
        }

        /// <summary>
        /// 验证实际值与预期符号一致。
        /// </summary>
        private static void AssertSameSign(
            double actualValue,
            double expectedSign,
            string valueName)
        {
            AssertTrue(
                Math.Sign(actualValue) ==
                Math.Sign(expectedSign),
                $"{valueName}方向错误：actual={actualValue:F6}，" +
                $"expectedSign={expectedSign:F0}。");
        }

        /// <summary>
        /// 条件不成立时抛出异常，使测试进程以非零状态结束。
        /// </summary>
        private static void AssertTrue(
            bool condition,
            string failureMessage)
        {
            if (!condition)
            {
                throw new InvalidOperationException(
                    failureMessage);
            }
        }
    }
}