using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;

namespace AGVNavigationCore.PathFinding.Planning
{
    /// <summary>
    /// AGV 경로 계획기
    /// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
    /// </summary>
    public class AGVPathfinder
    {

        private readonly List<MapNode> _mapNodes;
        private readonly AStarPathfinder _basicPathfinder;
        private readonly DirectionalPathfinder _directionPathfinder;
        private readonly JunctionAnalyzer _junctionAnalyzer;
        private readonly DirectionChangePlanner _directionChangePlanner;

        public AGVPathfinder(List<MapNode> mapNodes)
        {
            _mapNodes = mapNodes ?? new List<MapNode>();
            _basicPathfinder = new AStarPathfinder();
            _basicPathfinder.SetMapNodes(_mapNodes);
            _junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
            _directionChangePlanner = new DirectionChangePlanner(_mapNodes);
            _directionPathfinder = new DirectionalPathfinder();
        }

        /// <summary>
        /// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
        /// </summary>
        /// <param name="startNode">기준이 되는 노드</param>
        /// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
        public MapNode FindNearestJunction(MapNode startNode)
        {
            if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
                return null;

            // 교차로: 3개 이상의 노드가 연결된 노드
            var junctions = _mapNodes.Where(n =>
                n.IsActive &&
                n.IsNavigationNode() &&
                n.ConnectedNodes != null &&
                n.DisableCross == false &&
                n.ConnectedNodes.Count >= 3 &&
                n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
                n.Id != startNode.Id
            ).ToList();

            // docking 포인트가 연결된 노드는 제거한다.


            if (junctions.Count == 0)
                return null;

            // 직선 거리 기반으로 가장 가까운 교차로 찾기
            MapNode nearestJunction = null;
            float minDistance = float.MaxValue;

            foreach (var junction in junctions)
            {
                float dx = junction.Position.X - startNode.Position.X;
                float dy = junction.Position.Y - startNode.Position.Y;
                float distance = (float)Math.Sqrt(dx * dx + dy * dy);

                if (distance < minDistance)
                {
                    minDistance = distance;
                    nearestJunction = junction;
                }
            }

            return nearestJunction;
        }

        /// <summary>
        /// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
        /// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
        /// </summary>
        /// <param name="startNode">시작 노드</param>
        /// <param name="targetNode">목적지 노드</param>
        /// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
        public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
        {
            if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
                return null;

            // 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
            var StartNode = pathResult.Path.First();
            foreach (var pathNode in pathResult.Path)
            {
                if (pathNode != null &&
                    pathNode.IsActive &&
                    pathNode.IsNavigationNode() &&
                    pathNode.DisableCross == false &&
                    pathNode.ConnectedNodes != null &&
                    pathNode.ConnectedNodes.Count >= 3 &&
                    pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
                {
                    if (pathNode.Id.Equals(StartNode.Id) == false)
                        return pathNode;
                }
            }

            return null;
        }


        public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
        {
            // 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
            return _basicPathfinder.FindPathAStar(startNode, targetNode);
        }


        /// <summary>
        /// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
        /// </summary>

        /// <summary>
        /// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환)
        /// </summary>
        public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode _prevNode, AgvDirection prevDirection)
        {
            // 1. 입력 검증
            if (startNode == null || targetNode == null)
                return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0);

            // 2. A* 경로 탐색
            var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
            pathResult.PrevNode = _prevNode;
            pathResult.PrevDirection = prevDirection;

            if (!pathResult.Success)
                return AGVPathResult.CreateFailure(pathResult.Message ?? "경로 없음", 0, 0);

            // 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
            // 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
            if (pathResult.Path != null && pathResult.Path.Count > 0)
            {
                var detailedPath = new List<NodeMotorInfo>();
                for (int i = 0; i < pathResult.Path.Count; i++)
                {
                    var node = pathResult.Path[i];
                    var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;

                    // 마그넷 방향 계산 (갈림길인 경우)
                    // 마그넷 방향 계산 (갈림길인 경우)
                    MagnetDirection magnetDirection = MagnetDirection.Straight;

                    //갈림길에 있다면 미리 방향을 저장해준다.
                    if ((node.ConnectedNodes?.Count ?? 0) > 2 && nextNode != null)
                    {
                        //다음 노드ID를 확인해서 마그넷 방향 데이터를 찾는다.
                        if (node.MagnetDirections.ContainsKey(nextNode.Id) == false)
                        {
                            return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
                        }
                        else
                        {
                            var magdir = node.MagnetDirections[nextNode.Id].ToString();
                            if (magdir == "L") magnetDirection = MagnetDirection.Left;
                            else if (magdir == "R") magnetDirection = MagnetDirection.Right;

                        }


                    }

                    var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);

                    // 속도 설정
                    var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
                    if (mapNode != null)
                    {
                        nodeInfo.Speed = mapNode.SpeedLimit;
                        detailedPath.Add(nodeInfo);
                    }


                }
                pathResult.DetailedPath = detailedPath;
            }

            return pathResult;
        }




        /// <summary>
        /// 새로운 경로 계산 로직 (방향성 A* + 제약조건)
        /// 1. 180도 회전은 RFID 3번에서만 가능
        /// 2. 120도 미만 예각 회전 불가 (단, RFID 3번에서 스위치백은 가능)
        /// 3. 목적지 도킹 방향 준수
        /// </summary>
        public AGVPathResult CalculatePath_new(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
        {
            if (startNode == null || targetNode == null)
                return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");

            // 초기 상태 설정
            var openSet = new List<SearchState>();
            var closedSet = new HashSet<string>(); // Key: "CurrentID_PrevID"
            
            // 시작 상태 생성
            var startState = new SearchState
            {
                CurrentNode = startNode,
                PreviousNode = prevNode, // 진입 방향 계산용
                CurrentDirection = prevDir, // 현재 모터 방향
                GCost = 0,
                HCost = CalculateHeuristic(startNode, targetNode),
                Parent = null
            };

            openSet.Add(startState);

            while (openSet.Count > 0)
            {
                // F Cost가 가장 낮은 상태 선택
                var currentState = GetLowestFCostState(openSet);
                openSet.Remove(currentState);

                // 방문 기록 (상태 기반: 현재노드 + 진입노드)
                string stateKey = GetStateKey(currentState);
                if (closedSet.Contains(stateKey)) continue;
                closedSet.Add(stateKey);

                // 목적지 도달 검사
                if (currentState.CurrentNode.Id == targetNode.Id)
                {
                    // 도킹 방향 제약 조건 확인
                    if (IsDockingDirectionValid(currentState, targetNode))
                    {
                        return ReconstructPath_New(currentState);
                    }
                    // 도킹 방향이 안 맞으면? -> 이 경로로는 불가. 다른 경로 탐색 계속.
                    // (단, 제자리 회전이 가능한 경우라면 여기서 추가 처리를 할 수도 있음)
                    // 현재 로직상 도착 후 제자리 회전은 없으므로 Pass.
                }

                // 이웃 노드 탐색
                foreach (var nextNodeId in currentState.CurrentNode.ConnectedNodes)
                {
                    var nextNode = _mapNodes.FirstOrDefault(n => n.Id == nextNodeId);
                    if (nextNode == null || !nextNode.IsActive) continue;

                    // 이동 가능 여부 및 비용 계산 (회전 제약 포함)
                    var moveTry = CheckMove(currentState, nextNode);

                    if (moveTry.IsPossible)
                    {
                        var newState = new SearchState
                        {
                            CurrentNode = nextNode,
                            PreviousNode = currentState.CurrentNode,
                            CurrentDirection = moveTry.NextDirection,
                            GCost = currentState.GCost + moveTry.Cost,
                            HCost = CalculateHeuristic(nextNode, targetNode),
                            Parent = currentState,
                            TurnType = moveTry.TurnType // 디버깅용
                        };

                        // 이미 방문한 더 나은 경로가 있는지 확인
                        // (여기서는 ClosedSet만 체크하고 OpenSet 내 중복 처리는 생략 - 간단 구현)
                        // A* 최적화를 위해 OpenSet 내 동일 상태(Key)가 있고 G Cost가 더 낮다면 Skip해야 함.
                        
                        string newStateKey = GetStateKey(newState);
                        if (closedSet.Contains(newStateKey)) continue;

                        var existingOpen = openSet.FirstOrDefault(s => GetStateKey(s) == newStateKey);
                        if (existingOpen != null)
                        {
                            if (newState.GCost < existingOpen.GCost)
                            {
                                openSet.Remove(existingOpen);
                                openSet.Add(newState);
                            }
                        }
                        else
                        {
                            openSet.Add(newState);
                        }
                    }
                }
            }

            return AGVPathResult.CreateFailure("조건을 만족하는 경로를 찾을 수 없습니다.");
        }

        #region Helper Classes & Methods for CalculatePath_new

        private class SearchState
        {
            public MapNode CurrentNode { get; set; }
            public MapNode PreviousNode { get; set; }
            public AgvDirection CurrentDirection { get; set; } // 현재 모터 방향 (Forward/Backward)
            
            public float GCost { get; set; }
            public float HCost { get; set; }
            public float FCost => GCost + HCost;
            
            public SearchState Parent { get; set; }
            public string TurnType { get; set; } // Debug info
        }

        private struct MoveResult
        {
            public bool IsPossible;
            public float Cost;
            public AgvDirection NextDirection;
            public string TurnType;
        }

        private string GetStateKey(SearchState state)
        {
            string prevId = state.PreviousNode?.Id ?? "null";
            // 모터 방향도 상태에 포함해야 함 (같은 노드, 같은 진입이라도 모터방향 다르면 다른 상태 - 스위치백 때문)
            return $"{state.CurrentNode.Id}_{prevId}_{state.CurrentDirection}";
        }

        private SearchState GetLowestFCostState(List<SearchState> openSet)
        {
            SearchState lowest = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].FCost < lowest.FCost)
                    lowest = openSet[i];
            }
            return lowest;
        }

        private float CalculateHeuristic(MapNode from, MapNode to)
        {
            // 유클리드 거리
            float dx = from.Position.X - to.Position.X;
            float dy = from.Position.Y - to.Position.Y;
            return (float)Math.Sqrt(dx * dx + dy * dy);
        }

        private MoveResult CheckMove(SearchState current, MapNode next)
        {
            var res = new MoveResult { IsPossible = false, Cost = float.MaxValue };

            // 1. 기본 거리 비용
            float dist = (float)Math.Sqrt(Math.Pow(current.CurrentNode.Position.X - next.Position.X, 2) + Math.Pow(current.CurrentNode.Position.Y - next.Position.Y, 2));
            
            // 2. 회전 각도 계산
            // Vector Prev -> Curr
            // Vector Curr -> Next
            
            double angle = 180.0; // Straight
            if (current.PreviousNode != null)
            {
                // Vector 1 (Prev -> Curr)
                double v1x = current.CurrentNode.Position.X - current.PreviousNode.Position.X;
                double v1y = current.CurrentNode.Position.Y - current.PreviousNode.Position.Y;

                // Vector 2 (Curr -> Next)
                double v2x = next.Position.X - current.CurrentNode.Position.X;
                double v2y = next.Position.Y - current.CurrentNode.Position.Y;

                // 각도 계산 (0 ~ 180)
                // 내적 이용: a·b = |a||b|cosθ
                double dot = v1x * v2x + v1y * v2y;
                double mag1 = Math.Sqrt(v1x * v1x + v1y * v1y);
                double mag2 = Math.Sqrt(v2x * v2x + v2y * v2y);

                if (mag1 > 0 && mag2 > 0)
                {
                    double cosTheta = dot / (mag1 * mag2);
                    // 부동소수점 오차 보정
                    if (cosTheta > 1.0) cosTheta = 1.0;
                    if (cosTheta < -1.0) cosTheta = -1.0;
                    
                    double rad = Math.Acos(cosTheta);
                    // 외적을 이용해 좌/우 판별이 가능하지만, 여기서는 "꺾인 정도"만 중요하므로 내적각(0~180)만 사용
                    // 180도 = 직진, 90도 = 직각, 0도 = U턴(완전 뒤로)
                    // 주의: 벡터 방향 기준임. 
                    // Prev->Curr 가 (1,0) 이고 Curr->Next가 (1,0) 이면 각도 0도? 아니면 180?
                    // 위 내적 계산에서 같은 방향이면 cos=1 -> acos=0. 즉 0도가 직진임.
                    // 180도가 U턴(역방향).
                    
                    // 변환: 사용자가 "120도 이상 완만해야" 라고 했음.
                    // 그림상 11->3->4는 예각(Sharp turn).
                    // 직선 주행시 각도 변화량(Deviation)으로 생각하면:
                    // 직진 = 0도 변화.
                    // 90도 턴 = 90도 변화.
                    // U턴 = 180도 변화.
                    
                    angle = rad * (180.0 / Math.PI); // 0(직진) ~ 180(U턴)
                }
            }
            else
            {
                angle = 0; // 시작점에서는 직진으로 간주 (또는 자유)
            }

            // 제약 조건 체크
            // 조건 2: 120도 미만 예각 회전 불가? 
            // 사용자 멘트: "11->3->4 ... 120도 이상 완만해야 이동 가능... 3->4는 각도가 훨씬 작아서 커브 못틈"
            // 여기서 "각도"가 내각(Inner Angle)을 말하는지, 진행 경로의 꺾임각을 말하는지 중요.
            // 11->3->4 그림을 보면 "V"자 형태에 가까움. (Sharp Turn)
            // 즉, "진행 방향의 꺾임각"이 크다(예: 150도 꺾임) = "내각"이 작다(30도).
            // "120도 이상 완만해야" -> 내각이 120도 이상(점잖은 턴)이어야 한다.
            // 즉, 꺾임각(Deviation)은 60도 이하여야 한다.
            
            // 내 알고리즘의 `angle`은 "꺾임각(Deviation)"임. (0=직진, 180=U턴)
            // 따라서 "내각 120도 이상" == "꺾임각 60도 이하".
            // 그러므로 angle <= 60 이어야 Normal Turn 가능.
            
            // 3번 노드 (RFID 3) 특수성:
            // "3번 노드에서만 180도 턴 가능"
            // "3->4 처럼 각이 좁은 경우 3번으로 가서 모터방향 바꿔서 4번으로 감"
            
            bool isRfid3 = current.CurrentNode.RfidId == 3;
            bool isSharpTurn = angle > 60.0; // 60도보다 많이 꺾이면 Sharp Turn (내각 120도 미만)

            // Case A: Normal Move (완만한 턴)
            if (!isSharpTurn)
            {
                res.IsPossible = true;
                res.Cost = dist;
                res.NextDirection = current.CurrentDirection; // 방향 유지
                res.TurnType = "Normal";
                return res;
            }

            // Case B: Sharp Turn or U-Turn
            if (isSharpTurn)
            {
                if (isRfid3)
                {
                    // 3번 노드에서는 무슨 짓이든(?) 가능하다. (Switch Back)
                    // 멈춰서 방향을 바꾸므로, 모터 방향도 바뀜 (FWD -> BWD or BWD -> FWD)
                    // 비용 패널티 추가 (멈추고 바꾸는 시간 등)
                    res.IsPossible = true;
                    res.Cost = dist + 5000; // 큰 비용 (Switch Penalty)
                    res.NextDirection = (current.CurrentDirection == AgvDirection.Forward) 
                                        ? AgvDirection.Backward 
                                        : AgvDirection.Forward;
                    res.TurnType = "SwitchBack";
                    return res;
                }
                else
                {
                    // 3번 노드가 아닌데 Sharp Turn 하려고 함 -> 불가
                    res.IsPossible = false;
                    return res;
                }
            }

            return res;
        }

        private bool IsDockingDirectionValid(SearchState state, MapNode target)
        {
            if (target.DockDirection == DockingDirection.DontCare) return true;

            // 도착 시 모터 방향 확인
            // SearchState에 저장된 CurrentDirection이 도착 시 모터 방향임.
            
            if (target.DockDirection == DockingDirection.Forward)
                return state.CurrentDirection == AgvDirection.Forward;

            if (target.DockDirection == DockingDirection.Backward)
                return state.CurrentDirection == AgvDirection.Backward;

            return true;
        }

        private AGVPathResult ReconstructPath_New(SearchState endState)
        {
            var result = new AGVPathResult { Success = true, TotalDistance = endState.GCost };
            var pathList = new List<MapNode>();
            var detailedList = new List<NodeMotorInfo>();

            var current = endState;
            var pathStack = new Stack<SearchState>();
            
            while (current != null)
            {
                pathStack.Push(current);
                current = current.Parent;
            }

            // Path 재구성
            int seq = 1;
            while (pathStack.Count > 0)
            {
                var step = pathStack.Pop();
                pathList.Add(step.CurrentNode);
                
                // Detailed Info (마그넷 방향 계산 등은 후처리 필요할 수도 있으나 여기선 단순화)
                // SearchState에는 "어떤 방향으로 왔는지"가 저장되어 있음.
                
                // 마그넷 방향 계산 (다음 노드가 있을 때)
                MagnetDirection magDir = MagnetDirection.Straight;
                if (pathStack.Count > 0)
                {
                    var nextStep = pathStack.Peek();
                    // Current -> NextStep 이동 시 마그넷 방향
                    if (step.CurrentNode.MagnetDirections.ContainsKey(nextStep.CurrentNode.Id))
                    {
                        var magPos = step.CurrentNode.MagnetDirections[nextStep.CurrentNode.Id];
                         if (magPos == MagnetPosition.L) magDir = MagnetDirection.Left;
                         else if (magPos == MagnetPosition.R) magDir = MagnetDirection.Right;

                         // 만약 SwitchBack 상황이라면? 
                         // 모터 방향이 바뀌었다면 마그넷 방향도 그에 맞춰야 하나?
                         // 기존 로직 참고: MagnetDirection은 "진행 방향 기준" 좌/우 인가? 아님 절대적?
                         // 보통 "갈림길에서 어느 쪽" 인지 나타냄.
                    }
                }
                
                detailedList.Add(new NodeMotorInfo(seq++, step.CurrentNode.Id, step.CurrentNode.RfidId, step.CurrentDirection, null, magDir)); // NextNode는 일단 null
            }
            
            // NextNode 정보 채우기
            for (int i = 0; i < detailedList.Count - 1; i++)
            {
                detailedList[i].NextNode = _mapNodes.FirstOrDefault(n => n.Id == detailedList[i+1].NodeId);
            }

            result.Path = pathList;
            result.DetailedPath = detailedList;
            
            return result;
        }
        
        #endregion

        public enum MapZone
        {
            None,
            Buffer,   // 91 ~ 07
            Charger,  // 73 ~ 10
            Plating,  // 72 ~ 05
            Loader,   // 71 ~ 04
            Cleaner,  // 70 ~ 01
            Junction  // Hub (11, 12, etc)
        }

        public MapZone GetMapZone(MapNode node)
        {
            if (node == null) return MapZone.None;
            int rfid = node.RfidId;

            // Buffer: 91~07 (Linear)
            // Assuming 91 is start, 07 is end. 
            // Range check might be tricky if IDs are not sequential.
            // Using precise list based on map description if possible, acts as a catch-all for now.
            if (rfid == 91 || (rfid >= 31 && rfid <= 36) || (rfid >= 7 && rfid <= 9)) return MapZone.Buffer;
            
            // Charger: 73~10
            if (rfid == 73 || rfid == 6 || rfid == 10) return MapZone.Charger;

            // Plating: 72~5
            if (rfid == 72 || rfid == 5) return MapZone.Plating;

            // Loader: 71~4
            if (rfid == 71 || rfid == 4) return MapZone.Loader;

            // Cleaner: 70~1
            if (rfid == 70 || rfid == 1 || rfid == 2 || rfid == 3) return MapZone.Cleaner;

            // Junction (Hub)
            if (rfid == 11 || rfid == 12) return MapZone.Junction;

            return MapZone.None;
        }

        public AGVPathResult CalculateScriptedPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
        {
            var startZone = GetMapZone(startNode);
            var targetZone = GetMapZone(targetNode);

            // 1. Same Zone or Trivial Case -> Use CalculatePath_new
            if (startZone == targetZone && startZone != MapZone.None && startZone != MapZone.Junction)
            {
                return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
            }

            // 2. Hub Logic (Buffer -> Hub -> Target, etc.)
            // Logic: Start -> ExitNode -> Hub -> EntryNode -> Target
            
            MapNode exitNode = GetZoneExitNode(startZone);
            MapNode entryNode = GetZoneEntryNode(targetZone);

            // If Start/Target are in Junction or Unknown, handle gracefully
            if (startZone == MapZone.Junction) exitNode = startNode;
            if (targetZone == MapZone.Junction) entryNode = targetNode;

            if (exitNode == null || entryNode == null)
            {
                // Fallback to normal search if zone logic fails
                return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
            }

            // Path 1: Start -> Exit
            var path1 = CalculatePath_new(startNode, exitNode, prevNode, prevDir);
            if (!path1.Success) return AGVPathResult.CreateFailure($"Zone Exit Failure: {startNode.ID2}->{exitNode.ID2}");

            // Path 2: Exit -> Entry (Hub Crossing)
            // Use CalculatePath_new for Hub crossing relative to Arrival Direction
            var lastNode1 = path1.Path.Last();
            var lastDir1 = path1.DetailedPath.Last().MotorDirection;
            var prevNode1 = path1.Path.Count > 1 ? path1.Path[path1.Path.Count - 2] : prevNode;

            var path2 = CalculatePath_new(exitNode, entryNode, prevNode1, lastDir1);
            if (!path2.Success) return AGVPathResult.CreateFailure($"Hub Crossing Failure: {exitNode.ID2}->{entryNode.ID2}");

            // Path 3: Entry -> Target
            var lastNode2 = path2.Path.Last();
            var lastDir2 = path2.DetailedPath.Last().MotorDirection;
            var prevNode2 = path2.Path.Count > 1 ? path2.Path[path2.Path.Count - 2] : lastNode1;

            var path3 = CalculatePath_new(entryNode, targetNode, prevNode2, lastDir2);
            if (!path3.Success) return AGVPathResult.CreateFailure($"Zone Entry Failure: {entryNode.ID2}->{targetNode.ID2}");

            // Merge Paths
            var merged = Utility.CombineResults(path1, path2);
            merged = Utility.CombineResults(merged, path3);

            return merged;
        }

        private MapNode GetZoneExitNode(MapZone zone)
        {
            int exitRfid = -1;
            switch (zone)
            {
                case MapZone.Buffer: exitRfid = 7; break;
                case MapZone.Charger: exitRfid = 10; break; // Or 6? Assuming 10 based on flow
                case MapZone.Plating: exitRfid = 5; break;
                case MapZone.Loader: exitRfid = 4; break;
                case MapZone.Cleaner: exitRfid = 1; break;
                case MapZone.Junction: return null;
            }
            return _mapNodes.FirstOrDefault(n => n.RfidId == exitRfid);
        }

        private MapNode GetZoneEntryNode(MapZone zone)
        {
             int entryRfid = -1;
            switch (zone)
            {
                case MapZone.Buffer: entryRfid = 7; break; // Bi-directional entry/exit? 
                // Usually Buffer entry might be different (e.g. 91?). 
                // But user didn't specify directional flow constraints for zones other than turn logic.
                // Let's assume Entry = Exit for single-lane spurs, or define specific entry points.
                // If Buffer is 91~07, maybe Entry is 7?
                case MapZone.Charger: entryRfid = 10; break;
                case MapZone.Plating: entryRfid = 5; break;
                case MapZone.Loader: entryRfid = 4; break;
                case MapZone.Cleaner: entryRfid = 1; break;
            }
            return _mapNodes.FirstOrDefault(n => n.RfidId == entryRfid);
        }

        /// <summary>
        /// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관)
        /// </summary>
        public AGVPathResult CalculatePath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
        {
            AGVPathResult Retval;
            // var o_StartNode = startNode;
            // startNode, targetNode는 이미 인자로 받음

            if (startNode == null || targetNode == null) return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");

            try
            {
                // 종료노드라면 이전위치로 이동시켜야한다. (Simulator Logic)
                // 만약 시작노드가 끝단(ConnectedMapNodes.Count == 1)이라면, 
                // AGV가 해당 노드에 '도착'한 상태가 아니라 '작업' 중일 수 있으므로 
                // 이전 노드(진입점)로 위치를 보정하여 경로를 계산한다.
                AGVPathResult LimitPath = null;
                if (startNode.ConnectedMapNodes.Count == 1)
                {
                    // 시작점 -> 이전점 경로 (보통 후진이나 전진 1칸)
                    LimitPath = this.FindPathAStar(startNode, prevNode);
                    if (LimitPath.Success)
                    {
                        for (int i = 0; i < LimitPath.Path.Count; i++)
                        {
                            var nodeinfo = LimitPath.Path[i];
                            var dir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
                            LimitPath.DetailedPath.Add(new NodeMotorInfo(i + 1, nodeinfo.Id, nodeinfo.RfidId, dir));
                        }

                        // 시작 위치 및 방향 변경
                        // var org_start = startNode; // Unused
                        startNode = prevNode;
                        prevNode = LimitPath.Path.First(); // startNode (original)
                        prevDir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
                    }
                    else
                    {
                        // 경로 생성 실패 시 보정 없이 진행하거나 에러 처리
                        // 여기서는 일단 기존 로직대로 진행
                    }
                }

                // 2. Buffer-to-Buffer 예외 처리
                // 05~31 구간 체크
                var node_buff_sta = _mapNodes.Where(t => t.StationType == StationType.Buffer).OrderBy(s => s.RfidId).FirstOrDefault();// (n => n.RfidId == 5);
                var node_buff_end = _mapNodes.Where(t => t.StationType == StationType.Buffer).OrderByDescending(s => s.RfidId).FirstOrDefault();//

                bool fixpath = false;
                Retval = null;
                MapNode gatewayNode = null;

                if (node_buff_sta != null && node_buff_end != null)
                {
                    // 버퍼 구간 경로 테스트
                    var rlt = this.FindPathAStar(node_buff_sta, node_buff_end);
                    if (rlt.Success)
                    {
                        // 버퍼구간내에 시작과 종료가 모두 포함되어있다
                        if (rlt.Path.Find(n => n.Id == startNode.Id) != null &&
                            rlt.Path.Find(n => n.Id == targetNode.Id) != null)
                        {
                            Retval = CalcPathBufferToBuffer(startNode, targetNode, prevNode, prevDir);
                            fixpath = true;
                        }
                    }
                }

                if (!fixpath)
                {
                

                    if (targetNode.StationType == StationType.Limit || targetNode.StationType == StationType.Normal)
                    {
                        //일반노드라면 방향 무관하게 그냥 이동하게한다.
                        Retval = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
                    }
                    else
                    {

                        //목적지까지 그대로 방향을 계산한다.
                        var pathToTaget = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
                        if (pathToTaget.Success == false)
                            return AGVPathResult.CreateFailure($"Target({targetNode.ID2})까지 경로 실패: {pathToTaget.Message}");

                        // 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
                        //다음이동방향이 이전노드와 동일하다면? 되돌아가야한다는것이다
                        var predictNext = pathToTaget.Path[1];
                        if (predictNext.Id == prevNode.Id)
                        {
                            var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
                            foreach (var item in pathToTaget.DetailedPath)
                                item.MotorDirection = reverseDir;
                        }

                        if (targetNode.DockDirection != DockingDirection.DontCare)
                        {
                            Retval = pathToTaget;
                        }
                        //현재 진행방향과 목적지의 도킹방향이 일치하는지 확인한다.
                        else if ((pathToTaget.DetailedPath.Last().MotorDirection == AgvDirection.Backward && targetNode.DockDirection == DockingDirection.Backward) ||
                           (pathToTaget.DetailedPath.Last().MotorDirection == AgvDirection.Forward && targetNode.DockDirection == DockingDirection.Forward))
                        {
                            //일치하는 경우 그대로 사용하낟.
                            Retval = pathToTaget;
                        }
                        else
                        {
                            //불일치하는경우라면  Turn 가능노드를 찾아서 이동한 후 턴을 한다.

                            // 3. 목적지별 Turn 및 진입 조건 확인
                            gatewayNode = GetTurnNode(targetNode);


                            // Gateway Node 찾음
                            // 4. Start -> Gateway 경로 계산 (A*)
                            var pathToGateway = this.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
                            if (pathToGateway.Success == false)
                                return AGVPathResult.CreateFailure($"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.Message}");

                            // 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
                            if (pathToGateway.Path.Count > 1)
                            {
                                //다음이동방향이 이전노드와 동일하다면? 되돌아가야한다는것이다
                                 predictNext = pathToGateway.Path[1];
                                if (predictNext.Id == prevNode.Id)
                                {
                                    var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
                                    foreach (var item in pathToGateway.DetailedPath)
                                        item.MotorDirection = reverseDir;
                                }
                            }

                            // 마지막 경로는 게이트웨이이므로 제거 (Gateway 진입 후 처리는 GetPathFromGateway에서 담당)
                            if (pathToGateway.Path.Count > 0 && pathToGateway.Path.Last().Id == gatewayNode.Id)
                            {
                                var idx = pathToGateway.Path.Count - 1;
                                pathToGateway.Path.RemoveAt(idx);
                                pathToGateway.DetailedPath.RemoveAt(idx);
                            }

                            // 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
                            MapNode GateprevNode = pathToGateway.Path.LastOrDefault() ?? prevNode;
                            NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.LastOrDefault();

                            var arrivalOrientation = GatePrevDetail?.MotorDirection ?? prevDir;
                            var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);

                            if (!gatewayPathResult.Success)
                                return AGVPathResult.CreateFailure($"{gatewayPathResult.Message}");

                            Retval = CombinePaths(pathToGateway, gatewayPathResult);
                        }
                    }
                }

                //게이트웨이
                Retval.Gateway = gatewayNode;

                // 경로 오류 검사
                if (Retval == null || Retval.Success == false) return Retval ?? AGVPathResult.CreateFailure("경로 계산 결과 없음");

                if (LimitPath != null)
                {
                    Retval = CombinePaths(LimitPath, Retval);
                }

                // 해당 경로와 대상의 도킹포인트 방향 검사
                if (targetNode.DockDirection != DockingDirection.DontCare)
                {
                    var lastPath = Retval.DetailedPath.LastOrDefault();
                    if (lastPath != null)
                    {
                        if (targetNode.DockDirection == DockingDirection.Forward && lastPath.MotorDirection != AgvDirection.Forward)
                        {
                            return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(FWD) Target:{targetNode.DockDirection}");
                        }
                        if (targetNode.DockDirection == DockingDirection.Backward && lastPath.MotorDirection != AgvDirection.Backward)
                        {
                            return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(BWD) Target:{targetNode.DockDirection}");
                        }
                    }
                }

                // 경로 최적화: A -> B -> A 패턴 제거
                // 6[F][R] → 13[B][L] → 6[F][L] 같은 경우 제거
                while (fixpath == false)
                {
                    var updatecount = 0;
                    for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
                    {
                        var n1 = Retval.DetailedPath[i];
                        var n2 = Retval.DetailedPath[i + 1];
                        var n3 = Retval.DetailedPath[i + 2];

                        if (n1.NodeId == n3.NodeId)
                        {
                            bool isInverse = false;
                            // 1. 모터 방향이 반대인가? (F <-> B)
                            bool isMotorInverse = (n1.MotorDirection != n2.MotorDirection) &&
                                                  (n1.MotorDirection == AgvDirection.Forward || n1.MotorDirection == AgvDirection.Backward) &&
                                                  (n2.MotorDirection == AgvDirection.Forward || n2.MotorDirection == AgvDirection.Backward);

                            if (isMotorInverse)
                            {
                                // 2. 마그넷 방향이 반대인가? (L <-> R, S <-> S)
                                bool isMagnetInverse = false;
                                if (n1.MagnetDirection == MagnetDirection.Straight && n2.MagnetDirection == MagnetDirection.Straight) isMagnetInverse = true;
                                else if (n1.MagnetDirection == MagnetDirection.Left && n2.MagnetDirection == MagnetDirection.Right) isMagnetInverse = true;
                                else if (n1.MagnetDirection == MagnetDirection.Right && n2.MagnetDirection == MagnetDirection.Left) isMagnetInverse = true;

                                if (isMagnetInverse) isInverse = true;
                            }

                            if (isInverse)
                            {
                                // 제자리 회귀 경로 발견 -> 앞의 두 노드(n1, n2)를 제거하여 n3만 남김
                                Retval.DetailedPath.RemoveAt(i);
                                Retval.DetailedPath.RemoveAt(i);

                                if (Retval.Path.Count > i + 1)
                                {
                                    Retval.Path.RemoveAt(i);
                                    Retval.Path.RemoveAt(i);
                                }
                                i--; // 인덱스 재조정
                                updatecount += 1;
                            }
                        }
                    }
                    if (updatecount == 0) break;
                }

                // 불가능한 회전 경로 검사 (사용자 요청 로직 반영)
                for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
                {
                    var n1 = Retval.DetailedPath[i];
                    var n2 = Retval.DetailedPath[i + 1];
                    var n3 = Retval.DetailedPath[i + 2];

                    if (n1.NodeId == n3.NodeId &&
                        n1.MotorDirection == n3.MotorDirection &&
                        n1.MotorDirection == n2.MotorDirection) // Fix: 중간 노드 방향도 같을 때만 에러
                    {
                        return AGVPathResult.CreateFailure($"불가능한 회전 경로가 포함되어있습니다. {n1.RfidId}->{n2.RfidId}->{n3.RfidId}");
                    }
                }

                // 기타 검증 로직 (마지막 노드 도킹, 시작노드 일치 등)
                var lastnode = Retval.Path.Last();
                if (lastnode.StationType != StationType.Normal)
                {
                    var lastnodePath = Retval.DetailedPath.Last();
                    if (lastnode.DockDirection == DockingDirection.Forward && lastnodePath.MotorDirection != AgvDirection.Forward)
                        return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
                    if (lastnode.DockDirection == DockingDirection.Backward && lastnodePath.MotorDirection != AgvDirection.Backward)
                        return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
                }

                // 첫번째 노드 일치 검사 - 필요시 수행 (startNode가 변경될 수 있어서 o_StartNode 등 필요할 수도 있음)
                // 여기서는 생략 혹은 간단히 체크

                // 되돌아가는 길 방향 일치 검사
                if (Retval.DetailedPath.Count > 1)
                {
                    var FirstDetailPath = Retval.DetailedPath[0];
                    var NextDetailPath = Retval.DetailedPath[1];
                    AgvDirection? PredictNextDir = null;

                    if (NextDetailPath.NodeId == prevNode.Id)
                    {
                        if (NextDetailPath.MagnetDirection == MagnetDirection.Straight)
                            PredictNextDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
                    }

                    if (PredictNextDir != null && (FirstDetailPath.MotorDirection != (AgvDirection)PredictNextDir))
                    {
                        // return AGVPathResult.CreateFailure($"되돌아가는 길인데 방향이 일치하지않음");
                        // 경고 수준이나 무시 가능한 경우도 있음
                    }
                }

                // 연결성 검사
                for (int i = 0; i < Retval.DetailedPath.Count - 1; i++)
                {
                    var cnode = Retval.Path[i];
                    var nnode = Retval.Path[i + 1];

                    if (cnode.ConnectedNodes.Contains(nnode.Id) == false && cnode.Id != nnode.Id)
                    {
                        return AGVPathResult.CreateFailure($"[{cnode.RfidId}] 노드에 연결되지 않은 [{nnode.RfidId}]노드가 지정됨");
                    }
                }

                //각 도킹포인트별로 절대 움직이면 안되는 조건확인
                var firstnode = Retval.Path.FirstOrDefault();
                var firstDet = Retval.DetailedPath.First();
                var failmessage = $"[{firstnode.ID2}] 노드의 시작모터 방향({firstDet.MotorDirection})이 올바르지 않습니다";
                if (firstnode.StationType == StationType.Charger && firstDet.MotorDirection != AgvDirection.Forward)
                    return AGVPathResult.CreateFailure(failmessage);
                else if (firstnode.StationType == StationType.Loader && firstDet.MotorDirection != AgvDirection.Backward)
                    return AGVPathResult.CreateFailure(failmessage);
                else if (firstnode.StationType == StationType.Cleaner && firstDet.MotorDirection != AgvDirection.Backward)
                    return AGVPathResult.CreateFailure(failmessage);
                else if (firstnode.StationType == StationType.Plating && firstDet.MotorDirection != AgvDirection.Backward)
                    return AGVPathResult.CreateFailure(failmessage);
                else if (firstnode.StationType == StationType.Buffer)
                {
                    //버퍼는 도킹이되어잇느닞 확인하고. 그때 방향을 체크해야한다.
                }


                return Retval;
            }
            catch (Exception ex)
            {
                return AGVPathResult.CreateFailure($"[계산오류] {ex.Message}");
            }
        }

        private AGVPathResult CalcPathBufferToBuffer(MapNode start, MapNode target, MapNode prev, AgvDirection prevDir)
        {
            // Monitor Side 판단 및 Buffer 간 이동 로직
            int deltaX = 0;
            int deltaY = 0;
            if (prev == null) return AGVPathResult.CreateFailure("이전 노드 정보가 없습니다");
            else
            {
                deltaX = start.Position.X - prev.Position.X;
                deltaY = -(start.Position.Y - prev.Position.Y);
            }

            if (Math.Abs(deltaY) > Math.Abs(deltaX))
                deltaX = deltaY;

            bool isMonitorLeft = false;

            if (deltaX > 0) // 오른쪽(Forward)으로 이동해 옴
                isMonitorLeft = (prevDir == AgvDirection.Backward);
            else if (deltaX < 0) // 왼쪽(Reverse)으로 이동해 옴
                isMonitorLeft = (prevDir == AgvDirection.Forward);
            else
                return AGVPathResult.CreateFailure("이전 노드와의 방향을 알 수 없습니다");

            if (isMonitorLeft)
            {
                // Monitor Left -> Gateway 탈출
                var GateWayNode = _mapNodes.FirstOrDefault(n => n.RfidId == 6);
                var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;

                AGVPathResult escPath = null;
                if (start.Position.X > prev.Position.X)
                    escPath = this.FindBasicPath(start, GateWayNode, prev, prevDir);
                else
                    escPath = this.FindBasicPath(start, GateWayNode, prev, reverseDir);

                if (!escPath.Success) return AGVPathResult.CreateFailure("버퍼 탈출 경로 실패");

                var lastNode = escPath.Path.Last();
                var lastPrev = escPath.Path[escPath.Path.Count - 2];
                var lastDir = escPath.DetailedPath.Last().MotorDirection;

                var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir);

                escPath.Path.RemoveAt(escPath.Path.Count - 1);
                escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1);

                return CombinePaths(escPath, gateToTarget);
            }
            else
            {
                // Monitor Right -> 직접 진입 또는 Overshoot
                bool isTargetLeft = target.Position.X < start.Position.X;

                if (target == start)
                {
                    // 제자리 재정렬 (Same as Simulator logic)
                    var list = new List<MapNode>();
                    var retval = AGVPathResult.CreateSuccess(list, new List<AgvDirection>(), 0, 0);
                    var resversedir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;

                    retval.Path.Add(target);

                    if (deltaX < 0)
                    {
                        var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
                        if (nextNode != null)
                        {
                            retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
                            retval.Path.Add(nextNode);
                            var lastDefailt = retval.DetailedPath.Last();
                            retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Forward)
                            {
                                Speed = SpeedLevel.M,
                            });
                            retval.Path.Add(target);
                            retval.DetailedPath.Add(new NodeMotorInfo((retval.DetailedPath.Max(t => t.seq) + 1), target.Id, target.RfidId, AgvDirection.Forward));
                            retval.Path.Add(target);
                            retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
                        }
                        else
                        {
                            retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, resversedir));
                            retval.Path.Add(prev);
                            retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Last().seq + 1, prev.Id, prev.RfidId, prevDir)
                            {
                                Speed = SpeedLevel.M,
                            });
                            retval.Path.Add(target);
                            retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, prevDir));
                        }
                    }
                    else
                    {
                        retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
                        var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
                        retval.Path.Add(nextNode);
                        var lastDefailt = retval.DetailedPath.Last();
                        retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Backward)
                        {
                            Speed = SpeedLevel.L,
                        });
                        retval.Path.Add(target);
                        retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
                    }
                    return retval;
                }
                else if (isTargetLeft)
                {
                    return this.FindBasicPath(start, target, prev, AgvDirection.Backward);
                }
                else
                {
                    // Overshoot
                    var path1 = this.FindBasicPath(start, target, prev, AgvDirection.Forward);
                    if (path1.Path.Count < 2) return AGVPathResult.CreateFailure("Overshoot 경로 생성 실패");

                    var last = path1.Path.Last();
                    var lastD = path1.DetailedPath.Last();
                    path1.Path.RemoveAt(path1.Path.Count - 1);
                    path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1);

                    path1.Path.Add(last);
                    path1.DetailedPath.Add(new NodeMotorInfo(lastD.seq + 1, lastD.NodeId, lastD.RfidId, AgvDirection.Backward)
                    {
                        Speed = SpeedLevel.L,
                    });

                    return path1;
                }
            }
        }

        private AGVPathResult GetPathFromGateway(MapNode GTNode, MapNode targetNode, MapNode PrevNode, AgvDirection PrevDirection)
        {
            AGVPathResult resultPath = null;
            var deltaX = GTNode.Position.X - PrevNode.Position.X;
            var isMonitorLeft = false;

            if (deltaX > 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
            else isMonitorLeft = PrevDirection == AgvDirection.Forward;

            if (targetNode.StationType == StationType.Loader)
            {
                deltaX = GTNode.Position.Y - PrevNode.Position.Y;
                if (deltaX < 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
                else isMonitorLeft = PrevDirection == AgvDirection.Forward;
            }

            switch (targetNode.StationType)
            {
                case StationType.Loader:
                case StationType.Charger:
                case StationType.Cleaner:
                case StationType.Plating:
                case StationType.Buffer:
                    var rlt1 = new AGVPathResult();
                    rlt1.Success = true;

                    //단순 경로를 찾는다
                    var motdir = targetNode.DockDirection == DockingDirection.Backward ? AgvDirection.Backward : AgvDirection.Forward;
                    var pathtarget = this.FindBasicPath(GTNode, targetNode, PrevNode, motdir);

                    if ((targetNode.DockDirection == DockingDirection.Backward && isMonitorLeft) ||
                        (targetNode.DockDirection == DockingDirection.Forward && !isMonitorLeft))
                    {
                        var turnPatterns = GetTurnaroundPattern(GTNode, targetNode);
                        if (turnPatterns == null || !turnPatterns.Any()) return new AGVPathResult { Success = false, Message = $"회차 패턴 없음: Dir {PrevDirection}" };

                        foreach (var item in turnPatterns)
                        {
                            var rfidvalue = ushort.Parse(item.Substring(0, 4));
                            var node = _mapNodes.FirstOrDefault(t => t.RfidId == rfidvalue);
                            rlt1.Path.Add(node);

                            AgvDirection nodedir = item.Substring(4, 1) == "F" ? AgvDirection.Forward : AgvDirection.Backward;
                            MagnetDirection magnet = MagnetDirection.Straight;
                            var magchar = item.Substring(5, 1);
                            if (magchar == "L") magnet = MagnetDirection.Left;
                            else if (magchar == "R") magnet = MagnetDirection.Right;

                            rlt1.DetailedPath.Add(new NodeMotorInfo(rlt1.DetailedPath.Count, node.Id, node.RfidId, nodedir, null, magnet)
                            {
                                Speed = SpeedLevel.L,
                            });
                        }

                        if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId ||
                            pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection)
                        {
                            // Gateway 턴 마지막 주소 불일치 경고 (로깅 등)
                        }

                        pathtarget.Path.RemoveAt(0);
                        pathtarget.DetailedPath.RemoveAt(0);
                    }
                    return CombinePaths(rlt1, pathtarget);

                default:
                    return AGVPathResult.CreateFailure($"지원되지 않는 StationType: {targetNode.StationType}");
            }
        }

        private MapNode GetTurnNode(MapNode node)
        {
            var rfid = 0;
            if (node.StationType == StationType.Cleaner) rfid = 3;
            else if (node.StationType == StationType.Charger) rfid = 3;
            else if (node.StationType == StationType.Plating) rfid = 3;
            else if (node.StationType == StationType.Loader) rfid = 3;
            else if (node.StationType == StationType.Buffer) rfid = 3;

            if (rfid == 0) return null;
            return _mapNodes.FirstOrDefault(t => t.RfidId == rfid);
        }

        private List<string> GetTurnaroundPattern(MapNode gatewayNode, MapNode targetNode)
        {
            switch (gatewayNode.RfidId)
            {
                case 6:
                    if (targetNode.StationType == StationType.Buffer)
                        return new List<string> { "0006BL", "0007FS", "0013BL", "0006BL" };
                    else
                        return new List<string> { "0006BL", "0007FS", "0013BL", "0006BS" };
                case 9: return new List<string> { "0009FL", "0010BS", "0007FL", "0009FS" };
                case 10: return new List<string> { "0010BR", "0009FR", "0007BS", "0010BS" };
                case 13: return new List<string> { "0013BL", "0006FL", "0007BS", "0013BS" };
                default: return null;
            }
        }

        private AGVPathResult CombinePaths(AGVPathResult p1, AGVPathResult p2)
        {
            var res = new AGVPathResult();
            res.Success = true;

            var p1last = p1.DetailedPath.LastOrDefault();
            var p2fist = p2.DetailedPath.FirstOrDefault();

            if (p1last != null && p2fist != null &&
                (p1last.NodeId == p2fist.NodeId && p1last.MotorDirection == p2fist.MotorDirection && p1last.MagnetDirection == p2fist.MagnetDirection))
            {
                p1.Path.RemoveAt(p1.Path.Count - 1);
                p1.DetailedPath.RemoveAt(p1.DetailedPath.Count - 1);
            }

            foreach (var item in p1.Path) res.Path.Add(item);
            foreach (var item in p2.Path) res.Path.Add(item);

            foreach (var item in p1.DetailedPath)
            {
                var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
                item.seq = maxseq + 1;
                res.DetailedPath.Add(item);
            }
            foreach (var item in p2.DetailedPath)
            {
                var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
                item.seq = maxseq + 1;
                res.DetailedPath.Add(item);
            }

            return res;
        }
    }
}