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

namespace TrajectoryOutputDemo;

/// <summary>供外部控制模块消费的单个只读轨迹点；全部字段直接来自已验证的 <see cref="EmTrajectoryPoint"/>。</summary>
public sealed class ControlTrajectoryPoint
{
    internal ControlTrajectoryPoint(EmTrajectoryPoint source)
    {
        TimeFromStartSeconds = source.TimeFromStart;
        XMeters = source.X;
        YMeters = source.Y;
        YawRadians = source.Yaw;
        SignedLongitudinalVelocityMetersPerSecond = source.SignedLongitudinalVelocity;
        YawRateRadiansPerSecond = source.YawRate;
        CurvaturePerMeter = source.VehicleCurvature;
        Direction = source.Direction;
        SegmentIndex = source.SegmentIndex;
        PathSMeters = source.PathS;
        BoundaryType = source.BoundaryType;
    }

    public double TimeFromStartSeconds { get; }
    public double XMeters { get; }
    public double YMeters { get; }
    public double YawRadians { get; }
    public double SignedLongitudinalVelocityMetersPerSecond { get; }
    public double YawRateRadiansPerSecond { get; }
    public double CurvaturePerMeter { get; }
    public TravelDirection Direction { get; }
    public int SegmentIndex { get; }
    public double PathSMeters { get; }
    public EmBoundaryType BoundaryType { get; }
}

/// <summary>轨迹元数据与控制点序列的不可变组合；它不包含也不发送任何硬件命令。</summary>
public sealed class ControlTrajectorySequence
{
    internal ControlTrajectorySequence(EmTrajectoryMetadata metadata, IReadOnlyList<ControlTrajectoryPoint> points)
    {
        Metadata = metadata ?? throw new ArgumentNullException(nameof(metadata));
        Points = points ?? throw new ArgumentNullException(nameof(points));
    }

    public EmTrajectoryMetadata Metadata { get; }
    public IReadOnlyList<ControlTrajectoryPoint> Points { get; }
}

/// <summary>将不可变 EM 轨迹投影为控制模块可引用的只读序列，不进行采样、插值或底盘协议转换。</summary>
public sealed class ControlModuleTrajectoryAdapter
{
    /// <summary>逐点复制公开控制字段；调用方只能在完整非空轨迹上调用此方法。</summary>
    public ControlTrajectorySequence Create(EmTrajectory trajectory)
    {
        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
            throw new ArgumentException("必须提供完整非空的 EM 轨迹。", nameof(trajectory));

        var points = new List<ControlTrajectoryPoint>(trajectory.Points.Count);
        for (int index = 0; index < trajectory.Points.Count; index++)
            points.Add(new ControlTrajectoryPoint(trajectory.Points[index]));
        return new ControlTrajectorySequence(trajectory.Metadata,
            new ReadOnlyCollection<ControlTrajectoryPoint>(points));
    }
}