using System;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
internal sealed class CurvatureTransition
{
    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));
        }

        SegmentIndex = segmentIndex;
        LeftCoarsePathIndex = leftCoarsePathIndex;
        RightCoarsePathIndex = rightCoarsePathIndex;
        LocalArcLengthMeters = localArcLengthMeters;
        X = x;
        Y = y;
        VehicleHeadingRadians = vehicleHeadingRadians;
        LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
        RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
        CurvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
    }

    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; }
}