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

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

        public static void Run()
        {
            VerifyRigidStateIsRecovered();
            VerifyOlderSamplesAreAligned();
            VerifyYawWrapAroundIsAveraged();
            VerifySmallLayoutErrorIsReported();
            VerifyInconsistentCentersAreRejected();
            VerifyMissingMemberIsRejected();
            VerifyInvalidVelocityRemainsExplicit();

            Console.WriteLine(
                "FleetStateEstimator车队状态估计测试通过。共7个场景。");
        }

        private static void VerifyRigidStateIsRecovered()
        {
            var layout = CreateLayout();
            var fleetPose = new Pose2D(4.0, -2.0, 0.4);
            var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
            var result = CreateEstimator().Estimate(
                layout,
                CreateRigidMemberStates(
                    layout,
                    fleetPose,
                    fleetTwist,
                    sampleTimestampSeconds: 5.0,
                    hasValidVelocityEstimate: true),
                targetTimestampSeconds: 5.0);

            var state = RequireState(result, "刚体状态还原");
            AssertPose(
                state.FleetPoseInWorld,
                fleetPose,
                "刚体状态还原");
            AssertTwist(
                state.TwistAtFleetOriginInWorld,
                fleetTwist,
                "刚体速度还原");

            for (var index = 0;
                 index < result.MemberErrors.Count;
                 index++)
            {
                AssertPose(
                    result.MemberErrors[index]
                        .ActualPoseInExpectedVehicleFrame,
                    Pose2D.Identity,
                    "刚体布局误差");
            }
        }

        private static void VerifyOlderSamplesAreAligned()
        {
            var layout = CreateLayout();
            var sampleFleetPose =
                new Pose2D(1.0, 2.0, 0.3);
            var fleetTwist =
                new Twist2D(0.4, -0.2, 0.0);
            var result = CreateEstimator().Estimate(
                layout,
                CreateRigidMemberStates(
                    layout,
                    sampleFleetPose,
                    fleetTwist,
                    sampleTimestampSeconds: 0.9,
                    hasValidVelocityEstimate: true),
                targetTimestampSeconds: 1.0);

            var state = RequireState(result, "成员时间对齐");
            AssertPose(
                state.FleetPoseInWorld,
                new Pose2D(1.04, 1.98, 0.3),
                "成员时间对齐");
            AssertTwist(
                state.TwistAtFleetOriginInWorld,
                fleetTwist,
                "时间对齐后速度");
        }

        private static void VerifySmallLayoutErrorIsReported()
        {
            var layout = CreateLayout();
            var result = CreateEstimator().Estimate(
                layout,
                new[]
                {
                    CreateMemberState(
                        1,
                        new Pose2D(-0.98, 0.0, 0.0),
                        Twist2D.Zero,
                        1.0,
                        true),
                    CreateMemberState(
                        2,
                        new Pose2D(0.99, 0.0, Math.PI),
                        Twist2D.Zero,
                        1.0,
                        true)
                },
                targetTimestampSeconds: 1.0);

            var state = RequireState(result, "小范围布局误差");
            AssertNear(
                state.FleetPoseInWorld.XMeters,
                0.005,
                "小范围布局误差中心X");
            AssertNear(
                FindError(result.MemberErrors, 1)
                    .ActualPoseInExpectedVehicleFrame.XMeters,
                0.015,
                "车辆1布局误差X");
            AssertNear(
                FindError(result.MemberErrors, 2)
                    .ActualPoseInExpectedVehicleFrame.XMeters,
                0.015,
                "车辆2布局误差X");
        }

        private static void VerifyYawWrapAroundIsAveraged()
        {
            var layout = CreateLayout();
            var firstCandidate = new Pose2D(
                0.0,
                0.0,
                AngleMath.DegreesToRadians(179.0));
            var secondCandidate = new Pose2D(
                0.0,
                0.0,
                AngleMath.DegreesToRadians(-179.0));
            var result = CreateEstimator().Estimate(
                layout,
                new[]
                {
                    CreateMemberState(
                        1,
                        FrameTransform2D.Compose(
                            firstCandidate,
                            layout.Vehicles[0].PoseInFleet),
                        Twist2D.Zero,
                        1.0,
                        true),
                    CreateMemberState(
                        2,
                        FrameTransform2D.Compose(
                            secondCandidate,
                            layout.Vehicles[1].PoseInFleet),
                        Twist2D.Zero,
                        1.0,
                        true)
                },
                targetTimestampSeconds: 1.0);

            var state = RequireState(result, "跨正负π航向平均");
            AssertNear(
                Math.Abs(state.FleetPoseInWorld.YawRadians),
                Math.PI,
                "跨正负π航向平均");
        }

        private static void VerifyInconsistentCentersAreRejected()
        {
            var result = CreateEstimator().Estimate(
                CreateLayout(),
                new[]
                {
                    CreateMemberState(
                        1,
                        new Pose2D(-1.0, 0.0, 0.0),
                        Twist2D.Zero,
                        1.0,
                        true),
                    CreateMemberState(
                        2,
                        new Pose2D(1.3, 0.0, Math.PI),
                        Twist2D.Zero,
                        1.0,
                        true)
                },
                targetTimestampSeconds: 1.0);

            AssertUnavailable(result, "候选中心冲突");
        }

        private static void VerifyMissingMemberIsRejected()
        {
            var result = CreateEstimator().Estimate(
                CreateLayout(),
                new[]
                {
                    CreateMemberState(
                        1,
                        new Pose2D(-1.0, 0.0, 0.0),
                        Twist2D.Zero,
                        1.0,
                        true)
                },
                targetTimestampSeconds: 1.0);

            AssertUnavailable(result, "成员缺失");
        }

        private static void VerifyInvalidVelocityRemainsExplicit()
        {
            var layout = CreateLayout();
            var result = CreateEstimator().Estimate(
                layout,
                CreateRigidMemberStates(
                    layout,
                    Pose2D.Identity,
                    new Twist2D(0.4, 0.0, 0.0),
                    sampleTimestampSeconds: 1.0,
                    hasValidVelocityEstimate: false),
                targetTimestampSeconds: 1.0);

            var state = RequireState(result, "速度未初始化");
            if (state.HasValidVelocityEstimate)
            {
                throw new InvalidOperationException(
                    "成员速度无效时车队速度不应标记为有效。");
            }

            AssertTwist(
                state.TwistAtFleetOriginInWorld,
                Twist2D.Zero,
                "速度未初始化");
        }

        private static FleetStateEstimator CreateEstimator()
        {
            return new FleetStateEstimator(
                maximumMemberStateAgeSeconds: 0.25,
                maximumPositionDisagreementMeters: 0.1,
                maximumYawDisagreementRadians:
                    AngleMath.DegreesToRadians(5.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 FleetMemberStateSample[]
            CreateRigidMemberStates(
                FleetLayout layout,
                Pose2D fleetPoseInWorld,
                Twist2D twistAtFleetOriginInWorld,
                double sampleTimestampSeconds,
                bool hasValidVelocityEstimate)
        {
            var states =
                new FleetMemberStateSample[layout.VehicleCount];

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

                states[index] = new FleetMemberStateSample(
                    vehicle.VehicleId,
                    sampleTimestampSeconds,
                    memberPoseInWorld,
                    memberTwistInWorld,
                    isStateAvailable: true,
                    hasValidVelocityEstimate:
                        hasValidVelocityEstimate);
            }

            return states;
        }

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

        private static FleetState RequireState(
            FleetStateEstimateResult result,
            string scenario)
        {
            if (!result.IsAvailable || !result.State.HasValue)
            {
                throw new InvalidOperationException(
                    $"{scenario}应产生可用状态：" +
                    result.UnavailableReason);
            }

            return result.State.Value;
        }

        private static void AssertUnavailable(
            FleetStateEstimateResult result,
            string scenario)
        {
            if (result.IsAvailable ||
                string.IsNullOrWhiteSpace(
                    result.UnavailableReason))
            {
                throw new InvalidOperationException(
                    $"{scenario}应返回带原因的不可用结果。");
            }
        }

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

            throw new InvalidOperationException(
                $"没有找到车辆{vehicleId}的布局误差。");
        }

        private static void AssertPose(
            Pose2D actual,
            Pose2D expected,
            string scenario)
        {
            AssertNear(
                actual.XMeters,
                expected.XMeters,
                scenario + " X");
            AssertNear(
                actual.YMeters,
                expected.YMeters,
                scenario + " Y");
            AssertNear(
                AngleMath.ShortestDifferenceRadians(
                    actual.YawRadians,
                    expected.YawRadians),
                0.0,
                scenario + " Yaw");
        }

        private static void AssertTwist(
            Twist2D actual,
            Twist2D expected,
            string scenario)
        {
            AssertNear(
                actual.VxMetersPerSecond,
                expected.VxMetersPerSecond,
                scenario + " Vx");
            AssertNear(
                actual.VyMetersPerSecond,
                expected.VyMetersPerSecond,
                scenario + " Vy");
            AssertNear(
                actual.OmegaRadiansPerSecond,
                expected.OmegaRadiansPerSecond,
                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}。");
            }
        }
    }
}