using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;

namespace Flyshot.Core.Planning.Export;

/// <summary>
/// 将规划结果导出为与 Python demo 格式兼容的文本文件。
///
/// 为什么用空格分隔而不是 CSV？
/// ---
/// 空格分隔是最简单的列对齐格式，不需要额外引号处理，也方便用 numpy.loadtxt
/// 或 C++ 的 fscanf 直接读取。真机侧的状态日志通常也是空格分隔的纯文本，
/// 保持一致便于后续做自动化 diff。
/// </summary>
public static class TrajectoryExporter
{
    /// <summary>
    /// 导出规划关节轨迹关键点到文本文件。
    /// </summary>
    public static void WriteJointTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        WriteRows(path, rows);
    }

    /// <summary>
    /// 导出稠密关节轨迹到文本文件。
    /// </summary>
    public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        WriteRows(path, rows);
    }

    /// <summary>
    /// 导出规划笛卡尔轨迹关键点到文本文件。
    /// </summary>
    public static void WriteCartesianTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        WriteRows(path, rows);
    }

    /// <summary>
    /// 导出稠密笛卡尔轨迹到文本文件。
    /// </summary>
    public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        WriteRows(path, rows);
    }

    /// <summary>
    /// 导出触发事件到 JSON 文件。
    /// </summary>
    public static void WriteShotEvents(string path, IReadOnlyList<ShotEvent> events)
    {
        WriteShotEvents(path, events, bindings: null);
    }

    /// <summary>
    /// 导出触发事件到 JSON 文件，并可选附带最终绑定到的实际 sample 信息。
    /// </summary>
    public static void WriteShotEvents(
        string path,
        IReadOnlyList<ShotEvent> events,
        IReadOnlyList<TriggerSampleBinding>? bindings)
    {
        var bindingByWaypointIndex = bindings?
            .GroupBy(static binding => binding.Trigger.WaypointIndex)
            .ToDictionary(static group => group.Key, static group => group.First());

        var payload = events.Select(e => new
        {
            waypoint_index = e.WaypointIndex,
            trigger_time = Math.Round(e.TriggerTime, 6),
            sample_index = e.SampleIndex,
            sample_time = Math.Round(e.SampleTime, 6),
            addrs = e.AddressGroup.Addresses.ToList(),
            trigger_window_seconds = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out var binding)
                ? Math.Round(TriggerSampleBinder.TriggerBindingToleranceSeconds, 6)
                : (double?)null,
            selected_sample_index = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                ? binding.SampleIndex
                : (int?)null,
            selected_send_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                ? Math.Round(binding.Sample.SendTime, 6)
                : (double?)null,
            selected_trajectory_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                ? Math.Round(binding.Sample.TrajectoryTime, 6)
                : (double?)null,
            io_mask = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                ? ComputeIoValue(binding.Trigger.AddressGroup)
                : (ushort?)null,
            reference_joints_deg = bindingByWaypointIndex is not null
                && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                && binding.Trigger.ReferenceJointsDegrees is { Count: > 0 }
                ? binding.Trigger.ReferenceJointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
                : null,
            trigger_joints_deg = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
                ? binding.Sample.JointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
                : null
        }).ToList();

        var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions
        {
            WriteIndented = true
        });

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

    private static void WriteRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        var sb = new StringBuilder();
        foreach (var row in rows)
        {
            sb.AppendLine(string.Join(" ", row.Select(v => $"{v:F6}")));
        }

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

    /// <summary>
    /// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码，和运行时 / saveTrajectory 导出保持一致。
    /// </summary>
    private 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;
    }
}
