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 => "path-S (m)";

    public string VerticalAxisLabel => "lateral offset (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)
    {
        worldPainter.Clear();
        if (bootstrap == null)
        {
            worldPainter.DrawText(Color.LightYellow, "Trajectory bootstrap unavailable.", 0f, 0f);
            return;
        }

        DrawMap(bootstrap.Map);
        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, Color.LimeGreen, "start");
        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, Color.Orange, "goal");
        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,
            Color.DeepSkyBlue, "real pose");
        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)
        {
            worldPainter.DrawText(Color.LightYellow, "No published trajectory available.", 0f, 0f);
            return;
        }
        DrawEmPath(trajectory.Points);
    }

    public void DrawLs(TrajectoryObservationCharts charts)
    {
        lsPainter.Clear();
        if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
        {
            lsPainter.DrawText(Color.LightYellow, "No published trajectory available for L-S chart.", 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)
    {
        stPainter.Clear();
        if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
        {
            stPainter.DrawText(Color.LightYellow, "No published trajectory available for T-S/T-V charts.", 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);
        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 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, Color color, string label)
    {
        if (pose == null) return;
        float x = ToMillimeters(pose.X);
        float y = ToMillimeters(pose.Y);
        worldPainter.DrawCircle(color, x, y, 80f);
        worldPainter.DrawLine(color, x, y, x + (float)Math.Cos(pose.Heading) * 250f,
            y + (float)Math.Sin(pose.Heading) * 250f, endArrow: true, width: 3);
        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: 4);
    }

    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: 3);
    }

    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: 4);
    }

    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);
}