using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationSettings
{
    public double MapPaddingMeters { get; set; } = 2d;
    public float MapResolutionMillimeters { get; set; } = 50f;
    public double ReplanPeriodSeconds { get; set; } = 0.20d;
    public double ObserverPeriodSeconds { get; set; } = 0.05d;
    public double VehicleLengthMeters { get; set; } = 0.80d;
    public double VehicleWidthMeters { get; set; } = 0.60d;
    public double SafetyMarginMeters { get; set; } = 0.05d;
    public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;

    public TrajectoryObservationSettings CreateValidatedSnapshot()
    {
        var snapshot = new TrajectoryObservationSettings
        {
            MapPaddingMeters = MapPaddingMeters,
            MapResolutionMillimeters = MapResolutionMillimeters,
            ReplanPeriodSeconds = ReplanPeriodSeconds,
            ObserverPeriodSeconds = ObserverPeriodSeconds,
            VehicleLengthMeters = VehicleLengthMeters,
            VehicleWidthMeters = VehicleWidthMeters,
            SafetyMarginMeters = SafetyMarginMeters,
            MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
        };
        snapshot.Validate();
        return snapshot;
    }

    public void Validate()
    {
        EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
        EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
        EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));
        EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds));
        EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters));
        EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters));
        EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters));
        EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter));
    }

    public VehicleParameters CreateVehicle()
    {
        Validate();
        return new VehicleParameters
        {
            LengthMeters = VehicleLengthMeters,
            WidthMeters = VehicleWidthMeters,
            SafetyMarginMeters = SafetyMarginMeters,
            MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
        };
    }

    private static void EnsurePositiveFinite(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d)
            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite and positive.");
    }
}

public sealed class TrajectoryObservationObstacle
{
    private readonly bool _isCircle;
    private readonly double _first;
    private readonly double _second;
    private readonly double _third;
    private readonly double _fourth;

    private TrajectoryObservationObstacle(bool isCircle, double first, double second, double third, double fourth)
    {
        _isCircle = isCircle;
        _first = first;
        _second = second;
        _third = third;
        _fourth = fourth;
    }

    public static TrajectoryObservationObstacle Circle(double centerXMillimeters, double centerYMillimeters,
        double radiusMillimeters)
    {
        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
        return new TrajectoryObservationObstacle(true, centerXMillimeters, centerYMillimeters, radiusMillimeters, 0d);
    }

    public static TrajectoryObservationObstacle Rectangle(double xMinMillimeters, double xMaxMillimeters,
        double yMinMillimeters, double yMaxMillimeters)
    {
        EnsureFinite(xMinMillimeters, nameof(xMinMillimeters));
        EnsureFinite(xMaxMillimeters, nameof(xMaxMillimeters));
        EnsureFinite(yMinMillimeters, nameof(yMinMillimeters));
        EnsureFinite(yMaxMillimeters, nameof(yMaxMillimeters));
        if (xMaxMillimeters <= xMinMillimeters || yMaxMillimeters <= yMinMillimeters)
            throw new ArgumentOutOfRangeException(nameof(xMaxMillimeters), "Rectangle bounds must be non-degenerate.");
        return new TrajectoryObservationObstacle(false, xMinMillimeters, xMaxMillimeters, yMinMillimeters, yMaxMillimeters);
    }

    public MapBoundsMm GetBounds()
    {
        return _isCircle
            ? new MapBoundsMm(ToFiniteFloat(_first - _third), ToFiniteFloat(_first + _third),
                ToFiniteFloat(_second - _third), ToFiniteFloat(_second + _third))
            : new MapBoundsMm(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third), ToFiniteFloat(_fourth));
    }

    public IMapObstacle ToMapObstacle()
    {
        return _isCircle
            ? new CircleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third))
            : new AxisAlignedRectangleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second),
                ToFiniteFloat(_third), ToFiniteFloat(_fourth));
    }

    private static float ToFiniteFloat(double value)
    {
        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(value), "Value cannot be represented as a finite millimeter coordinate.");
        return (float)value;
    }

    private static void EnsureFinite(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
    }

    private static void EnsurePositiveFinite(double value, string parameterName)
    {
        EnsureFinite(value, parameterName);
        if (value <= 0d) throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
    }
}

public static class TrajectoryObservationSetupFactory
{
    public static CoarsePathPlanningJob CreateBootstrapJob(Pose2D start, Pose2D goal,
        TrajectoryObservationSettings settings, IReadOnlyList<TrajectoryObservationObstacle> obstacles,
        long obstacleSnapshotVersion)
    {
        if (start == null) throw new ArgumentNullException(nameof(start));
        if (goal == null) throw new ArgumentNullException(nameof(goal));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
        ValidatePose(start, nameof(start));
        ValidatePose(goal, nameof(goal));
        settings.Validate();

        double padMm = settings.MapPaddingMeters * 1000d;
        var bounds = new MapBoundsMm(
            ToGridLower(Math.Min(start.X, goal.X) * 1000d - padMm, settings.MapResolutionMillimeters),
            ToGridUpper(Math.Max(start.X, goal.X) * 1000d + padMm, settings.MapResolutionMillimeters),
            ToGridLower(Math.Min(start.Y, goal.Y) * 1000d - padMm, settings.MapResolutionMillimeters),
            ToGridUpper(Math.Max(start.Y, goal.Y) * 1000d + padMm, settings.MapResolutionMillimeters));

        var mapObstacles = new List<IMapObstacle>(obstacles.Count);
        for (int index = 0; index < obstacles.Count; index++)
        {
            TrajectoryObservationObstacle obstacle = obstacles[index] ?? throw new ArgumentNullException(nameof(obstacles));
            MapBoundsMm obstacleBounds = obstacle.GetBounds();
            if (obstacleBounds.XMin < bounds.XMin || obstacleBounds.XMax > bounds.XMax ||
                obstacleBounds.YMin < bounds.YMin || obstacleBounds.YMax > bounds.YMax)
                throw new ArgumentOutOfRangeException(nameof(obstacles), "Obstacle envelope must fit within map bounds.");
            mapObstacles.Add(obstacle.ToMapObstacle());
        }

        IReadOnlyList<IMapObstacleSource> sources;
        bool allowExplicitEmptyMap = mapObstacles.Count == 0;
        if (allowExplicitEmptyMap)
            sources = Array.Empty<IMapObstacleSource>();
        else
        {
            if (obstacleSnapshotVersion <= 0L)
                throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
            sources = new IMapObstacleSource[]
            {
                new ManualObstacleSource("trajectory-observer-manual", obstacleSnapshotVersion, true, mapObstacles),
            };
        }

        return new CoarsePathPlanningJob
        {
            MapRequest = new PlanningMapRequest
            {
                Bounds = bounds,
                ResolutionMm = settings.MapResolutionMillimeters,
                ObstacleSources = sources,
                AllowExplicitEmptyMap = allowExplicitEmptyMap,
            },
            Start = start,
            Goal = goal,
            Vehicle = settings.CreateVehicle(),
            Configuration = new HybridAStarConfiguration(),
            StartDirection = null,
            GoalDirection = GoalDirectionConstraint.Any,
        };
    }

    private static float ToGridLower(double millimeters, float resolutionMm)
    {
        return ToFiniteFloat(Math.Floor(millimeters / resolutionMm) * resolutionMm, nameof(millimeters));
    }

    private static float ToGridUpper(double millimeters, float resolutionMm)
    {
        return ToFiniteFloat(Math.Ceiling(millimeters / resolutionMm) * resolutionMm, nameof(millimeters));
    }

    private static float ToFiniteFloat(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
            throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
        return (float)value;
    }

    private static void ValidatePose(Pose2D pose, string parameterName)
    {
        if (double.IsNaN(pose.X) || double.IsInfinity(pose.X) || double.IsNaN(pose.Y) || double.IsInfinity(pose.Y) ||
            double.IsNaN(pose.Heading) || double.IsInfinity(pose.Heading))
            throw new ArgumentOutOfRangeException(parameterName, "Pose values must be finite.");
    }
}