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

namespace MultiWheelC.Control.Execution
{
    // 单周期轨迹控制结果。
    public enum ParkingControlCycleResult
    {
        Inactive = 0,
        CommandSent = 1,
        Completed = 2,
        StateUnavailable = 3,
        Faulted = 4
    }

    // 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。
    public readonly struct ParkingControlCycleTiming
    {
        public ParkingControlCycleTiming(
            long cycleIndex,
            double cycleIntervalMilliseconds,
            double stateReadMilliseconds,
            double projectionMilliseconds,
            double controllerComputeMilliseconds,
            double commandSendMilliseconds,
            double totalCycleMilliseconds,
            bool hasStateTimestamp,
            double stateTimestampSeconds,
            bool stateTimestampChanged,
            ParkingControlCycleResult result)
        {
            CycleIndex = cycleIndex;
            CycleIntervalMilliseconds =
                cycleIntervalMilliseconds;
            StateReadMilliseconds = stateReadMilliseconds;
            ProjectionMilliseconds = projectionMilliseconds;
            ControllerComputeMilliseconds =
                controllerComputeMilliseconds;
            CommandSendMilliseconds = commandSendMilliseconds;
            TotalCycleMilliseconds = totalCycleMilliseconds;
            HasStateTimestamp = hasStateTimestamp;
            StateTimestampSeconds = stateTimestampSeconds;
            StateTimestampChanged = stateTimestampChanged;
            Result = result;
        }

        public long CycleIndex { get; }
        public double CycleIntervalMilliseconds { get; }
        public double StateReadMilliseconds { get; }
        public double ProjectionMilliseconds { get; }
        public double ControllerComputeMilliseconds { get; }
        public double CommandSendMilliseconds { get; }
        public double TotalCycleMilliseconds { get; }
        public bool HasStateTimestamp { get; }
        public double StateTimestampSeconds { get; }
        public bool StateTimestampChanged { get; }
        public ParkingControlCycleResult Result { get; }
    }

    // 负责单车状态读取、公共轨迹计算和真实底盘命令发送。
    public sealed class ParkingGeometricController
    {
        private readonly IVehicleStateProvider _stateProvider;
        private readonly GcpCommandExecutor _commandExecutor;
        private readonly PathTrackingCore _trackingCore;

        private long _cycleIndex;
        private bool _hasPreviousStateTimestamp;
        private double _previousStateTimestampSeconds;

        // 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。
        public ParkingGeometricController(
            IVehicleStateProvider stateProvider,
            ILateralController lateralController,
            ILongitudinalController longitudinalController,
            GcpCommandAllocator gcpAllocator,
            GcpCommandExecutor commandExecutor,
            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)
        {
            _stateProvider = stateProvider ??
                throw new ArgumentNullException(
                    nameof(stateProvider));
            _commandExecutor = commandExecutor ??
                throw new ArgumentNullException(
                    nameof(commandExecutor));
            _trackingCore = new PathTrackingCore(
                lateralController,
                longitudinalController,
                gcpAllocator,
                finishDistanceMeters,
                finishSpeedMetersPerSecond,
                finishHeadingToleranceRadians,
                maximumDistanceToTrajectoryMeters,
                terminalBrakingPreviewMeters,
                terminalApproachDistanceMeters,
                terminalApproachGainPerSecond,
                maximumTerminalApproachSpeedMetersPerSecond,
                curvaturePreviewSeconds,
                maximumCurvaturePreviewMeters,
                motionDirectionInBodyRadians);
        }

        // 以下控制参数由公共轨迹核心统一持有。
        public double FinishDistanceMeters =>
            _trackingCore.FinishDistanceMeters;

        public double FinishSpeedMetersPerSecond =>
            _trackingCore.FinishSpeedMetersPerSecond;

        public double FinishHeadingToleranceRadians =>
            _trackingCore.FinishHeadingToleranceRadians;

        public double MaximumDistanceToTrajectoryMeters =>
            _trackingCore.MaximumDistanceToTrajectoryMeters;

        public double TerminalBrakingPreviewMeters =>
            _trackingCore.TerminalBrakingPreviewMeters;

        public double TerminalApproachDistanceMeters =>
            _trackingCore.TerminalApproachDistanceMeters;

        public double TerminalApproachGainPerSecond =>
            _trackingCore.TerminalApproachGainPerSecond;

        public double MaximumTerminalApproachSpeedMetersPerSecond =>
            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;

        public double CurvaturePreviewSeconds =>
            _trackingCore.CurvaturePreviewSeconds;

        public double MaximumCurvaturePreviewMeters =>
            _trackingCore.MaximumCurvaturePreviewMeters;

        public bool IsActive => _trackingCore.IsActive;

        public bool IsCompleted => _trackingCore.IsCompleted;

        public string LastFailureReason =>
            _trackingCore.LastFailureReason;

        public Exception LastException =>
            _trackingCore.LastException;

        // 最近一次有效车辆状态仍由单车外层保存。
        public VehicleState? LastVehicleState { get; private set; }

        public TrajectoryProjection? LastProjection =>
            _trackingCore.LastProjection;

        public GcpMotionCommand? LastRequestedCommand =>
            _trackingCore.LastRequestedCommand;

        // 经过GCP角速度限制后实际发送给底盘的最近一次命令。
        public GcpMotionCommand? LastCommand { get; private set; }

        public double? LastControlReferenceSpeedMetersPerSecond =>
            _trackingCore.LastControlReferenceSpeedMetersPerSecond;

        public double? LastCurvaturePreviewDistanceMeters =>
            _trackingCore.LastCurvaturePreviewDistanceMeters;

        public double? LastFeedforwardCurvaturePerMeter =>
            _trackingCore.LastFeedforwardCurvaturePerMeter;

        public ParkingControlCycleTiming? LastCycleTiming { get; private set; }

        // 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。
        public void Start(Trajectory2D trajectory)
        {
            _commandExecutor.Stop();
            _trackingCore.Start(trajectory);
            ClearExecutionDiagnostics();
        }

        // 读取车辆状态、调用公共核心并发送一次真实底盘命令。
        public ParkingControlCycleResult ExecuteCycle(
            double deltaTimeSeconds)
        {
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (!_trackingCore.IsActive)
            {
                return ParkingControlCycleResult.Inactive;
            }

            var cycleIndex = ++_cycleIndex;
            var cycleStartTimestamp =
                Stopwatch.GetTimestamp();
            var stateReadMilliseconds = 0.0;
            var projectionMilliseconds = 0.0;
            var controllerComputeMilliseconds = 0.0;
            var commandSendMilliseconds = 0.0;
            var hasStateTimestamp = false;
            var stateTimestampSeconds = 0.0;
            var stateTimestampChanged = false;
            var cycleResult =
                ParkingControlCycleResult.Faulted;

            try
            {
                var stateReadStartTimestamp =
                    Stopwatch.GetTimestamp();
                bool stateAvailable;
                VehicleState vehicleState;
                try
                {
                    stateAvailable =
                        _stateProvider.TryGetState(
                            out vehicleState);
                }
                finally
                {
                    stateReadMilliseconds =
                        GetElapsedMilliseconds(
                            stateReadStartTimestamp);
                }

                if (!stateAvailable)
                {
                    var stopStartTimestamp =
                        Stopwatch.GetTimestamp();
                    try
                    {
                        StopForUnavailableState();
                    }
                    finally
                    {
                        commandSendMilliseconds =
                            GetElapsedMilliseconds(
                                stopStartTimestamp);
                    }

                    cycleResult = ParkingControlCycleResult
                        .StateUnavailable;
                    return cycleResult;
                }

                LastVehicleState = vehicleState;
                hasStateTimestamp = true;
                stateTimestampSeconds =
                    vehicleState.SampleTimestampSeconds;
                stateTimestampChanged =
                    !_hasPreviousStateTimestamp ||
                    stateTimestampSeconds !=
                    _previousStateTimestampSeconds;
                _previousStateTimestampSeconds =
                    stateTimestampSeconds;
                _hasPreviousStateTimestamp = true;

                var output = _trackingCore.Compute(
                    vehicleState.PoseInWorld,
                    vehicleState.TwistInBody,
                    vehicleState.HasValidVelocityEstimate,
                    deltaTimeSeconds);
                projectionMilliseconds =
                    output.ProjectionMilliseconds;
                controllerComputeMilliseconds =
                    output.ControllerComputeMilliseconds;

                if (output.Result ==
                    PathTrackingCycleResult.Completed)
                {
                    commandSendMilliseconds =
                        StopForCompletedTrajectory();
                    cycleResult =
                        ParkingControlCycleResult.Completed;
                    return cycleResult;
                }

                if (output.Result ==
                    PathTrackingCycleResult.Faulted)
                {
                    commandSendMilliseconds =
                        StopForTrackingFault();
                    cycleResult =
                        ParkingControlCycleResult.Faulted;
                    return cycleResult;
                }

                if (output.Result !=
                        PathTrackingCycleResult.CommandGenerated ||
                    !output.Command.HasValue)
                {
                    cycleResult =
                        ParkingControlCycleResult.Inactive;
                    return cycleResult;
                }

                var commandSendStartTimestamp =
                    Stopwatch.GetTimestamp();
                bool commandSucceeded;
                try
                {
                    commandSucceeded =
                        _commandExecutor.Execute(
                            output.Command.Value,
                            deltaTimeSeconds);
                }
                finally
                {
                    commandSendMilliseconds =
                        GetElapsedMilliseconds(
                            commandSendStartTimestamp);
                }

                if (!commandSucceeded)
                {
                    cycleResult = EnterFault(
                        string.IsNullOrWhiteSpace(
                            _commandExecutor.LastFailureReason)
                            ? "GCP底盘命令执行失败。"
                            : _commandExecutor.LastFailureReason);
                    return cycleResult;
                }

                LastCommand =
                    _commandExecutor.LastSentCommand;
                cycleResult =
                    ParkingControlCycleResult.CommandSent;
                return cycleResult;
            }
            catch (Exception exception)
            {
                cycleResult = EnterFault(
                    "停车机器人轨迹控制周期异常：" +
                    exception.Message,
                    exception);
                return cycleResult;
            }
            finally
            {
                LastCycleTiming =
                    new ParkingControlCycleTiming(
                        cycleIndex,
                        deltaTimeSeconds * 1000.0,
                        stateReadMilliseconds,
                        projectionMilliseconds,
                        controllerComputeMilliseconds,
                        commandSendMilliseconds,
                        GetElapsedMilliseconds(
                            cycleStartTimestamp),
                        hasStateTimestamp,
                        stateTimestampSeconds,
                        stateTimestampChanged,
                        cycleResult);
            }
        }

        // 主动取消当前轨迹、立即停车并清除全部控制状态。
        public void Cancel()
        {
            _commandExecutor.Stop();
            _trackingCore.Cancel();
            ClearExecutionDiagnostics();
        }

        // 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。
        private void StopForUnavailableState()
        {
            _commandExecutor.Stop();
            _trackingCore.PauseForUnavailableState(
                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。");
            LastCommand = null;
        }

        // 公共核心完成轨迹后发送停车，并保留零命令供实验记录。
        private double StopForCompletedTrajectory()
        {
            var startTimestamp = Stopwatch.GetTimestamp();
            _commandExecutor.Stop();
            LastCommand = new GcpMotionCommand(
                0.0,
                0.0,
                0.0);
            return GetElapsedMilliseconds(startTimestamp);
        }

        // 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。
        private double StopForTrackingFault()
        {
            var startTimestamp = Stopwatch.GetTimestamp();
            _commandExecutor.Stop();
            LastCommand = null;
            return GetElapsedMilliseconds(startTimestamp);
        }

        // 将底盘执行或外层异常同步到公共核心，并立即停车。
        private ParkingControlCycleResult EnterFault(
            string reason,
            Exception exception = null)
        {
            _commandExecutor.Stop();
            _trackingCore.Fail(reason, exception);
            LastCommand = null;
            return ParkingControlCycleResult.Faulted;
        }

        // 清除只属于单车状态读取、发送和周期计时的诊断。
        private void ClearExecutionDiagnostics()
        {
            _cycleIndex = 0;
            _hasPreviousStateTimestamp = false;
            _previousStateTimestampSeconds = 0.0;
            LastVehicleState = null;
            LastCommand = null;
            LastCycleTiming = null;
        }

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