using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
    /// </summary>
    public readonly struct VehicleState
    {
        /// <summary>
        /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
        /// </summary>
        public VehicleState(
            double sampleTimestampSeconds,
            Pose2D poseInWorld,
            Twist2D twistInWorld,
            bool hasValidVelocityEstimate)
        {
            NumericGuard.EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.EnsureFinite(
                twistInWorld,
                nameof(twistInWorld));

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

            // 第一帧或定位重置后的速度不可用于闭环控制，
            // 此时显式置零，避免调用方误用残留速度。
            TwistInWorld = hasValidVelocityEstimate
                ? twistInWorld
                : Twist2D.Zero;

            var worldPoseInBody =
                FrameTransform2D.Inverse(
                    PoseInWorld);
            TwistInBody =
                FrameTransform2D.TransformTwistAtSamePoint(
                    worldPoseInBody,
                    TwistInWorld);
        }

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

        /// <summary>
        /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
        /// </summary>
        public Pose2D PoseInWorld { get; }

        /// <summary>
        /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
        /// </summary>
        public Twist2D TwistInWorld { get; }

        /// <summary>
        /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
        /// </summary>
        public Twist2D TwistInBody { get; }

        /// <summary>
        /// 获取当前速度估计是否已经初始化并可用于闭环控制。
        /// </summary>
        public bool HasValidVelocityEstimate { get; }

    }
}