using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 轨迹中的单个时间样本。世界位置单位 m、航向单位 rad、速度单位 m/s、曲率单位 1/m。
/// </summary>
public sealed class EmTrajectoryPoint
{
    /// <summary>
    /// 创建一个轨迹时间样本。所有浮点输入必须有限；时间、段内弧长和路径弧长必须非负，段索引和枚举值必须有效。派生的绝对速度、世界速度分量和偏航角速度由有符号纵向速度、航向和曲率计算。
    /// </summary>
    public EmTrajectoryPoint(
        double x,
        double y,
        double yaw,
        double signedLongitudinalVelocity,
        double timeFromStart,
        double vehicleCurvature,
        int segmentIndex,
        double segmentLocalS,
        double pathS,
        TravelDirection direction,
        EmBoundaryType boundaryType,
        double longitudinalAcceleration,
        double longitudinalJerk)
    {
        ContractNumeric.RequireFinite(x, nameof(x));
        ContractNumeric.RequireFinite(y, nameof(y));
        ContractNumeric.RequireFinite(yaw, nameof(yaw));
        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
        ContractNumeric.RequireFinite(pathS, nameof(pathS));
        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
        if (segmentIndex < 0)
            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (timeFromStart < 0d)
            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
        if (segmentLocalS < 0d)
            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
        if (pathS < 0d)
            throw new ArgumentOutOfRangeException(nameof(pathS));
        if (!Enum.IsDefined(typeof(TravelDirection), direction))
            throw new ArgumentOutOfRangeException(nameof(direction));
        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
            throw new ArgumentOutOfRangeException(nameof(boundaryType));

        X = x;
        Y = y;
        Yaw = yaw;
        SignedLongitudinalVelocity = signedLongitudinalVelocity;
        Speed = Math.Abs(signedLongitudinalVelocity);
        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
        YawRate = signedLongitudinalVelocity * vehicleCurvature;
        TimeFromStart = timeFromStart;
        VehicleCurvature = vehicleCurvature;
        SegmentIndex = segmentIndex;
        SegmentLocalS = segmentLocalS;
        PathS = pathS;
        Direction = direction;
        BoundaryType = boundaryType;
        LongitudinalAcceleration = longitudinalAcceleration;
        LongitudinalJerk = longitudinalJerk;
    }

    /// <summary>
    /// 世界坐标系 X 位置，单位 m。
    /// </summary>
    public double X { get; }

    /// <summary>
    /// 世界坐标系 Y 位置，单位 m。
    /// </summary>
    public double Y { get; }

    /// <summary>
    /// 车辆在世界坐标系中的航向，单位 rad；构造器仅要求有限，不归一化角度。
    /// </summary>
    public double Yaw { get; }

    /// <summary>
    /// 沿车辆前向轴的有符号纵向速度，单位 m/s；符号表示前进或倒车。
    /// </summary>
    public double SignedLongitudinalVelocity { get; }

    /// <summary>
    /// 有符号纵向速度的绝对值，单位 m/s。
    /// </summary>
    public double Speed { get; }

    /// <summary>
    /// 由有符号纵向速度和航向导出的世界坐标 X 速度分量，单位 m/s。
    /// </summary>
    public double VelocityX { get; }

    /// <summary>
    /// 由有符号纵向速度和航向导出的世界坐标 Y 速度分量，单位 m/s。
    /// </summary>
    public double VelocityY { get; }

    /// <summary>
    /// 由有符号纵向速度乘车辆曲率导出的偏航角速度，单位 rad/s。
    /// </summary>
    public double YawRate { get; }

    /// <summary>
    /// 从本条轨迹开始执行起累计的非负时间，单位 s。
    /// </summary>
    public double TimeFromStart { get; }

    /// <summary>
    /// 车辆路径曲率，单位 1/m；符号约定由上游几何计算定义。
    /// </summary>
    public double VehicleCurvature { get; }

    /// <summary>
    /// 此点所属的非负方向段索引。
    /// </summary>
    public int SegmentIndex { get; }

    /// <summary>
    /// 从该方向段开始沿参考路径累计的非负弧长，单位 m。
    /// </summary>
    public double SegmentLocalS { get; }

    /// <summary>
    /// 从整条参考路径开始累计的非负弧长，单位 m。
    /// </summary>
    public double PathS { get; }

    /// <summary>
    /// 此点的已验证行驶方向。
    /// </summary>
    public TravelDirection Direction { get; }

    /// <summary>
    /// 此点的已验证边界语义；普通内部点使用 <see cref="EmBoundaryType.None"/>。
    /// </summary>
    public EmBoundaryType BoundaryType { get; }

    /// <summary>
    /// 沿车辆前向轴的纵向加速度，单位 m/s²；仅供同程序集的轨迹验证和执行逻辑读取。
    /// </summary>
    internal double LongitudinalAcceleration { get; }

    /// <summary>
    /// 沿车辆前向轴的纵向加加速度，单位 m/s³；仅供同程序集的轨迹验证和执行逻辑读取。
    /// </summary>
    internal double LongitudinalJerk { get; }
}