using UnityEngine;

namespace UnityMoveIt2.Core
{
    /// <summary>
    /// UR5 手动配置工具 / UR5 Manual Setup Tool
    /// 用于在Unity中手动创建UR5机器人结构（无需URDF Importer包）
    /// </summary>
    public class UR5ManualSetup : MonoBehaviour
    {
        [Header("UR5 配置参数 / UR5 Configuration")]
        [SerializeField]
        [Tooltip("是否创建碰撞体")]
        private bool createColliders = true;

        [SerializeField]
        [Tooltip("是否使用重力")]
        private bool useGravity = false;

        [SerializeField]
        [Tooltip("关节刚度 / Joint Stiffness")]
        private float jointStiffness = 10000f;

        [SerializeField]
        [Tooltip("关节阻尼 / Joint Damping")]
        private float jointDamping = 100f;

        [SerializeField]
        [Tooltip("最大关节力 / Max Joint Force")]
        private float maxJointForce = 150f;

        /// <summary>
        /// UR5的DH参数和几何数据
        /// </summary>
        private readonly float[] linkLengths = { 0.089159f, 0.13585f, 0.425f, 0.39225f, 0.10915f, 0.09465f, 0.0823f };
        private readonly string[] linkNames = { "base_link", "shoulder_link", "upper_arm_link", "forearm_link", "wrist_1_link", "wrist_2_link", "wrist_3_link", "tool0" };

        /// <summary>
        /// 创建完整的UR5机器人结构
        /// </summary>
        [ContextMenu("创建 UR5 结构 / Create UR5 Structure")]
        public void CreateUR5Structure()
        {
            Debug.Log("[UR5ManualSetup] 开始创建UR5机器人结构...");

            // 清理现有子对象
            ClearChildren();

            // 创建根Articulation Body
            ArticulationBody rootBody = GetOrAddComponent<ArticulationBody>();
            rootBody.immovable = true;
            rootBody.useGravity = useGravity;

            // 创建base_link（作为根的第一个子对象）
            GameObject baseLink = CreateLink("base_link", Vector3.zero, Quaternion.identity, transform);
            // 注意：只有根Articulation Body可以设置immovable
            // base_link作为子对象不能设置immovable

            // 创建肩部关节 (shoulder_pan_joint)
            GameObject shoulderLink = CreateRevoluteJoint(
                "shoulder_link",
                baseLink.transform,
                new Vector3(0, 0, linkLengths[0]),
                Vector3.forward,  // Z轴旋转
                -Mathf.PI, Mathf.PI
            );

            // 创建肩部抬升关节 (shoulder_lift_joint)
            GameObject upperArmLink = CreateRevoluteJoint(
                "upper_arm_link",
                shoulderLink.transform,
                new Vector3(0, linkLengths[1], 0),
                Vector3.up,  // Y轴旋转
                -Mathf.PI, Mathf.PI
            );

            // 创建肘关节 (elbow_joint)
            GameObject forearmLink = CreateRevoluteJoint(
                "forearm_link",
                upperArmLink.transform,
                new Vector3(0, -0.1197f, linkLengths[2]),
                Vector3.up,
                -Mathf.PI, Mathf.PI
            );

            // 创建腕关节1 (wrist_1_joint)
            GameObject wrist1Link = CreateRevoluteJoint(
                "wrist_1_link",
                forearmLink.transform,
                new Vector3(0, 0, linkLengths[3]),
                Vector3.up,
                -Mathf.PI, Mathf.PI
            );

            // 创建腕关节2 (wrist_2_joint)
            GameObject wrist2Link = CreateRevoluteJoint(
                "wrist_2_link",
                wrist1Link.transform,
                new Vector3(0, linkLengths[4], 0),
                Vector3.forward,  // Z轴旋转
                -Mathf.PI, Mathf.PI
            );

            // 创建腕关节3 (wrist_3_joint)
            GameObject wrist3Link = CreateRevoluteJoint(
                "wrist_3_link",
                wrist2Link.transform,
                new Vector3(0, 0, linkLengths[5]),
                Vector3.up,
                -Mathf.PI, Mathf.PI
            );

            // 创建tool0（末端执行器）
            GameObject tool0 = new GameObject("tool0");
            tool0.transform.SetParent(wrist3Link.transform);
            tool0.transform.localPosition = new Vector3(0, linkLengths[6], 0);
            tool0.transform.localRotation = Quaternion.identity;

            // 添加可视化标记
            AddVisualMarker(tool0.transform, Color.red, 0.02f);

            // 配置机器人适配器
            ConfigureRobotAdapter(baseLink.transform, tool0.transform);

            Debug.Log("[UR5ManualSetup] ✓ UR5结构创建完成！");
            Debug.LogWarning("[UR5ManualSetup] 下一步：请在Assets/UR5/meshes目录下手动为每个link添加mesh模型");
        }

        /// <summary>
        /// 创建旋转关节
        /// </summary>
        private GameObject CreateRevoluteJoint(string name, Transform parent, Vector3 localPos, Vector3 axis, float lowerLimit, float upperLimit)
        {
            GameObject link = CreateLink(name, localPos, Quaternion.identity, parent);
            ArticulationBody body = link.GetComponent<ArticulationBody>();

            // 配置为旋转关节
            body.jointType = ArticulationJointType.RevoluteJoint;

            // 设置关节轴
            body.anchorRotation = GetAxisRotation(axis);

            // 配置关节驱动
            ArticulationDrive drive = body.xDrive;
            drive.lowerLimit = lowerLimit * Mathf.Rad2Deg;
            drive.upperLimit = upperLimit * Mathf.Rad2Deg;
            drive.stiffness = jointStiffness;
            drive.damping = jointDamping;
            drive.forceLimit = maxJointForce;
            drive.target = 0;
            body.xDrive = drive;

            return link;
        }

        /// <summary>
        /// 创建连杆
        /// </summary>
        private GameObject CreateLink(string name, Vector3 localPos, Quaternion localRot, Transform parent)
        {
            GameObject link = new GameObject(name);
            link.transform.SetParent(parent);
            link.transform.localPosition = localPos;
            link.transform.localRotation = localRot;

            // 添加Articulation Body
            ArticulationBody body = link.AddComponent<ArticulationBody>();
            body.useGravity = useGravity;

            // 添加占位可视化（临时）
            AddVisualMarker(link.transform, Color.cyan, 0.03f);

            // 添加碰撞体
            if (createColliders && name != "tool0")
            {
                SphereCollider collider = link.AddComponent<SphereCollider>();
                collider.radius = 0.05f;
            }

            return link;
        }

        /// <summary>
        /// 添加可视化标记
        /// </summary>
        private void AddVisualMarker(Transform parent, Color color, float size)
        {
            GameObject marker = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            marker.name = "Marker";
            marker.transform.SetParent(parent);
            marker.transform.localPosition = Vector3.zero;
            marker.transform.localScale = Vector3.one * size;

            Renderer renderer = marker.GetComponent<Renderer>();
            Material mat = new Material(Shader.Find("Standard"));
            mat.color = color;
            renderer.material = mat;

            // 移除碰撞体（仅用于可视化）
            // 根据是否在编辑器模式使用不同的销毁方法
            Collider collider = marker.GetComponent<Collider>();
            if (collider != null)
            {
                if (Application.isPlaying)
                    Destroy(collider);
                else
                    DestroyImmediate(collider);
            }
        }

        /// <summary>
        /// 配置机器人适配器
        /// </summary>
        private void ConfigureRobotAdapter(Transform baseLink, Transform tool0)
        {
            UniversalRobotAdapter adapter = GetOrAddComponent<UniversalRobotAdapter>();

            // 使用反射自动设置私有字段
            var baseLinkField = typeof(UniversalRobotAdapter).GetField("baseLink",
                System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);
            var endEffectorField = typeof(UniversalRobotAdapter).GetField("endEffector",
                System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);
            var dofField = typeof(UniversalRobotAdapter).GetField("degreesOfFreedom",
                System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);
            var robotNameField = typeof(UniversalRobotAdapter).GetField("robotName",
                System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);

            if (baseLinkField != null && baseLink != null)
            {
                baseLinkField.SetValue(adapter, baseLink);
                Debug.Log($"[UR5ManualSetup] ✓ 自动设置 Base Link: {baseLink.name}");
            }

            if (endEffectorField != null && tool0 != null)
            {
                endEffectorField.SetValue(adapter, tool0);
                Debug.Log($"[UR5ManualSetup] ✓ 自动设置 End Effector: {tool0.name}");
            }

            if (dofField != null)
            {
                dofField.SetValue(adapter, 6);
                Debug.Log("[UR5ManualSetup] ✓ 自动设置 DOF: 6");
            }

            if (robotNameField != null)
            {
                robotNameField.SetValue(adapter, "UR5");
                Debug.Log("[UR5ManualSetup] ✓ 自动设置 Robot Name: UR5");
            }

            Debug.Log("[UR5ManualSetup] ✓ UniversalRobotAdapter 已自动配置完成!");
        }

        /// <summary>
        /// 获取关节轴旋转
        /// </summary>
        private Quaternion GetAxisRotation(Vector3 axis)
        {
            if (axis == Vector3.up)
                return Quaternion.Euler(0, 90, 0);
            else if (axis == Vector3.forward)
                return Quaternion.identity;
            else if (axis == Vector3.right)
                return Quaternion.Euler(90, 0, 0);
            else
                return Quaternion.identity;
        }

        /// <summary>
        /// 清理子对象
        /// </summary>
        private void ClearChildren()
        {
            while (transform.childCount > 0)
            {
                DestroyImmediate(transform.GetChild(0).gameObject);
            }
        }

        /// <summary>
        /// 获取或添加组件
        /// </summary>
        private T GetOrAddComponent<T>() where T : Component
        {
            T component = GetComponent<T>();
            if (component == null)
                component = gameObject.AddComponent<T>();
            return component;
        }

        /// <summary>
        /// 在Scene视图中绘制辅助线
        /// </summary>
        private void OnDrawGizmos()
        {
            if (transform.childCount > 0)
            {
                DrawJointHierarchy(transform);
            }
        }

        private void DrawJointHierarchy(Transform parent)
        {
            foreach (Transform child in parent)
            {
                Gizmos.color = Color.yellow;
                Gizmos.DrawLine(parent.position, child.position);

                Gizmos.color = Color.green;
                Gizmos.DrawWireSphere(child.position, 0.02f);

                DrawJointHierarchy(child);
            }
        }
    }
}