using MyParking.Shared;

namespace MultiWheelC.Fleet
{
    // 一次经过校验的虚拟车队原点状态快照。
    public readonly struct FleetState
    {
        public FleetState(
            double sampleTimestampSeconds,
            Pose2D fleetPoseInWorld,
            Twist2D twistAtFleetOriginInWorld,
            bool hasValidVelocityEstimate)
        {
            NumericGuard.EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));
            NumericGuard.EnsureFinite(
                fleetPoseInWorld,
                nameof(fleetPoseInWorld));
            NumericGuard.EnsureFinite(
                twistAtFleetOriginInWorld,
                nameof(twistAtFleetOriginInWorld));

            SampleTimestampSeconds =
                sampleTimestampSeconds;
            FleetPoseInWorld = new Pose2D(
                fleetPoseInWorld.XMeters,
                fleetPoseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    fleetPoseInWorld.YawRadians));
            HasValidVelocityEstimate =
                hasValidVelocityEstimate;

            // 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
            TwistAtFleetOriginInWorld =
                hasValidVelocityEstimate
                    ? twistAtFleetOriginInWorld
                    : Twist2D.Zero;

            var worldPoseInFleet =
                FrameTransform2D.Inverse(
                    FleetPoseInWorld);
            TwistAtFleetOriginInFleet =
                FrameTransform2D.TransformTwistAtSamePoint(
                    worldPoseInFleet,
                    TwistAtFleetOriginInWorld);
        }

        // 状态源单调时钟中的采样时刻，单位为s。
        public double SampleTimestampSeconds { get; }

        // 车队坐标系原点在世界坐标系中的实际位姿。
        public Pose2D FleetPoseInWorld { get; }

        // 车队原点处的实际刚体速度，在世界坐标系中表达。
        public Twist2D TwistAtFleetOriginInWorld { get; }

        // 同一刚体速度在车队坐标系中表达，供车队控制器使用。
        public Twist2D TwistAtFleetOriginInFleet { get; }

        // 速度是否已经初始化并可用于闭环控制。
        public bool HasValidVelocityEstimate { get; }
    }
}