using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 描述按执行点序和累计弧长参数化的车体中心参考轨迹点，统一使用SI单位。
    /// </summary>
    public readonly struct TrajectoryPoint
    {
        /// <summary>
        /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
        /// </summary>
        public TrajectoryPoint(
            double arcLengthMeters,
            Pose2D poseInWorld,
            double curvaturePerMeter,
            double referenceSpeedMetersPerSecond)
        {
            NumericGuard.EnsureFiniteNonNegative(
                arcLengthMeters,
                nameof(arcLengthMeters));
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.EnsureFinite(
                curvaturePerMeter,
                nameof(curvaturePerMeter));
            NumericGuard.EnsureFinite(
                referenceSpeedMetersPerSecond,
                nameof(referenceSpeedMetersPerSecond));
            ArcLengthMeters = arcLengthMeters;
            PoseInWorld = new Pose2D(
                poseInWorld.XMeters,
                poseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    poseInWorld.YawRadians));
            CurvaturePerMeter = curvaturePerMeter;
            ReferenceSpeedMetersPerSecond =
                referenceSpeedMetersPerSecond;
        }

        /// <summary>
        /// 获取沿预定执行点序从轨迹起点累计到当前点的弧长，单位为m。
        /// </summary>
        public double ArcLengthMeters { get; }

        /// <summary>
        /// 获取车体中心参考坐标系在世界坐标系中的位姿；航向始终表示车头方向。
        /// </summary>
        public Pose2D PoseInWorld { get; }

        /// <summary>
        /// 获取沿累计弧长增加方向的车体中心参考轨迹曲率，单位为1/m，左弯为正。
        /// </summary>
        public double CurvaturePerMeter { get; }

        /// <summary>
        /// 获取车体纵向有符号参考速度，单位为m/s；正值前进、负值倒车、零值停车。
        /// </summary>
        public double ReferenceSpeedMetersPerSecond { get; }

    }
}