using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationStaticSnapshotBuilder
{
    public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap,
        EmPlannerConfiguration effectiveConfiguration, TrajectoryObservationSettings settings, long obstacleSnapshotVersion)
    {
        if (bootstrap == null) throw new ArgumentNullException(nameof(bootstrap));
        if (effectiveConfiguration == null) throw new ArgumentNullException(nameof(effectiveConfiguration));
        if (settings == null) throw new ArgumentNullException(nameof(settings));
        var map = bootstrap.Map ?? throw new ArgumentException("Bootstrap map is required.", nameof(bootstrap));
        var bounds = new VisualizationBounds(map.Bounds.XMin / 1000d, map.Bounds.XMax / 1000d,
            map.Bounds.YMin / 1000d, map.Bounds.YMax / 1000d);
        return new PlanningVisualizationStaticSnapshot("EM轨迹规划观察", bounds, CreateGrid(map, bounds),
            CreatePolylines(bootstrap), CreateMarkers(bootstrap), CreateSegments(bootstrap),
            CreateConfiguration(effectiveConfiguration, settings, map, obstacleSnapshotVersion));
    }

    private static VisualizationOccupancyGrid CreateGrid(Mapping.PlanningGridMap map, VisualizationBounds bounds)
    {
        var bits = new byte[(map.Rows * map.Cols + 7) / 8];
        for (int row = 0; row < map.Rows; row++)
            for (int column = 0; column < map.Cols; column++)
                if (map.IsOccupied(row, column)) bits[(row * map.Cols + column) >> 3] |= (byte)(1 << ((row * map.Cols + column) & 7));
        return new VisualizationOccupancyGrid(bounds, map.ResolutionMeters, map.Rows, map.Cols, bits);
    }

    private static IReadOnlyList<VisualizationPolyline> CreatePolylines(TrajectoryObservationBootstrapResult bootstrap)
    {
        var lines = new List<VisualizationPolyline>();
        var coarse = new List<VisualizationPoint>();
        foreach (var point in bootstrap.CoarseResult.PlanningResult.Path) coarse.Add(new VisualizationPoint(point.X, point.Y));
        if (coarse.Count > 0) lines.Add(new VisualizationPolyline("coarse-path", "粗路径", "coarse", VisualizationLineStyle.Dashed, coarse));
        var smooth = new List<VisualizationPoint>();
        foreach (var point in bootstrap.SmoothedPath.Path) smooth.Add(new VisualizationPoint(point.X, point.Y));
        if (smooth.Count > 0) lines.Add(new VisualizationPolyline("local-g2-path", "Local G2路径", "local-g2", VisualizationLineStyle.Solid, smooth));
        return lines;
    }

    private static IReadOnlyList<VisualizationMarker> CreateMarkers(TrajectoryObservationBootstrapResult bootstrap)
    {
        var markers = new List<VisualizationMarker>();
        foreach (DirectionSegmentView segment in bootstrap.Segments)
            if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
            {
                var point = segment.Points[segment.Points.Count - 1];
                markers.Add(new VisualizationMarker("gear-switch-" + segment.SegmentIndex, "gear-switch",
                    "换向 " + (segment.SegmentIndex + 1), new VisualizationPoint(point.X, point.Y)));
            }
        return markers;
    }

    private static IReadOnlyList<VisualizationDirectionSegment> CreateSegments(TrajectoryObservationBootstrapResult bootstrap)
    {
        var result = new List<VisualizationDirectionSegment>();
        foreach (DirectionSegmentView segment in bootstrap.Segments)
        {
            var points = new List<VisualizationPoint>();
            foreach (var point in segment.Points) points.Add(new VisualizationPoint(point.X, point.Y));
            result.Add(new VisualizationDirectionSegment(segment.SegmentIndex, segment.Direction.ToString(),
                segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture,
                segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points));
        }
        return result;
    }

    private static IReadOnlyList<VisualizationConfigurationGroup> CreateConfiguration(EmPlannerConfiguration configuration,
        TrajectoryObservationSettings settings, Mapping.PlanningGridMap map, long obstacleSnapshotVersion)
    {
        string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
        var groups = new List<VisualizationConfigurationGroup>();
        groups.Add(Group("调度", V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
            V("时域", "TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds), "s"),
            V("距离视界", "DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters), "m"),
            V("输出步长", "OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds), "s"),
            V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s")));
        groups.Add(Group("OSQP", V("最大外层迭代", "MaximumOuterIterations", configuration.Solver.MaximumOuterIterations.ToString(CultureInfo.InvariantCulture), ""),
            V("最大OSQP迭代", "MaximumOsqpIterations", configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture), ""),
            V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""), V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""), V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), "")));
        groups.Add(Group("地图", V("快照版本", "ObstacleSnapshotVersion", obstacleSnapshotVersion.ToString(CultureInfo.InvariantCulture), ""), V("分辨率", "ResolutionMm", F(map.ResolutionMm), "mm"), V("X最小", "Bounds.XMin", F(map.Bounds.XMin), "mm"), V("X最大", "Bounds.XMax", F(map.Bounds.XMax), "mm"), V("Y最小", "Bounds.YMin", F(map.Bounds.YMin), "mm"), V("Y最大", "Bounds.YMax", F(map.Bounds.YMax), "mm")));
        groups.Add(Group("可视化与换向确认", V("网页刷新率", "WebRefreshRateHz", settings.WebRefreshRateHz.ToString(CultureInfo.InvariantCulture), "Hz"), V("历史容量", "VisualizationHistoryCycleLimit", settings.VisualizationHistoryCycleLimit.ToString(CultureInfo.InvariantCulture), "cycles"), V("方向确认速度", "DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond), "m/s"), V("方向确认样本", "DirectionConfirmationSamples", settings.DirectionConfirmationSamples.ToString(CultureInfo.InvariantCulture), "")));
        return groups;
    }

    private static VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values) => new VisualizationConfigurationGroup(title, values);
    private static VisualizationValue V(string chinese, string raw, string value, string unit) => new VisualizationValue(chinese, raw, value, unit, "normal");
}