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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class DirectionSegmentView
{
    public DirectionSegmentView(
        int segmentIndex,
        TravelDirection direction,
        IReadOnlyList<SmoothedPathPoint> points,
        ReferenceBoundary startBoundary,
        ReferenceBoundary endBoundary,
        double sourceStartArcLength)
    {
        if (segmentIndex < 0)
            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (points == null || points.Count == 0)
            throw new ArgumentException("A direction segment requires points.", nameof(points));
        if (startBoundary == null || endBoundary == null)
            throw new ArgumentNullException(startBoundary == null ? nameof(startBoundary) : nameof(endBoundary));
        if (startBoundary.SegmentIndex != segmentIndex || endBoundary.SegmentIndex != segmentIndex)
            throw new ArgumentException("Boundary segment identity must match the segment.");
        if (points[0] == null || Math.Abs(points[0].ArcLength) > 1e-12d)
            throw new ArgumentException("A direction segment must begin at local S zero.", nameof(points));

        var copy = new List<SmoothedPathPoint>(points.Count);
        double previousS = -1d;
        for (int index = 0; index < points.Count; index++)
        {
            SmoothedPathPoint point = points[index];
            if (point == null || point.Direction != direction || point.ArcLength < 0d || point.ArcLength < previousS)
                throw new ArgumentException("Direction segment points are invalid.", nameof(points));
            copy.Add(point);
            previousS = point.ArcLength;
        }
        if (Math.Abs(endBoundary.SegmentLocalS - previousS) > 1e-12d)
            throw new ArgumentException("End boundary must match the final local S.", nameof(endBoundary));

        SegmentIndex = segmentIndex;
        Direction = direction;
        Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
        StartBoundary = startBoundary;
        EndBoundary = endBoundary;
        SourceStartArcLength = sourceStartArcLength;
    }

    public int SegmentIndex { get; }

    public TravelDirection Direction { get; }

    public IReadOnlyList<SmoothedPathPoint> Points { get; }

    public ReferenceBoundary StartBoundary { get; }

    public ReferenceBoundary EndBoundary { get; }

    public double SourceStartArcLength { get; }

    public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
}