using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Microsoft.Extensions.Logging;
using System.Text;

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
/// </summary>
public sealed class FlyshotTrajectoryArtifactWriter
{
    /// <summary>
    /// 旧 Data 明细点位文件使用的默认采样周期，单位为秒。
    /// </summary>
    private const double LegacyDetailSamplePeriodSeconds = 0.016;

    /// <summary>
    /// FANUC J519 实际下发的固定伺服周期，单位为秒。
    /// </summary>
    private const double ActualSendServoPeriodSeconds = 0.008;

    private readonly ControllerClientCompatOptions _options;
    private readonly RobotModelLoader _robotModelLoader;
    private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;

    /// <summary>
    /// 初始化规划结果点位导出器。
    /// </summary>
    /// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
    /// <param name="robotModelLoader">机器人模型加载器，用于生成笛卡尔点位。</param>
    /// <param name="logger">日志记录器；允许 null。</param>
    public FlyshotTrajectoryArtifactWriter(
        ControllerClientCompatOptions options,
        RobotModelLoader robotModelLoader,
        ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
    {
        _options = options ?? throw new ArgumentNullException(nameof(options));
        _robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
        _logger = logger;
    }

    /// <summary>
    /// 将飞拍规划结果导出到 Config/Data/name。
    /// </summary>
    /// <param name="trajectoryName">飞拍轨迹名称。</param>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="bundle">规划结果包；旧规划导出和实发诊断使用同一份视图。</param>
    /// <param name="speedRatio">请求速度倍率，仅作为诊断列保留；最终采样倍率已在规划阶段处理。</param>
    public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
    {
        WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
    }

    /// <summary>
    /// 将飞拍规划结果导出到 Config/Data/name，并显式区分旧规划导出视图和实发诊断视图。
    /// </summary>
    /// <param name="trajectoryName">飞拍轨迹名称。</param>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="exportPlanningBundle">旧格式规划导出使用的结果包，不应包含运行时 speedRatio 折算。</param>
    /// <param name="executionBundle">实发诊断使用的结果包，应与运行时最终发送队列保持一致。</param>
    /// <param name="speedRatio">请求速度倍率，仅作为 ActualSend 诊断列保留。</param>
    public void WriteUploadedFlyshot(
        string trajectoryName,
        RobotProfile robot,
        PlannedExecutionBundle exportPlanningBundle,
        PlannedExecutionBundle executionBundle,
        double speedRatio = 1.0)
    {
        if (string.IsNullOrWhiteSpace(trajectoryName))
        {
            throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
        }

        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(exportPlanningBundle);
        ArgumentNullException.ThrowIfNull(executionBundle);

        var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
        Directory.CreateDirectory(outputDir);
        if (exportPlanningBundle.Result.DenseJointTrajectory is null)
        {
            throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
        }

        // 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图，避免和 ActualSend 实发诊断混在一起。
        var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
        var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
        _logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
        var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
        var jointDetailTrajectory = DownsampleDenseRows(
            executionDenseTrajectory,
            samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
        var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
        var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);

        TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
        TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
        TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
        TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
        WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);

        _logger?.LogInformation(
            "saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
            trajectoryName,
            outputDir,
            jointTrajectory.Count,
            jointDetailTrajectory.Count,
            speedRatio);
    }

    /// <summary>
    /// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。
    /// </summary>
    private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
    {
        ArgumentNullException.ThrowIfNull(robot);

        var preparedExecution = result.PreparedFlyshotExecution;
        if (result.DenseJointTrajectory is null && preparedExecution is null)
        {
            return;
        }

        var samples = preparedExecution is null
            ? J519SendTrajectorySampler.SampleDenseJointTrajectory(
                result.DenseJointTrajectory!,
                result.Duration.TotalSeconds,
                ActualSendServoPeriodSeconds,
                speedRatio: 1.0)
            : preparedExecution.Samples.Select(static sample => new J519SendSample(
                sample.SampleIndex,
                sample.SendTime,
                sample.TrajectoryTime,
                sample.SpeedRatio,
                sample.JointsDegrees)).ToArray();
        var triggerBindings = preparedExecution is null
            ? TriggerSampleBinder.Bind(
                result.TriggerTimeline,
                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();
        TrajectoryLimitValidator.ValidateJ519SendSamples(
            robot,
            samples,
            trajectoryName: result.ProgramName,
            validateJerk: false);
        var jointRows = new List<IReadOnlyList<double>>(samples.Count);
        List<IReadOnlyList<double>> timingRows = preparedExecution is null
            ? new List<IReadOnlyList<double>>(samples.Count)
            : preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
        List<IReadOnlyList<double>> jerkRows = preparedExecution is null
            ? []
            : preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
        double? previousSendTime = null;
        double[]? previousJoints = null;
        double[]? previousVelocity = null;
        double[]? previousAcceleration = null;

        foreach (var sample in samples)
        {
            jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
            if (preparedExecution is null)
            {
                timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));

                if (previousSendTime is not null && previousJoints is not null)
                {
                    jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
                        previousSendTime.Value,
                        sample.SendTime,
                        previousJoints,
                        sample.JointsDegrees,
                        ref previousVelocity,
                        ref previousAcceleration));
                }
            }

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

        WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
        WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
        WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
        TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
    }

    /// <summary>
    /// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
    /// </summary>
    private static IReadOnlyList<double> BuildActualSendJointRow(double sendTime, IReadOnlyList<double> joints)
    {
        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] = 0.0;
        row[^1] = 0.0;
        return row;
    }

    /// <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>
    /// 构造 JointTraj.txt 行数据，格式为 time + 关节弧度。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
    {
        var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
        for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
        {
            var row = new List<double>(trajectory.PlannedWaypoints[index].Positions.Count + 1)
            {
                Math.Round(trajectory.WaypointTimes[index], 6)
            };
            row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6)));
            rows.Add(row);
        }

        return rows;
    }

    /// <summary>
    /// 构造 CartTraj.txt 行数据，格式为 time + x/y/z/qx/qy/qz/qw。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
        PlannedTrajectory trajectory,
        RobotKinematicsModel kinematicsModel)
    {
        var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
        for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
        {
            var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray());
            var row = new List<double>(pose.Length + 1)
            {
                Math.Round(trajectory.WaypointTimes[index], 6)
            };
            row.AddRange(pose.Select(static value => Math.Round(value, 6)));
            rows.Add(row);
        }

        return rows;
    }

    /// <summary>
    /// 基于执行侧稠密关节轨迹生成笛卡尔导出行，保持与 JointDetialTraj.txt 同一来源。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRowsFromJointDense(
        IReadOnlyList<IReadOnlyList<double>> jointDenseRows,
        RobotKinematicsModel kinematicsModel)
    {
        var rows = new List<IReadOnlyList<double>>(jointDenseRows.Count);
        foreach (var jointRow in jointDenseRows)
        {
            var jointPositions = jointRow.Skip(1).ToArray();
            var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPositions);
            var row = new List<double>(pose.Length + 1)
            {
                Math.Round(jointRow[0], 6)
            };
            row.AddRange(pose.Select(static value => Math.Round(value, 6)));
            rows.Add(row);
        }

        return rows;
    }

    /// <summary>
    /// 将 8ms 执行稠密轨迹按指定周期抽稀为低频兼容视图，并始终保留终点。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
        IReadOnlyList<IReadOnlyList<double>> denseRows,
        double samplePeriodSeconds)
    {
        var result = new List<IReadOnlyList<double>>();
        var epsilon = 1e-6;
        var nextSampleTime = 0.0;

        foreach (var row in denseRows)
        {
            var sampleTime = row[0];
            if (sampleTime + epsilon < nextSampleTime)
            {
                continue;
            }

            if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
            {
                result.Add(row);
                nextSampleTime += samplePeriodSeconds;
            }
        }

        if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
        {
            result.Add(denseRows[^1]);
        }

        return result;
    }

    /// <summary>
    /// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。
    /// </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);
    }
}