using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class EmPlanningRequestValidationResult
{
    internal EmPlanningRequestValidationResult(EmPlanningStatus status, string failureReason, EmPlanningRequestSnapshot snapshot)
    {
        Status = status;
        FailureReason = failureReason ?? string.Empty;
        Snapshot = snapshot;
    }

    public EmPlanningStatus Status { get; }

    public string FailureReason { get; }

    public bool IsValid { get { return Status == EmPlanningStatus.Success; } }

    internal EmPlanningRequestSnapshot Snapshot { get; }
}

internal sealed class EmPlanningRequestSnapshot
{
    public EmPlanningRequestSnapshot(EmPlanningRequest request, EmPlannerConfiguration configuration)
    {
        Request = request;
        Configuration = configuration;
    }

    public EmPlanningRequest Request { get; }

    public EmPlannerConfiguration Configuration { get; }
}

public static class EmPlanningRequestValidator
{
    public static EmPlanningRequestValidationResult Validate(EmPlanningRequest request)
    {
        if (request == null)
            return Invalid("Request is required.");
        if (request.ReferencePath == null || request.Map == null || request.Vehicle == null || request.VehicleState == null || request.Configuration == null)
            return Invalid("Request members ReferencePath, Map, Vehicle, VehicleState, and Configuration are required.");
        if (request.MotionModel == EmMotionModel.CrabTranslation || request.MotionModel == EmMotionModel.InPlaceRotation)
            return Result(EmPlanningStatus.UnsupportedMotionMode, "Only nonholonomic forward/reverse motion is supported.");
        if (request.MotionModel != EmMotionModel.NonholonomicForwardReverse)
            return Invalid("Motion model is invalid.");
        if (!Enum.IsDefined(typeof(EmPlanningScope), request.PlanningScope))
            return Invalid("Planning scope is invalid.");
        if (!TryValidateConfiguration(request.Configuration, request.PlanningScope, out string configurationFailure))
            return Invalid(configurationFailure);
        if (!request.Map.PlanningReady)
            return Invalid("Planning map is not ready.");
        if (!IsConsumable(request.ReferencePath.Status))
            return Result(EmPlanningStatus.InvalidReferencePath, "Reference path status is not consumable.");
        if (request.ReferencePath.Path == null || request.ReferencePath.Segments == null ||
            request.SegmentIndex < 0 || request.SegmentIndex >= request.ReferencePath.Segments.Count)
            return Result(EmPlanningStatus.InvalidReferencePath, "Requested direction segment is out of range.");
        if (!IsValidVehicleState(request.VehicleState))
            return Invalid("Vehicle state contains invalid values.");
        if (!IsValidVehicle(request.Vehicle))
            return Invalid("Vehicle geometry or curvature limit is invalid.");
        if (request.RequestedAtUtc - request.VehicleState.CapturedAtUtc >
            TimeSpan.FromSeconds(request.Configuration.Scheduling.MaximumVehicleStateAgeSeconds))
            return Result(EmPlanningStatus.StaleVehicleState, "Vehicle state is stale.");

        return new EmPlanningRequestValidationResult(
            EmPlanningStatus.Success,
            string.Empty,
            new EmPlanningRequestSnapshot(request, request.Configuration.Copy()));
    }

    private static EmPlanningRequestValidationResult Invalid(string reason)
    {
        return Result(EmPlanningStatus.InvalidInput, reason);
    }

    private static EmPlanningRequestValidationResult Result(EmPlanningStatus status, string reason)
    {
        return new EmPlanningRequestValidationResult(status, reason, null);
    }

    private static bool IsConsumable(PathSmoothingStatus status)
    {
        return status == PathSmoothingStatus.Complete ||
            status == PathSmoothingStatus.PartialImprovement ||
            status == PathSmoothingStatus.NotNeeded ||
            status == PathSmoothingStatus.Unchanged;
    }

    private static bool IsValidVehicleState(VehicleMotionState state)
    {
        return state.Pose != null &&
            NumericGuard.IsFinite(state.Pose.X) &&
            NumericGuard.IsFinite(state.Pose.Y) &&
            NumericGuard.IsFinite(state.Pose.Heading) &&
            NumericGuard.IsFinite(state.SignedLongitudinalSpeedMetersPerSecond) &&
            (!state.LongitudinalAccelerationMetersPerSecondSquared.HasValue || NumericGuard.IsFinite(state.LongitudinalAccelerationMetersPerSecondSquared.Value)) &&
            state.SequenceId >= 0;
    }

    private static bool IsValidVehicle(MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters vehicle)
    {
        return NumericGuard.IsPositiveFinite(vehicle.LengthMeters) &&
            NumericGuard.IsPositiveFinite(vehicle.WidthMeters) &&
            NumericGuard.IsFinite(vehicle.SafetyMarginMeters) && vehicle.SafetyMarginMeters >= 0d &&
            VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _);
    }

    private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, EmPlanningScope planningScope,
        out string failureReason)
    {
        failureReason = "Configuration is invalid.";
        if (configuration.Scheduling == null || configuration.Corridor == null || configuration.Frenet == null ||
            configuration.Lateral == null || configuration.Longitudinal == null || configuration.Solver == null ||
            configuration.Validation == null || configuration.Lateral.Weights == null || configuration.Longitudinal.Weights == null)
            return false;

        SchedulingConfiguration scheduling = configuration.Scheduling;
        CorridorConfiguration corridor = configuration.Corridor;
        FrenetConfiguration frenet = configuration.Frenet;
        LateralConfiguration lateral = configuration.Lateral;
        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
        SolverConfiguration solver = configuration.Solver;
        ValidationConfiguration validation = configuration.Validation;

        if (!Positive(scheduling.ReplanPeriodSeconds) || !Positive(scheduling.TimeHorizonSeconds) || !Positive(scheduling.DistanceHorizonMeters) ||
            !Positive(scheduling.OutputTimeStepSeconds) || !Positive(scheduling.SolverTimeoutSeconds) || !Positive(scheduling.HandoffLookaheadSeconds) || !Positive(scheduling.MaximumVehicleStateAgeSeconds) ||
            !Positive(corridor.LongitudinalSampleSpacingMeters) || !Positive(corridor.LateralSampleSpacingMeters) || !Positive(corridor.MaximumLateralOffsetMeters) ||
            !NonNegative(corridor.AdditionalClearanceReserveMeters) || !Positive(corridor.MaximumCollisionCheckStepMeters) ||
            !Positive(frenet.MaximumProjectionDistanceMeters) || !NumericGuard.IsFinite(frenet.MinimumFrenetDenominator) ||
            frenet.MinimumFrenetDenominator <= 0d || frenet.MinimumFrenetDenominator >= 1d || !Positive(frenet.BoundaryAnchorToleranceMeters) ||
            !Positive(lateral.MaximumLateralStepPerIterationMeters) || !Positive(lateral.MaximumLateralSlope) ||
            !Positive(lateral.MaximumLateralSecondDerivativePerMeter) || !Positive(lateral.MaximumLateralThirdDerivativePerSquareMeter) ||
            !Positive(longitudinal.MaximumForwardSpeedMetersPerSecond) || !Positive(longitudinal.MaximumReverseSpeedMetersPerSecond) ||
            !Positive(longitudinal.DesiredForwardSpeedMetersPerSecond) ||
            longitudinal.DesiredForwardSpeedMetersPerSecond > longitudinal.MaximumForwardSpeedMetersPerSecond ||
            !Positive(longitudinal.DesiredReverseSpeedMetersPerSecond) ||
            longitudinal.DesiredReverseSpeedMetersPerSecond > longitudinal.MaximumReverseSpeedMetersPerSecond ||
            !Positive(longitudinal.MaximumAccelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
            !Positive(longitudinal.MaximumJerkMetersPerSecondCubed) || !Positive(longitudinal.MaximumLateralAccelerationMetersPerSecondSquared) ||
            !Positive(longitudinal.MaximumCurvatureRatePerMeterPerSecond) || !NonNegative(longitudinal.StopSpeedToleranceMetersPerSecond) ||
            !Positive(longitudinal.ZeroSpeedHoldSeconds) || solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
            !Positive(solver.AbsoluteTolerance) || !Positive(solver.RelativeTolerance) || !Positive(solver.StrictResidualTolerance) ||
            !Positive(validation.SpatialToleranceMeters) || !Positive(validation.KinematicTolerance) ||
            !Positive(scheduling.MaximumOptimizationTimeStepSeconds) ||
            !Positive(scheduling.MaximumOptimizationSpatialStepMeters) ||
            scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 ||
            !Positive(validation.TerminalPositionToleranceMeters) ||
            !Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI ||
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;

        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
                longitudinal.MaximumAccelerationMetersPerSecondSquared,
                longitudinal.MaximumDecelerationMetersPerSecondSquared,
                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
                out _) ||
            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
                longitudinal.MaximumAccelerationMetersPerSecondSquared,
                longitudinal.MaximumDecelerationMetersPerSecondSquared,
                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
                out _))
        {
            failureReason = "Configuration cannot construct the jerk-limited stopping model.";
            return false;
        }

        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
            longitudinal.MaximumReverseSpeedMetersPerSecond);
        double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
            maximumSpeed * scheduling.ReplanPeriodSeconds;
        if (planningScope == EmPlanningScope.RollingHorizon &&
            scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
        {
            failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
                ";configured=" + scheduling.DistanceHorizonMeters;
            return false;
        }

        try
        {
            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
                scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
            LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
                scheduling.OutputTimeStepSeconds);
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return false;
        }

        return true;
    }

    private static bool Positive(double value) { return NumericGuard.IsPositiveFinite(value); }

    private static bool NonNegative(double value) { return NumericGuard.IsFinite(value) && value >= 0d; }

    private static bool WeightsAreValid(LateralWeights weights)
    {
        return NonNegative(weights.ReferenceOffset) && NonNegative(weights.HeadingDeviation) &&
            NonNegative(weights.SecondDerivative) && NonNegative(weights.ThirdDerivative) &&
            NonNegative(weights.Curvature) && NonNegative(weights.CurvatureVariation) &&
            NonNegative(weights.PreviousTrajectory) && NonNegative(weights.RollingTerminal);
    }

    private static bool WeightsAreValid(LongitudinalWeights weights)
    {
        return NonNegative(weights.ReferenceSpeed) && NonNegative(weights.Acceleration) && NonNegative(weights.Jerk) &&
            NonNegative(weights.PreviousTrajectory) && NonNegative(weights.TerminalAcceleration);
    }
}