using System;
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,
    TerminalYawRateNotZero,
    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 terminalPathS, EmBoundaryType terminalBoundary)
    {
        if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
            configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
            !IsFinite(terminalPathS) || terminalPathS < 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 > terminalPathS + limits.SpatialTolerance ||
                point.PathS > terminalPathS + 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 = FindTerminalAnchor(trajectory, terminalPathS, terminalBoundary, limits.SpatialTolerance);
        if (terminalIndex < 0)
        {
            return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
                FindFirstTerminalPathIndex(trajectory, terminalPathS, 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.YawRate) > limits.KinematicTolerance)
            return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
                "Terminal yaw rate must be zero.");

        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.");
            }
            if (hasPreviousAcceleration && Math.Abs((acceleration - previousAcceleration) / dt) > limits.MaximumJerk +
                limits.KinematicTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
                    "Trajectory finite-difference jerk exceeds its limit.");
            }
            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 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.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.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 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 configuredCollisionStepMeters)
        {
            MaximumSpeed = maximumSpeed;
            MaximumCurvature = maximumCurvature;
            MaximumAcceleration = maximumAcceleration;
            MaximumDeceleration = maximumDeceleration;
            MaximumJerk = maximumJerk;
            MaximumCurvatureRate = maximumCurvatureRate;
            SpatialTolerance = spatialTolerance;
            KinematicTolerance = kinematicTolerance;
            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 ConfiguredCollisionStepMeters { get; }
    }
}
