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

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

        public static void Run()
        {
            var layout = CreateTailToTailLayout();

            VerifyTranslation(layout);
            VerifyRotationAroundFleetCenter(layout);
            VerifyRotationAroundFirstVehicle(layout);
            VerifyStop(layout);

            Console.WriteLine(
                "FleetKinematics刚体速度分配测试通过。共4个场景。");
        }

        private static FleetLayout CreateTailToTailLayout()
        {
            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 void VerifyTranslation(FleetLayout layout)
        {
            var commands = FleetKinematics.Decompose(
                layout,
                new FleetMotionCommand(
                    Point2D.Zero,
                    new Twist2D(0.4, 0.0, 0.0)));

            AssertTwist(Find(commands, 1), 0.4, 0.0, 0.0);
            AssertTwist(Find(commands, 2), -0.4, 0.0, 0.0);
        }

        private static void VerifyRotationAroundFleetCenter(
            FleetLayout layout)
        {
            var commands = FleetKinematics.Decompose(
                layout,
                FleetMotionCommand.RotateAround(
                    Point2D.Zero,
                    0.2));

            AssertTwist(Find(commands, 1), 0.0, 0.2, 0.2);
            AssertTwist(Find(commands, 2), 0.0, 0.2, 0.2);
        }

        private static void VerifyRotationAroundFirstVehicle(
            FleetLayout layout)
        {
            var commands = FleetKinematics.Decompose(
                layout,
                FleetMotionCommand.RotateAround(
                    new Point2D(1.0, 0.0),
                    0.2));

            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.2);
            AssertTwist(Find(commands, 2), 0.0, 0.4, 0.2);
        }

        private static void VerifyStop(FleetLayout layout)
        {
            var commands = FleetKinematics.Decompose(
                layout,
                FleetMotionCommand.Stop());

            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.0);
            AssertTwist(Find(commands, 2), 0.0, 0.0, 0.0);
        }

        private static FleetMemberCommand Find(
            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 AssertTwist(
            FleetMemberCommand command,
            double expectedVx,
            double expectedVy,
            double expectedOmega)
        {
            AssertNear(
                command.TwistInVehicleBody.VxMetersPerSecond,
                expectedVx,
                command.VehicleId,
                "Vx");
            AssertNear(
                command.TwistInVehicleBody.VyMetersPerSecond,
                expectedVy,
                command.VehicleId,
                "Vy");
            AssertNear(
                command.TwistInVehicleBody.OmegaRadiansPerSecond,
                expectedOmega,
                command.VehicleId,
                "Omega");
        }

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