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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 在纵横向优化前选定的参考距离终端，保证不会跨越当前方向段。
/// 所有参考站量以当前段局部 S（m）表示；边界类型和纵向模式明确滚动延续、接近停车或精确停车语义。
/// </summary>
public sealed class PlanningHorizonSelection
{
    /// <summary>
    /// 创建仅供本程序集发布的窗口与停车边界选择。
    /// 参数：两个参考站均为局部 S（m），boundary/terminal/mode 为已判定枚举；hasStopBoundary 表示终端是否受真实停车边界约束。
    /// </summary>
    internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
        EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
        double stopBoundaryReferenceS, bool hasStopBoundary)
    {
        WindowEndReferenceS = windowEndReferenceS;
        WindowEndBoundaryType = windowEndBoundaryType;
        TerminalType = terminalType;
        LongitudinalMode = longitudinalMode;
        StopBoundaryReferenceS = stopBoundaryReferenceS;
        HasStopBoundary = hasStopBoundary;
    }

    /// <summary>
    /// 本次优化窗口末端的局部参考弧长 S，单位 m。
    /// </summary>
    public double WindowEndReferenceS { get; }

    /// <summary>
    /// 规划输出的终端参考弧长 S，当前与窗口末端相同，单位 m。
    /// </summary>
    public double TerminalReferenceS => WindowEndReferenceS;

    /// <summary>
    /// 窗口末端的边界语义；中途截断时为滚动安全停车边界。
    /// </summary>
    public EmBoundaryType WindowEndBoundaryType { get; }

    /// <summary>
    /// 供终端约束使用的目标、换向或滚动终端类型。
    /// </summary>
    public EmTerminalType TerminalType { get; }

    /// <summary>
    /// 纵向候选应滚动延续、接近停车边界还是在边界精确停车的模式。
    /// </summary>
    public EmLongitudinalMode LongitudinalMode { get; }

    /// <summary>
    /// 实际目标或换向停车边界的局部参考弧长 S，单位 m；无此边界时仍保存段末。
    /// </summary>
    public double StopBoundaryReferenceS { get; }

    /// <summary>
    /// 是否存在必须在当前方向段内处理的目标或换向停车边界。
    /// </summary>
    public bool HasStopBoundary { get; }
}

/// <summary>
/// 根据段边界、初始纵向状态和调度窗口选择 EM 优化地平线。
/// 输入速度为 m/s、加速度为 m/s²、距离为 m；无效输入、越界初态或不足以停车的段会以明确状态拒绝。
/// </summary>
public sealed class PlanningHorizonSelector
{
    /// <summary>
    /// 段终点、初态限值和停车距离比较使用的局部 S/距离容差，单位 m。
    /// </summary>
    private const double BoundaryTolerance = 1e-8d;

    /// <summary>
    /// 为当前方向段选择不跨界的参考距离窗口和纵向终端模式。
    /// 参数：currentSegmentReferenceS 为 m，初速为 m/s，初加速度为 m/s²，configuration 提供 m、s 制限值；planningScope 必须为定义的范围枚举。
    /// 返回：成功时给出 selection；配置/初态非法返回 InvalidInput，停车距离越段返回 StoppingDistanceInsufficient，非法全段终边界返回 InvalidReferencePath。
    /// </summary>
    public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
        EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
        out string failureReason)
    {
        selection = null;
        failureReason = string.Empty;
        if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
            !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
            currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
            !IsFinite(initialAccelerationMetersPerSecondSquared) ||
            !Enum.IsDefined(typeof(EmPlanningScope), planningScope))
        {
            failureReason = "Planning horizon inputs are invalid.";
            return EmPlanningStatus.InvalidInput;
        }

        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
        SchedulingConfiguration scheduling = configuration.Scheduling;
        double directionMaximum = segment.Direction == TravelDirection.Forward
            ? longitudinal.MaximumForwardSpeedMetersPerSecond
            : longitudinal.MaximumReverseSpeedMetersPerSecond;
        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
            !IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
            !IsPositiveFinite(scheduling.DistanceHorizonMeters))
        {
            failureReason = "Planning horizon configuration is invalid.";
            return EmPlanningStatus.InvalidInput;
        }
        if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
            initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
            initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
        {
            failureReason = "The initial state violates longitudinal bounds.";
            return EmPlanningStatus.InvalidInput;
        }

        double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
        if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
                initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile initialStop,
                out failureReason))
        {
            return EmPlanningStatus.InvalidInput;
        }
        if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
        {
            failureReason = "The current segment lacks the jerk-limited stopping distance.";
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }

        if (planningScope == EmPlanningScope.FullDirectionSegment)
        {
            EmBoundaryType boundary = segment.EndBoundary.BoundaryType;
            if (!IsStopBoundary(boundary))
            {
                failureReason = "A full direction segment must end at Goal or GearSwitch.";
                return EmPlanningStatus.InvalidReferencePath;
            }
            selection = new PlanningHorizonSelection(segment.LengthMeters, boundary,
                ToTerminalType(boundary), EmLongitudinalMode.ExactStopAtBoundary,
                segment.LengthMeters, true);
            return EmPlanningStatus.Success;
        }

        double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
        bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
        bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
        EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
            ? segment.EndBoundary.BoundaryType
            : EmBoundaryType.RollingSafetyStop;
        if (!hasStopBoundary)
        {
            selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
                EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
                segment.LengthMeters, false);
            return EmPlanningStatus.Success;
        }

        IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
            scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            knotTimes, scheduling.OutputTimeStepSeconds);
        double availableMotionTime = knotTimes[stabilizationStart];
        double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
            initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
            longitudinal.MaximumAccelerationMetersPerSecondSquared,
            longitudinal.MaximumDecelerationMetersPerSecondSquared,
            longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
        EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
            ? EmLongitudinalMode.ExactStopAtBoundary
            : EmLongitudinalMode.ApproachStopBoundary;
        selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
            ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
        return EmPlanningStatus.Success;
    }

    /// <summary>
    /// 判断边界是否要求车辆在当前段内停止。
    /// 参数：boundaryType 为边界枚举；返回：Goal 或 GearSwitchApproach 时为 true，其余边界允许滚动延续。
    /// </summary>
    private static bool IsStopBoundary(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    /// <summary>
    /// 将参考边界语义映射为纵向终端类型。
    /// 参数：boundaryType 为边界枚举；返回：目标、任一换向边界分别映射到 Goal、GearSwitch，其余映射为 RollingSafetyStop。
    /// </summary>
    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal
            ? EmTerminalType.Goal
            : boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
                ? EmTerminalType.GearSwitch
                : EmTerminalType.RollingSafetyStop;
    }

    /// <summary>
    /// 判断配置中的正量是否可用于时间、距离或动力学计算。
    /// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
    /// </summary>
    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    /// <summary>
    /// 判断规划输入是否为有限实数。
    /// 参数：value 为任意标量；返回：NaN 与无穷均返回 false。
    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}