using System; using System.Collections.Generic; using System.Globalization; using System.IO; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.Mapping; using MultiWheelC.TrajectoryPlanning.PathSmoothing; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Test; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization; internal static class Program { private static readonly byte[] PngSignature = { 137, 80, 78, 71, 13, 10, 26, 10 }; private static int Main(string[] arguments) { try { if (arguments.Length == 3 && arguments[0] == "--export-fixtures") { ExportFixtureReports(arguments[1], arguments[2]); return 0; } if (arguments.Length == 2 && arguments[0] == "--export-end-to-end") { ExportEndToEndReports(arguments[1]); return 0; } Require(arguments.Length == 1 && File.Exists(arguments[0]), "A fixture path is required."); Verify(arguments[0]); Console.WriteLine("PNG verification host completed."); return 0; } catch (Exception exception) { Console.Error.WriteLine(exception.ToString()); return 1; } } private static void ExportFixtureReports(string fixturePath, string outputDirectory) { Require(File.Exists(fixturePath), "Fixture path was not found."); PrintReports(new PathSmoothingComparisonDemo().ExportFixtureReports(fixturePath, outputDirectory)); } private static void ExportEndToEndReports(string outputDirectory) { PrintReports(new PathSmoothingComparisonDemo().ExportEndToEndReports(outputDirectory)); } private static void PrintReports(IReadOnlyList scenarios) { for (int scenarioIndex = 0; scenarioIndex < scenarios.Count; scenarioIndex++) { PathSmoothingComparisonScenarioResult scenario = scenarios[scenarioIndex]; if (scenario.Comparison == null) { Console.WriteLine(scenario.ScenarioId + " coarse=" + scenario.CoarsePathStatus + " " + scenario.Diagnostic); continue; } PrintEntry(scenario.ScenarioId, scenario.Comparison.RawPathBaseline); for (int entryIndex = 0; entryIndex < scenario.Comparison.Entries.Count; entryIndex++) PrintEntry(scenario.ScenarioId, scenario.Comparison.Entries[entryIndex]); Console.WriteLine(scenario.ScenarioId + " report=" + scenario.Report.Status + " " + scenario.Report.Reason); } } private static void PrintEntry(string scenarioId, PathSmoothingComparisonEntry entry) { Console.WriteLine(string.Format( CultureInfo.InvariantCulture, "{0} {1} status={2} length={3:F4} peakKappa={4:F4} clearance={5:F4}", scenarioId, entry.IsRawPathBaseline ? "RawPath" : entry.Method.ToString(), entry.Status, entry.Metrics.PathLengthMeters, entry.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter, entry.Metrics.MinimumBodyClearanceMeters)); } private static void Verify(string fixturePath) { PathSmoothingComparisonRequest request = CreateComparisonRequest(); PathSmoothingComparisonResult comparison = new PathSmoothingComparisonService().Compare(request); Require(!comparison.IsCancelled, "Fixture comparison was cancelled."); Require(request.SmoothingRequest.CoarsePath.Count > 1, "Fixture coarse path is too short."); CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0]; CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1]; var model = new SmoothingFigureModelBuilder().Build( comparison, request.SmoothingRequest.Map, new Pose2D(first.X, first.Y, first.Heading), new Pose2D(last.X, last.Y, last.Heading), "straight", "Straight"); VerifyObstacleRectanglesAreMerged(fixturePath, comparison, new Pose2D(first.X, first.Y, first.Heading), new Pose2D(last.X, last.Y, last.Heading)); VerifySixFigureDefinitionContract(model); VerifyPointOnlyRendererContract(model); var resolver = new SmoothingFontResolver(); Require(resolver.TryResolve("SimSun", "Times New Roman", out SmoothingFontResolution fonts, out string fontReason), "Required report fonts are unavailable: " + fontReason); byte[] png; using (fonts) { Require(fonts.ChineseFamilyName == "SimSun", "Chinese font must resolve to exact SimSun family."); Require(fonts.LatinFamilyName == "Times New Roman", "Latin font must resolve to exact Times New Roman family."); Require(resolver.MeasureMixedText(fonts, "粗路径 κ(s) X (m) −π", 9f).Width > 0f, "Mixed Chinese/Latin sample must have nonempty measured bounds."); png = new SmoothingPngRenderer().Render(model, fonts); } VerifyPng(png); string outputDirectory = Path.Combine(Path.GetTempPath(), "path-smoothing-png-" + Guid.NewGuid().ToString("N")); try { var exporter = new SmoothingReportExporter(); SmoothingReportExportResult report = exporter.Export(new SmoothingReportExportRequest { Model = model, OutputDirectory = outputDirectory, FileStem = "straight-report", }); Require(report.Status == SmoothingReportExportStatus.Success, "Report export failed: " + report.Reason); Require(report.SvgPaths.Count == 6 && report.PngPaths.Count == 6 && File.Exists(report.CsvPath), "Successful export must publish six SVGs, six PNGs and one CSV."); VerifyPublishedSixFigureFiles(report, outputDirectory); Require(!ContainsTemporaryFiles(outputDirectory), "Successful export must not leave temporary files."); string unavailableDirectory = Path.Combine(outputDirectory, "missing-font"); SmoothingReportExportResult unavailable = exporter.Export(new SmoothingReportExportRequest { Model = model, OutputDirectory = unavailableDirectory, FileStem = "must-not-write", ChineseFontFamilyName = "Missing report font", LatinFontFamilyName = "Times New Roman", }); Require(unavailable.Status == SmoothingReportExportStatus.FontUnavailable, "Missing exact font must report FontUnavailable."); Require(!Directory.Exists(unavailableDirectory), "Missing-font export must remain atomic and create no output directory."); } finally { if (Directory.Exists(outputDirectory)) Directory.Delete(outputDirectory, true); } } private static PathSmoothingComparisonRequest CreateComparisonRequest() { PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(new PlanningMapRequest { Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f), ResolutionMm = 50f, AllowExplicitEmptyMap = true, }); Require(mapResult.Succeeded && mapResult.Map != null, "PNG verification map must be created."); var vehicle = new VehicleParameters { LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d, MaximumCurvaturePerMeter = 100d, }; var coarsePath = new List { CreatePoint(0.5d, 0.5d, 0d), CreatePoint(1.0d, 0.5d, 0.5d), CreatePoint(1.0d, 1.0d, 1.0d), CreatePoint(1.5d, 1.0d, 1.5d), }; var segments = new List { new PathSegment(0, TravelDirection.Forward, 0, coarsePath.Count - 1, false, false), }; return new PathSmoothingComparisonRequest(new PathSmoothingRequest( coarsePath, segments, mapResult.Map, vehicle, new PathSmoothingConfiguration())); } private static void VerifySixFigureDefinitionContract(SmoothingFigureModel model) { SmoothingFigureSet figureSet = new SmoothingFigureSetBuilder().Build(model); var stems = new List(); for (int index = 0; index < figureSet.Figures.Count; index++) stems.Add(figureSet.Figures[index].FileStem); string expected = string.Join(",", new[] { "01-coarse-path-overview", "02-all-paths-comparison", "03-cubic-bspline-overview", "04-local-cubic-bezier-overview", "05-piecewise-quintic-overview", "06-curvature-comparison", }); Require(string.Join(",", stems) == expected, "Six-figure report stems must be stable and ordered."); Require(!figureSet.Figures[1].ShowsMapContext, "All-path comparison must contain only trajectories, axes and legend."); Require(figureSet.Figures[2].ShowsMapContext && figureSet.Figures[2].Series[0].Opacity < 1d, "Individual smoother figures must retain a faded raw-path map reference."); Require(figureSet.Figures[0].WorldScalePointsPerMeter > 0d && Math.Abs((figureSet.Figures[0].WorldXMaxMeters - figureSet.Figures[0].WorldXMinMeters) / (figureSet.Figures[0].WorldYMaxMeters - figureSet.Figures[0].WorldYMinMeters) - figureSet.Figures[0].PlotWidthPoints / figureSet.Figures[0].PlotHeightPoints) < 0.000001d, "Overhead figures must preserve equal X/Y scale."); Require(figureSet.Figures[0].LegendYPoints - (figureSet.Figures[0].PlotYPoints + figureSet.Figures[0].PlotHeightPoints + 31d) >= 8d, "Legend must leave vertical clearance below the X-axis unit label."); } private static void VerifyObstacleRectanglesAreMerged(string fixturePath, PathSmoothingComparisonResult comparison, Pose2D start, Pose2D goal) { IReadOnlyList fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath); IReadOnlyList requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath); int rectangleIndex = -1; for (int index = 0; index < fixtures.Count; index++) { if (fixtures[index].Id == "rectangle-detour") { rectangleIndex = index; break; } } Require(rectangleIndex >= 0, "Fixture suite must include rectangle-detour for obstacle rendering verification."); SmoothingFigureModel model = new SmoothingFigureModelBuilder().Build( comparison, requests[rectangleIndex].SmoothingRequest.Map, start, goal, "obstacle", "obstacle"); Require(model.Obstacles.Count > 0, "Rectangle-detour fixture must produce report obstacles."); for (int firstIndex = 0; firstIndex < model.Obstacles.Count; firstIndex++) { SmoothingFigureObstacle first = model.Obstacles[firstIndex]; for (int secondIndex = firstIndex + 1; secondIndex < model.Obstacles.Count; secondIndex++) { SmoothingFigureObstacle second = model.Obstacles[secondIndex]; bool matchingColumn = Math.Abs(first.X - second.X) < 0.0000001d && Math.Abs(first.Width - second.Width) < 0.0000001d; bool verticallyAdjacent = Math.Abs((first.Y + first.Height) - second.Y) < 0.0000001d || Math.Abs((second.Y + second.Height) - first.Y) < 0.0000001d; Require(!(matchingColumn && verticallyAdjacent), "Adjacent occupied rows must merge into a single obstacle rectangle."); } } } private static void VerifyPointOnlyRendererContract(SmoothingFigureModel model) { SmoothingFigureDefinition figure = new SmoothingFigureSetBuilder().Build(model).Figures[1]; string svg = new SmoothingSvgRenderer().Render(figure); Require(svg.Contains("class=\"trajectory-point\""), "Trajectory samples must render as discrete SVG point markers."); Require(!svg.Contains("stroke-dasharray") && !svg.Contains("-path\""), "Trajectory SVG output must not use dashed or joined path strokes."); Require(svg.Contains("X (m)") && svg.Contains("Y (m)"), "Overhead SVG must label metre coordinate axes."); } private static void VerifyPublishedSixFigureFiles(SmoothingReportExportResult report, string outputDirectory) { var expectedStems = new[] { "01-coarse-path-overview", "02-all-paths-comparison", "03-cubic-bspline-overview", "04-local-cubic-bezier-overview", "05-piecewise-quintic-overview", "06-curvature-comparison", }; VerifyPublishedPaths(report.SvgPaths, expectedStems, ".svg", outputDirectory); VerifyPublishedPaths(report.PngPaths, expectedStems, ".png", outputDirectory); Require(!File.Exists(Path.Combine(outputDirectory, "comparison.svg")) && !File.Exists(Path.Combine(outputDirectory, "comparison.png")), "Six-figure export must not leave legacy composite comparison images."); } private static void VerifyPublishedPaths(IReadOnlyList paths, string[] expectedStems, string extension, string outputDirectory) { var actual = new List(); for (int index = 0; index < paths.Count; index++) actual.Add(paths[index]); Require(actual.Count == expectedStems.Length, "Six-figure export must publish six " + extension + " files."); for (int index = 0; index < expectedStems.Length; index++) { string expected = Path.Combine(outputDirectory, expectedStems[index] + extension); Require(actual[index] == expected && File.Exists(actual[index]), "Published " + extension + " path must match the stable figure stem."); } } private static CoarsePathPoint CreatePoint(double x, double y, double arcLength) { return new CoarsePathPoint( x, y, 0d, 0d, arcLength, TravelDirection.Forward, 0d, 1d, false, CoarsePathPointSource.Start); } private static bool ContainsTemporaryFiles(string directory) { foreach (string ignored in Directory.EnumerateFiles(directory, "*.tmp")) return true; return false; } private static void VerifyPng(byte[] png) { Require(png != null && png.Length > PngSignature.Length, "PNG output is empty."); byte[] data = png ?? throw new InvalidOperationException("PNG output is empty."); for (int index = 0; index < PngSignature.Length; index++) Require(data[index] == PngSignature[index], "PNG signature is invalid."); bool sawHeader = false; bool sawPhysicalResolution = false; int offset = PngSignature.Length; while (offset < data.Length) { Require(offset + 12 <= data.Length, "PNG chunk header is truncated."); int length = checked((int)ReadUInt32BigEndian(data, offset)); int dataStart = offset + 8; int crcStart = checked(dataStart + length); Require(crcStart + 4 <= data.Length, "PNG chunk data is truncated."); uint expectedCrc = ReadUInt32BigEndian(data, crcStart); uint actualCrc = ComputeCrc32(data, offset + 4, length + 4); Require(expectedCrc == actualCrc, "PNG chunk CRC is invalid."); string type = System.Text.Encoding.ASCII.GetString(data, offset + 4, 4); if (type == "IHDR") { Require(length == 13, "IHDR length must be 13."); Require(ReadUInt32BigEndian(data, dataStart) == SmoothingPngRenderer.WidthPixels, "PNG width must be 4296."); Require(ReadUInt32BigEndian(data, dataStart + 4) == SmoothingPngRenderer.HeightPixels, "PNG height must be 3120."); sawHeader = true; } else if (type == "pHYs") { Require(length == 9, "pHYs length must be 9."); Require(ReadUInt32BigEndian(data, dataStart) == SmoothingPngRenderer.PixelsPerMeter, "PNG horizontal density must be 23622 pixels/meter."); Require(ReadUInt32BigEndian(data, dataStart + 4) == SmoothingPngRenderer.PixelsPerMeter, "PNG vertical density must be 23622 pixels/meter."); Require(data[dataStart + 8] == 1, "PNG pHYs unit must be meter."); sawPhysicalResolution = true; } offset = crcStart + 4; } Require(sawHeader && sawPhysicalResolution, "PNG must contain IHDR and pHYs chunks."); } private static uint ReadUInt32BigEndian(byte[] data, int offset) { return ((uint)data[offset] << 24) | ((uint)data[offset + 1] << 16) | ((uint)data[offset + 2] << 8) | data[offset + 3]; } private static uint ComputeCrc32(byte[] data, int offset, int length) { uint crc = 0xffffffffu; for (int index = 0; index < length; index++) { crc ^= data[offset + index]; for (int bit = 0; bit < 8; bit++) crc = (crc & 1u) == 0u ? crc >> 1 : (crc >> 1) ^ 0xedb88320u; } return crc ^ 0xffffffffu; } private static void Require(bool condition, string message) { if (!condition) throw new InvalidOperationException(message); } }