using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal sealed class LongitudinalEnvelopeTrustRegion
{
    internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
        IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
        IReadOnlyList<int> envelopeSegmentIndex, double scale)
    {
        MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
        MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
        SpeedSlope = Copy(speedSlope, nameof(speedSlope));
        SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
        EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
        Scale = scale;
    }

    internal IReadOnlyList<double> MinimumPathS { get; }

    internal IReadOnlyList<double> MaximumPathS { get; }

    internal IReadOnlyList<double> SpeedSlope { get; }

    internal IReadOnlyList<double> SpeedIntercept { get; }

    internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }

    internal double Scale { get; }

    internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
    {
        failureReason = string.Empty;
        if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
        {
            failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
            return false;
        }
        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
        {
            failureReason = "Minimum active width must be finite and positive.";
            return false;
        }
        double ratio = nextScale / Scale;
        for (int index = 0; index < MinimumPathS.Count; index++)
        {
            double currentWidth = MaximumPathS[index] - MinimumPathS[index];
            if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
            {
                failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
                return false;
            }
        }
        return true;
    }

    private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
    {
        if (source == null)
            throw new ArgumentNullException(parameterName);
        var copy = new List<double>(source.Count);
        for (int index = 0; index < source.Count; index++)
            copy.Add(source[index]);
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
    {
        if (source == null)
            throw new ArgumentNullException(parameterName);
        var copy = new List<int>(source.Count);
        for (int index = 0; index < source.Count; index++)
            copy.Add(source[index]);
        return new ReadOnlyCollection<int>(copy);
    }

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

internal sealed class LongitudinalEnvelopeTrustRegionBuilder
{
    private const double ScheduleProgressTolerance = 1e-12d;
    private const double StationSelectionTolerance = 1e-12d;

    internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
        IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
        double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
        out string failureReason)
    {
        region = null;
        failureReason = string.Empty;
        if (speedLimit == null || anchor == null || referencePathS == null)
        {
            failureReason = "Trust region inputs must be present.";
            return false;
        }
        if (!IsSupportedScale(scale))
        {
            failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
            return false;
        }
        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
        {
            failureReason = "Minimum active width must be finite and positive.";
            return false;
        }
        if (referencePathS.Count != anchor.S.Count)
        {
            failureReason = "Reference PathS count must match the anchor knot count.";
            return false;
        }
        if (terminalHoldStartIndex == -1)
            terminalHoldStartIndex = anchor.S.Count;
        if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
        {
            failureReason = "Terminal-hold start index is outside the anchor knot range.";
            return false;
        }
        if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
            return false;

        int knotCount = anchor.S.Count;
        var minimumPathS = new double[knotCount];
        var maximumPathS = new double[knotCount];
        var speedSlope = new double[knotCount];
        var speedIntercept = new double[knotCount];
        var segmentIndex = new int[knotCount];
        for (int index = 0; index < knotCount; index++)
        {
            double anchorS = anchor.S[index];
            bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
            int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
            FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
                out double slope, out double intercept);
            double lower = speedLimit.PathS[firstSegment];
            double upper = speedLimit.PathS[lastSegment + 1];
            if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
                lower = anchorS;
            if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
                upper = anchorS;
            double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
            double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
            if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
            {
                failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
                return false;
            }

            minimumPathS[index] = trustedLower;
            maximumPathS[index] = trustedUpper;
            speedSlope[index] = slope;
            speedIntercept[index] = intercept;
            segmentIndex[index] = segment;
        }

        region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
            segmentIndex, scale);
        return true;
    }

    private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
        IReadOnlyList<double> referencePathS, out string failureReason)
    {
        if (anchor.S[0] != 0d)
        {
            failureReason = "The anchor must begin at exact PathS zero.";
            return false;
        }

        double minimumPathS = speedLimit.PathS[0];
        double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
        double previousAnchorS = double.NegativeInfinity;
        for (int index = 0; index < anchor.S.Count; index++)
        {
            double anchorS = anchor.S[index];
            double referenceS = referencePathS[index];
            if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
            {
                failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
                return false;
            }
            if (anchorS < previousAnchorS)
            {
                failureReason = "Anchor PathS must be nondecreasing.";
                return false;
            }
            if (!IsFinite(referenceS))
            {
                failureReason = "Reference PathS must be finite.";
                return false;
            }
            previousAnchorS = anchorS;
        }

        failureReason = string.Empty;
        return true;
    }

    private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
        int knotIndex, bool terminalHold)
    {
        int lastSegment = speedLimit.PathS.Count - 2;
        if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
            return 0;
        if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
            return lastSegment;
        for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
        {
            if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
            {
                if (terminalHold)
                    return index;
                double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
                if (scheduleDelta > ScheduleProgressTolerance)
                    return index;
                if (scheduleDelta < -ScheduleProgressTolerance)
                    return index - 1;
                double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
                double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
                return rightWidth >= leftWidth ? index : index - 1;
            }
            if (anchorS < speedLimit.PathS[index])
                return index - 1;
        }
        return lastSegment;
    }

    private static void GetAffineLine(PathSpeedLimit limit, int segment,
        out double slope, out double intercept)
    {
        double lower = limit.PathS[segment];
        double upper = limit.PathS[segment + 1];
        slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
            limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
        intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
    }

    private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
        out int firstSegment, out int lastSegment, out double slope, out double intercept)
    {
        GetAffineLine(limit, selectedSegment, out slope, out intercept);
        firstSegment = selectedSegment;
        while (firstSegment > 0)
        {
            GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
            if (candidateSlope != slope || candidateIntercept != intercept)
                break;
            firstSegment--;
        }
        lastSegment = selectedSegment;
        while (lastSegment < limit.PathS.Count - 2)
        {
            GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
            if (candidateSlope != slope || candidateIntercept != intercept)
                break;
            lastSegment++;
        }
    }

    private static bool IsSupportedScale(double scale)
    {
        return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
    }

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