using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;

namespace Flyshot.Core.Config;

/// <summary>
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
/// </summary>
public sealed class RobotModelLoader
{
    private const uint JsonChunkType = 0x4E4F534A;
    private readonly ILogger<RobotModelLoader>? _logger;

    /// <summary>
    /// 初始化 RobotModelLoader。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null。</param>
    public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
    {
        _logger = logger;
    }

    /// <summary>
    /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
    /// </summary>
    /// <param name="modelPath">.robot 文件路径。</param>
    /// <param name="accLimitScale">加速度全局倍率。</param>
    /// <param name="jerkLimitScale">Jerk 全局倍率。</param>
    /// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
    public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
    {
        if (string.IsNullOrWhiteSpace(modelPath))
        {
            throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
        }

        if (accLimitScale <= 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
        }

        if (jerkLimitScale <= 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
        }

        _logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);

        var resolvedModelPath = Path.GetFullPath(modelPath);
        var jsonText = ReadJsonChunk(resolvedModelPath);
        using var document = JsonDocument.Parse(jsonText);

        var robotBody = FindRobotBody(document.RootElement);
        var profileName = robotBody.TryGetProperty("name", out var nameElement)
            ? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
            : Path.GetFileNameWithoutExtension(resolvedModelPath);

        var jointLimits = new List<JointLimit>();
        var jointCouplings = new List<JointCoupling>();

        foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
        {
            if (!IsPlanningJoint(jointElement))
            {
                continue;
            }

            var jointName = jointElement.GetProperty("name").GetString() ?? throw new InvalidDataException("关节缺少 name。");
            var limitElement = jointElement.GetProperty("limit");

            jointLimits.Add(new JointLimit(
                jointName: jointName,
                velocityLimit: limitElement.GetProperty("velocity").GetDouble(),
                accelerationLimit: limitElement.GetProperty("acceleration").GetDouble() * accLimitScale,
                jerkLimit: limitElement.GetProperty("jerk").GetDouble() * jerkLimitScale));

            if (jointElement.TryGetProperty("couple", out var coupleElement))
            {
                var masterJointName = coupleElement.GetProperty("master_joint").GetString()
                    ?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");

                jointCouplings.Add(new JointCoupling(
                    slaveJointName: jointName,
                    masterJointName: masterJointName,
                    multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
                    offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
            }
        }

        _logger?.LogInformation(
            "RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
            profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);

        return new RobotProfile(
            name: profileName,
            modelPath: resolvedModelPath,
            degreesOfFreedom: jointLimits.Count,
            jointLimits: jointLimits,
            jointCouplings: jointCouplings,
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 从 GLB 文件中提取 JSON chunk 文本。
    /// </summary>
    private static string ReadJsonChunk(string modelPath)
    {
        using var stream = File.OpenRead(modelPath);
        using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false);

        var magic = Encoding.ASCII.GetString(reader.ReadBytes(4));
        if (!string.Equals(magic, "glTF", StringComparison.Ordinal))
        {
            throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。");
        }

        var version = reader.ReadUInt32();
        if (version != 2)
        {
            throw new NotSupportedException($"当前仅支持 GLB 2.0，实际版本为 {version}。");
        }

        var totalLength = reader.ReadUInt32();
        while (stream.Position < totalLength)
        {
            var chunkLength = reader.ReadUInt32();
            var chunkType = reader.ReadUInt32();
            var chunkBytes = reader.ReadBytes((int)chunkLength);
            if (chunkType == JsonChunkType)
            {
                return Encoding.UTF8.GetString(chunkBytes);
            }
        }

        throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。");
    }

    /// <summary>
    /// 在 robotics.bodies 中找到 type=2 的机器人主体。
    /// </summary>
    private static JsonElement FindRobotBody(JsonElement root)
    {
        var bodies = root
            .GetProperty("scenes")[0]
            .GetProperty("extras")
            .GetProperty("rvbust")
            .GetProperty("robotics")
            .GetProperty("bodies");

        foreach (var body in bodies.EnumerateArray())
        {
            if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
            {
                return body;
            }
        }

        throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。");
    }

    /// <summary>
    /// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
    /// </summary>
    /// <param name="modelPath">.robot 文件路径。</param>
    /// <returns>包含完整关节几何链的运动学模型。</returns>
    public RobotKinematicsModel LoadKinematicsModel(string modelPath)
    {
        if (string.IsNullOrWhiteSpace(modelPath))
        {
            throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
        }

        _logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", modelPath);

        var resolvedModelPath = Path.GetFullPath(modelPath);
        var jsonText = ReadJsonChunk(resolvedModelPath);
        using var document = JsonDocument.Parse(jsonText);

        var robotBody = FindRobotBody(document.RootElement);
        var profileName = robotBody.TryGetProperty("name", out var nameElement)
            ? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
            : Path.GetFileNameWithoutExtension(resolvedModelPath);

        var joints = new List<RobotJointGeometry>();
        foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
        {
            var jointName = jointElement.GetProperty("name").GetString()
                ?? throw new InvalidDataException("关节缺少 name。");
            var jointType = jointElement.TryGetProperty("type", out var typeElement)
                ? typeElement.GetInt32()
                : 0;
            var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
            var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
            // axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
            var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
            var originXyz = origin.Length >= 3 ? origin[..3] : origin;
            var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 };

            string? coupleMaster = null;
            double coupleMultiplier = 0.0;
            double coupleOffset = 0.0;
            if (jointElement.TryGetProperty("couple", out var coupleElement))
            {
                coupleMaster = coupleElement.GetProperty("master_joint").GetString();
                coupleMultiplier = coupleElement.TryGetProperty("multiplier", out var m) ? m.GetDouble() : 0.0;
                coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
            }

            joints.Add(new RobotJointGeometry(
                name: jointName,
                parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
                child: jointElement.GetProperty("child").GetString() ?? string.Empty,
                jointType: jointType,
                axis: axisVector,
                originXyz: originXyz,
                originQuatXyzw: originQuat,
                coupleMaster: coupleMaster,
                coupleMultiplier: coupleMultiplier,
                coupleOffset: coupleOffset));
        }

        _logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);

        return new RobotKinematicsModel(name: profileName, joints: joints);
    }

    /// <summary>
    /// 判断当前 joint 是否属于规划侧需要保留的旋转关节。
    /// </summary>
    private static bool IsPlanningJoint(JsonElement jointElement)
    {
        return jointElement.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2;
    }
}