Studio/Assets/Scripts/Examples/AutoFactory/AGVMap.cs

using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using UnityEditor;
using UnityEngine;
using XRLib;

namespace Studio.VirtualFactory
{
    public class AGVMap : MonoBehaviour, ISingle
    {
        public List<AGVNode> nodes = new();

#if UNITY_EDITOR
        public void OnDrawGizmos()
        {
            HashSet<(Vector3, Vector3)> drawnLines = new HashSet<(Vector3, Vector3)>();

            foreach (var n in nodes)
            {
                if (n == null)
                    continue;
                    Handles.Label(n.transform.position + Vector3.up * 0.5f, n.name);
                foreach (var l in n.linkedNodes)
                {
                    Vector3 start = n.transform.position;
                    if (l == null)
                        continue;
                    Vector3 end = l.transform.position;

                    // Ensure the line is always stored in a consistent order
                    var line = start.x < end.x || (start.x == end.x && start.y < end.y) || (start.x == end.x && start.y == end.y && start.z < end.z)
                        ? (start, end)
                        : (end, start);

                    if (!drawnLines.Contains(line))
                    {
                        Gizmos.DrawLine(start, end);
                        drawnLines.Add(line);
                    }
                }
            }
        }

#endif
        public List<AGVPortNode> GetPortNodes()
        {
            var result = new List<AGVPortNode>();
            foreach(var n in nodes)
            {
                if (n is AGVPortNode pn)
                {
                    result.Add(pn);
                }
            }

            return result;
        }

        public bool TryGetEmptyInputPortNode(out AGVPortNode portNode)
        {
            foreach (var n in nodes)
            {
                if (n is not AGVPortNode pn)
                    continue;
                
                if (pn.portType != AGVPortNode.PortType.Input)
                    continue;

                if (pn.loader is not InputPort)
                    continue;

                Debug.Log($"TryGetEmptyInputPortNode: {pn.name}");
                if (pn.loader.isEmpty)
                {
                    portNode = pn;
                    return true;
                }
            }
            portNode = null;
            return false;
        }
        internal List<AGVNode> FindPath(AGVNode startNode, AGVNode targetNode)
        {
            var visited = new HashSet<AGVNode>();
            var path = new List<AGVNode>();
            if (DFS(startNode, targetNode, visited, path))
            {
                return path;
            }
            return null; // <20><><EFBFBD>θ<EFBFBD> ã<><C3A3> <20><> <20><><EFBFBD><EFBFBD>
        }

        private bool DFS(AGVNode current, AGVNode target, HashSet<AGVNode> visited, List<AGVNode> path)
        {
            visited.Add(current);
            path.Add(current);

            if (current == target)
            {
                return true;
            }

            foreach (var neighbor in current.linkedNodes)
            {
                if (neighbor == null || visited.Contains(neighbor))
                    continue;

                if (DFS(neighbor, target, visited, path))
                {
                    return true;
                }
            }

            path.RemoveAt(path.Count - 1);
            return false;
        }
    }
}