unity2moveit2/unity-project/Assets/Scripts/Core/VisualizationEngine.cs

using System;
using System.Collections.Generic;
using UnityEngine;

namespace UnityMoveIt2.Core
{
    /// <summary>
    /// 可视化引擎 - Visualization Engine
    /// 主类:管理轨迹、碰撞、关节状态和工作空间的可视化
    /// Main class: Manages trajectory, collision, joint state, and workspace visualization
    /// </summary>
    public partial class VisualizationEngine : MonoBehaviour
    {
        #region Inspector Fields

        [Header("可视化设置 / Visualization Settings")]
        [SerializeField]
        private bool enableVisualization = true;

        [SerializeField]
        private bool showTrajectory = true;

        [SerializeField]
        private bool showCollisions = true;

        [SerializeField]
        private bool showJointStates = false;

        [SerializeField]
        private bool showWorkspace = true;

        [Header("性能设置 / Performance Settings")]
        [SerializeField]
        [Tooltip("可视化更新频率(Hz) / Visualization Update Rate (Hz)")]
        private float updateRate = 30f;

        [SerializeField]
        [Tooltip("最大轨迹点数 / Max Trajectory Points")]
        private int maxTrajectoryPoints = 1000;

        [SerializeField]
        [Tooltip("使用LOD优化 / Use LOD Optimization")]
        private bool useLOD = true;

        [Header("引用 / References")]
        [SerializeField]
        private UniversalRobotAdapter robotAdapter;

        #endregion

        #region Private Fields

        private float lastUpdateTime;
        private float updateInterval;

        // 子模块容器 / Sub-module containers
        private GameObject trajectoryContainer;
        private GameObject collisionContainer;
        private GameObject jointStateContainer;
        private GameObject workspaceContainer;

        // 缓存 / Caching
        private Dictionary<string, GameObject> visualizationCache;
        private bool isInitialized = false;

        #endregion

        #region Properties

        public bool IsInitialized => isInitialized;
        public bool EnableVisualization
        {
            get => enableVisualization;
            set
            {
                enableVisualization = value;
                UpdateVisualizationVisibility();
            }
        }

        #endregion

        #region Events

        public event Action OnVisualizationUpdated;
        public event Action<string> OnVisualizationError;

        #endregion

        #region Unity Lifecycle

        private void Awake()
        {
            InitializeVisualizationEngine();
        }

        private void Start()
        {
            if (robotAdapter == null)
            {
                robotAdapter = GetComponentInParent<UniversalRobotAdapter>();
                if (robotAdapter == null)
                {
                    Debug.LogError("[VisualizationEngine] 未找到UniversalRobotAdapter / UniversalRobotAdapter not found");
                }
            }

            StartVisualization();
        }

        private void Update()
        {
            if (!enableVisualization || !isInitialized)
                return;

            // 限制更新频率 / Limit update frequency
            if (Time.time - lastUpdateTime < updateInterval)
                return;

            UpdateAllVisualizations();
            lastUpdateTime = Time.time;
        }

        private void OnDestroy()
        {
            CleanupVisualization();
        }

        #endregion

        #region Initialization

        private void InitializeVisualizationEngine()
        {
            Debug.Log("[VisualizationEngine] 初始化可视化引擎 / Initializing Visualization Engine");

            // 计算更新间隔 / Calculate update interval
            updateInterval = 1f / updateRate;

            // 创建容器 / Create containers
            CreateContainers();

            // 初始化缓存 / Initialize cache
            visualizationCache = new Dictionary<string, GameObject>();

            // 初始化子模块 / Initialize sub-modules
            InitializeTrajectoryVisualization();
            InitializeCollisionVisualization();
            InitializeJointStateVisualization();
            InitializeWorkspaceVisualization();

            isInitialized = true;
            Debug.Log("[VisualizationEngine] 可视化引擎初始化完成 / Visualization Engine Initialized");
        }

        private void CreateContainers()
        {
            // 轨迹容器 / Trajectory container
            trajectoryContainer = new GameObject("TrajectoryVisualization");
            trajectoryContainer.transform.SetParent(transform);
            trajectoryContainer.transform.localPosition = Vector3.zero;

            // 碰撞容器 / Collision container
            collisionContainer = new GameObject("CollisionVisualization");
            collisionContainer.transform.SetParent(transform);
            collisionContainer.transform.localPosition = Vector3.zero;

            // 关节状态容器 / Joint state container
            jointStateContainer = new GameObject("JointStateVisualization");
            jointStateContainer.transform.SetParent(transform);
            jointStateContainer.transform.localPosition = Vector3.zero;

            // 工作空间容器 / Workspace container
            workspaceContainer = new GameObject("WorkspaceVisualization");
            workspaceContainer.transform.SetParent(transform);
            workspaceContainer.transform.localPosition = Vector3.zero;
        }

        private void StartVisualization()
        {
            UpdateVisualizationVisibility();
        }

        #endregion

        #region Update Methods

        private void UpdateAllVisualizations()
        {
            try
            {
                if (showTrajectory)
                {
                    UpdateTrajectoryVisualization();
                }

                if (showCollisions)
                {
                    UpdateCollisionVisualization();
                }

                if (showJointStates && robotAdapter != null)
                {
                    UpdateJointStateVisualization(robotAdapter.GetCurrentJointPositions());
                }

                if (showWorkspace)
                {
                    UpdateWorkspaceVisualization();
                }

                OnVisualizationUpdated?.Invoke();
            }
            catch (Exception ex)
            {
                Debug.LogError($"[VisualizationEngine] 更新可视化时发生错误 / Error updating visualization: {ex.Message}");
                OnVisualizationError?.Invoke(ex.Message);
            }
        }

        private void UpdateVisualizationVisibility()
        {
            if (trajectoryContainer != null)
                trajectoryContainer.SetActive(enableVisualization && showTrajectory);

            if (collisionContainer != null)
                collisionContainer.SetActive(enableVisualization && showCollisions);

            if (jointStateContainer != null)
                jointStateContainer.SetActive(enableVisualization && showJointStates);

            if (workspaceContainer != null)
                workspaceContainer.SetActive(enableVisualization && showWorkspace);
        }

        #endregion

        #region Public Control Methods

        /// <summary>
        /// 启用/禁用特定可视化类型 / Enable/Disable Specific Visualization Type
        /// </summary>
        public void SetVisualizationType(VisualizationType type, bool enabled)
        {
            switch (type)
            {
                case VisualizationType.Trajectory:
                    showTrajectory = enabled;
                    break;
                case VisualizationType.Collision:
                    showCollisions = enabled;
                    break;
                case VisualizationType.JointState:
                    showJointStates = enabled;
                    break;
                case VisualizationType.Workspace:
                    showWorkspace = enabled;
                    break;
            }

            UpdateVisualizationVisibility();
        }

        /// <summary>
        /// 清除所有可视化 / Clear All Visualizations
        /// </summary>
        public void ClearAllVisualizations()
        {
            ClearTrajectoryVisualization();
            ClearCollisionVisualization();
            ClearJointStateVisualization();
            ClearWorkspaceVisualization();

            Debug.Log("[VisualizationEngine] 已清除所有可视化 / Cleared all visualizations");
        }

        /// <summary>
        /// 设置更新频率 / Set Update Rate
        /// </summary>
        public void SetUpdateRate(float rate)
        {
            updateRate = Mathf.Clamp(rate, 1f, 120f);
            updateInterval = 1f / updateRate;
        }

        #endregion

        #region Cleanup

        private void CleanupVisualization()
        {
            ClearAllVisualizations();

            if (visualizationCache != null)
            {
                foreach (var kvp in visualizationCache)
                {
                    if (kvp.Value != null)
                    {
                        Destroy(kvp.Value);
                    }
                }
                visualizationCache.Clear();
            }

            Debug.Log("[VisualizationEngine] 可视化引擎已清理 / Visualization Engine cleaned up");
        }

        #endregion

        #region Utility Methods

        /// <summary>
        /// 创建或获取缓存的GameObject / Create or Get Cached GameObject
        /// </summary>
        protected GameObject GetOrCreateVisualizationObject(string key, Transform parent)
        {
            if (visualizationCache.ContainsKey(key) && visualizationCache[key] != null)
            {
                return visualizationCache[key];
            }

            GameObject obj = new GameObject(key);
            obj.transform.SetParent(parent);
            obj.transform.localPosition = Vector3.zero;
            visualizationCache[key] = obj;

            return obj;
        }

        /// <summary>
        /// 销毁缓存的GameObject / Destroy Cached GameObject
        /// </summary>
        protected void DestroyVisualizationObject(string key)
        {
            if (visualizationCache.ContainsKey(key))
            {
                if (visualizationCache[key] != null)
                {
                    Destroy(visualizationCache[key]);
                }
                visualizationCache.Remove(key);
            }
        }

        /// <summary>
        /// 计算LOD级别 / Calculate LOD Level
        /// </summary>
        protected int CalculateLODLevel(Vector3 position)
        {
            if (!useLOD)
                return 0;

            Camera mainCam = Camera.main;
            if (mainCam == null)
                return 0;

            float distance = Vector3.Distance(mainCam.transform.position, position);

            if (distance < 2f)
                return 0; // 高细节 / High detail
            else if (distance < 5f)
                return 1; // 中等细节 / Medium detail
            else
                return 2; // 低细节 / Low detail
        }

        #endregion
    }

    #region Enums

    /// <summary>
    /// 可视化类型 / Visualization Type
    /// </summary>
    public enum VisualizationType
    {
        Trajectory,
        Collision,
        JointState,
        Workspace
    }

    #endregion
}