ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Output/Visualization/SmoothingFigureSetBuilder.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;

/// <summary>Builds the stable four-figure Local G2 report set and an optional diagnostic figure.</summary>
public sealed class SmoothingFigureSetBuilder
{
    private const double MinimumExtentMeters = 0.25d;
    private const double PaddingFraction = 0.10d;

    public SmoothingFigureSet Build(SmoothingFigureModel model)
    {
        if (model == null) throw new ArgumentNullException(nameof(model));
        SmoothingFigureSeries raw = Find(model, "raw");
        SmoothingFigureSeries localG2 = Find(model, "local-g2");
        var figures = new List<SmoothingFigureDefinition>
        {
            BuildOverhead(SmoothingFigureKind.CoarsePathOverview, "01-coarse-path-overview", "Raw coarse path", model, true, View(raw, 1d)),
            BuildOverhead(SmoothingFigureKind.AllPathsComparison, "02-all-paths-comparison", "Raw path and Local G2", model, false, View(raw, 1d), View(localG2, 1d)),
            BuildOverhead(SmoothingFigureKind.LocalG2Overview, "03-local-g2-overview", "Local G2 smoothing", model, true, View(raw, 0.28d), View(localG2, 1d)),
            BuildCurvature(model, View(raw, 1d), View(localG2, 1d)),
        };
        if (TryFind(model, "local-g2-diagnostic", out SmoothingFigureSeries diagnostic))
        {
            figures.Add(BuildOverhead(
                SmoothingFigureKind.LocalG2DiagnosticCandidate,
                "05-local-g2-diagnostic-candidate",
                "Local G2 diagnostic candidate (not published)",
                model,
                true,
                View(raw, 1d, 2.10d),
                View(diagnostic, 1d)));
        }
        return new SmoothingFigureSet(figures);
    }

    private static SmoothingFigureDefinition BuildOverhead(
        SmoothingFigureKind kind,
        string stem,
        string title,
        SmoothingFigureModel model,
        bool mapContext,
        params SmoothingFigureSeriesView[] series)
    {
        Bounds bounds = CalculateWorldBounds(model, mapContext, series);
        return new SmoothingFigureDefinition(
            kind, stem, title, model, mapContext, series,
            bounds.XMin, bounds.XMax, bounds.YMin, bounds.YMax,
            BuildTicks(bounds.XMin, bounds.XMax), BuildTicks(bounds.YMin, bounds.YMax),
            1d, -1d, 1d, Array.Empty<double>(), Array.Empty<double>());
    }

    private static SmoothingFigureDefinition BuildCurvature(
        SmoothingFigureModel model,
        params SmoothingFigureSeriesView[] series)
    {
        double arcMaximum = 0d;
        double curvatureMinimum = 0d;
        double curvatureMaximum = 0d;
        for (int viewIndex = 0; viewIndex < series.Length; viewIndex++)
        {
            IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
            for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
            {
                SmoothingFigurePoint point = points[pointIndex];
                if (point.ArcLength > arcMaximum) arcMaximum = point.ArcLength;
                if (point.VehicleCurvature < curvatureMinimum) curvatureMinimum = point.VehicleCurvature;
                if (point.VehicleCurvature > curvatureMaximum) curvatureMaximum = point.VehicleCurvature;
            }
        }
        arcMaximum = ExpandMaximum(arcMaximum, MinimumExtentMeters);
        ExpandRange(ref curvatureMinimum, ref curvatureMaximum, MinimumExtentMeters);
        return new SmoothingFigureDefinition(
            SmoothingFigureKind.CurvatureComparison,
            "04-curvature-comparison",
            "Vehicle-curvature comparison",
            model,
            false,
            series,
            0d,
            1d,
            0d,
            1d,
            Array.Empty<double>(),
            Array.Empty<double>(),
            arcMaximum,
            curvatureMinimum,
            curvatureMaximum,
            BuildTicks(0d, arcMaximum),
            BuildTicks(curvatureMinimum, curvatureMaximum));
    }

    private static Bounds CalculateWorldBounds(
        SmoothingFigureModel model,
        bool includesEndpoints,
        IReadOnlyList<SmoothingFigureSeriesView> series)
    {
        bool hasPoint = false;
        double xMin = 0d;
        double xMax = 0d;
        double yMin = 0d;
        double yMax = 0d;
        for (int viewIndex = 0; viewIndex < series.Count; viewIndex++)
        {
            IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
            for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
                Include(points[pointIndex].X, points[pointIndex].Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
        }
        if (includesEndpoints)
        {
            Include(model.Start.X, model.Start.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
            Include(model.Goal.X, model.Goal.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
        }
        if (!hasPoint)
        {
            xMin = model.WorldXMinMeters;
            xMax = model.WorldXMaxMeters;
            yMin = model.WorldYMinMeters;
            yMax = model.WorldYMaxMeters;
        }
        ExpandRange(ref xMin, ref xMax, MinimumExtentMeters);
        ExpandRange(ref yMin, ref yMax, MinimumExtentMeters);
        double xPadding = (xMax - xMin) * PaddingFraction;
        double yPadding = (yMax - yMin) * PaddingFraction;
        xMin -= xPadding;
        xMax += xPadding;
        yMin -= yPadding;
        yMax += yPadding;
        const double desiredAspect = 400d / 245d;
        double width = xMax - xMin;
        double height = yMax - yMin;
        if (width / height < desiredAspect)
        {
            double halfWidth = height * desiredAspect / 2d;
            double center = (xMin + xMax) / 2d;
            xMin = center - halfWidth;
            xMax = center + halfWidth;
        }
        else
        {
            double halfHeight = width / desiredAspect / 2d;
            double center = (yMin + yMax) / 2d;
            yMin = center - halfHeight;
            yMax = center + halfHeight;
        }
        return new Bounds(xMin, xMax, yMin, yMax);
    }

    private static void Include(double x, double y, ref bool hasPoint, ref double xMin, ref double xMax, ref double yMin, ref double yMax)
    {
        if (!hasPoint)
        {
            hasPoint = true;
            xMin = x;
            xMax = x;
            yMin = y;
            yMax = y;
            return;
        }
        if (x < xMin) xMin = x;
        if (x > xMax) xMax = x;
        if (y < yMin) yMin = y;
        if (y > yMax) yMax = y;
    }

    private static void ExpandRange(ref double minimum, ref double maximum, double minimumExtent)
    {
        double extent = maximum - minimum;
        if (extent >= minimumExtent) return;
        double center = (minimum + maximum) / 2d;
        minimum = center - minimumExtent / 2d;
        maximum = center + minimumExtent / 2d;
    }

    private static double ExpandMaximum(double value, double minimum)
    {
        return value < minimum ? minimum : value * (1d + PaddingFraction);
    }

    private static IReadOnlyList<double> BuildTicks(double minimum, double maximum)
    {
        double span = maximum - minimum;
        if (span <= 0d) return new[] { minimum, maximum };
        double roughStep = span / 5d;
        double magnitude = Math.Pow(10d, Math.Floor(Math.Log10(roughStep)));
        double normalized = roughStep / magnitude;
        double nice = normalized <= 1d ? 1d : (normalized <= 2d ? 2d : (normalized <= 5d ? 5d : 10d));
        double step = nice * magnitude;
        var ticks = new List<double>();
        double first = Math.Ceiling(minimum / step) * step;
        for (double value = first; value <= maximum + step * 0.001d; value += step) ticks.Add(value);
        if (ticks.Count < 2)
        {
            ticks.Clear();
            ticks.Add(minimum);
            ticks.Add(maximum);
        }
        return new ReadOnlyCollection<double>(ticks);
    }

    private static SmoothingFigureSeries Find(SmoothingFigureModel model, string key)
    {
        for (int index = 0; index < model.Series.Count; index++)
        {
            if (model.Series[index].Key == key) return model.Series[index];
        }
        throw new InvalidOperationException("Figure model is missing path series: " + key);
    }

    private static bool TryFind(SmoothingFigureModel model, string key, out SmoothingFigureSeries series)
    {
        for (int index = 0; index < model.Series.Count; index++)
        {
            if (model.Series[index].Key == key)
            {
                series = model.Series[index];
                return true;
            }
        }
        series = null;
        return false;
    }

    private static SmoothingFigureSeriesView View(
        SmoothingFigureSeries series,
        double opacity,
        double pointRadiusPoints = 1.35d)
    {
        return new SmoothingFigureSeriesView(series, opacity, pointRadiusPoints);
    }

    private readonly struct Bounds
    {
        public Bounds(double xMin, double xMax, double yMin, double yMax)
        {
            XMin = xMin;
            XMax = xMax;
            YMin = yMin;
            YMax = yMax;
        }

        public double XMin { get; }
        public double XMax { get; }
        public double YMin { get; }
        public double YMax { get; }
    }
}

/// <summary>Stable normal figure order; an optional Local G2 diagnostic figure is appended.</summary>
public sealed class SmoothingFigureSet
{
    internal SmoothingFigureSet(IReadOnlyList<SmoothingFigureDefinition> figures)
    {
        var copy = new List<SmoothingFigureDefinition>(figures == null ? 0 : figures.Count);
        if (figures != null)
        {
            for (int index = 0; index < figures.Count; index++) copy.Add(figures[index]);
        }
        Figures = new ReadOnlyCollection<SmoothingFigureDefinition>(copy);
    }

    public IReadOnlyList<SmoothingFigureDefinition> Figures { get; }
}