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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Builds immutable world-space trajectory points from validated LS/ST results.</summary>
public sealed class EmTrajectoryAssembler
{
    private readonly double outputTimeStepSeconds;
    private readonly double zeroSpeedHoldSeconds;

    public EmTrajectoryAssembler()
        : this(EmPlannerConfiguration.CreateDefault())
    {
    }

    public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
            throw new ArgumentNullException(nameof(configuration));
        outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
        zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
            zeroSpeedHoldSeconds < 0d)
        {
            throw new ArgumentOutOfRangeException(nameof(configuration));
        }
    }

    public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
    {
        if (longitudinal == null || longitudinal.Candidate == null ||
            (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
        {
            throw new ArgumentException("Trajectory assembly requires a successful longitudinal result.", nameof(longitudinal));
        }
        if (metadata == null)
            throw new ArgumentNullException(nameof(metadata));

        var interpolator = new LateralPathInterpolator(path);
        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds);
        double terminalPathS = path.Points[path.Points.Count - 1].PathS;
        var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
        double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;

        for (int index = 0; index < schedule.Samples.Count; index++)
        {
            TrajectorySample sample = schedule.Samples[index];
            if (sample.PathS > terminalPathS + 1e-10d)
                throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal));

            InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS);
            bool isTerminalAnchor = index == longitudinal.Candidate.KnotTimes.Count - 1;
            EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
            double signedSpeed = directionSign * sample.ProgressSpeed;
            points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
                geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
                boundaryType, sample.Acceleration, sample.Jerk));
        }

        return new EmTrajectory(metadata, points);
    }

    private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)
    {
        switch (terminalType)
        {
            case EmTerminalType.RollingSafetyStop:
                return EmBoundaryType.RollingSafetyStop;
            case EmTerminalType.GearSwitch:
                return EmBoundaryType.GearSwitchApproach;
            case EmTerminalType.Goal:
                return EmBoundaryType.Goal;
            default:
                throw new ArgumentOutOfRangeException(nameof(terminalType));
        }
    }

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