using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
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, bootstrap.Vehicle, 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))
                {
                    int bitIndex = row * map.Cols + column;
                    bits[bitIndex >> 3] |= (byte)(1 << (bitIndex & 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>();
        if (bootstrap.SmoothedPath.Path.Count > 0)
        {
            var start = bootstrap.SmoothedPath.Path[0];
            markers.Add(new VisualizationMarker("smooth-start", "smooth-start",
                "平滑路径起点", new VisualizationPoint(start.X, start.Y)));
        }
        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-end-" + segment.SegmentIndex, "gear-switch-end",
                    "换向点 " + (segment.SegmentIndex + 1) + " / s_end",
                    new VisualizationPoint(point.X, point.Y)));
            }
            else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
            {
                var point = segment.Points[segment.Points.Count - 1];
                markers.Add(new VisualizationMarker("final-goal-" + segment.SegmentIndex, "final-goal",
                    "终点 / s_end", 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, VehicleParameters vehicle, Mapping.PlanningGridMap map,
        long obstacleSnapshotVersion)
    {
        string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
        string I(int value) => value.ToString(CultureInfo.InvariantCulture);
        string L(long value) => value.ToString(CultureInfo.InvariantCulture);
        string B(bool value) => value ? "true" : "false";
        string N(double? value) => value.HasValue ? F(value.Value) : "不可用";

        return new List<VisualizationConfigurationGroup>
        {
            Group("调度",
                V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
                V("ST 时域", "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"),
                V("交接前视", "HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds), "s"),
                V("状态最大年龄", "MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds), "s")),
            Group("OSQP",
                V("最大外层迭代", "MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations), ""),
                V("最大 OSQP 迭代", "MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations), ""),
                V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""),
                V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""),
                V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), ""),
                V("Warm start", "WarmStart", B(configuration.Solver.WarmStart), ""),
                V("Polish", "Polish", B(configuration.Solver.Polish), ""),
                V("原生详细输出", "NativeVerbose", B(configuration.Solver.NativeVerbose), "")),
            Group("车辆与安全",
                V("车辆长度", "LengthMeters", F(vehicle.LengthMeters), "m"),
                V("车辆宽度", "WidthMeters", F(vehicle.WidthMeters), "m"),
                V("安全余量", "SafetyMarginMeters", F(vehicle.SafetyMarginMeters), "m"),
                V("最大曲率", "MaximumCurvaturePerMeter", N(vehicle.MaximumCurvaturePerMeter), "1/m"),
                V("最小转弯半径", "MinimumTurningRadiusMeters", N(vehicle.MinimumTurningRadiusMeters), "m")),
            Group("纵向限制",
                V("最大前进速度", "MaximumForwardSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond), "m/s"),
                V("最大倒车速度", "MaximumReverseSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond), "m/s"),
                V("最大加速度", "MaximumAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared), "m/s²"),
                V("最大减速度", "MaximumDecelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared), "m/s²"),
                V("最大加加速度", "MaximumJerkMetersPerSecondCubed", F(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed), "m/s³"),
                V("最大横向加速度", "MaximumLateralAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared), "m/s²"),
                V("最大曲率变化率", "MaximumCurvatureRatePerMeterPerSecond", F(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond), "1/(m·s)"),
                V("停车速度容差", "StopSpeedToleranceMetersPerSecond", F(configuration.Longitudinal.StopSpeedToleranceMetersPerSecond), "m/s"),
                V("零速保持", "ZeroSpeedHoldSeconds", F(configuration.Longitudinal.ZeroSpeedHoldSeconds), "s")),
            Group("ST 权重",
                V("参考速度", "ReferenceSpeed", F(configuration.Longitudinal.Weights.ReferenceSpeed), ""),
                V("加速度", "Acceleration", F(configuration.Longitudinal.Weights.Acceleration), ""),
                V("加加速度", "Jerk", F(configuration.Longitudinal.Weights.Jerk), ""),
                V("上一轨迹", "PreviousTrajectory", F(configuration.Longitudinal.Weights.PreviousTrajectory), ""),
                V("终端加速度", "TerminalAcceleration", F(configuration.Longitudinal.Weights.TerminalAcceleration), "")),
            Group("横向限制",
                V("单次横向步长", "MaximumLateralStepPerIterationMeters", F(configuration.Lateral.MaximumLateralStepPerIterationMeters), "m"),
                V("最大横向斜率", "MaximumLateralSlope", F(configuration.Lateral.MaximumLateralSlope), ""),
                V("最大二阶导", "MaximumLateralSecondDerivativePerMeter", F(configuration.Lateral.MaximumLateralSecondDerivativePerMeter), "1/m"),
                V("最大三阶导", "MaximumLateralThirdDerivativePerSquareMeter", F(configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter), "1/m²")),
            Group("LS 权重",
                V("参考偏移", "ReferenceOffset", F(configuration.Lateral.Weights.ReferenceOffset), ""),
                V("航向偏差", "HeadingDeviation", F(configuration.Lateral.Weights.HeadingDeviation), ""),
                V("二阶导", "SecondDerivative", F(configuration.Lateral.Weights.SecondDerivative), ""),
                V("三阶导", "ThirdDerivative", F(configuration.Lateral.Weights.ThirdDerivative), ""),
                V("曲率", "Curvature", F(configuration.Lateral.Weights.Curvature), ""),
                V("曲率变化", "CurvatureVariation", F(configuration.Lateral.Weights.CurvatureVariation), ""),
                V("上一轨迹", "PreviousTrajectory", F(configuration.Lateral.Weights.PreviousTrajectory), ""),
                V("滚动终端", "RollingTerminal", F(configuration.Lateral.Weights.RollingTerminal), "")),
            Group("走廊与投影",
                V("纵向采样间隔", "LongitudinalSampleSpacingMeters", F(configuration.Corridor.LongitudinalSampleSpacingMeters), "m"),
                V("横向采样间隔", "LateralSampleSpacingMeters", F(configuration.Corridor.LateralSampleSpacingMeters), "m"),
                V("最大横向偏移", "MaximumLateralOffsetMeters", F(configuration.Corridor.MaximumLateralOffsetMeters), "m"),
                V("附加净空储备", "AdditionalClearanceReserveMeters", F(configuration.Corridor.AdditionalClearanceReserveMeters), "m"),
                V("最大碰撞检查步长", "MaximumCollisionCheckStepMeters", F(configuration.Corridor.MaximumCollisionCheckStepMeters), "m"),
                V("最大投影距离", "MaximumProjectionDistanceMeters", F(configuration.Frenet.MaximumProjectionDistanceMeters), "m"),
                V("最小 Frenet 分母", "MinimumFrenetDenominator", F(configuration.Frenet.MinimumFrenetDenominator), ""),
                V("边界锚点容差", "BoundaryAnchorToleranceMeters", F(configuration.Frenet.BoundaryAnchorToleranceMeters), "m"),
                V("空间容差", "SpatialToleranceMeters", F(configuration.Validation.SpatialToleranceMeters), "m"),
                V("运动学容差", "KinematicTolerance", F(configuration.Validation.KinematicTolerance), "")),
            Group("地图",
                V("障碍快照版本", "ObstacleSnapshotVersion", L(obstacleSnapshotVersion), ""),
                V("地图快照编号", "MapSnapshotId", L(map.SnapshotId), ""),
                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"),
                V("行数", "Rows", I(map.Rows), ""),
                V("列数", "Cols", I(map.Cols), "")),
            Group("可视化与换向确认",
                V("规划范围", "PlanningScope", settings.PlanningScope.ToString(), ""),
                V("启用网页", "EnableWebVisualization", B(settings.EnableWebVisualization), ""),
                V("自动打开网页", "AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization), ""),
                V("网页端口", "WebVisualizationPort", I(settings.WebVisualizationPort), ""),
                V("网页刷新率", "WebRefreshRateHz", F(settings.WebRefreshRateHz), "Hz"),
                V("历史容量", "VisualizationHistoryCycleLimit", I(settings.VisualizationHistoryCycleLimit), "cycles"),
                V("启用 Painter", "EnableNativePainterVisualization", B(settings.EnableNativePainterVisualization), ""),
                V("方向确认速度", "DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond), "m/s"),
                V("方向确认样本", "DirectionConfirmationSamples", I(settings.DirectionConfirmationSamples), ""),
                V("换向投影容差", "GearSwitchProjectionToleranceMeters", F(settings.GearSwitchProjectionToleranceMeters), "m"),
                V("换向停车保持", "GearSwitchStopHoldSeconds", F(settings.GearSwitchStopHoldSeconds), "s")),
        };
    }

    private static VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values)
    {
        return new VisualizationConfigurationGroup(title, values);
    }

    private static VisualizationValue V(string chinese, string raw, string value, string unit)
    {
        return new VisualizationValue(chinese, raw, value, unit, "normal");
    }
}
