using System;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
internal sealed class CurvatureTransition
{
    /// <summary>记录同一方向段内相邻粗路径采样点之间的车辆曲率跳变。</summary>
    /// <param name="segmentIndex">所属方向段在预处理路径中的从零开始索引。</param>
    /// <param name="leftCoarsePathIndex">跳变左侧粗路径采样点索引。</param>
    /// <param name="rightCoarsePathIndex">跳变右侧粗路径采样点索引，必须等于左索引加一。</param>
    /// <param name="localArcLengthMeters">跳变位置在方向段内的局部弧长，单位 m。</param>
    /// <param name="x">跳变位置世界 X 坐标，单位 m。</param>
    /// <param name="y">跳变位置世界 Y 坐标，单位 m。</param>
    /// <param name="vehicleHeadingRadians">跳变位置的车辆航向，单位 rad。</param>
    /// <param name="leftVehicleCurvaturePerMeter">左侧车辆曲率，单位 1/m。</param>
    /// <param name="rightVehicleCurvaturePerMeter">右侧车辆曲率，单位 1/m。</param>
    internal CurvatureTransition(
        int segmentIndex,
        int leftCoarsePathIndex,
        int rightCoarsePathIndex,
        double localArcLengthMeters,
        double x,
        double y,
        double vehicleHeadingRadians,
        double leftVehicleCurvaturePerMeter,
        double rightVehicleCurvaturePerMeter)
    {
        if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
            throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
        if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
            !NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
            !NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
        {
            throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
        }

        double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
        if (!NumericGuard.IsFinite(curvatureJumpPerMeter))
            throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter));

        SegmentIndex = segmentIndex;
        LeftCoarsePathIndex = leftCoarsePathIndex;
        RightCoarsePathIndex = rightCoarsePathIndex;
        LocalArcLengthMeters = localArcLengthMeters;
        X = x;
        Y = y;
        VehicleHeadingRadians = vehicleHeadingRadians;
        LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
        RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
        CurvatureJumpPerMeter = curvatureJumpPerMeter;
    }

    internal int SegmentIndex { get; }
    internal int LeftCoarsePathIndex { get; }
    internal int RightCoarsePathIndex { get; }
    internal double LocalArcLengthMeters { get; }
    internal double X { get; }
    internal double Y { get; }
    internal double VehicleHeadingRadians { get; }
    internal double LeftVehicleCurvaturePerMeter { get; }
    internal double RightVehicleCurvaturePerMeter { get; }
    internal double CurvatureJumpPerMeter { get; }
}
