using System;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public enum EmTrajectoryValidationFailure
{
    None,
    InvalidInput,
    NonFinite,
    TimeNotStrictlyIncreasing,
    PathSDecreased,
    SegmentBoundaryExceeded,
    MissingTerminalAnchor,
    TerminalSpeedNotZero,
    TerminalAccelerationNotZero,
    TerminalYawRateNotZero,
    TerminalPoseMismatch,
    DirectionMismatch,
    DirectionSignMismatch,
    RedundantSpeedMismatch,
    WorldVelocityMismatch,
    YawRateMismatch,
    SpeedLimitExceeded,
    AccelerationLimitExceeded,
    JerkLimitExceeded,
    CurvatureLimitExceeded,
    CurvatureRateLimitExceeded,
    PoseCollision,
    SweptCollision,
}

public sealed class EmTrajectoryValidationResult
{
    private EmTrajectoryValidationResult(EmTrajectoryValidationFailure failure, int pointIndex, string message)
    {
        Failure = failure;
        PointIndex = pointIndex;
        Message = message ?? string.Empty;
    }

    public bool IsValid { get { return Failure == EmTrajectoryValidationFailure.None; } }

    public EmTrajectoryValidationFailure Failure { get; }

    public int PointIndex { get; }

    public string Message { get; }

    internal static EmTrajectoryValidationResult Success()
    {
        return new EmTrajectoryValidationResult(EmTrajectoryValidationFailure.None, -1, string.Empty);
    }

    internal static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int pointIndex,
        string message)
    {
        return new EmTrajectoryValidationResult(failure, pointIndex, message);
    }
}

/// <summary>Independently checks the public world-space trajectory before it can be published.</summary>
public sealed class EmTrajectoryValidator
{
    private const double MaximumSweptCollisionStepMeters = 0.025d;
    private readonly FootprintCollisionChecker collisionChecker;

    public EmTrajectoryValidator()
        : this(new FootprintCollisionChecker())
    {
    }

    public EmTrajectoryValidator(FootprintCollisionChecker collisionChecker)
    {
        this.collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
    {
        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null,
            terminalBoundary, false);
    }

    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary)
    {
        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose,
            terminalBoundary, true);
    }

    private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary, bool requiresTerminalPose)
    {
        if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
            configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
            !IsFinite(pathUpperBoundS) || pathUpperBoundS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
            !TryReadLimits(configuration, vehicle, trajectory.Metadata.Direction, out ValidationLimits limits))
        {
            return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
                "Trajectory publication inputs or validation limits are invalid.");
        }

        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point == null || !HasOnlyFiniteValues(point))
                return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
                point.PathS > pathUpperBoundS + limits.SpatialTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,
                    "Trajectory point lies outside the current direction segment.");
            }
            if (index == 0)
                continue;

            EmTrajectoryPoint previous = trajectory.Points[index - 1];
            if (point.TimeFromStart <= previous.TimeFromStart)
                return Reject(EmTrajectoryValidationFailure.TimeNotStrictlyIncreasing, index,
                    "Trajectory time must be strictly increasing.");
            if (point.PathS + limits.SpatialTolerance < previous.PathS ||
                point.SegmentLocalS + limits.SpatialTolerance < previous.SegmentLocalS)
            {
                return Reject(EmTrajectoryValidationFailure.PathSDecreased, index,
                    "Trajectory PathS must not decrease.");
            }
        }

        int terminalIndex = -1;
        if (trajectory.Metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary)
        {
            terminalIndex = FindTerminalAnchor(trajectory, pathUpperBoundS, terminalBoundary, limits.SpatialTolerance);
            if (terminalIndex < 0)
            {
                return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
                    FindFirstTerminalPathIndex(trajectory, pathUpperBoundS, limits.SpatialTolerance),
                    "Trajectory does not contain the exact terminal boundary anchor.");
            }

            EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
            if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance)
                return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex,
                    "Terminal signed speed must be zero.");
            if (Math.Abs(terminal.LongitudinalAcceleration) > limits.KinematicTolerance)
                return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex,
                    "Terminal stored acceleration must be zero.");
            if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance)
                return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
                    "Terminal yaw rate must be zero.");
            for (int index = terminalIndex; index < trajectory.Points.Count; index++)
            {
                EmTrajectoryPoint tail = trajectory.Points[index];
                if (Math.Abs(tail.PathS - terminal.PathS) > limits.SpatialTolerance)
                {
                    return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor, index,
                        "Exact-stop tail must remain at the terminal PathS.");
                }
                if (Math.Abs(tail.SignedLongitudinalVelocity) > limits.KinematicTolerance)
                    return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, index,
                        "Exact-stop tail signed speed must remain zero.");
                if (Math.Abs(tail.LongitudinalAcceleration) > limits.KinematicTolerance)
                    return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, index,
                        "Exact-stop tail stored acceleration must remain zero.");
            }
            if (IsRealTerminalBoundary(terminalBoundary))
            {
                if (requiresTerminalPose && terminalPose == null)
                {
                    return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex,
                        "A real terminal anchor requires its expected world pose.");
                }
                if (terminalPose != null)
                {
                    if (!IsFinite(terminalPose.X) || !IsFinite(terminalPose.Y) || !IsFinite(terminalPose.Heading))
                    {
                        return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex,
                            "The expected terminal world pose is non-finite.");
                    }
                    double dx = terminal.X - terminalPose.X;
                    double dy = terminal.Y - terminalPose.Y;
                    double positionError = Math.Sqrt(dx * dx + dy * dy);
                    double yawError = Math.Abs(NormalizeAngle(terminal.Yaw - terminalPose.Heading));
                    if (positionError > limits.TerminalPositionTolerance || yawError > limits.TerminalYawTolerance)
                    {
                        return Reject(EmTrajectoryValidationFailure.TerminalPoseMismatch, terminalIndex,
                            "TerminalPoseMismatch: position=" + Format(positionError) + ";yaw=" + Format(yawError));
                    }
                }
            }
        }

        double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
        double previousAcceleration = 0d;
        bool hasPreviousAcceleration = false;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point.Direction != trajectory.Metadata.Direction)
                return Reject(EmTrajectoryValidationFailure.DirectionMismatch, index,
                    "Trajectory point direction differs from trajectory metadata.");
            if (Math.Abs(point.SignedLongitudinalVelocity) > limits.KinematicTolerance &&
                point.SignedLongitudinalVelocity * directionSign < 0d)
            {
                return Reject(EmTrajectoryValidationFailure.DirectionSignMismatch, index,
                    "Trajectory signed speed has the wrong direction sign.");
            }
            if (!NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed, limits.KinematicTolerance))
                return Reject(EmTrajectoryValidationFailure.RedundantSpeedMismatch, index,
                    "Trajectory Speed is inconsistent with signed speed.");
            if (!NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX,
                    limits.KinematicTolerance) ||
                !NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY,
                    limits.KinematicTolerance))
            {
                return Reject(EmTrajectoryValidationFailure.WorldVelocityMismatch, index,
                    "Trajectory world velocity is inconsistent with signed speed and yaw.");
            }
            if (!NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate,
                    limits.KinematicTolerance))
            {
                return Reject(EmTrajectoryValidationFailure.YawRateMismatch, index,
                    "Trajectory yaw rate is inconsistent with signed speed and curvature.");
            }
            if (point.Speed > limits.MaximumSpeed + limits.KinematicTolerance)
                return Reject(EmTrajectoryValidationFailure.SpeedLimitExceeded, index,
                    "Trajectory speed exceeds its direction limit.");
            if (Math.Abs(point.VehicleCurvature) > limits.MaximumCurvature + limits.KinematicTolerance)
                return Reject(EmTrajectoryValidationFailure.CurvatureLimitExceeded, index,
                    "Trajectory vehicle curvature exceeds the vehicle limit.");

            if (index == 0)
                continue;

            EmTrajectoryPoint previous = trajectory.Points[index - 1];
            double dt = point.TimeFromStart - previous.TimeFromStart;
            double previousProgressSpeed = directionSign * previous.SignedLongitudinalVelocity;
            double progressSpeed = directionSign * point.SignedLongitudinalVelocity;
            double acceleration = (progressSpeed - previousProgressSpeed) / dt;
            if (acceleration > limits.MaximumAcceleration + limits.KinematicTolerance ||
                -acceleration > limits.MaximumDeceleration + limits.KinematicTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.AccelerationLimitExceeded, index,
                    "Trajectory finite-difference acceleration exceeds its limit.");
            }
            double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt;
            if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk +
                limits.KinematicTolerance)
            {
                double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d;
                double storedCurrentJerk = previous.LongitudinalJerk;
                return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
                    "Trajectory finite-difference jerk exceeds its limit" +
                    " (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" +
                    ", previousAcceleration=" + Format(previousAcceleration) + "m/s2" +
                    ", acceleration=" + Format(acceleration) + "m/s2" +
                    ", jerk=" + Format(finiteDifferenceJerk) + "m/s3" +
                    ", limit=" + Format(limits.MaximumJerk) + "m/s3" +
                    ", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" +
                    ", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" +
                    ", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3).");
            }
            if (Math.Abs(point.VehicleCurvature - previous.VehicleCurvature) / dt > limits.MaximumCurvatureRate +
                limits.KinematicTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, index,
                    "Trajectory finite-difference curvature rate exceeds its limit.");
            }
            previousAcceleration = acceleration;
            hasPreviousAcceleration = true;
        }

        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (!collisionChecker.IsPoseCollisionFree(new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, 0d, out _))
                return Reject(EmTrajectoryValidationFailure.PoseCollision, index,
                    "Trajectory point fails the full-body world-space collision check.");
        }
        double sweptStepMeters = Math.Min(MaximumSweptCollisionStepMeters, limits.ConfiguredCollisionStepMeters);
        for (int index = 1; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint previous = trajectory.Points[index - 1];
            EmTrajectoryPoint point = trajectory.Points[index];
            if (!collisionChecker.IsSweptMotionCollisionFree(new Pose2D(previous.X, previous.Y, previous.Yaw),
                    new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, sweptStepMeters, out _))
            {
                return Reject(EmTrajectoryValidationFailure.SweptCollision, index,
                    "Trajectory segment fails the full-body swept world-space collision check.");
            }
        }

        return EmTrajectoryValidationResult.Success();
    }

    private static int FindTerminalAnchor(EmTrajectory trajectory, double terminalPathS, EmBoundaryType terminalBoundary,
        double spatialTolerance)
    {
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point.BoundaryType == terminalBoundary && Math.Abs(point.PathS - terminalPathS) <= spatialTolerance)
                return index;
        }
        return -1;
    }

    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2d * Math.PI;
        while (angle < -Math.PI) angle += 2d * Math.PI;
        return angle;
    }

    private static int FindFirstTerminalPathIndex(EmTrajectory trajectory, double terminalPathS, double spatialTolerance)
    {
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            if (Math.Abs(trajectory.Points[index].PathS - terminalPathS) <= spatialTolerance)
                return index;
        }
        return trajectory.Points.Count - 1;
    }

    private static bool HasOnlyFiniteValues(EmTrajectoryPoint point)
    {
        return IsFinite(point.X) && IsFinite(point.Y) && IsFinite(point.Yaw) &&
            IsFinite(point.SignedLongitudinalVelocity) && IsFinite(point.Speed) && IsFinite(point.VelocityX) &&
            IsFinite(point.VelocityY) && IsFinite(point.YawRate) && IsFinite(point.TimeFromStart) &&
            IsFinite(point.VehicleCurvature) && IsFinite(point.SegmentLocalS) && IsFinite(point.PathS) &&
            IsFinite(point.LongitudinalAcceleration) && IsFinite(point.LongitudinalJerk);
    }

    private static bool TryReadLimits(EmPlannerConfiguration configuration, VehicleParameters vehicle,
        TravelDirection direction, out ValidationLimits limits)
    {
        limits = default;
        double maximumCurvature;
        if (vehicle.MaximumCurvaturePerMeter.HasValue)
            maximumCurvature = vehicle.MaximumCurvaturePerMeter.Value;
        else if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
            maximumCurvature = 1d / vehicle.MinimumTurningRadiusMeters.Value;
        else
            return false;

        if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
            return false;
        double maximumSpeed = direction == TravelDirection.Forward
            ? configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
            : configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
        if (!IsFinite(maximumSpeed) || maximumSpeed <= 0d || !IsFinite(maximumCurvature) || maximumCurvature <= 0d ||
            !IsFinite(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
            configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared <= 0d ||
            !IsFinite(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
            configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared <= 0d ||
            !IsFinite(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed) ||
            configuration.Longitudinal.MaximumJerkMetersPerSecondCubed <= 0d ||
            !IsFinite(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond) ||
            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond <= 0d ||
            !IsFinite(configuration.Validation.SpatialToleranceMeters) || configuration.Validation.SpatialToleranceMeters < 0d ||
            !IsFinite(configuration.Validation.KinematicTolerance) || configuration.Validation.KinematicTolerance < 0d ||
            !IsFinite(configuration.Validation.TerminalPositionToleranceMeters) ||
            configuration.Validation.TerminalPositionToleranceMeters < 0d ||
            !IsFinite(configuration.Validation.TerminalYawToleranceRadians) ||
            configuration.Validation.TerminalYawToleranceRadians < 0d ||
            !IsFinite(configuration.Corridor.MaximumCollisionCheckStepMeters) ||
            configuration.Corridor.MaximumCollisionCheckStepMeters <= 0d)
        {
            return false;
        }

        limits = new ValidationLimits(maximumSpeed, maximumCurvature,
            configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared,
            configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
            configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond,
            configuration.Validation.SpatialToleranceMeters, configuration.Validation.KinematicTolerance,
            configuration.Validation.TerminalPositionToleranceMeters,
            configuration.Validation.TerminalYawToleranceRadians,
            configuration.Corridor.MaximumCollisionCheckStepMeters);
        return true;
    }

    private static bool NearlyEqual(double expected, double actual, double tolerance)
    {
        double scale = Math.Max(1d, Math.Max(Math.Abs(expected), Math.Abs(actual)));
        return Math.Abs(expected - actual) <= tolerance + tolerance * scale;
    }

    private static string Format(double value)
    {
        return value.ToString("G17", CultureInfo.InvariantCulture);
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    private static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int index, string message)
    {
        return EmTrajectoryValidationResult.Reject(failure, index, message);
    }

    private readonly struct ValidationLimits
    {
        public ValidationLimits(double maximumSpeed, double maximumCurvature, double maximumAcceleration,
            double maximumDeceleration, double maximumJerk, double maximumCurvatureRate, double spatialTolerance,
            double kinematicTolerance, double terminalPositionTolerance, double terminalYawTolerance,
            double configuredCollisionStepMeters)
        {
            MaximumSpeed = maximumSpeed;
            MaximumCurvature = maximumCurvature;
            MaximumAcceleration = maximumAcceleration;
            MaximumDeceleration = maximumDeceleration;
            MaximumJerk = maximumJerk;
            MaximumCurvatureRate = maximumCurvatureRate;
            SpatialTolerance = spatialTolerance;
            KinematicTolerance = kinematicTolerance;
            TerminalPositionTolerance = terminalPositionTolerance;
            TerminalYawTolerance = terminalYawTolerance;
            ConfiguredCollisionStepMeters = configuredCollisionStepMeters;
        }

        public double MaximumSpeed { get; }
        public double MaximumCurvature { get; }
        public double MaximumAcceleration { get; }
        public double MaximumDeceleration { get; }
        public double MaximumJerk { get; }
        public double MaximumCurvatureRate { get; }
        public double SpatialTolerance { get; }
        public double KinematicTolerance { get; }
        public double TerminalPositionTolerance { get; }
        public double TerminalYawTolerance { get; }
        public double ConfiguredCollisionStepMeters { get; }
    }
}