using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
internal sealed class LocalG2RegionWorkOrder
{
    internal bool TryCreate(
        IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
        out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
        out string reason)
    {
        workOrder = Empty();
        reason = string.Empty;
        if (reportOrder == null)
        {
            reason = "局部 G2 区域工作顺序输入无效。";
            return false;
        }

        var indexed = new List<IndexedRegion>(reportOrder.Count);
        for (int index = 0; index < reportOrder.Count; index++)
        {
            LocalG2SmoothingRegion region = reportOrder[index];
            if (!TryValidate(region, out double firstArc, out reason))
                return false;
            indexed.Add(new IndexedRegion(region, index, firstArc));
        }

        indexed.Sort(Compare);
        var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
        for (int index = 0; index < indexed.Count; index++)
            ordered.Add(indexed[index].Region);
        workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
        return true;
    }

    private static bool TryValidate(
        LocalG2SmoothingRegion region,
        out double firstArc,
        out string reason)
    {
        firstArc = 0d;
        reason = string.Empty;
        if (region == null || region.SegmentIndex < 0 ||
            region.Transitions == null || region.Transitions.Count == 0 ||
            region.WindowVariants == null || region.WindowVariants.Count == 0)
        {
            reason = "局部 G2 工作顺序包含空区域或空窗口集。";
            return false;
        }

        double previousArc = -1d;
        for (int index = 0; index < region.Transitions.Count; index++)
        {
            CurvatureTransition transition = region.Transitions[index];
            if (transition == null ||
                transition.SegmentIndex != region.SegmentIndex ||
                !NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
                transition.LocalArcLengthMeters < previousArc)
            {
                reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
                return false;
            }
            previousArc = transition.LocalArcLengthMeters;
        }

        firstArc = region.Transitions[0].LocalArcLengthMeters;
        return true;
    }

    private static int Compare(IndexedRegion left, IndexedRegion right)
    {
        int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
        if (segment != 0) return segment;
        int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
        return descendingArc != 0
            ? descendingArc
            : left.ReportIndex.CompareTo(right.ReportIndex);
    }

    private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
    {
        return new ReadOnlyCollection<LocalG2SmoothingRegion>(
            new List<LocalG2SmoothingRegion>());
    }

    private sealed class IndexedRegion
    {
        internal IndexedRegion(
            LocalG2SmoothingRegion region,
            int reportIndex,
            double firstArc)
        {
            Region = region;
            ReportIndex = reportIndex;
            FirstArc = firstArc;
        }

        internal LocalG2SmoothingRegion Region { get; }
        internal int ReportIndex { get; }
        internal double FirstArc { get; }
    }
}