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

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationSettings
{
    public EmPlanningScope PlanningScope { get; set; } = EmPlanningScope.FullDirectionSegment;
    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 SolverTimeoutSeconds { get; set; } = 5d;
    public int MaximumOsqpIterations { get; set; } = 100000;
    /// <summary>仅滚动视窗兼容字段；完整方向段模式不用于截断。单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。</summary>
    public double TimeHorizonSeconds { get; set; } = 2d;
    /// <summary>发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。</summary>
    public double OutputTimeStepSeconds { get; set; } = 0.10d;
    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 bool EnableWebVisualization { get; set; } = true;
    public bool AutoOpenWebVisualization { get; set; } = true;
    public int WebVisualizationPort { get; set; } = 0;
    public double WebRefreshRateHz { get; set; } = 10d;
    public int VisualizationHistoryCycleLimit { get; set; } = 60;
    public bool EnableNativePainterVisualization { get; set; } = false;
    public double DirectionConfirmationSpeedMetersPerSecond { get; set; } = 0.02d;
    public int DirectionConfirmationSamples { get; set; } = 3;
    public double GearSwitchProjectionToleranceMeters { get; set; } = 0.50d;
    public double GearSwitchStopHoldSeconds { get; set; } = 0.20d;

    public TrajectoryObservationSettings CreateValidatedSnapshot()
    {
        var snapshot = new TrajectoryObservationSettings
        {
            PlanningScope = PlanningScope,
            MapPaddingMeters = MapPaddingMeters,
            MapResolutionMillimeters = MapResolutionMillimeters,
            ReplanPeriodSeconds = ReplanPeriodSeconds,
            ObserverPeriodSeconds = ObserverPeriodSeconds,
            SolverTimeoutSeconds = SolverTimeoutSeconds,
            MaximumOsqpIterations = MaximumOsqpIterations,
            TimeHorizonSeconds = TimeHorizonSeconds,
            OutputTimeStepSeconds = OutputTimeStepSeconds,
            VehicleLengthMeters = VehicleLengthMeters,
            VehicleWidthMeters = VehicleWidthMeters,
            SafetyMarginMeters = SafetyMarginMeters,
            MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
            EnableWebVisualization = EnableWebVisualization,
            AutoOpenWebVisualization = AutoOpenWebVisualization,
            WebVisualizationPort = WebVisualizationPort,
            WebRefreshRateHz = WebRefreshRateHz,
            VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
            EnableNativePainterVisualization = EnableNativePainterVisualization,
            DirectionConfirmationSpeedMetersPerSecond = DirectionConfirmationSpeedMetersPerSecond,
            DirectionConfirmationSamples = DirectionConfirmationSamples,
            GearSwitchProjectionToleranceMeters = GearSwitchProjectionToleranceMeters,
            GearSwitchStopHoldSeconds = GearSwitchStopHoldSeconds,
        };
        snapshot.Validate();
        return snapshot;
    }

    public void Validate()
    {
        if (!Enum.IsDefined(typeof(EmPlanningScope), PlanningScope))
            throw new ArgumentOutOfRangeException(nameof(PlanningScope), "Value must be a defined planning scope.");
        EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
        EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
        EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));
        EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds));
        EnsurePositiveFinite(SolverTimeoutSeconds, nameof(SolverTimeoutSeconds));
        if (MaximumOsqpIterations <= 0)
            throw new ArgumentOutOfRangeException(nameof(MaximumOsqpIterations), "Value must be positive.");
        EnsurePositiveFinite(TimeHorizonSeconds, nameof(TimeHorizonSeconds));
        EnsurePositiveFinite(OutputTimeStepSeconds, nameof(OutputTimeStepSeconds));
        if (OutputTimeStepSeconds > TimeHorizonSeconds)
            throw new ArgumentOutOfRangeException(nameof(OutputTimeStepSeconds),
                "The trajectory timestamp spacing cannot exceed the ST time horizon.");
        EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters));
        EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters));
        EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters));
        EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter));
        if (WebVisualizationPort != 0 && (WebVisualizationPort < 1024 || WebVisualizationPort > 65535))
            throw new ArgumentOutOfRangeException(nameof(WebVisualizationPort),
                "Port must be zero or in the inclusive range 1024 through 65535.");
        EnsurePositiveFinite(WebRefreshRateHz, nameof(WebRefreshRateHz));
        if (VisualizationHistoryCycleLimit <= 0)
            throw new ArgumentOutOfRangeException(nameof(VisualizationHistoryCycleLimit), "Value must be positive.");
        EnsurePositiveFinite(DirectionConfirmationSpeedMetersPerSecond,
            nameof(DirectionConfirmationSpeedMetersPerSecond));
        if (DirectionConfirmationSamples <= 0)
            throw new ArgumentOutOfRangeException(nameof(DirectionConfirmationSamples), "Value must be positive.");
        EnsurePositiveFinite(GearSwitchProjectionToleranceMeters, nameof(GearSwitchProjectionToleranceMeters));
        EnsurePositiveFinite(GearSwitchStopHoldSeconds, nameof(GearSwitchStopHoldSeconds));
    }

    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.");
    }
}