using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;

/// <summary>Transforms the raw-path and Local G2 results into shared report-figure data.</summary>
public sealed class SmoothingFigureModelBuilder
{
    private const double MarginPoints = 18d;
    private const double PanelGapPoints = 12d;

    public SmoothingFigureModel Build(
        PathSmoothingComparisonResult comparison,
        PlanningGridMap map,
        Pose2D start,
        Pose2D goal,
        string scenarioId,
        string scenarioLabel)
    {
        if (comparison == null) throw new ArgumentNullException(nameof(comparison));
        if (map == null) throw new ArgumentNullException(nameof(map));
        if (start == null) throw new ArgumentNullException(nameof(start));
        if (goal == null) throw new ArgumentNullException(nameof(goal));

        double worldXMin = map.Bounds.XMin / 1000d;
        double worldXMax = map.Bounds.XMax / 1000d;
        double worldYMin = map.Bounds.YMin / 1000d;
        double worldYMax = map.Bounds.YMax / 1000d;
        double innerWidth = IeeeFigureStyle.FigureWidthPoints - 2d * MarginPoints;
        double innerHeight = IeeeFigureStyle.FigureHeightPoints - 2d * MarginPoints;
        double pathWidth = innerWidth * 0.60d;
        double rightWidth = innerWidth - pathWidth - PanelGapPoints;
        double rightHeight = (innerHeight - PanelGapPoints) / 2d;

        var model = new SmoothingFigureModel(
            scenarioId, scenarioLabel, worldXMin, worldXMax, worldYMin, worldYMax,
            MarginPoints, MarginPoints, pathWidth, innerHeight,
            MarginPoints + pathWidth + PanelGapPoints, MarginPoints, rightWidth, rightHeight,
            MarginPoints + pathWidth + PanelGapPoints, MarginPoints + rightHeight + PanelGapPoints, rightWidth, rightHeight,
            BuildObstacles(map), BuildSeries(comparison, map), BuildMetricRows(comparison),
            new SmoothingFigurePoint(start.X, start.Y, 0d, 0d), new SmoothingFigurePoint(goal.X, goal.Y, 0d, 0d));

        double worldWidth = worldXMax - worldXMin;
        double worldHeight = worldYMax - worldYMin;
        if (worldWidth <= 0d || worldHeight <= 0d)
            throw new ArgumentOutOfRangeException(nameof(map), "Map bounds must be finite and non-degenerate.");
        double scale = Math.Min(pathWidth / worldWidth, innerHeight / worldHeight);
        model.PathScaleX = scale;
        model.PathScaleY = scale;
        return model;
    }

    private static IReadOnlyList<SmoothingFigureObstacle> BuildObstacles(PlanningGridMap map)
    {
        var runs = new List<ObstacleRun>();
        double resolution = map.ResolutionMeters;
        double xMin = map.Bounds.XMin / 1000d;
        double yMin = map.Bounds.YMin / 1000d;
        for (int row = 0; row < map.Rows; row++)
        {
            int runStart = -1;
            for (int column = 0; column <= map.Cols; column++)
            {
                bool occupied = column < map.Cols && map.IsOccupied(row, column);
                if (occupied && runStart < 0) { runStart = column; continue; }
                if (!occupied && runStart >= 0)
                {
                    AddOrExtendRun(runs, xMin + runStart * resolution, yMin + row * resolution,
                        (column - runStart) * resolution, resolution);
                    runStart = -1;
                }
            }
        }
        var obstacles = new List<SmoothingFigureObstacle>(runs.Count);
        for (int index = 0; index < runs.Count; index++)
        {
            ObstacleRun run = runs[index];
            obstacles.Add(new SmoothingFigureObstacle(run.X, run.Y, run.Width, run.Height));
        }
        return obstacles;
    }

    private static void AddOrExtendRun(IList<ObstacleRun> runs, double x, double y, double width, double height)
    {
        for (int index = runs.Count - 1; index >= 0; index--)
        {
            ObstacleRun candidate = runs[index];
            if (NearlyEqual(candidate.X, x) && NearlyEqual(candidate.Width, width) && NearlyEqual(candidate.Y + candidate.Height, y))
            {
                candidate.Height += height;
                return;
            }
        }
        runs.Add(new ObstacleRun(x, y, width, height));
    }

    private static bool NearlyEqual(double first, double second) => Math.Abs(first - second) < 1e-9d;

    private static IReadOnlyList<SmoothingFigureSeries> BuildSeries(PathSmoothingComparisonResult comparison, PlanningGridMap map)
    {
        return new List<SmoothingFigureSeries>(2)
        {
            CreateSeries(comparison.RawPathBaseline, null, "raw", "Raw coarse path", IeeeFigureStyle.RawColor, string.Empty, true, map),
            CreateSeries(FindLocalG2(comparison), SmoothingMethod.LocalG2Quintic, "local-g2", "Local G2", IeeeFigureStyle.LocalG2Color, string.Empty, false, map),
        };
    }

    private static IReadOnlyList<SmoothingFigureMetricRow> BuildMetricRows(PathSmoothingComparisonResult comparison)
    {
        return new List<SmoothingFigureMetricRow>
        {
            CreateRow(comparison.RawPathBaseline, "RawPath", "Raw coarse path"),
            CreateRow(FindLocalG2(comparison), "LocalG2Quintic", "Local G2"),
        };
    }

    private static PathSmoothingComparisonEntry FindLocalG2(PathSmoothingComparisonResult comparison)
    {
        for (int index = 0; index < comparison.Entries.Count; index++)
        {
            PathSmoothingComparisonEntry entry = comparison.Entries[index];
            if (entry.Method == SmoothingMethod.LocalG2Quintic) return entry;
        }
        return null;
    }

    private static SmoothingFigureMetricRow CreateRow(PathSmoothingComparisonEntry entry, string method, string label)
    {
        return entry == null
            ? new SmoothingFigureMetricRow(method, label, PathSmoothingStatus.Failed, new PathQualityMetrics(), null)
            : new SmoothingFigureMetricRow(method, label, entry.Status, entry.Metrics, entry.Timing);
    }

    private static SmoothingFigureSeries CreateSeries(
        PathSmoothingComparisonEntry entry,
        SmoothingMethod? method,
        string key,
        string label,
        string color,
        string dashArray,
        bool isRawPathBaseline,
        PlanningGridMap map)
    {
        var points = new List<SmoothingFigurePoint>();
        var violations = new List<SmoothingFigurePoint>();
        PathSmoothingStatus status = entry == null ? PathSmoothingStatus.Failed : entry.Status;
        if (entry != null)
        {
            for (int index = 0; index < entry.Path.Count; index++)
            {
                SmoothedPathPoint point = entry.Path[index];
                var figurePoint = new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature);
                points.Add(figurePoint);
                if (status == PathSmoothingStatus.Infeasible && map.IsOccupiedWorld(point.X, point.Y)) violations.Add(figurePoint);
            }
        }
        if (status == PathSmoothingStatus.Infeasible && points.Count > 0 && violations.Count == 0)
            violations.Add(points[points.Count / 2]);
        return new SmoothingFigureSeries(method, key, label, status, color, dashArray, isRawPathBaseline, points, violations);
    }

    private sealed class ObstacleRun
    {
        public ObstacleRun(double x, double y, double width, double height)
        {
            X = x;
            Y = y;
            Width = width;
            Height = height;
        }

        public double X { get; }
        public double Y { get; }
        public double Width { get; }
        public double Height { get; set; }
    }
}