using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Resamples a compatible published trajectory onto the current ST knots as soft longitudinal references.</summary>
public sealed class LongitudinalPreviousTrajectorySeed
{
    private static readonly LongitudinalPreviousTrajectorySeed empty = new LongitudinalPreviousTrajectorySeed(
        Array.Empty<double>(), Array.Empty<double>());

    public LongitudinalPreviousTrajectorySeed(IReadOnlyList<double> pathS,
        IReadOnlyList<double> progressSpeedMetersPerSecond)
    {
        if (pathS == null)
            throw new ArgumentNullException(nameof(pathS));
        if (progressSpeedMetersPerSecond == null)
            throw new ArgumentNullException(nameof(progressSpeedMetersPerSecond));
        if (pathS.Count != progressSpeedMetersPerSecond.Count)
            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.");

        var copiedPathS = new List<double>(pathS.Count);
        var copiedSpeed = new List<double>(progressSpeedMetersPerSecond.Count);
        for (int index = 0; index < pathS.Count; index++)
        {
            if (!IsFinite(pathS[index]) || pathS[index] < 0d ||
                !IsFinite(progressSpeedMetersPerSecond[index]) || progressSpeedMetersPerSecond[index] < 0d)
            {
                throw new ArgumentOutOfRangeException(nameof(pathS));
            }
            copiedPathS.Add(pathS[index]);
            copiedSpeed.Add(progressSpeedMetersPerSecond[index]);
        }

        PathS = new ReadOnlyCollection<double>(copiedPathS);
        ProgressSpeedMetersPerSecond = new ReadOnlyCollection<double>(copiedSpeed);
    }

    public IReadOnlyList<double> PathS { get; }

    public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }

    public static LongitudinalPreviousTrajectorySeed Empty { get { return empty; } }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}

/// <summary>Builds monotone PathS and progress-speed soft references from a prior published trajectory.</summary>
public sealed class LongitudinalPreviousTrajectorySeedBuilder
{
    private const double ProjectionTolerance = 1e-10d;

    public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
        DateTimeOffset newEffectiveAtUtc, LongitudinalKnotSchedule knotSchedule, int segmentIndex,
        TravelDirection direction)
    {
        if (knotSchedule == null)
            return LongitudinalPreviousTrajectorySeed.Empty;
        return Build(previous, currentPath, newEffectiveAtUtc, knotSchedule.KnotTimes, segmentIndex, direction);
    }

    public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
        DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
        TravelDirection direction)
    {
        try
        {
            if (!IsCompatible(previous, currentPath, newKnotTimes, segmentIndex, direction))
                return LongitudinalPreviousTrajectorySeed.Empty;

            var pathS = new List<double>(newKnotTimes.Count);
            var progressSpeed = new List<double>(newKnotTimes.Count);
            double previousProjectedPathS = double.NegativeInfinity;
            for (int index = 0; index < newKnotTimes.Count; index++)
            {
                DateTimeOffset sampleUtc = newEffectiveAtUtc.AddSeconds(newKnotTimes[index]);
                double previousTimeSeconds = (sampleUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds;
                if (!TryInterpolate(previous.Points, previousTimeSeconds, out InterpolatedPreviousSample sample) ||
                    !TryProjectMonotonically(currentPath, sample.X, sample.Y, previousProjectedPathS,
                        out double projectedPathS))
                {
                    return LongitudinalPreviousTrajectorySeed.Empty;
                }

                pathS.Add(projectedPathS);
                progressSpeed.Add(Math.Abs(sample.SignedSpeedMetersPerSecond));
                previousProjectedPathS = projectedPathS;
            }

            return new LongitudinalPreviousTrajectorySeed(pathS, progressSpeed);
        }
        catch
        {
            return LongitudinalPreviousTrajectorySeed.Empty;
        }
    }

    private static bool IsCompatible(EmTrajectory previous, LateralPath currentPath,
        IReadOnlyList<double> newKnotTimes, int segmentIndex, TravelDirection direction)
    {
        if (previous == null || currentPath == null || newKnotTimes == null || segmentIndex < 0 ||
            !Enum.IsDefined(typeof(TravelDirection), direction) || !currentPath.IsIndependentlyValidated ||
            currentPath.Points.Count < 2 || previous.Metadata == null || previous.Points.Count < 2 ||
            previous.Metadata.SegmentIndex != segmentIndex || previous.Metadata.Direction != direction)
        {
            return false;
        }

        double previousKnotTime = double.NegativeInfinity;
        for (int index = 0; index < newKnotTimes.Count; index++)
        {
            if (!IsFinite(newKnotTimes[index]) || newKnotTimes[index] < 0d ||
                newKnotTimes[index] <= previousKnotTime)
            {
                return false;
            }
            previousKnotTime = newKnotTimes[index];
        }
        if (newKnotTimes.Count == 0)
            return false;

        double previousTime = double.NegativeInfinity;
        for (int index = 0; index < previous.Points.Count; index++)
        {
            EmTrajectoryPoint point = previous.Points[index];
            if (point == null || point.Direction != direction || !IsFinite(point.TimeFromStart) ||
                !IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.SignedLongitudinalVelocity) ||
                point.TimeFromStart <= previousTime)
            {
                return false;
            }
            previousTime = point.TimeFromStart;
        }

        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < currentPath.Points.Count; index++)
        {
            LateralPathPoint point = currentPath.Points[index];
            if (point == null || !IsFinite(point.PathS) || !IsFinite(point.X) || !IsFinite(point.Y) ||
                point.PathS <= previousPathS)
            {
                return false;
            }
            previousPathS = point.PathS;
        }
        return true;
    }

    private static bool TryInterpolate(IReadOnlyList<EmTrajectoryPoint> points, double sampleTimeSeconds,
        out InterpolatedPreviousSample sample)
    {
        sample = default;
        if (!IsFinite(sampleTimeSeconds) || sampleTimeSeconds < points[0].TimeFromStart - ProjectionTolerance ||
            sampleTimeSeconds > points[points.Count - 1].TimeFromStart + ProjectionTolerance)
        {
            return false;
        }

        if (sampleTimeSeconds <= points[0].TimeFromStart + ProjectionTolerance)
        {
            sample = InterpolatedPreviousSample.From(points[0]);
            return true;
        }

        for (int index = 1; index < points.Count; index++)
        {
            EmTrajectoryPoint right = points[index];
            if (sampleTimeSeconds <= right.TimeFromStart + ProjectionTolerance)
            {
                EmTrajectoryPoint left = points[index - 1];
                double ratio = (sampleTimeSeconds - left.TimeFromStart) /
                    (right.TimeFromStart - left.TimeFromStart);
                ratio = Math.Max(0d, Math.Min(1d, ratio));
                sample = new InterpolatedPreviousSample(
                    Linear(left.X, right.X, ratio),
                    Linear(left.Y, right.Y, ratio),
                    Linear(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, ratio));
                return true;
            }
        }

        return false;
    }

    private static bool TryProjectMonotonically(LateralPath path, double x, double y, double minimumPathS,
        out double projectedPathS)
    {
        projectedPathS = 0d;
        double bestDistanceSquared = double.PositiveInfinity;
        bool found = false;
        for (int index = 1; index < path.Points.Count; index++)
        {
            LateralPathPoint left = path.Points[index - 1];
            LateralPathPoint right = path.Points[index];
            double dx = right.X - left.X;
            double dy = right.Y - left.Y;
            double lengthSquared = dx * dx + dy * dy;
            if (!IsFinite(lengthSquared) || lengthSquared <= ProjectionTolerance)
                continue;

            double ratio = ((x - left.X) * dx + (y - left.Y) * dy) / lengthSquared;
            ratio = Math.Max(0d, Math.Min(1d, ratio));
            double candidatePathS = Linear(left.PathS, right.PathS, ratio);
            if (candidatePathS + ProjectionTolerance < minimumPathS)
                continue;

            double projectedX = Linear(left.X, right.X, ratio);
            double projectedY = Linear(left.Y, right.Y, ratio);
            double distanceSquared = (x - projectedX) * (x - projectedX) + (y - projectedY) * (y - projectedY);
            if (!found || distanceSquared < bestDistanceSquared - ProjectionTolerance ||
                (Math.Abs(distanceSquared - bestDistanceSquared) <= ProjectionTolerance && candidatePathS < projectedPathS))
            {
                projectedPathS = candidatePathS;
                bestDistanceSquared = distanceSquared;
                found = true;
            }
        }

        if (!found)
            return false;
        if (minimumPathS > double.NegativeInfinity)
            projectedPathS = Math.Max(minimumPathS, projectedPathS);
        return true;
    }

    private static double Linear(double left, double right, double ratio)
    {
        return left + (right - left) * ratio;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    private readonly struct InterpolatedPreviousSample
    {
        public InterpolatedPreviousSample(double x, double y, double signedSpeedMetersPerSecond)
        {
            X = x;
            Y = y;
            SignedSpeedMetersPerSecond = signedSpeedMetersPerSecond;
        }

        public double X { get; }
        public double Y { get; }
        public double SignedSpeedMetersPerSecond { get; }

        public static InterpolatedPreviousSample From(EmTrajectoryPoint point)
        {
            return new InterpolatedPreviousSample(point.X, point.Y, point.SignedLongitudinalVelocity);
        }
    }
}