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>
        /// 길목(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 node05 = _mapNodes.FirstOrDefault(n => n.RfidId == 5);
                var node31 = _mapNodes.FirstOrDefault(n => n.RfidId == 31);

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

                if (node05 != null && node31 != null)
                {
                    // 버퍼 구간 경로 테스트
                    var rlt = this.FindPathAStar(node05, node31);
                    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)
                {
                    // 3. 목적지별 Gateway 및 진입 조건 확인
                    gatewayNode = GetGatewayNode(targetNode);

                    if (gatewayNode == null)
                    {
                        // 게이트웨이가 없는 경우라면(일반 노드 등), Gateway 로직 없이 기본 경로 탐색
                        Retval = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
                    }
                    else
                    {
                        // 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)
                        {
                            var 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.Charger1 && 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.UnLoader && firstDet.MotorDirection != AgvDirection.Backward)
                    return AGVPathResult.CreateFailure(failmessage);
                else if (firstnode.StationType == StationType.Clearner && 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 || targetNode.StationType == StationType.Charger2)
            {
                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.Charger2:
                case StationType.Charger1:
                case StationType.UnLoader:
                case StationType.Clearner:
                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 GetGatewayNode(MapNode node)
        {
            var rfid = 0;
            if (node.StationType == StationType.UnLoader) rfid = 10;
            else if (node.StationType == StationType.Charger1) rfid = 9;
            else if (node.StationType == StationType.Clearner) rfid = 6;
            else if (node.StationType == StationType.Charger2) rfid = 13;
            else if (node.StationType == StationType.Loader) rfid = 13;
            else if (node.StationType == StationType.Buffer) rfid = 6;

            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;
        }
    }
}