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

namespace Flyshot.Core.Config;

/// <summary>
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
/// </summary>
public sealed class RobotModelLoader
{
    private readonly ILogger<RobotModelLoader>? _logger;

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

    /// <summary>
    /// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
    /// </summary>
    /// <param name="modelPath">机器人 JSON 文件路径。</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)
    {
        return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile;
    }

    /// <summary>
    /// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。
    /// </summary>
    /// <param name="modelPath">机器人 JSON 文件路径。</param>
    /// <param name="accLimitScale">加速度全局倍率。</param>
    /// <param name="jerkLimitScale">Jerk 全局倍率。</param>
    /// <returns>包含规划约束视图和运动学几何视图的加载结果。</returns>
    public LoadedRobotModel LoadProfileAndKinematics(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
    {
        if (string.IsNullOrWhiteSpace(modelPath))
        {
            throw new ArgumentException("机器人 JSON 路径不能为空。", 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 JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);

        var resolvedModelPath = Path.GetFullPath(modelPath);
        using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));

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

        var profile = BuildProfile(robotBody, profileName, resolvedModelPath, accLimitScale, jerkLimitScale);
        var kinematicsModel = BuildKinematicsModel(robotBody, profileName);

        _logger?.LogInformation(
            "RobotModel JSON 加载完成: profileName={ProfileName}, dof={Dof}, 几何关节数={JointCount}, resolvedPath={ResolvedPath}",
            profile.Name, profile.DegreesOfFreedom, kinematicsModel.Joints.Count, resolvedModelPath);

        return new LoadedRobotModel(profile, kinematicsModel);
    }

    /// <summary>
    /// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。
    /// </summary>
    /// <param name="modelPath">机器人 JSON 文件路径。</param>
    /// <returns>包含完整关节几何链的运动学模型。</returns>
    public RobotKinematicsModel LoadKinematicsModel(string modelPath)
    {
        return LoadProfileAndKinematics(modelPath).KinematicsModel;
    }

    /// <summary>
    /// 在 robotics.bodies 中找到当前现场机器人主体。
    /// </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;
            }
        }

        foreach (var body in bodies.EnumerateArray())
        {
            if (body.TryGetProperty("joints", out _) && body.TryGetProperty("name", out _))
            {
                return body;
            }
        }

        throw new InvalidDataException("未在机器人 JSON 中找到包含 joints 的机器人主体。");
    }

    /// <summary>
    /// 从机器人主体构造规划约束视图。
    /// </summary>
    private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale)
    {
        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。");
                var multiplier = coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0;
                var offset = coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0;
                jointCouplings.Add(new JointCoupling(
                    slaveJointName: jointName,
                    masterJointName: masterJointName,
                    multiplier: multiplier,
                    offset: offset));
                _logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
            }
        }

        foreach (var jointLimit in jointLimits)
        {
            _logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
        }

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

    /// <summary>
    /// 从机器人主体构造正运动学几何视图。
    /// </summary>
    private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
    {
        var joints = new List<RobotJointGeometry>();
        foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
        {
            var jointName = jointElement.GetProperty("name").GetString()
                ?? throw new InvalidDataException("关节缺少 name。");
            // jointType: 关节类型编码；用于区分旋转关节/其他结构关节，后续几何链路可据此决定求解策略。
            var jointType = jointElement.TryGetProperty("type", out var typeElement)
                ? typeElement.GetInt32()
                : 0;
            // origin: 关节局部原点配置，格式通常为 [x, y, z, qx, qy, qz, qw]，定义父坐标到关节坐标的位姿。
            var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
            // axis: 关节运动轴；部分模型为 4 元组 [x, y, z, scale]，其中方向向量用于正运动学雅可比计算。
            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;
            // originXyz: 平移分量 (x,y,z)，用于构建关节在父链路下的位置偏移。
            var originXyz = origin.Length >= 3 ? origin[..3] : origin;
            // originQuat: 旋转分量 (qx,qy,qz,qw)，用于构建关节在父链路下的姿态；缺省时回退单位四元数。
            var originQuat = origin.Length >= 7 ? origin[3..7] : [0.0, 0.0, 0.0, 1.0];

            // coupleMaster/coupleMultiplier/coupleOffset: 关节耦合参数，描述 slave 关节如何由 master 关节线性映射得到。
            // 典型关系: slave = master * multiplier + offset。
            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;
            }

            var parentLink = jointElement.GetProperty("parent").GetString() ?? string.Empty;
            var childLink = jointElement.GetProperty("child").GetString() ?? string.Empty;
            _logger?.LogInformation(
                "几何关节解析: name={JointName}, parent={Parent}, child={Child}, type={JointType}, axis={Axis}, originXyz={OriginXyz}, originQuat={OriginQuat}, coupleMaster={CoupleMaster}, coupleMultiplier={CoupleMultiplier}, coupleOffset={CoupleOffset}",
                jointName,
                parentLink,
                childLink,
                jointType,
                string.Join(", ", axisVector.Select(static v => v.ToString("G17"))),
                string.Join(", ", originXyz.Select(static v => v.ToString("G17"))),
                string.Join(", ", originQuat.Select(static v => v.ToString("G17"))),
                coupleMaster ?? "<none>",
                coupleMultiplier,
                coupleOffset);

            joints.Add(new RobotJointGeometry(
                // name: 当前关节名，作为几何链和耦合关系的主键。
                name: jointName,
                // parent: 父 link 名称，用于串起机器人树结构。
                parent: parentLink,
                // child: 子 link 名称，标识该关节输出到哪个连杆。
                child: childLink,
                // jointType: 关节类型编码，供运动学模型区分计算路径。
                jointType: jointType,
                // axis: 关节轴方向向量，决定旋转/平移沿哪个局部方向发生。
                axis: axisVector,
                // originXyz: 关节原点平移分量。
                originXyz: originXyz,
                // originQuatXyzw: 关节原点旋转四元数分量。
                originQuatXyzw: originQuat,
                // coupleMaster: 耦合主关节名（无耦合时为 null）。
                coupleMaster: coupleMaster,
                // coupleMultiplier: 耦合线性比例系数。
                coupleMultiplier: coupleMultiplier,
                // coupleOffset: 耦合常量偏移量。
                coupleOffset: coupleOffset));
        }

        _logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={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;
    }
}