using System;
using System.Collections.Generic;
using MultiWheelC.Trajectory;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MyParking.Shared;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

/// <summary>将冻结的单方向段 EM 轨迹转换为几何控制器使用的 SI 单位有符号速度轨迹。</summary>
public sealed class EmControlTrajectoryAdapter
{
    private const double MinimumSegmentLengthMeters = 1e-6d;

    /// <summary>按世界位置重建从零开始的弧长，同时保留航向、曲率以及前进为正倒车为负的参考速度。</summary>
    public Trajectory2D Create(EmTrajectory trajectory)
    {
        if (trajectory == null)
            throw new ArgumentNullException(nameof(trajectory));

        var points = new List<TrajectoryPoint>(trajectory.Points.Count);
        double arcLengthMeters = 0d;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint source = trajectory.Points[index];
            var pose = new Pose2D(source.X, source.Y, source.Yaw);
            var converted = new TrajectoryPoint(arcLengthMeters, pose,
                source.VehicleCurvature, source.SignedLongitudinalVelocity);

            if (points.Count == 0)
            {
                points.Add(converted);
                continue;
            }

            TrajectoryPoint previous = points[points.Count - 1];
            double deltaX = pose.XMeters - previous.PoseInWorld.XMeters;
            double deltaY = pose.YMeters - previous.PoseInWorld.YMeters;
            double distanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
            if (distanceMeters < MinimumSegmentLengthMeters)
            {
                points[points.Count - 1] = new TrajectoryPoint(
                    previous.ArcLengthMeters, pose, source.VehicleCurvature,
                    source.SignedLongitudinalVelocity);
                continue;
            }

            arcLengthMeters += distanceMeters;
            points.Add(new TrajectoryPoint(arcLengthMeters, pose,
                source.VehicleCurvature, source.SignedLongitudinalVelocity));
        }

        if (points.Count < 2)
        {
            throw new ArgumentException(
                "EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));
        }

        return new Trajectory2D(points);
    }
}