using System;
using System.Collections.Generic;

namespace MyParking.Shared
{
    // 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
    public static class FleetKinematics
    {
        public static IReadOnlyList<FleetMemberCommand> Decompose(
            FleetLayout layout,
            FleetMotionCommand command)
        {
            if (layout == null)
            {
                throw new ArgumentNullException(nameof(layout));
            }

            EnsureCommandIsFinite(command);

            var commands =
                new FleetMemberCommand[layout.VehicleCount];
            var referencePoint = command.ReferencePointInFleet;
            var referenceTwist = command.TwistAtReferencePoint;

            for (var index = 0; index < layout.VehicleCount; index++)
            {
                var vehicle = layout.Vehicles[index];
                var offsetXMeters =
                    vehicle.PoseInFleet.XMeters -
                    referencePoint.XMeters;
                var offsetYMeters =
                    vehicle.PoseInFleet.YMeters -
                    referencePoint.YMeters;

                var twistAtVehicleInFleet = new Twist2D(
                    referenceTwist.VxMetersPerSecond -
                    referenceTwist.OmegaRadiansPerSecond *
                    offsetYMeters,
                    referenceTwist.VyMetersPerSecond +
                    referenceTwist.OmegaRadiansPerSecond *
                    offsetXMeters,
                    referenceTwist.OmegaRadiansPerSecond);

                var fleetPoseInVehicle =
                    FrameTransform2D.Inverse(
                        vehicle.PoseInFleet);
                var twistInVehicleBody =
                    FrameTransform2D.TransformTwistAtSamePoint(
                        fleetPoseInVehicle,
                        twistAtVehicleInFleet);

                commands[index] = new FleetMemberCommand(
                    vehicle.VehicleId,
                    twistInVehicleBody);
            }

            return Array.AsReadOnly(commands);
        }

        private static void EnsureCommandIsFinite(
            FleetMotionCommand command)
        {
            NumericGuard.EnsureFinite(
                command.ReferencePointInFleet.XMeters,
                nameof(command.ReferencePointInFleet));
            NumericGuard.EnsureFinite(
                command.ReferencePointInFleet.YMeters,
                nameof(command.ReferencePointInFleet));
            NumericGuard.EnsureFinite(
                command.TwistAtReferencePoint,
                nameof(command.TwistAtReferencePoint));
        }
    }
}