using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

internal static class CurvatureLimitPolicy
{
    internal static bool TryGetAllowedMaximumVehicleCurvaturePerMeter(
        VehicleParameters vehicle,
        double radiusToleranceMeters,
        out double allowedMaximumCurvaturePerMeter)
    {
        allowedMaximumCurvaturePerMeter = 0d;
        if (!NumericGuard.IsFinite(radiusToleranceMeters) || radiusToleranceMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double nominalMaximumCurvature))
        {
            return false;
        }

        double nominalMinimumRadius = 1d / nominalMaximumCurvature;
        double toleratedMinimumRadius = nominalMinimumRadius - radiusToleranceMeters;
        if (!NumericGuard.IsPositiveFinite(toleratedMinimumRadius)) return false;

        allowedMaximumCurvaturePerMeter = 1d / toleratedMinimumRadius;
        return NumericGuard.IsPositiveFinite(allowedMaximumCurvaturePerMeter);
    }
}