경로 계산부분 일단 백업(오류있음)

AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs

@@ -473,6 +473,7 @@ namespace AGVNavigationCore.PathFinding.Planning
                 Path = new List<string>();
                 Path.AddRange(_path);
             }
+            
         }
 
         public List<PathData> GetMapZonePathData()
@@ -795,11 +796,22 @@ namespace AGVNavigationCore.PathFinding.Planning
 
             // 시작 태그 검색용 (예: "91F")
             string startTag = $"{startNode.RfidId}{motDir}";
+            string targetTag = $"{targetNode.RfidId}";
+            string targetTag1;
+            if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt)
+            {
+                targetTag += "B"; //모든 스테이션은 후진으로 도킹을 해야한다
+                targetTag1 = "";
+            }
+            else
+            {
+                targetTag1 = targetTag + (motDir == 'F' ? "B" : "F");
+                targetTag += motDir;
+            }
 
             // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다
             var zonepath = GetMapZonePathData();
 
-
             //목표가 일반노드라면 단순 경로 생성해서 반환한다.
             if (targetNode.StationType == Station.Normal)
             {
@@ -807,9 +819,122 @@ namespace AGVNavigationCore.PathFinding.Planning
                 return simplepath;
             }
 
-
             IEnumerable<PathData> candidates;
 
+            //시작과 목표가 포함된 경로르 모두 찾아서 이용하도록 한다(zonepath 내에서 모두 검색한다)
+            //모니터의 방향이 동일하고 20F -> 70B (목표노드의 방향에 따라서 목적지으 ㅣFB는 결정한다 일반노드라면 방향상관없이 검색한다 - 기본은 시작이 방향과 동일하게 한다)
+            //경로의 시작이 경로의 끝보다 index가 먼저 나와야한다.
+            //현재 진행방향의 경로와, 반대방향의 경로를 2개 추출해서.. 이전에 지나온 경로를 체크한다.
+            string SearchTagS, SearchTagE, SearchTagE1;
+
+            for (int i = 0; i < 2; i++)
+            {
+                //진입순서에 따라서 검색하는 대상을 바꿔준다(진행방향 반대방향)
+                if (i == 0)
+                {
+                    SearchTagS = $"{startNode.RfidId}{motDir}";
+                    SearchTagE = $"{targetNode.RfidId}";
+                    if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt)
+                    {
+                        SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다
+                        SearchTagE1 = "";
+                    }
+                    else
+                    {
+                        SearchTagE += motDir;
+                        SearchTagE1 = targetTag + (motDir == 'F' ? "B" : "F");
+                    }
+                }
+                else
+                {
+                    SearchTagS = $"{startNode.RfidId}{(motDir == 'F' ? 'B' : 'F')}";
+                    SearchTagE = $"{targetNode.RfidId}";
+                    if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt)
+                    {
+                        SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다
+                        SearchTagE1 = "";
+                    }
+                    else
+                    {
+                        SearchTagE += motDir;
+                        SearchTagE1 = targetTag + (motDir == 'F' ? "B" : "F"); // 오히려 반대로 처리해준다.
+                    }
+                }
+
+
+
+                candidates = zonepath.Where(d =>
+                    d.Monitor == monitorMode &&
+                    d.Path.Contains(SearchTagS) &&
+                    d.Path.Contains(SearchTagE)
+                ).Where(d =>
+                {
+                    int startIndex = d.Path.FindIndex(p => p.Equals(SearchTagS));
+                    int endIndex = d.Path.FindLastIndex(p => p.Equals(SearchTagE));
+                    if (endIndex == -1 && SearchTagE1 != "")
+                        endIndex = d.Path.FindLastIndex(p => p.Equals(SearchTagE1));
+
+                    return startIndex != -1 && endIndex != -1 && startIndex < endIndex;
+                }).ToList();
+
+                //찾아진 값이 있다면 slice 해서 그 경로를반환한다.
+                if (candidates.Any())
+                {
+                    PathData bestPath = null;
+                    int bestStartIndex = -1;
+                    int bestEndIndex = -1;
+                    int minPathLength = int.MaxValue;
+
+                    foreach (var candidate in candidates)
+                    {
+                        int startIndex = candidate.Path.FindIndex(p => p.Equals(SearchTagS));
+                        int endIndex = candidate.Path.FindLastIndex(p => p.Equals(SearchTagE));
+                        if (endIndex == -1 && SearchTagE1 != "")
+                            endIndex = candidate.Path.FindLastIndex(p => p.Equals(SearchTagE1));
+
+                        int length = endIndex - startIndex;
+                        if (length < minPathLength)
+                        {
+                            minPathLength = length;
+                            bestPath = candidate;
+                            bestStartIndex = startIndex;
+                            bestEndIndex = endIndex;
+                        }
+                    }
+
+                    if (bestPath != null)
+                    {
+                        var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList();
+
+                        // 검증: 첫 번째 이동이 이전 노드(prevNode) 방향으로 가는 것이라면, 모터 방향을 반전시켜야 할 수 있음.
+                        if (slicedPath.Count > 1 && prevNode != null)
+                        {
+                            var nextNodeInPath = _mapNodes.FirstOrDefault(n => n.RfidId.ToString() == new string(slicedPath[1].Where(char.IsDigit).ToArray()));
+                            if (nextNodeInPath != null && nextNodeInPath.Id == prevNode.Id)
+                            {
+                                // 되돌아가는 상황: 첫 노드의 방향 플래그를 반전시킨다.
+                                string firstTag = slicedPath[0];
+                                char currentFlag = firstTag.Last();
+                                if (currentFlag == 'F' || currentFlag == 'B')
+                                {
+                                    char reversedFlag = currentFlag == 'F' ? 'B' : 'F';
+                                    slicedPath[0] = firstTag.Substring(0, firstTag.Length - 1) + reversedFlag;
+                                }
+                            }
+                        }
+
+                        return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
+                    }
+                }
+
+
+            }
+
+
+
+
+
+
             //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다
             //그런경우는 복잡하게 추가 계산할 필요가 없으니까
             var exactMatchList = zonepath.Where(d =>
@@ -1134,7 +1259,7 @@ namespace AGVNavigationCore.PathFinding.Planning
 
             if (startNode.RfidId == 8 || startNode.RfidId == 9 || startNode.RfidId == 20)
             {
-                if(prevNode.Position.X > startNode.Position.X)  //오른쪽에서 왔다. 
+                if (prevNode.Position.X > startNode.Position.X)  //오른쪽에서 왔다. 
                 {
                     if (prevDir == AgvDirection.Forward) return MonDir.LeftTop; //오른쪽에서 전진으로 왔다면 모니터는 좌측에있다.
                     else return MonDir.RightBtm;    //오른쪽에서 후진으로 왔다면 모니터는 우측에 있다.
@@ -1142,7 +1267,7 @@ namespace AGVNavigationCore.PathFinding.Planning
                 else //왼쪽에서 왔다
                 {
                     if (prevDir == AgvDirection.Forward) return MonDir.RightBtm;
-                    else return MonDir.LeftTop;  
+                    else return MonDir.LeftTop;
                 }
             }