using System;
using System.IO;
using TrajectoryOutputDemo;

namespace TrajectoryOutputDemoTests;

internal static class Program
{
    private static int Main()
    {
        string temporaryDirectory = Path.Combine(Path.GetTempPath(), "trajectory-output-demo-tests", Guid.NewGuid().ToString("N"));
        try
        {
            Directory.CreateDirectory(temporaryDirectory);
            string csvPath = Path.Combine(temporaryDirectory, "trajectory.csv");
            var configuration = TrajectoryOutputDemoConfiguration.CreateDefault(csvPath);
            TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(configuration);

            if (!result.Succeeded)
                throw new InvalidOperationException("真实 OSQP 规划未成功：" + result.Diagnostic);
            if (!File.Exists(csvPath))
                throw new InvalidOperationException("成功规划必须导出 CSV。");
            ControlTrajectorySequence sequence = result.ControlTrajectory ??
                throw new InvalidOperationException("成功规划必须提供控制模块序列。");
            var trajectory = result.Trajectory ??
                throw new InvalidOperationException("成功规划必须提供 EM 轨迹。");
            if (sequence.Points.Count != trajectory.Points.Count)
                throw new InvalidOperationException("控制序列必须逐点对应 EM 轨迹。");

            string[] lines = File.ReadAllLines(csvPath);
            if (lines.Length < 2)
                throw new InvalidOperationException("CSV 必须包含表头和至少一个轨迹点。");
            if (lines[0] != "time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type")
                throw new InvalidOperationException("CSV 表头不符合控制模块契约。");

            Console.WriteLine("PASS trajectory-output-demo");
            return 0;
        }
        finally
        {
            if (Directory.Exists(temporaryDirectory))
                Directory.Delete(temporaryDirectory, true);
        }
    }
}