using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Drawing;
using System.Globalization;
using ClumsyCore;
using ClumsyCore.DTools;
using MDCSToolBox.Clumsy.Pilot;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public static class TrajectoryObservationPresentationText
{
    public static string Create(TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
    {
        string projectionFailures = charts == null
            ? "unavailable"
            : charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
        if (observation == null || observation.SelectedPoint == null)
        {
            return "OBSERVE_ONLY: no chassis command is sent.\n" +
                "selected trajectory point unavailable\n" +
                "LS projection failures=" + projectionFailures;
        }

        EmTrajectoryPoint point = observation.SelectedPoint;
        return "OBSERVE_ONLY: no chassis command is sent.\n" +
            "selected t=" + point.TimeFromStart.ToString("F2", CultureInfo.InvariantCulture) +
            " s, path-S=" + point.PathS.ToString("F2", CultureInfo.InvariantCulture) + " m\n" +
            "signed speed=" + point.SignedLongitudinalVelocity.ToString("F2", CultureInfo.InvariantCulture) +
            " m/s, yaw rate=" + point.YawRate.ToString("F2", CultureInfo.InvariantCulture) + " rad/s\n" +
            "LS projection failures=" + projectionFailures;
    }
}

public sealed class TrajectoryObservationLsPresentationPoint
{
    public TrajectoryObservationLsPresentationPoint(double horizontalPathS, double verticalLateralOffset)
    {
        HorizontalPathS = horizontalPathS;
        VerticalLateralOffset = verticalLateralOffset;
    }

    public double HorizontalPathS { get; }

    public double VerticalLateralOffset { get; }
}

public sealed class TrajectoryObservationLsPresentationModel
{
    private TrajectoryObservationLsPresentationModel(IReadOnlyList<TrajectoryObservationLsPresentationPoint> samples)
    {
        Samples = new ReadOnlyCollection<TrajectoryObservationLsPresentationPoint>(
            new List<TrajectoryObservationLsPresentationPoint>(samples));
    }

    public string HorizontalAxisLabel => "ReferenceS (m)";

    public string VerticalAxisLabel => "l (m)";

    public IReadOnlyList<TrajectoryObservationLsPresentationPoint> Samples { get; }

    public static TrajectoryObservationLsPresentationModel Create(TrajectoryObservationCharts charts)
    {
        if (charts == null) throw new ArgumentNullException(nameof(charts));
        var samples = new List<TrajectoryObservationLsPresentationPoint>(charts.LsSamples.Count);
        for (int index = 0; index < charts.LsSamples.Count; index++)
        {
            TrajectoryObservationLsSample sample = charts.LsSamples[index];
            samples.Add(new TrajectoryObservationLsPresentationPoint(sample.PathS, sample.LateralOffset));
        }
        return new TrajectoryObservationLsPresentationModel(samples);
    }
}

public sealed class TrajectoryObservationPresentation
{
    private const float MillimetersPerMeter = 1000f;
    private const int MaximumVisibleGridLines = 80;
    private const float VelocitySeriesBaseMillimeters = -2500f;

    private readonly Painter worldPainter = UI.GetPainter("TrajectoryObserver.World", true);
    private readonly Painter lsPainter = UI.GetPainter("TrajectoryObserver.LS", true);
    private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);

    public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState,
        string diagnosticText)
    {
        worldPainter.Clear();
        if (bootstrap == null)
        {
            worldPainter.DrawText(Color.LightYellow, "Trajectory bootstrap unavailable.", 0f, 0f);
            return;
        }

        VehicleParameters vehicle = bootstrap.Vehicle;
        DrawMap(bootstrap.Map);
        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, vehicle, Color.LimeGreen, "计划起点");
        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, vehicle, Color.Orange, "终点");
        DrawCoarsePath(bootstrap.CoarseResult == null ? null : bootstrap.CoarseResult.PlanningResult.Path);
        DrawLocalG2Path(bootstrap.SmoothedPath == null ? null : bootstrap.SmoothedPath.Path);
        DrawPose(observation == null || observation.VehicleState == null ? null : observation.VehicleState.Pose,
            vehicle, Color.DeepSkyBlue, "车辆");
        if (runtimeState != null && runtimeState.WorldNotice.Length > 0)
        {
            float noticeX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
            float noticeY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMax - 200f;
            worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice, noticeX, noticeY);
        }

        EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
        {
            float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
            float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
            DrawBoundaryMarkers(bootstrap.Segments);
            worldPainter.DrawText(Color.LightYellow,
                EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
            return;
        }
        DrawEmPath(trajectory.Points);
        DrawBoundaryMarkers(bootstrap.Segments);
    }

    public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
    {
        lsPainter.Clear();
        if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
        {
            lsPainter.DrawText(Color.LightYellow,
                EmptyChartMessage("No published trajectory available for L-S chart.\n", diagnosticText), 0f, 0f);
            return;
        }

        TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts);
        float maxS = MaximumPathS(charts.LsSamples);
        float maxL = MaximumLateralOffset(charts.LsSamples);
        lsPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxS, 0f, width: 2);
        lsPainter.DrawLine(Color.Gainsboro, 0f, -maxL, 0f, maxL, width: 2);
        lsPainter.DrawText(Color.White, "S-L", maxS + 100f, maxL + 150f);
        lsPainter.DrawText(Color.White, model.HorizontalAxisLabel, maxS + 100f, -200f);
        lsPainter.DrawText(Color.White, model.VerticalAxisLabel, 100f, maxL + 300f);
        for (int index = 1; index < model.Samples.Count; index++)
        {
            TrajectoryObservationLsPresentationPoint previous = model.Samples[index - 1];
            TrajectoryObservationLsPresentationPoint current = model.Samples[index];
            lsPainter.DrawLine(Color.MediumPurple, ToMillimeters(previous.HorizontalPathS),
                ToMillimeters(previous.VerticalLateralOffset), ToMillimeters(current.HorizontalPathS),
                ToMillimeters(current.VerticalLateralOffset), width: 3);
        }
    }

    public void DrawSt(TrajectoryObservationCharts charts, string diagnosticText)
    {
        stPainter.Clear();
        if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
        {
            stPainter.DrawText(Color.LightYellow,
                EmptyChartMessage("No published trajectory available for T-S/T-V charts.\n", diagnosticText), 0f, 0f);
            return;
        }

        float maxTime = MaximumTime(charts.StSamples, charts.SpeedSamples);
        float maxS = MaximumPathS(charts.StSamples);
        stPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxTime, 0f, width: 2);
        stPainter.DrawLine(Color.Gainsboro, 0f, 0f, 0f, maxS, width: 2);
        stPainter.DrawText(Color.White, "T-S", 0f, maxS + 150f);
        stPainter.DrawText(Color.White, "t (s)", maxTime + 100f, -200f);
        stPainter.DrawText(Color.White, "PathS (m)", 100f, maxS + 300f);
        DrawStSeries(charts.StSamples);

        stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters,
            maxTime, VelocitySeriesBaseMillimeters, width: 2);
        stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters, 0f,
            VelocitySeriesBaseMillimeters + MillimetersPerMeter, width: 2);
        stPainter.DrawText(Color.White, "T-V", 0f, VelocitySeriesBaseMillimeters - 200f);
        DrawSpeedSeries(charts.SpeedSamples);
        DrawStLegend(maxTime, maxS);
    }

    public void ClearAll()
    {
        worldPainter.Clear();
        lsPainter.Clear();
        stPainter.Clear();
    }

    private static string EmptyChartMessage(string label, string diagnosticText)
    {
        return string.IsNullOrWhiteSpace(diagnosticText)
            ? label
            : label + diagnosticText;
    }

    private void DrawMap(PlanningGridMap map)
    {
        if (map == null) return;
        float xMin = map.Bounds.XMin;
        float xMax = map.Bounds.XMax;
        float yMin = map.Bounds.YMin;
        float yMax = map.Bounds.YMax;
        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) /
            (double)MaximumVisibleGridLines));
        for (int column = 0; column <= map.Cols; column += gridStride)
        {
            float x = Math.Min(xMax, xMin + column * map.ResolutionMm);
            worldPainter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
        }
        for (int row = 0; row <= map.Rows; row += gridStride)
        {
            float y = Math.Min(yMax, yMin + row * map.ResolutionMm);
            worldPainter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
        }
        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
        for (int row = 0; row < map.Rows; row++)
        {
            for (int column = 0; column < map.Cols; column++)
            {
                if (!map.IsOccupied(row, column)) continue;
                float x = xMin + column * map.ResolutionMm + map.ResolutionMm / 2f;
                float y = yMin + row * map.ResolutionMm;
                worldPainter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + map.ResolutionMm,
                    width: Math.Max(1, (int)Math.Round(map.ResolutionMm)));
            }
        }
    }

    private void DrawPose(Pose2D pose, VehicleParameters vehicle, Color color, string label)
    {
        if (pose == null) return;
        double lengthMeters = vehicle == null || vehicle.LengthMeters <= 0d ? 0.8d : vehicle.LengthMeters;
        double widthMeters = vehicle == null || vehicle.WidthMeters <= 0d ? 0.6d : vehicle.WidthMeters;
        double heading = pose.Heading;
        double cosHeading = Math.Cos(heading);
        double sinHeading = Math.Sin(heading);
        float x = ToMillimeters(pose.X);
        float y = ToMillimeters(pose.Y);
        float halfLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter / 2d);
        float halfWidthMillimeters = (float)(widthMeters * MillimetersPerMeter / 2d);
        float frontLeftX = x + (float)(cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
        float frontLeftY = y + (float)(sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
        float frontRightX = x + (float)(cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
        float frontRightY = y + (float)(sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
        float rearRightX = x + (float)(-cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
        float rearRightY = y + (float)(-sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
        float rearLeftX = x + (float)(-cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
        float rearLeftY = y + (float)(-sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
        DrawVehicleOutline(frontLeftX, frontLeftY, frontRightX, frontRightY,
            rearRightX, rearRightY, rearLeftX, rearLeftY, color);
        float headingLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter * 0.25d);
        worldPainter.DrawLine(color, x, y,
            x + (float)cosHeading * headingLengthMillimeters,
            y + (float)sinHeading * headingLengthMillimeters, endArrow: false, width: 1);
        worldPainter.DrawText(color, label, x + 100f, y + 100f);
    }

    private void DrawVehicleOutline(float frontLeftX, float frontLeftY, float frontRightX, float frontRightY,
        float rearRightX, float rearRightY, float rearLeftX, float rearLeftY, Color color)
    {
        worldPainter.DrawLine(color, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 1);
        worldPainter.DrawLine(color, frontRightX, frontRightY, rearRightX, rearRightY, width: 1);
        worldPainter.DrawLine(color, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 1);
        worldPainter.DrawLine(color, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 1);
    }

    private void DrawBoundaryMarkers(IReadOnlyList<DirectionSegmentView> segments)
    {
        if (segments == null) return;
        for (int index = 0; index < segments.Count; index++)
        {
            DirectionSegmentView segment = segments[index];
            if (segment.Points.Count == 0) continue;
            var point = segment.Points[segment.Points.Count - 1];
            if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
            {
                DrawDiamondMarker(point, Color.Orange,
                    "换向点 " + (segment.SegmentIndex + 1).ToString(CultureInfo.InvariantCulture) + " / s_end");
            }
            else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
            {
                DrawCrossMarker(point, Color.OrangeRed, "终点 / s_end");
            }
        }
    }

    private void DrawDiamondMarker(SmoothedPathPoint point, Color color, string label)
    {
        float x = ToMillimeters(point.X);
        float y = ToMillimeters(point.Y);
        const float radius = 50f;
        worldPainter.DrawLine(color, x, y + radius, x + radius, y, width: 1);
        worldPainter.DrawLine(color, x + radius, y, x, y - radius, width: 1);
        worldPainter.DrawLine(color, x, y - radius, x - radius, y, width: 1);
        worldPainter.DrawLine(color, x - radius, y, x, y + radius, width: 1);
        worldPainter.DrawText(color, label, x + 100f, y + 100f);
    }

    private void DrawCrossMarker(SmoothedPathPoint point, Color color, string label)
    {
        float x = ToMillimeters(point.X);
        float y = ToMillimeters(point.Y);
        const float radius = 50f;
        worldPainter.DrawLine(color, x - radius, y - radius, x + radius, y + radius, width: 1);
        worldPainter.DrawLine(color, x - radius, y + radius, x + radius, y - radius, width: 1);
        worldPainter.DrawText(color, label, x + 100f, y + 100f);
    }

    private void DrawCoarsePath(IReadOnlyList<CoarsePathPoint> path)
    {
        if (path == null || path.Count == 0) return;
        for (int index = 1; index < path.Count; index++)
            worldPainter.DrawLine(Color.LimeGreen, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
    }

    private void DrawLocalG2Path(IReadOnlyList<SmoothedPathPoint> path)
    {
        if (path == null || path.Count == 0) return;
        for (int index = 1; index < path.Count; index++)
            worldPainter.DrawLine(Color.Gold, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 1);
    }

    private void DrawEmPath(IReadOnlyList<EmTrajectoryPoint> path)
    {
        for (int index = 1; index < path.Count; index++)
            worldPainter.DrawLine(Color.DeepPink, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
    }

    private void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
    {
        worldPainter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
        worldPainter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
        worldPainter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
        worldPainter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
    }

    private void DrawStSeries(IReadOnlyList<TrajectoryObservationStSample> samples)
    {
        for (int index = 1; index < samples.Count; index++)
            stPainter.DrawLine(Color.Cyan, ToMillimeters(samples[index - 1].TimeFromStart),
                ToMillimeters(samples[index - 1].PathS), ToMillimeters(samples[index].TimeFromStart),
                ToMillimeters(samples[index].PathS), width: 3);
    }

    private void DrawSpeedSeries(IReadOnlyList<TrajectoryObservationSpeedSample> samples)
    {
        if (samples == null) return;
        for (int index = 1; index < samples.Count; index++)
            stPainter.DrawLine(Color.Orange, ToMillimeters(samples[index - 1].TimeFromStart),
                VelocitySeriesBaseMillimeters + ToMillimeters(samples[index - 1].SignedLongitudinalVelocity),
                ToMillimeters(samples[index].TimeFromStart),
                VelocitySeriesBaseMillimeters + ToMillimeters(samples[index].SignedLongitudinalVelocity), width: 3);
    }

    private void DrawStLegend(float maxTime, float maxS)
    {
        float x = maxTime + 250f;
        stPainter.DrawLine(Color.Cyan, x, maxS, x + 200f, maxS, width: 4);
        stPainter.DrawText(Color.Cyan, "t-s", x + 250f, maxS - 50f);
        stPainter.DrawLine(Color.Orange, x, VelocitySeriesBaseMillimeters, x + 200f,
            VelocitySeriesBaseMillimeters, width: 4);
        stPainter.DrawText(Color.Orange, "t-v", x + 250f, VelocitySeriesBaseMillimeters - 50f);
    }

    private static float MaximumPathS(IReadOnlyList<TrajectoryObservationLsSample> samples)
    {
        double maximum = 1d;
        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
        return ToMillimeters(maximum);
    }

    private static float MaximumPathS(IReadOnlyList<TrajectoryObservationStSample> samples)
    {
        double maximum = 1d;
        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
        return ToMillimeters(maximum);
    }

    private static float MaximumLateralOffset(IReadOnlyList<TrajectoryObservationLsSample> samples)
    {
        double maximum = 1d;
        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, Math.Abs(samples[index].LateralOffset));
        return ToMillimeters(maximum);
    }

    private static float MaximumTime(IReadOnlyList<TrajectoryObservationStSample> stSamples,
        IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples)
    {
        double maximum = 1d;
        for (int index = 0; index < stSamples.Count; index++) maximum = Math.Max(maximum, stSamples[index].TimeFromStart);
        if (speedSamples != null)
            for (int index = 0; index < speedSamples.Count; index++)
                maximum = Math.Max(maximum, speedSamples[index].TimeFromStart);
        return ToMillimeters(maximum);
    }

    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
}