From 177309c5c91f7661ff62a0f865940088f82850c5 Mon Sep 17 00:00:00 2001
From: backuppc <backuppc@mail.com>
Date: Mon, 27 Oct 2025 17:13:04 +0900
Subject: [PATCH] feat: Add comprehensive debug logging to path validation
 functions
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Added detailed debug output to GetNextNodeByDirection() with:
  - Initial node information (positions, movement vectors)
  - All candidate nodes evaluation with step-by-step scoring
  - Score calculations: base score → motor direction → magnet direction
  - Final selection with best score

- Added detailed debug output to ValidateDockingDirection() with:
  - Path validation stage information
  - Expected vs actual next node comparison
  - Movement vector analysis for mismatch debugging
  - Success/failure status for each path segment

Debug output includes:
- Node IDs, RFIDs, positions, and vectors
- Normalized vectors and dot products
- Score progression through each bonus/penalty application
- Direction consistency analysis
- Final scoring results with selection indicators

This enables detailed tracing of path prediction and validation issues.

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude <noreply@anthropic.com>
---
 .../Utils/DirectionalHelper.cs                | 39 ++++++++++++++++++-
 .../Utils/DockingValidator.cs                 | 33 +++++++++++++++-
 2 files changed, 70 insertions(+), 2 deletions(-)

diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
index c88967b..d8cf020 100644
--- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
+++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
@@ -88,6 +88,19 @@ namespace AGVNavigationCore.Utils
             MapNode bestNode = null;
             float bestScore = float.MinValue;
 
+            System.Diagnostics.Debug.WriteLine(
+                $"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
+            System.Diagnostics.Debug.WriteLine(
+                $"  현재노드: {currentNode.NodeId}({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
+            System.Diagnostics.Debug.WriteLine(
+                $"  이전노드: {prevNode.NodeId}({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
+            System.Diagnostics.Debug.WriteLine(
+                $"  이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
+            System.Diagnostics.Debug.WriteLine(
+                $"  현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}");
+            System.Diagnostics.Debug.WriteLine(
+                $"  후보노드 개수: {candidateNodes.Count}");
+
             foreach (var candidate in candidateNodes)
             {
                 var toNextVector = new PointF(
@@ -113,7 +126,7 @@ namespace AGVNavigationCore.Utils
                                   (normalizedMovement.Y * normalizedToNext.Y);
 
                 float score;
-                if (direction == AgvDirection.Forward)
+                if (direction == prevDirection)
                 {
                     // Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
                     score = dotProduct;
@@ -124,15 +137,30 @@ namespace AGVNavigationCore.Utils
                     score = -dotProduct;
                 }
 
+                System.Diagnostics.Debug.WriteLine(
+                    $"\n  [후보] {candidate.NodeId}({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
+                System.Diagnostics.Debug.WriteLine(
+                    $"    벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
+                System.Diagnostics.Debug.WriteLine(
+                    $"    정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})");
+                System.Diagnostics.Debug.WriteLine(
+                    $"    내적(dotProduct): {dotProduct:F4}");
+                System.Diagnostics.Debug.WriteLine(
+                    $"    기본점수 ({(direction == prevDirection ? "방향유지" : "방향변경")}): {score:F4}");
+
                 // 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가
+                var scoreBeforeMotor = score;
                 score = ApplyMotorDirectionConsistencyBonus(
                     score,
                     direction,
                     prevDirection,
                     dotProduct
                 );
+                System.Diagnostics.Debug.WriteLine(
+                    $"    모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}");
 
                 // 마그넷 방향을 고려한 점수 조정
+                var scoreBeforeMagnet = score;
                 score = ApplyMagnetDirectionBonus(
                     score,
                     magnetDirection,
@@ -141,14 +169,23 @@ namespace AGVNavigationCore.Utils
                     currentNode,
                     candidate
                 );
+                System.Diagnostics.Debug.WriteLine(
+                    $"    마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");
 
                 if (score > bestScore)
                 {
                     bestScore = score;
                     bestNode = candidate;
+                    System.Diagnostics.Debug.WriteLine(
+                        $"    ⭐ 현재 최고점수 선택됨!");
                 }
             }
 
+            System.Diagnostics.Debug.WriteLine(
+                $"\n  최종선택: {bestNode?.NodeId ?? "null"} (점수: {bestScore:F4})");
+            System.Diagnostics.Debug.WriteLine(
+                $"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");
+
             return bestNode;
         }
 
diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
index cf954c0..e44e533 100644
--- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
+++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
@@ -69,6 +69,17 @@ namespace AGVNavigationCore.Utils
                     if (prevNode != null)
                     {
                         // DirectionalHelper를 사용하여 예상되는 다음 노드 확인
+                        System.Diagnostics.Debug.WriteLine(
+                            $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  이전→현재→다음: {prevNode.NodeId}({prevNode.RfidId}) → {curNode.NodeId}({curNode.RfidId}) → {nextNode.NodeId}({nextNode.RfidId})");
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");
+
                         var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
                             curNode,
                             prevNode,
@@ -78,13 +89,28 @@ namespace AGVNavigationCore.Utils
                             mapNodes
                         );
 
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  [예상] GetNextNodeByDirection 결과: {expectedNextNode?.NodeId ?? "null"}");
+                        System.Diagnostics.Debug.WriteLine(
+                            $"  [실제] DetailedPath 다음 노드: {nextNode.NodeId}");
+
                         if (expectedNextNode != null && !expectedNextNode.NodeId.Equals(nextNode.NodeId))
                         {
                             string error =
                                 $"[DockingValidator] ⚠️ 경로 방향 불일치: " +
                                 $"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
                                 $"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
-                            System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
+                            System.Diagnostics.Debug.WriteLine(
+                                $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
+                            System.Diagnostics.Debug.WriteLine(
+                                $"  이동 벡터:");
+                            System.Diagnostics.Debug.WriteLine(
+                                $"    이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
+                            System.Diagnostics.Debug.WriteLine(
+                                $"    현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
+                            System.Diagnostics.Debug.WriteLine(
+                                $"    현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
+                            System.Diagnostics.Debug.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
                             return DockingValidationResult.CreateInvalid(
                                 LastNode.NodeId,
                                 LastNode.Type,
@@ -94,6 +120,11 @@ namespace AGVNavigationCore.Utils
 
 
                         }
+                        else
+                        {
+                            System.Diagnostics.Debug.WriteLine(
+                                $"  ✅ 경로 방향 일치!");
+                        }
                     }
                 }
             }