using System;
using System.Diagnostics;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Control.Execution
{
    // 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
    public enum PathTrackingCycleResult
    {
        Inactive = 0,
        CommandGenerated = 1,
        Completed = 2,
        Faulted = 3
    }

    // 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
    public readonly struct PathTrackingCycleOutput
    {
        public PathTrackingCycleOutput(
            PathTrackingCycleResult result,
            GcpMotionCommand? command,
            TrajectoryProjection? projection,
            double projectionMilliseconds,
            double controllerComputeMilliseconds)
        {
            Result = result;
            Command = command;
            Projection = projection;
            ProjectionMilliseconds = projectionMilliseconds;
            ControllerComputeMilliseconds =
                controllerComputeMilliseconds;
        }

        public PathTrackingCycleResult Result { get; }

        public GcpMotionCommand? Command { get; }

        public TrajectoryProjection? Projection { get; }

        public double ProjectionMilliseconds { get; }

        public double ControllerComputeMilliseconds { get; }
    }

    // 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
    public sealed class PathTrackingCore
    {
        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
            1e-6;
        private const double StartupRegionMeters = 0.02;
        private const double StartupPreviewDistanceMeters = 0.05;
        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
        private const double ProjectionBackwardSearchDistanceMeters =
            0.10;
        private const double ProjectionForwardSearchDistanceMeters =
            1.00;

        private readonly ILateralController _lateralController;
        private readonly ILongitudinalController _longitudinalController;
        private readonly GcpCommandAllocator _gcpAllocator;
        private readonly double _motionDirectionInBodyRadians;

        private Trajectory2D _trajectory;
        private double _terminalTravelDirection = 1.0;

        public PathTrackingCore(
            ILateralController lateralController,
            ILongitudinalController longitudinalController,
            GcpCommandAllocator gcpAllocator,
            double finishDistanceMeters = 0.04,
            double finishSpeedMetersPerSecond = 0.02,
            double finishHeadingToleranceRadians =
                3.0 * Math.PI / 180.0,
            double maximumDistanceToTrajectoryMeters = 0.30,
            double terminalBrakingPreviewMeters = 0.02,
            double terminalApproachDistanceMeters = 0.10,
            double terminalApproachGainPerSecond = 0.8,
            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
            double curvaturePreviewSeconds = 0.20,
            double maximumCurvaturePreviewMeters = 0.12,
            double motionDirectionInBodyRadians = 0.0)
        {
            _lateralController = lateralController ??
                throw new ArgumentNullException(
                    nameof(lateralController));
            _longitudinalController = longitudinalController ??
                throw new ArgumentNullException(
                    nameof(longitudinalController));
            _gcpAllocator = gcpAllocator ??
                throw new ArgumentNullException(
                    nameof(gcpAllocator));

            NumericGuard.EnsureFinitePositive(
                finishDistanceMeters,
                nameof(finishDistanceMeters));
            NumericGuard.EnsureFiniteNonNegative(
                finishSpeedMetersPerSecond,
                nameof(finishSpeedMetersPerSecond));
            NumericGuard.EnsureFinitePositive(
                finishHeadingToleranceRadians,
                nameof(finishHeadingToleranceRadians));
            NumericGuard.EnsureFinitePositive(
                maximumDistanceToTrajectoryMeters,
                nameof(maximumDistanceToTrajectoryMeters));
            NumericGuard.EnsureFiniteNonNegative(
                terminalBrakingPreviewMeters,
                nameof(terminalBrakingPreviewMeters));
            NumericGuard.EnsureFinitePositive(
                terminalApproachDistanceMeters,
                nameof(terminalApproachDistanceMeters));
            NumericGuard.EnsureFinitePositive(
                terminalApproachGainPerSecond,
                nameof(terminalApproachGainPerSecond));
            NumericGuard.EnsureFinitePositive(
                maximumTerminalApproachSpeedMetersPerSecond,
                nameof(maximumTerminalApproachSpeedMetersPerSecond));
            NumericGuard.EnsureFiniteNonNegative(
                curvaturePreviewSeconds,
                nameof(curvaturePreviewSeconds));
            NumericGuard.EnsureFiniteNonNegative(
                maximumCurvaturePreviewMeters,
                nameof(maximumCurvaturePreviewMeters));
            NumericGuard.EnsureFinite(
                motionDirectionInBodyRadians,
                nameof(motionDirectionInBodyRadians));

            if (terminalApproachDistanceMeters <=
                finishDistanceMeters)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(terminalApproachDistanceMeters),
                    "终点单向逼近范围必须大于终点位置容差。");
            }

            FinishDistanceMeters = finishDistanceMeters;
            FinishSpeedMetersPerSecond =
                finishSpeedMetersPerSecond;
            FinishHeadingToleranceRadians =
                finishHeadingToleranceRadians;
            MaximumDistanceToTrajectoryMeters =
                maximumDistanceToTrajectoryMeters;
            TerminalBrakingPreviewMeters =
                terminalBrakingPreviewMeters;
            TerminalApproachDistanceMeters =
                terminalApproachDistanceMeters;
            TerminalApproachGainPerSecond =
                terminalApproachGainPerSecond;
            MaximumTerminalApproachSpeedMetersPerSecond =
                maximumTerminalApproachSpeedMetersPerSecond;
            CurvaturePreviewSeconds = curvaturePreviewSeconds;
            MaximumCurvaturePreviewMeters =
                maximumCurvaturePreviewMeters;
            _motionDirectionInBodyRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionInBodyRadians);
        }

        public double FinishDistanceMeters { get; }

        public double FinishSpeedMetersPerSecond { get; }

        public double FinishHeadingToleranceRadians { get; }

        public double MaximumDistanceToTrajectoryMeters { get; }

        public double TerminalBrakingPreviewMeters { get; }

        public double TerminalApproachDistanceMeters { get; }

        public double TerminalApproachGainPerSecond { get; }

        public double MaximumTerminalApproachSpeedMetersPerSecond { get; }

        public double CurvaturePreviewSeconds { get; }

        public double MaximumCurvaturePreviewMeters { get; }

        public bool IsActive { get; private set; }

        public bool IsCompleted { get; private set; }

        public string LastFailureReason { get; private set; } =
            string.Empty;

        public Exception LastException { get; private set; }

        public TrajectoryProjection? LastProjection { get; private set; }

        public GcpMotionCommand? LastRequestedCommand { get; private set; }

        public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }

        public double? LastCurvaturePreviewDistanceMeters { get; private set; }

        public double? LastFeedforwardCurvaturePerMeter { get; private set; }

        // 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
        public void Start(Trajectory2D trajectory)
        {
            if (trajectory == null)
            {
                throw new ArgumentNullException(
                    nameof(trajectory));
            }

            var terminalTravelDirection =
                ResolveTerminalTravelDirection(trajectory);

            ResetFeedbackControllers();
            _trajectory = trajectory;
            _terminalTravelDirection =
                terminalTravelDirection;
            IsActive = true;
            IsCompleted = false;
            ClearDiagnostics();
        }

        // 将受控刚体的位姿和速度转换为本周期GCP命令。
        public PathTrackingCycleOutput Compute(
            Pose2D poseInWorld,
            Twist2D actualTwistInBody,
            bool hasValidVelocityEstimate,
            double deltaTimeSeconds)
        {
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.EnsureFinite(
                actualTwistInBody,
                nameof(actualTwistInBody));
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (!IsActive || _trajectory == null)
            {
                return new PathTrackingCycleOutput(
                    PathTrackingCycleResult.Inactive,
                    null,
                    LastProjection,
                    0.0,
                    0.0);
            }

            var projectionStartTimestamp =
                Stopwatch.GetTimestamp();
            var projectionCompleted = false;
            var projectionMilliseconds = 0.0;
            var controllerComputeStartTimestamp = 0L;

            try
            {
                var projection = LastProjection.HasValue
                    ? TrajectoryProjector.Project(
                        _trajectory,
                        poseInWorld,
                        LastProjection.Value.ArcLengthMeters,
                        ProjectionBackwardSearchDistanceMeters,
                        ProjectionForwardSearchDistanceMeters)
                    : TrajectoryProjector.Project(
                        _trajectory,
                        poseInWorld);

                projectionMilliseconds =
                    GetElapsedMilliseconds(
                        projectionStartTimestamp);
                projectionCompleted = true;
                LastProjection = projection;
                controllerComputeStartTimestamp =
                    Stopwatch.GetTimestamp();

                if (projection.DistanceToTrajectoryMeters >
                    MaximumDistanceToTrajectoryMeters)
                {
                    Fail(
                        "受控刚体距离参考轨迹" +
                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
                        "超过允许值" +
                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
                    return CreateOutput(
                        PathTrackingCycleResult.Faulted,
                        null,
                        projection,
                        projectionMilliseconds,
                        controllerComputeStartTimestamp);
                }

                if (HasReachedEnd(
                        poseInWorld,
                        actualTwistInBody,
                        hasValidVelocityEstimate,
                        projection))
                {
                    CompleteTrajectory();
                    return CreateOutput(
                        PathTrackingCycleResult.Completed,
                        null,
                        projection,
                        projectionMilliseconds,
                        controllerComputeStartTimestamp);
                }

                if (HasStoppedAtUnsatisfiedTerminal(
                        poseInWorld,
                        actualTwistInBody,
                        hasValidVelocityEstimate,
                        projection,
                        out var terminalFailureReason))
                {
                    Fail(terminalFailureReason);
                    return CreateOutput(
                        PathTrackingCycleResult.Faulted,
                        null,
                        projection,
                        projectionMilliseconds,
                        controllerComputeStartTimestamp);
                }

                var controlReferenceSpeedMetersPerSecond =
                    ResolveControlReferenceSpeed(
                        poseInWorld,
                        projection);
                LastControlReferenceSpeedMetersPerSecond =
                    controlReferenceSpeedMetersPerSecond;
                var curvaturePreviewDistanceMeters =
                    ResolveCurvaturePreviewDistanceMeters(
                        actualTwistInBody,
                        hasValidVelocityEstimate,
                        controlReferenceSpeedMetersPerSecond);
                LastCurvaturePreviewDistanceMeters =
                    curvaturePreviewDistanceMeters;
                var feedforwardCurvaturePerMeter =
                    ResolveFeedforwardCurvaturePerMeter(
                        projection,
                        curvaturePreviewDistanceMeters);
                LastFeedforwardCurvaturePerMeter =
                    feedforwardCurvaturePerMeter;
                var context = new PathTrackingContext(
                    actualTwistInBody,
                    hasValidVelocityEstimate,
                    projection,
                    controlReferenceSpeedMetersPerSecond,
                    feedforwardCurvaturePerMeter,
                    deltaTimeSeconds,
                    _motionDirectionInBodyRadians);
                var lateralCommand =
                    _lateralController.Compute(context);
                var commandSpeedMetersPerSecond =
                    _longitudinalController
                        .ComputeSpeedMetersPerSecond(context);
                var command = _gcpAllocator.Allocate(
                    commandSpeedMetersPerSecond,
                    lateralCommand);

                LastRequestedCommand = command;
                LastFailureReason = string.Empty;
                LastException = null;

                return CreateOutput(
                    PathTrackingCycleResult.CommandGenerated,
                    command,
                    projection,
                    projectionMilliseconds,
                    controllerComputeStartTimestamp);
            }
            catch (Exception exception)
            {
                if (!projectionCompleted)
                {
                    projectionMilliseconds =
                        GetElapsedMilliseconds(
                            projectionStartTimestamp);
                }

                Fail(
                    "轨迹跟踪核心计算异常：" +
                    exception.Message,
                    exception);

                return CreateOutput(
                    PathTrackingCycleResult.Faulted,
                    null,
                    LastProjection,
                    projectionMilliseconds,
                    controllerComputeStartTimestamp);
            }
        }

        // 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
        public void PauseForUnavailableState(string reason)
        {
            ResetFeedbackControllers();
            LastRequestedCommand = null;
            LastFailureReason = reason ?? string.Empty;
            LastException = null;
        }

        // 将外层执行故障同步到公共核心，并终止当前轨迹。
        public void Fail(
            string reason,
            Exception exception = null)
        {
            ResetFeedbackControllers();
            _trajectory = null;
            IsActive = false;
            IsCompleted = false;
            LastRequestedCommand = null;
            LastFailureReason = reason ?? string.Empty;
            LastException = exception;
        }

        // 取消当前轨迹并清除全部跟踪状态。
        public void Cancel()
        {
            ResetFeedbackControllers();
            _trajectory = null;
            _terminalTravelDirection = 1.0;
            IsActive = false;
            IsCompleted = false;
            ClearDiagnostics();
        }

        private double ResolveControlReferenceSpeed(
            Pose2D poseInWorld,
            TrajectoryProjection projection)
        {
            if (projection.RemainingDistanceMeters >
                TerminalApproachDistanceMeters)
            {
                return ResolveReferenceSpeedForControl(
                    projection);
            }

            return ResolveTerminalApproachSpeed(
                poseInWorld);
        }

        private double ResolveCurvaturePreviewDistanceMeters(
            Twist2D actualTwistInBody,
            bool hasValidVelocityEstimate,
            double controlReferenceSpeedMetersPerSecond)
        {
            if (CurvaturePreviewSeconds <= 0.0 ||
                MaximumCurvaturePreviewMeters <= 0.0)
            {
                return 0.0;
            }

            var previewSpeedMetersPerSecond =
                hasValidVelocityEstimate
                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
                        actualTwistInBody)
                    : Math.Abs(
                        controlReferenceSpeedMetersPerSecond);

            return Math.Min(
                MaximumCurvaturePreviewMeters,
                previewSpeedMetersPerSecond *
                CurvaturePreviewSeconds);
        }

        private double ResolveFeedforwardCurvaturePerMeter(
            TrajectoryProjection projection,
            double previewDistanceMeters)
        {
            var previewArcLengthMeters = Math.Min(
                _trajectory.TotalLengthMeters,
                projection.ArcLengthMeters +
                previewDistanceMeters);

            return _trajectory
                .SampleAtArcLength(previewArcLengthMeters)
                .CurvaturePerMeter;
        }

        private double ResolveTerminalApproachSpeed(
            Pose2D poseInWorld)
        {
            var distanceToEndMeters =
                CalculateDistanceToEndMeters(
                    poseInWorld);
            var headingErrorToEndRadians =
                CalculateHeadingErrorToEndRadians(
                    poseInWorld);

            if (distanceToEndMeters <=
                    FinishDistanceMeters &&
                headingErrorToEndRadians <=
                    FinishHeadingToleranceRadians)
            {
                return 0.0;
            }

            var endPose = _trajectory.EndPoint.PoseInWorld;
            var deltaX = endPose.XMeters -
                         poseInWorld.XMeters;
            var deltaY = endPose.YMeters -
                         poseInWorld.YMeters;
            var longitudinalErrorMeters =
                deltaX * Math.Cos(endPose.YawRadians) +
                deltaY * Math.Sin(endPose.YawRadians);
            var remainingAlongTravelMeters =
                _terminalTravelDirection *
                longitudinalErrorMeters;

            // 越过终点后不生成与原轨迹方向相反的修正速度。
            if (remainingAlongTravelMeters <= 0.0)
            {
                return 0.0;
            }

            var speedMagnitudeMetersPerSecond =
                Math.Min(
                    MaximumTerminalApproachSpeedMetersPerSecond,
                    TerminalApproachGainPerSecond *
                    remainingAlongTravelMeters);

            return _terminalTravelDirection *
                   speedMagnitudeMetersPerSecond;
        }

        private double ResolveReferenceSpeedForControl(
            TrajectoryProjection projection)
        {
            var currentReferenceSpeed =
                ApplyTerminalBrakingPreview(
                    projection,
                    projection.ReferencePoint
                        .ReferenceSpeedMetersPerSecond);
            var isInStartupRegion =
                projection.ArcLengthMeters <=
                    StartupRegionMeters &&
                projection.RemainingDistanceMeters >
                    FinishDistanceMeters;

            if (!isInStartupRegion)
            {
                return currentReferenceSpeed;
            }

            var previewArcLengthMeters = Math.Min(
                _trajectory.TotalLengthMeters,
                projection.ArcLengthMeters +
                StartupPreviewDistanceMeters);
            var previewReferenceSpeed =
                _trajectory
                    .SampleAtArcLength(previewArcLengthMeters)
                    .ReferenceSpeedMetersPerSecond;

            if (Math.Abs(previewReferenceSpeed) <=
                ZeroReferenceSpeedToleranceMetersPerSecond)
            {
                return currentReferenceSpeed;
            }

            var startupReleaseSpeed =
                Math.Sign(previewReferenceSpeed) *
                Math.Min(
                    Math.Abs(previewReferenceSpeed),
                    MaximumStartupSpeedMetersPerSecond);

            if (Math.Sign(currentReferenceSpeed) ==
                    Math.Sign(startupReleaseSpeed) &&
                Math.Abs(currentReferenceSpeed) >=
                    Math.Abs(startupReleaseSpeed))
            {
                return currentReferenceSpeed;
            }

            return startupReleaseSpeed;
        }

        private double ApplyTerminalBrakingPreview(
            TrajectoryProjection projection,
            double currentReferenceSpeed)
        {
            if (TerminalBrakingPreviewMeters <= 0.0)
            {
                return currentReferenceSpeed;
            }

            var previewArcLengthMeters = Math.Min(
                _trajectory.TotalLengthMeters,
                projection.ArcLengthMeters +
                TerminalBrakingPreviewMeters);
            var previewReferenceSpeed =
                _trajectory
                    .SampleAtArcLength(previewArcLengthMeters)
                    .ReferenceSpeedMetersPerSecond;
            var previewIsStop =
                Math.Abs(previewReferenceSpeed) <=
                ZeroReferenceSpeedToleranceMetersPerSecond;
            var hasSameDirection =
                Math.Sign(previewReferenceSpeed) ==
                Math.Sign(currentReferenceSpeed);
            var previewIsSlower =
                Math.Abs(previewReferenceSpeed) <
                Math.Abs(currentReferenceSpeed);

            if (previewIsSlower &&
                (previewIsStop || hasSameDirection))
            {
                return previewReferenceSpeed;
            }

            return currentReferenceSpeed;
        }

        private static double ResolveTerminalTravelDirection(
            Trajectory2D trajectory)
        {
            for (var index = trajectory.Count - 1;
                 index >= 0;
                 index--)
            {
                var referenceSpeedMetersPerSecond =
                    trajectory[index]
                        .ReferenceSpeedMetersPerSecond;

                if (Math.Abs(referenceSpeedMetersPerSecond) >
                    ZeroReferenceSpeedToleranceMetersPerSecond)
                {
                    return Math.Sign(
                        referenceSpeedMetersPerSecond);
                }
            }

            throw new ArgumentException(
                "轨迹必须在终点前包含至少一个非零参考速度。",
                nameof(trajectory));
        }

        private bool HasReachedEnd(
            Pose2D poseInWorld,
            Twist2D actualTwistInBody,
            bool hasValidVelocityEstimate,
            TrajectoryProjection projection)
        {
            if (!hasValidVelocityEstimate)
            {
                return false;
            }

            return projection.RemainingDistanceMeters <=
                       FinishDistanceMeters &&
                   CalculateDistanceToEndMeters(poseInWorld) <=
                       FinishDistanceMeters &&
                   CalculateHeadingErrorToEndRadians(poseInWorld) <=
                       FinishHeadingToleranceRadians &&
                   CalculateActualLongitudinalSpeedMetersPerSecond(
                       actualTwistInBody) <=
                       FinishSpeedMetersPerSecond;
        }

        private bool HasStoppedAtUnsatisfiedTerminal(
            Pose2D poseInWorld,
            Twist2D actualTwistInBody,
            bool hasValidVelocityEstimate,
            TrajectoryProjection projection,
            out string failureReason)
        {
            failureReason = string.Empty;

            var isTerminalZeroSpeedReference =
                projection.RemainingDistanceMeters <=
                    FinishDistanceMeters &&
                Math.Abs(
                    projection.ReferencePoint
                        .ReferenceSpeedMetersPerSecond) <=
                    ZeroReferenceSpeedToleranceMetersPerSecond;

            if (!isTerminalZeroSpeedReference ||
                !hasValidVelocityEstimate ||
                CalculateActualLongitudinalSpeedMetersPerSecond(
                    actualTwistInBody) >
                FinishSpeedMetersPerSecond)
            {
                return false;
            }

            var positionErrorMeters =
                CalculateDistanceToEndMeters(
                    poseInWorld);
            var headingErrorRadians =
                CalculateHeadingErrorToEndRadians(
                    poseInWorld);

            failureReason =
                "受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
                $"位置误差={positionErrorMeters:F3}m，" +
                "航向误差=" +
                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
            return true;
        }

        private double CalculateDistanceToEndMeters(
            Pose2D poseInWorld)
        {
            var endPose = _trajectory.EndPoint.PoseInWorld;
            var deltaX = poseInWorld.XMeters -
                         endPose.XMeters;
            var deltaY = poseInWorld.YMeters -
                         endPose.YMeters;

            return Math.Sqrt(
                deltaX * deltaX +
                deltaY * deltaY);
        }

        private double CalculateHeadingErrorToEndRadians(
            Pose2D poseInWorld)
        {
            return Math.Abs(
                AngleMath.ShortestDifferenceRadians(
                    _trajectory.EndPoint
                        .PoseInWorld.YawRadians,
                    poseInWorld.YawRadians));
        }

        private double CalculateActualLongitudinalSpeedMetersPerSecond(
            Twist2D actualTwistInBody)
        {
            return Math.Abs(
                Math.Cos(_motionDirectionInBodyRadians) *
                    actualTwistInBody.VxMetersPerSecond +
                Math.Sin(_motionDirectionInBodyRadians) *
                    actualTwistInBody.VyMetersPerSecond);
        }

        private void CompleteTrajectory()
        {
            ResetFeedbackControllers();
            _trajectory = null;
            IsActive = false;
            IsCompleted = true;
            LastRequestedCommand = new GcpMotionCommand(
                0.0,
                0.0,
                0.0);
            LastFailureReason = string.Empty;
            LastException = null;
        }

        private void ResetFeedbackControllers()
        {
            _lateralController.Reset();
            _longitudinalController.Reset();
        }

        private void ClearDiagnostics()
        {
            LastProjection = null;
            LastRequestedCommand = null;
            LastControlReferenceSpeedMetersPerSecond = null;
            LastCurvaturePreviewDistanceMeters = null;
            LastFeedforwardCurvaturePerMeter = null;
            LastFailureReason = string.Empty;
            LastException = null;
        }

        private static PathTrackingCycleOutput CreateOutput(
            PathTrackingCycleResult result,
            GcpMotionCommand? command,
            TrajectoryProjection? projection,
            double projectionMilliseconds,
            long controllerComputeStartTimestamp)
        {
            var controllerComputeMilliseconds =
                controllerComputeStartTimestamp == 0L
                    ? 0.0
                    : GetElapsedMilliseconds(
                        controllerComputeStartTimestamp);

            return new PathTrackingCycleOutput(
                result,
                command,
                projection,
                projectionMilliseconds,
                controllerComputeMilliseconds);
        }

        private static double GetElapsedMilliseconds(
            long startTimestamp)
        {
            return (Stopwatch.GetTimestamp() - startTimestamp) *
                   1000.0 /
                   Stopwatch.Frequency;
        }
    }
}