using System.Diagnostics;
using System.Text;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;
using Microsoft.Extensions.Logging.Abstractions;

namespace Flyshot.Runtime.Fanuc;

/// <summary>
/// FANUC 控制器运行时，将上层兼容层指令转换为三条真实 Socket 通道的交互。
/// 仿真模式下仍保持内存桩行为，便于离线测试与回退。
/// </summary>
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
    private readonly object _stateLock = new();
    private readonly Dictionary<(string IoType, int Port), bool> _ioValues = new();
    private readonly FanucCommandClient _commandClient;
    private readonly FanucStateClient _stateClient;
    private readonly FanucJ519Client _j519Client;
    private readonly ILogger<FanucControllerRuntime>? _logger;

    private RobotProfile? _robot;
    private string? _robotName;
    private bool? _activeControllerIsSimulation;
    private string? _connectedRobotIp;
    private bool _isEnabled;
    private bool _isInMotion;
    private int _bufferSize;
    private double _speedRatio = 1.0;
    private double[] _tcp = [0.0, 0.0, 0.0];
    private double[] _jointPositions = Array.Empty<double>();
    private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
    private bool _disposed;
    private CancellationTokenSource? _sendCts;

    /// <summary>
    /// 初始化 FANUC 控制器运行时。
    /// </summary>
    /// <param name="logger">运行时日志记录器。</param>
    /// <param name="j519Logger">J519 客户端日志记录器。</param>
    public FanucControllerRuntime(ILogger<FanucControllerRuntime> logger, ILogger<FanucJ519Client> j519Logger)
    {
        ArgumentNullException.ThrowIfNull(logger);
        ArgumentNullException.ThrowIfNull(j519Logger);

        _commandClient = new FanucCommandClient();
        _stateClient = new FanucStateClient();
        _j519Client = new FanucJ519Client(j519Logger);
        _logger = logger;
    }

    /// <summary>
    /// 初始化一份无日志的 FANUC 控制器运行时，供离线测试或手工构造场景使用。
    /// </summary>
    public FanucControllerRuntime()
        : this(NullLogger<FanucControllerRuntime>.Instance, NullLogger<FanucJ519Client>.Instance)
    {
    }

    /// <summary>
    /// 供测试注入 mock 客户端的内部构造函数。
    /// </summary>
    internal FanucControllerRuntime(FanucCommandClient commandClient, FanucStateClient stateClient, FanucJ519Client j519Client, ILogger<FanucControllerRuntime>? logger = null)
    {
        _commandClient = commandClient;
        _stateClient = stateClient;
        _j519Client = j519Client;
        _logger = logger;
    }

    /// <inheritdoc />
    public void ResetRobot(RobotProfile robot, string robotName)
    {
        ArgumentNullException.ThrowIfNull(robot);
        if (string.IsNullOrWhiteSpace(robotName))
        {
            throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
        }

        _logger?.LogInformation("ResetRobot: robotName={RobotName}, dof={Dof}", robotName, robot.DegreesOfFreedom);

        lock (_stateLock)
        {
            DisconnectClients();
            _robot = robot;
            _robotName = robotName;
            _activeControllerIsSimulation = null;
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
            _bufferSize = 0;
            _speedRatio = 1.0;
            _tcp = [0.0, 0.0, 0.0];
            _jointPositions = new double[robot.DegreesOfFreedom];
            _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
            _ioValues.Clear();
        }
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
        _logger?.LogInformation("SetActiveController: sim={Sim}", sim);
        lock (_stateLock)
        {
            EnsureRobotSetup();
            DisconnectClients();
            _activeControllerIsSimulation = sim;
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
        }
    }

    /// <inheritdoc />
    public void Connect(string robotIp)
    {
        if (string.IsNullOrWhiteSpace(robotIp))
        {
            throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
        }

        _logger?.LogInformation("Connect 开始: robotIp={RobotIp}, 仿真={IsSim}", robotIp, _activeControllerIsSimulation);

        lock (_stateLock)
        {
            EnsureActiveControllerSelected();
            if (_activeControllerIsSimulation!.Value)
            {
                _connectedRobotIp = robotIp;
                _isEnabled = false;
                _isInMotion = false;
                _logger?.LogInformation("Connect 完成(仿真): robotIp={RobotIp}", robotIp);
                return;
            }

            // 真机模式：顺序建立三条通道 — 状态 → 命令 → 运动。
            _stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult();
            _commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult();
            _j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult();

            _connectedRobotIp = robotIp;
            _isEnabled = false;
            _isInMotion = false;
        }

        _logger?.LogInformation("Connect 完成(真机): robotIp={RobotIp}, 三条通道已建立", robotIp);
    }

    /// <inheritdoc />
    public void Disconnect()
    {
        _logger?.LogInformation("Disconnect 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            DisconnectClients();
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
        }
        _logger?.LogInformation("Disconnect 完成");
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
        if (bufferSize <= 0)
        {
            throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。");
        }

        _logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}, 仿真={IsSim}", bufferSize, _activeControllerIsSimulation);

        lock (_stateLock)
        {
            EnsureConnected();
            _bufferSize = bufferSize;
            _j519Client.SetSequenceBufferSize(bufferSize);

            if (IsSimulationMode)
            {
                _isEnabled = true;
                _logger?.LogInformation("EnableRobot 完成(仿真)");
                return;
            }

            // 真机模式：按 all-reconnect.pcap 的重连序列启动 RVBUSTSM，暂不发送 StopProg。
             _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
            _commandClient.ResetRobotAsync().GetAwaiter().GetResult();
            //_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
            _commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();

            _j519Client.StartMotion();
            _isEnabled = true;
        }

        _logger?.LogInformation("EnableRobot 完成(真机): RVBUSTSM 已启动, J519 运动循环已开启");
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
        _logger?.LogInformation("DisableRobot 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            if (!IsSimulationMode)
            {
                _j519Client.StopMotionAsync().GetAwaiter().GetResult();
                _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
            }

            _isEnabled = false;
            _isInMotion = false;
        }
        _logger?.LogInformation("DisableRobot 完成");
    }

    /// <inheritdoc />
    public void StopMove()
    {
        _logger?.LogInformation("StopMove 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            if (!IsSimulationMode)
            {
                _j519Client.StopMotionAsync().GetAwaiter().GetResult();
            }

            _isInMotion = false;
        }
        _logger?.LogInformation("StopMove 完成");
    }

    /// <inheritdoc />
    public double GetSpeedRatio()
    {
        lock (_stateLock)
        {
            EnsureConnected();
            if (!IsSimulationMode)
            {
                var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
                _speedRatio = response.Ratio;
            }

            return _speedRatio;
        }
    }

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
        if (double.IsNaN(ratio) || double.IsInfinity(ratio))
        {
            throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
        }

        _logger?.LogInformation("SetSpeedRatio: ratio={Ratio}", ratio);

        lock (_stateLock)
        {
            //EnsureConnected();
            var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
            if (!IsSimulationMode)
            {
                //_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
            }

            _speedRatio = clampedRatio;
        }

        _logger?.LogInformation("SetSpeedRatio 完成: clampedRatio={ClampedRatio}", _speedRatio);
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp))
            {
                var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult();
                _tcp = response.Pose.Take(3).ToArray();
            }

            return _tcp.ToArray();
        }
    }

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (_activeControllerIsSimulation is false)
            {
                EnsureConnected();
                _commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
            }

            _tcp = [x, y, z];
        }
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType)
    {
        if (port < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
        }

        var normalizedIoType = NormalizeIoType(ioType);

        lock (_stateLock)
        {
            EnsureConnected();
            if (!IsSimulationMode)
            {
                var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
                _ioValues[(normalizedIoType, port)] = response.Value;
                return response.Value;
            }

            return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value;
        }
    }

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
        if (port < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
        }

        var normalizedIoType = NormalizeIoType(ioType);

        lock (_stateLock)
        {
            EnsureEnabled();
            if (!IsSimulationMode)
            {
                _commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
            }

            _ioValues[(normalizedIoType, port)] = value;
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame?.JointRadians.Count >= _jointPositions.Length)
                {
                    return frame.JointRadians.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
                }
            }

            return _jointPositions.ToArray();
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame?.CartesianPose.Count >= 6)
                {
                    return frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
                }
            }

            return _pose.ToArray();
        }
    }

    /// <inheritdoc />
    public ControllerStateSnapshot GetSnapshot()
    {
        lock (_stateLock)
        {
            var jointPositions = _jointPositions;
            var cartesianPose = _pose;
            var isInMotion = _isInMotion;
            IReadOnlyList<uint> stateTailWords = Array.Empty<uint>();
            byte? j519Status = null;
            uint? j519Sequence = null;
            bool? j519AcceptsCommand = null;
            bool? j519ReceivedCommand = null;
            bool? j519SystemReady = null;
            bool? j519RobotInMotion = null;

            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame is not null)
                {
                    if (frame.JointRadians.Count >= jointPositions.Length)
                    {
                        jointPositions = frame.JointRadians.Take(jointPositions.Length).Select(v => (double)v).ToArray();
                    }

                    if (frame.CartesianPose.Count >= 6)
                    {
                        cartesianPose = frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
                    }

                    stateTailWords = frame.RawTailWords.ToArray();
                }

                var j519Response = _j519Client.GetLatestResponse();
                if (j519Response is not null)
                {
                    isInMotion = j519Response.RobotInMotion;
                    j519Status = j519Response.Status;
                    j519Sequence = j519Response.Sequence;
                    j519AcceptsCommand = j519Response.AcceptsCommand;
                    j519ReceivedCommand = j519Response.ReceivedCommand;
                    j519SystemReady = j519Response.SystemReady;
                    j519RobotInMotion = j519Response.RobotInMotion;
                }
            }

            return new ControllerStateSnapshot(
                capturedAt: DateTimeOffset.UtcNow,
                connectionState: ResolveConnectionState(),
                isEnabled: _isEnabled,
                isInMotion: isInMotion,
                speedRatio: _speedRatio,
                jointPositions: jointPositions,
                cartesianPose: cartesianPose,
                activeAlarms: Array.Empty<RuntimeAlarm>(),
                stateTailWords: stateTailWords,
                j519Status: j519Status,
                j519Sequence: j519Sequence,
                j519AcceptsCommand: j519AcceptsCommand,
                j519ReceivedCommand: j519ReceivedCommand,
                j519SystemReady: j519SystemReady,
                j519RobotInMotion: j519RobotInMotion);
        }
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
        ArgumentNullException.ThrowIfNull(result);
        ArgumentNullException.ThrowIfNull(finalJointPositions);
        CancellationToken denseSendCancellationToken = default;
        CancellationTokenSource? denseSendCancellationSource = null;
        var shouldRunDenseTrajectory = false;

        _logger?.LogInformation(
            "ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
            result.ProgramName, result.Method, result.Duration.TotalSeconds,
            result.DenseJointTrajectory is not null, result.TriggerTimeline.Count, _speedRatio);

        lock (_stateLock)
        {
            EnsureEnabled();
            EnsureValidTrajectory(result);
            EnsureJointCount(finalJointPositions.Count);
            CancelSendTaskLocked();

            if (!IsSimulationMode && result.DenseJointTrajectory is not null)
            {
                EnsureJ519ReadyForDenseExecution();

                // 真机模式且存在稠密路点：准备可被 StopMove 取消的同步发送任务。
                _isInMotion = true;
                _sendCts = new CancellationTokenSource();
                denseSendCancellationSource = _sendCts;
                denseSendCancellationToken = _sendCts.Token;
                shouldRunDenseTrajectory = true;

                _logger?.LogInformation("ExecuteTrajectory 开始同步稠密发送任务");
            }
            else
            {
                if (!IsSimulationMode)
                {
                    // 真机模式无稠密路点：回退到单点收敛。
                    var command = new FanucJ519Command(
                        sequence: 0,
                        targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
                    _j519Client.UpdateCommand(command);
                }

                _isInMotion = true;
                _jointPositions = finalJointPositions.ToArray();
                _isInMotion = false;
                _logger?.LogInformation("ExecuteTrajectory 完成(单点模式)");
            }
        }

        if (shouldRunDenseTrajectory)
        {
            try
            {
                // 稠密轨迹必须等 J519 队列被机器人状态包实际取完后再向上层返回。
                SendDenseTrajectory(result, finalJointPositions, denseSendCancellationToken);
                _logger?.LogInformation("ExecuteTrajectory 完成(稠密模式)");
            }
            finally
            {
                lock (_stateLock)
                {
                    if (ReferenceEquals(_sendCts, denseSendCancellationSource))
                    {
                        denseSendCancellationSource?.Dispose();
                        _sendCts = null;
                    }
                }
            }
        }
    }

    /// <inheritdoc />
    public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
    {
        ArgumentNullException.ThrowIfNull(result);
        ArgumentNullException.ThrowIfNull(finalPose);
        CancellationToken denseSendCancellationToken = default;
        CancellationTokenSource? denseSendCancellationSource = null;
        var shouldRunDenseTrajectory = false;

        _logger?.LogInformation(
            "ExecuteCartesianTrajectory 开始: program={ProgramName}, 时长={Duration}s, 稠密采样={HasDense}, speedRatio={SpeedRatio}",
            result.ProgramName, result.Duration.TotalSeconds, result.DenseCartesianTrajectory is not null, _speedRatio);

        lock (_stateLock)
        {
            EnsureEnabled();
            EnsureValidTrajectory(result);
            EnsurePoseCount(finalPose.Count);
            CancelSendTaskLocked();

            if (!IsSimulationMode && result.DenseCartesianTrajectory is not null)
            {
                EnsureJ519ReadyForDenseExecution();

                // 真机直角模式同样预装完整队列，由机器人状态包节拍驱动出队。
                _isInMotion = true;
                _sendCts = new CancellationTokenSource();
                denseSendCancellationSource = _sendCts;
                denseSendCancellationToken = _sendCts.Token;
                shouldRunDenseTrajectory = true;

                _logger?.LogInformation("ExecuteCartesianTrajectory 开始同步直角稠密发送任务");
            }
            else
            {
                if (!IsSimulationMode)
                {
                    var command = new FanucJ519Command(
                        sequence: 0,
                        targetValues: BuildCartesianTargetValues(finalPose),
                        dataStyle: 0);
                    _j519Client.UpdateCommand(command);
                }

                _isInMotion = true;
                _pose = MergeFinalCartesianPose(finalPose);
                _isInMotion = false;
                _logger?.LogInformation("ExecuteCartesianTrajectory 完成(单点模式)");
            }
        }

        if (shouldRunDenseTrajectory)
        {
            try
            {
                SendDenseCartesianTrajectory(result, finalPose, denseSendCancellationToken);
                _logger?.LogInformation("ExecuteCartesianTrajectory 完成(稠密模式)");
            }
            finally
            {
                lock (_stateLock)
                {
                    if (ReferenceEquals(_sendCts, denseSendCancellationSource))
                    {
                        denseSendCancellationSource?.Dispose();
                        _sendCts = null;
                    }
                }
            }
        }
    }

    /// <summary>
    /// 释放运行时持有的所有 Socket 客户端。
    /// </summary>
    public void Dispose()
    {
        if (_disposed)
        {
            return;
        }

        _disposed = true;
        CancelSendTaskLocked();
        DisconnectClients();
        _commandClient.Dispose();
        _stateClient.Dispose();
        _j519Client.Dispose();
    }

    /// <summary>
    /// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包驱动队列执行完成。
    /// </summary>
    private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
    {
        var triggers = result.TriggerTimeline;
        var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
        var preparedExecution = result.PreparedFlyshotExecution;
        var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds;
        var samples = preparedExecution is null
            ? J519SendTrajectorySampler.SampleDenseJointTrajectory(
                result.DenseJointTrajectory!,
                durationSeconds,
                servoPeriodSeconds,
                speedRatio: 1.0)
            : preparedExecution.Samples.Select(static sample => new J519SendSample(
                sample.SampleIndex,
                sample.SendTime,
                sample.TrajectoryTime,
                sample.SpeedRatio,
                sample.JointsDegrees)).ToArray();
        TrajectoryLimitValidator.ValidateJ519SendSamples(
            _robot!,
            samples,
            trajectoryName: result.ProgramName);
        var sampleCount = samples.Count;

        _logger?.LogInformation(
            "SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, runtimeSpeedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
            result.ProgramName, sampleCount, durationSeconds, _speedRatio, servoPeriodSeconds * 1000, triggers.Count);

        var triggerBindings = preparedExecution is null
            ? TriggerSampleBinder.Bind(
                triggers,
                samples,
                result.TriggerSampleIndexOffsetCycles)
            : preparedExecution.TriggerBindings.Select(static binding =>
                new TriggerSampleBinding(
                    binding.Trigger,
                    new J519SendSample(
                        binding.Sample.SampleIndex,
                        binding.Sample.SendTime,
                        binding.Sample.TrajectoryTime,
                        binding.Sample.SpeedRatio,
                        binding.Sample.JointsDegrees),
                    binding.SampleIndex,
                    binding.FoundInWindow)).ToArray();

        var stopwatch = Stopwatch.StartNew();
        ushort ioValue = 0;
        ushort ioMask = 0;
        int holdRemaining = -1;
        long logInterval = Math.Max(1, sampleCount / 10);
        int triggerFiredCount = 0;
        var nextBindingIndex = 0;
        var commands = new List<FanucJ519Command>(sampleCount);
        var sentJointRows = new List<IReadOnlyList<double>>(sampleCount);
        var sentTimingRows = new List<IReadOnlyList<double>>(sampleCount);
        var sentJerkRows = new List<IReadOnlyList<double>>();
        var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
        double? previousSendTime = null;
        double[]? previousJoints = null;
        double[]? previousVelocity = null;
        double[]? previousAcceleration = null;
        var preparedJerkRows = preparedExecution?.JerkRows;
        var diagnosticRequestedSpeedRatio = _speedRatio > 0.0 && !double.IsNaN(_speedRatio) && !double.IsInfinity(_speedRatio)
            ? _speedRatio
            : 1.0;

        try
        {
            foreach (var sample in samples)
            {
                cancellationToken.ThrowIfCancellationRequested();

                // 递减 IO 保持计数器；若已到期则清零。
                var clearMaskAfterSend = false;
                if (holdRemaining > 0)
                {
                    holdRemaining--;
                }
                else if (holdRemaining == 0)
                {
                    ioValue = 0;
                    holdRemaining = -1;
                    clearMaskAfterSend = true;
                }

                // 检查当前周期是否有新的触发事件。
                if (holdRemaining < 0 && !clearMaskAfterSend)
                {
                    while (nextBindingIndex < triggerBindings.Count
                        && triggerBindings[nextBindingIndex].SampleIndex < sample.SampleIndex)
                    {
                        nextBindingIndex++;
                    }

                    if (nextBindingIndex < triggerBindings.Count
                        && triggerBindings[nextBindingIndex].SampleIndex == sample.SampleIndex)
                    {
                        var binding = triggerBindings[nextBindingIndex];
                        ioMask = ComputeIoValue(binding.Trigger.AddressGroup);
                        ioValue = ioMask;
                        holdRemaining = Math.Max(binding.Trigger.HoldCycles - 1, 0);
                        triggerFiredCount++;
                        nextBindingIndex++;
                        _logger?.LogInformation(
                            "J519 IO触发: waypointIndex={WaypointIndex}, theoryTime={TheoryTime:F4}s, sampleIndex={SampleIndex}, sampleSendTime={SampleSendTime:F4}s, sampleTrajectoryTime={SampleTrajectoryTime:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
                            binding.Trigger.WaypointIndex,
                            binding.Trigger.TriggerTime,
                            binding.SampleIndex,
                            binding.Sample.SendTime,
                            binding.Sample.TrajectoryTime,
                            string.Join(",", binding.Trigger.AddressGroup.Addresses),
                            binding.Trigger.HoldCycles);
                    }
                }

                var command = new FanucJ519Command(
                    sequence: 0,
                    targetJoints: sample.JointsDegrees,
                    writeIoType: 2,
                    writeIoIndex: 1,
                    writeIoMask: ioMask,
                    writeIoValue: ioValue);

                commands.Add(command);
                sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
                sentTimingRows.Add(preparedExecution is null
                    ? J519SendTrajectorySampler.BuildTimingRow(sample, diagnosticRequestedSpeedRatio, stretchIterationCount: 0)
                    : J519SendTrajectorySampler.BuildTimingRow(
                        sample,
                        preparedExecution.RequestSpeedRatio,
                        preparedExecution.StretchIterationCount));

                if (preparedJerkRows is not null)
                {
                    if (sample.SampleIndex > 0 && sample.SampleIndex - 1 < preparedJerkRows.Count)
                    {
                        sentJerkRows.Add(preparedJerkRows[(int)sample.SampleIndex - 1]);
                    }
                }
                else if (previousSendTime is not null && previousJoints is not null)
                {
                    var jerkRow = J519SendTrajectorySampler.BuildJerkRow(
                        previousSendTime.Value,
                        sample.SendTime,
                        previousJoints,
                        sample.JointsDegrees,
                        ref previousVelocity,
                        ref previousAcceleration);
                    sentJerkRows.Add(jerkRow);
                }

                if (clearMaskAfterSend)
                {
                    ioMask = 0;
                }

                previousSendTime = sample.SendTime;
                previousJoints = sample.JointsDegrees.ToArray();

                // 周期性记录进度（Debug 级别，避免高频 Info 日志）。
                if (sample.SampleIndex > 0 && sample.SampleIndex % logInterval == 0)
                {
                    _logger?.LogDebug(
                        "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), sendTime={SendTime:F4}s, trajectoryTime={TrajectoryTime:F4}s",
                        (int)((double)sample.SampleIndex / sampleCount * 100),
                        sample.SampleIndex,
                        sampleCount,
                        sample.SendTime,
                        sample.TrajectoryTime);
                }
            }
            TryWriteDenseSendArtifacts(result, outputDir, sentJointRows, sentTimingRows, sentJerkRows, triggerBindings);

            // 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。
            _j519Client.LoadCommandQueue(commands);
            if (_j519Client.IsConnected)
            {
                _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
            }
            _logger?.LogInformation(
                "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 队列装载耗时={ElapsedMs}ms",
                sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
        }
        catch (OperationCanceledException)
        {
            _logger?.LogWarning(
                "SendDenseTrajectory 被取消: 已生成 {Current}/{Total} 条命令, 触发次数={TriggerFiredCount}",
                commands.Count, sampleCount, triggerFiredCount);
            // 正常取消，轨迹被中断。
        }
        finally
        {
            lock (_stateLock)
            {
                _isInMotion = false;
                _jointPositions = finalJointPositions.ToArray();
            }
        }
    }

    /// <summary>
    /// 直角稠密轨迹发送任务：预生成 Data format=0 的 J519 命令队列，并等待状态包驱动执行完成。
    /// </summary>
    private void SendDenseCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose, CancellationToken cancellationToken)
    {
        var rows = result.DenseCartesianTrajectory ?? throw new InvalidOperationException("Cartesian trajectory requires dense Cartesian samples.");
        var commands = new List<FanucJ519Command>(rows.Count);
        var sentPoseRows = new List<IReadOnlyList<double>>(rows.Count);
        var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
        var stopwatch = Stopwatch.StartNew();

        try
        {
            foreach (var row in rows)
            {
                cancellationToken.ThrowIfCancellationRequested();
                if (row.Count != 7)
                {
                    throw new InvalidOperationException("Cartesian dense trajectory rows must contain time plus six pose values.");
                }

                var pose = row.Skip(1).Take(6).ToArray();
                var command = new FanucJ519Command(
                    sequence: 0,
                    targetValues: BuildCartesianTargetValues(pose),
                    dataStyle: 0,
                    writeIoType: 2,
                    writeIoIndex: 1,
                    writeIoMask: 0,
                    writeIoValue: 0);

                commands.Add(command);
                sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose));
            }

            TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows);

            _j519Client.LoadCommandQueue(commands);
            if (_j519Client.IsConnected)
            {
                _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
            }

            _logger?.LogInformation(
                "SendDenseCartesianTrajectory 正常完成: 采样数={SampleCount}, 队列装载耗时={ElapsedMs}ms",
                commands.Count,
                stopwatch.ElapsedMilliseconds);
        }
        catch (OperationCanceledException)
        {
            _logger?.LogWarning(
                "SendDenseCartesianTrajectory 被取消: 已生成 {Current}/{Total} 条命令",
                commands.Count,
                rows.Count);
        }
        finally
        {
            lock (_stateLock)
            {
                _isInMotion = false;
                _pose = MergeFinalCartesianPose(finalPose);
            }
        }
    }

    /// <summary>
    /// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
    /// </summary>
    private void EnsureJ519ReadyForDenseExecution()
    {
        EnsureJ519ReadyForDenseExecutionCore(
            () => _j519Client.GetLatestResponse(),
            () =>
            {
                // 若当前状态未就绪，则先尝试一次 EnableRobot 走现场既有恢复链路。
                EnableRobot(_bufferSize);
            },
            static () => Thread.Sleep(TimeSpan.FromMilliseconds(500)));
    }

    /// <summary>
    /// 执行稠密发送前的 J519 就绪校验；未就绪时允许先做一次启机重试。
    /// </summary>
    /// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
    /// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
    /// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
    internal void EnsureJ519ReadyForDenseExecutionCore(
        Func<FanucJ519Response?> getLatestResponse,
        Action retryEnableRobot,
        Action waitAfterRetry)
    {
        ArgumentNullException.ThrowIfNull(getLatestResponse);
        ArgumentNullException.ThrowIfNull(retryEnableRobot);
        ArgumentNullException.ThrowIfNull(waitAfterRetry);

        var response = getLatestResponse();
       
        if (response is null || IsJ519ReadyForDenseExecution(response))
        {
            return;
        }
        _logger?.LogInformation("当前机器人不满足执行条件,重新启用机器人");
        try
        {
            retryEnableRobot();
            waitAfterRetry();
        }
        catch (InvalidOperationException exception)
        {
            // 重试链路自身失败时，优先保留 J519 状态位诊断，底层连接异常作为 inner exception 继续上抛。
            throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response), exception);
        }

        response = getLatestResponse();
        if (response is null || IsJ519ReadyForDenseExecution(response))
        {
            return;
        }

        throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response));
    }

    /// <summary>
    /// 判断当前 J519 响应是否已经满足稠密执行前置条件。
    /// </summary>
    /// <param name="response">待检查的 J519 状态响应。</param>
    /// <returns>接受命令且系统就绪时返回 true，否则返回 false。</returns>
    internal static bool IsJ519ReadyForDenseExecution(FanucJ519Response response)
    {
        ArgumentNullException.ThrowIfNull(response);
        return response is { AcceptsCommand: true, SystemReady: true };
    }

    /// <summary>
    /// 组装 J519 未就绪时的异常消息，便于保留现场关键状态位。
    /// </summary>
    /// <param name="response">最近一次收到的 J519 状态响应。</param>
    /// <returns>包含状态位和序号的诊断消息。</returns>
    internal static string BuildJ519DenseExecutionNotReadyMessage(FanucJ519Response response)
    {
        ArgumentNullException.ThrowIfNull(response);
        return "J519 status is not ready for dense execution: "
            + $"accept_cmd={response.AcceptsCommand}, "
            + $"received_cmd={response.ReceivedCommand}, "
            + $"sysrdy={response.SystemReady}, "
            + $"rbt_inmotion={response.RobotInMotion}, "
            + $"seq={response.Sequence}, "
            + $"status=0x{response.Status:X2}.";
    }

    /// <summary>
    /// 为单次稠密发送创建按日期时间归档的输出目录。
    /// </summary>
    private static string CreateDenseSendOutputDirectory(string programName)
    {
        var safeProgramName = SanitizeDirectoryName(string.IsNullOrWhiteSpace(programName) ? "unnamed-program" : programName);
        var timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
        var outputDir = Path.Combine(
            AppContext.BaseDirectory,
            "Config",
            "Data",
            safeProgramName,
            "DenseSend",
            timestamp);
        Directory.CreateDirectory(outputDir);
        return outputDir;
    }

    /// <summary>
    /// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
    /// </summary>
    private static IReadOnlyList<double> BuildDenseSendJointRow(double sendTime, IReadOnlyList<double> joints, ushort ioMask, ushort ioValue)
    {
        var row = new double[joints.Count + 3];
        row[0] = Math.Round(sendTime, 6);
        for (var index = 0; index < joints.Count; index++)
        {
            row[index + 1] = Math.Round(joints[index], 6);
        }

        row[^2] = ioMask;
        row[^1] = ioValue;
        return row;
    }

    /// <summary>
    /// 构造实际发送直角位姿文本行，格式为 send_time + X/Y/Z/W/P/R。
    /// </summary>
    private static IReadOnlyList<double> BuildDenseSendPoseRow(double sendTime, IReadOnlyList<double> pose)
    {
        var row = new double[pose.Count + 1];
        row[0] = Math.Round(sendTime, 6);
        for (var index = 0; index < pose.Count; index++)
        {
            row[index + 1] = Math.Round(pose[index], 6);
        }

        return row;
    }

    /// <summary>
    /// 构造 J519 直角命令的 9 个目标槽位，当前现场无扩展轴时 E1/E2/E3 补零。
    /// </summary>
    private static double[] BuildCartesianTargetValues(IReadOnlyList<double> pose)
    {
        EnsurePoseCount(pose.Count);
        return [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], 0.0, 0.0, 0.0];
    }

    /// <summary>
    /// 将六维 WPR 直角目标合并回运行时缓存，保留旧 GetPose 仿真路径的 7 维外形。
    /// </summary>
    private double[] MergeFinalCartesianPose(IReadOnlyList<double> pose)
    {
        EnsurePoseCount(pose.Count);
        return _pose.Length >= 7
            ? [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], _pose[6]]
            : pose.ToArray();
    }

    /// <summary>
    /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
    /// </summary>
    private void TryWriteDenseSendArtifacts(
        TrajectoryResult result,
        string outputDir,
        IReadOnlyList<IReadOnlyList<double>> sentJointRows,
        IReadOnlyList<IReadOnlyList<double>> sentTimingRows,
        IReadOnlyList<IReadOnlyList<double>> sentJerkRows,
        IReadOnlyList<TriggerSampleBinding> triggerBindings)
    {
        try
        {
            WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows);
            WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), sentTimingRows);
            WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows);
            TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
            _logger?.LogInformation(
                "SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}, triggerBindings={TriggerBindings}",
                outputDir,
                sentJointRows.Count,
                sentTimingRows.Count,
                sentJerkRows.Count,
                triggerBindings.Count);
        }
        catch (Exception exception)
        {
            _logger?.LogWarning(exception, "SendDenseTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
        }
    }

    /// <summary>
    /// 尝试把直角实际发送点位写入文本文件；若落盘失败，只记录日志，不影响运动主流程。
    /// </summary>
    private void TryWriteDenseCartesianSendArtifacts(string outputDir, IReadOnlyList<IReadOnlyList<double>> sentPoseRows)
    {
        try
        {
            WriteDenseRows(Path.Combine(outputDir, "ActualSendPoseTraj.txt"), sentPoseRows);
            _logger?.LogInformation(
                "SendDenseCartesianTrajectory 已写出实际发送记录: outputDir={OutputDir}, poseRows={PoseRows}",
                outputDir,
                sentPoseRows.Count);
        }
        catch (Exception exception)
        {
            _logger?.LogWarning(exception, "SendDenseCartesianTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
        }
    }

    /// <summary>
    /// 以旧轨迹文本兼容的空格分隔格式写出数值行。
    /// </summary>
    private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        var sb = new StringBuilder();
        foreach (var row in rows)
        {
            sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
        }

        File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
    }

    /// <summary>
    /// 清理程序名中的非法路径字符，避免输出目录受外部输入污染。
    /// </summary>
    private static string SanitizeDirectoryName(string name)
    {
        var invalidChars = Path.GetInvalidFileNameChars();
        var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
        return new string(chars);
    }

    /// <summary>
    /// 取消当前稠密发送任务，避免旧轨迹继续向机器人下发。
    /// </summary>
    private void CancelSendTaskLocked()
    {
        _sendCts?.Cancel();

        _sendCts?.Dispose();
        _sendCts = null;
    }

    /// <summary>
    /// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码。
    /// 手册约定 writeIoIndex=1 时，LSB 对应第 1 个 IO 点，因此地址 N 对应 bit(N-1)。
    /// </summary>
    internal static ushort ComputeIoValue(IoAddressGroup group)
    {
        ushort value = 0;
        foreach (var addr in group.Addresses)
        {
            if (addr is >= 1 and <= 16)
            {
                value |= (ushort)(1 << (addr - 1));
            }
        }

        return value;
    }

    /// <summary>
    /// 判断当前是否处于仿真模式；若尚未选择控制器则抛出异常。
    /// </summary>
    private bool IsSimulationMode
    {
        get
        {
            if (_activeControllerIsSimulation is null)
            {
                return false;
            }

            return _activeControllerIsSimulation.Value;
        }
    }

    /// <summary>
    /// 断开所有真实 Socket 通道，不影响内存状态。
    /// </summary>
    private void DisconnectClients()
    {
        _j519Client.Disconnect();
        _commandClient.Disconnect();
        _stateClient.Disconnect();
    }

    /// <summary>
    /// 归一化 IO 类型字符串，避免调用方大小写差异影响缓存键。
    /// </summary>
    private static string NormalizeIoType(string ioType)
    {
        if (string.IsNullOrWhiteSpace(ioType))
        {
            throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
        }

        return ioType.Trim().ToUpperInvariant();
    }

    /// <summary>
    /// 将 HTTP 层三维 TCP 请求扩展为 FANUC 命令通道需要的 7 维 Pose。
    /// </summary>
    private static double[] CreateTcpPose(double x, double y, double z)
    {
        return [x, y, z, 0.0, 0.0, 0.0, 1.0];
    }

    /// <summary>
    /// 校验轨迹规划结果可执行。
    /// </summary>
    private static void EnsureValidTrajectory(TrajectoryResult result)
    {
        if (!result.IsValid)
        {
            throw new InvalidOperationException(result.FailureReason ?? "Trajectory result is invalid.");
        }
    }

    /// <summary>
    /// 根据当前内部状态生成连接状态标签。
    /// </summary>
    private string ResolveConnectionState()
    {
        if (_robot is null)
        {
            return "NotConfigured";
        }

        if (string.IsNullOrWhiteSpace(_connectedRobotIp))
        {
            return "Disconnected";
        }

        return _activeControllerIsSimulation is false
            ? ResolveRealConnectionState(_stateClient.GetStatus())
            : "Connected";
    }

    /// <summary>
    /// 把真实 10010 状态通道健康度映射为上层快照连接状态。
    /// </summary>
    internal static string ResolveRealConnectionState(FanucStateClientStatus status)
    {
        ArgumentNullException.ThrowIfNull(status);

        return status.State switch
        {
            FanucStateConnectionState.Connected when status.IsFrameStale => "StateTimeout",
            FanucStateConnectionState.Connected => "Connected",
            FanucStateConnectionState.TimedOut => "StateTimeout",
            FanucStateConnectionState.Reconnecting => "Reconnecting",
            FanucStateConnectionState.Connecting => "Connecting",
            _ => "Disconnected",
        };
    }

    /// <summary>
    /// 判断 runtime 是否可以把某个状态通道帧作为当前机器人状态使用。
    /// </summary>
    internal static bool ShouldUseStateFrame(FanucStateClientStatus status)
    {
        ArgumentNullException.ThrowIfNull(status);
        return status.State == FanucStateConnectionState.Connected && !status.IsFrameStale;
    }

    /// <summary>
    /// 获取未超时的状态帧；超时或重连期间不把旧状态作为当前机器人状态使用。
    /// </summary>
    private FanucStateFrame? GetFreshStateFrame()
    {
        var status = _stateClient.GetStatus();
        return ShouldUseStateFrame(status) ? _stateClient.GetLatestFrame() : null;
    }

    /// <summary>
    /// 校验给定关节数组长度与当前机器人自由度一致。
    /// </summary>
    private void EnsureJointCount(int jointCount)
    {
        var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
        if (jointCount != expectedJointCount)
        {
            throw new InvalidOperationException($"Expected {expectedJointCount} joints but received {jointCount}.");
        }
    }

    /// <summary>
    /// 校验直角位姿固定为 [x,y,z,w,p,r] 六维。
    /// </summary>
    private static void EnsurePoseCount(int poseCount)
    {
        if (poseCount != 6)
        {
            throw new InvalidOperationException($"Expected 6 Cartesian pose values but received {poseCount}.");
        }
    }

    /// <summary>
    /// 校验机器人已经完成初始化。
    /// </summary>
    private void EnsureRobotSetup()
    {
        if (_robot is null || string.IsNullOrWhiteSpace(_robotName))
        {
            throw new InvalidOperationException("Robot has not been setup.");
        }
    }

    /// <summary>
    /// 校验活动控制器已经被选择。
    /// </summary>
    private void EnsureActiveControllerSelected()
    {
        EnsureRobotSetup();
        if (_activeControllerIsSimulation is null)
        {
            throw new InvalidOperationException("Active controller has not been selected.");
        }
    }

    /// <summary>
    /// 校验控制器已经建立连接。
    /// </summary>
    private void EnsureConnected()
    {
        EnsureActiveControllerSelected();
        if (string.IsNullOrWhiteSpace(_connectedRobotIp))
        {
            throw new InvalidOperationException("Controller has not been connected.");
        }
    }

    /// <summary>
    /// 校验机器人已经处于使能态。
    /// </summary>
    private void EnsureEnabled()
    {
        EnsureConnected();
        if (!_isEnabled || _bufferSize <= 0)
        {
            throw new InvalidOperationException("Robot has not been enabled.");
        }
    }
}
