# 방향 기반 경로 탐색 (DirectionalPathfinder) 구현 문서

## 📋 개요

**이전 위치 + 현재 위치 + 진행 방향**을 기반으로 **다음 노드 ID**를 반환하는 시스템 구현

### 핵심 요구사항
- ✅ VirtualAGV에 최소 **2개 위치 히스토리** 필요 (prev/current)
- ✅ 방향별 가중치 시스템 (Forward/Backward/Left/Right)
- ✅ Backward 시 좌/우 방향 **반전** 처리
- ✅ NewMap.agvmap 파일 기반 동작

---

## 🏗️ 구현 아키텍처

### 클래스 다이어그램

```
┌─────────────────────────────────────────┐
│   AGVDirectionCalculator                │
│   (메인 인터페이스)                      │
│                                          │
│ GetNextNodeId(                          │
│   prevPos, currentNode, currentPos,     │
│   direction, allNodes                   │
│ )                                        │
└──────────────┬──────────────────────────┘
               │ uses
               ▼
┌─────────────────────────────────────────┐
│   DirectionalPathfinder                 │
│   (핵심 알고리즘)                        │
│                                          │
│ - DirectionWeights 설정                 │
│ - 벡터 기반 방향 계산                    │
│ - 방향별 점수 계산                       │
└─────────────────────────────────────────┘

┌─────────────────────────────────────────┐
│   DirectionalPathfinderTest             │
│   (NewMap.agvmap 기반 테스트)           │
│                                          │
│ - 맵 파일 로드                           │
│ - 테스트 시나리오 실행                   │
│ - 결과 검증                              │
└─────────────────────────────────────────┘
```

---

## 🔧 구현 상세

### 1. DirectionalPathfinder.cs (PathFinding/Planning/)

**목적**: 벡터 기반 방향 계산 엔진

#### 핵심 메서드: `GetNextNodeId()`

```csharp
public string GetNextNodeId(
    Point previousPos,              // 이전 RFID 감지 위치
    MapNode currentNode,            // 현재 RFID 노드
    Point currentPos,               // 현재 위치
    AgvDirection direction,         // 요청된 이동 방향
    List<MapNode> allNodes          // 맵의 모든 노드
)
```

#### 실행 순서

1️⃣ **입력 검증**
```csharp
if (previousPos == Point.Empty || currentPos == Point.Empty)
    return null;  // 2개 위치 히스토리 필수
```

2️⃣ **연결된 노드 필터링**
```csharp
var candidateNodes = allNodes.Where(n =>
    currentNode.ConnectedNodes.Contains(n.NodeId)
).ToList();
```

3️⃣ **이동 벡터 계산**
```csharp
var movementVector = new PointF(
    currentPos.X - previousPos.X,    // Δx
    currentPos.Y - previousPos.Y     // Δy
);
```

4️⃣ **벡터 정규화** (길이 1로 만듦)
```csharp
float length = √(Δx² + Δy²);
normalizedMovement = (Δx/length, Δy/length);
```

5️⃣ **각 후보 노드에 대해 방향 점수 계산**
```
for each candidate in candidateNodes:
    score = CalculateDirectionalScore(
        이동방향,
        현재→다음 벡터,
        요청된 방향
    )
```

6️⃣ **가장 높은 점수 선택**
```csharp
return scoredCandidates
    .OrderByDescending(x => x.score)
    .First()
    .node.NodeId;
```

#### 방향 점수 계산 로직 (CalculateDirectionalScore)

**사용하는 벡터 연산:**
- **내적 (Dot Product)**: 두 벡터의 유사도 (-1 ~ 1)
- **외적 (Cross Product)**: 좌우 판별 (양수 = 좌, 음수 = 우)

```
내적 = v1.x * v2.x + v1.y * v2.y
외적 = v1.x * v2.y - v1.y * v2.x
```

##### 🔄 Forward (전진) 모드

```
직진(dotProduct ≈ 1)     → 점수 100 * 1.0
비슷한 방향(0.5~0.9)    → 점수 80 * 1.0
약간 다른(0~0.5)        → 점수 50 * 1.0
거의 반대(-0.5~0)       → 점수 20 * 2.0 (후진 가중치)
완전 반대(< -0.5)       → 점수 0
```

##### ↩️ Backward (후진) 모드

```
반대 방향(dotProduct < -0.9) → 점수 100 * 2.0
비슷하게 반대(-0.5~-0.9)    → 점수 80 * 2.0
약간 다른(-0~0.5)           → 점수 50 * 2.0
거의 같은(0~0.5)            → 점수 20 * 1.0
완전 같은(> 0.5)            → 점수 0
```

##### ⬅️ Left (좌측) 모드

**Forward 상태 (dotProduct > 0):**
```
좌측(crossProduct > 0.5)     → 점수 100 * 1.5
약간 좌측(0~0.5)            → 점수 70 * 1.5
직진(-0.5~0)                → 점수 50 * 1.0
우측 방향(-0.5~-1)          → 점수 30 * 1.5
```

**Backward 상태 (dotProduct < 0) - 좌우 반전:**
```
좌측(crossProduct < -0.5)    → 점수 100 * 1.5
약간 좌측(-0.5~0)           → 점수 70 * 1.5
역진(0~0.5)                 → 점수 50 * 2.0
우측 방향(> 0.5)            → 점수 30 * 1.5
```

##### ➡️ Right (우측) 모드

**Forward 상태 (dotProduct > 0):**
```
우측(crossProduct < -0.5)    → 점수 100 * 1.5
약간 우측(-0.5~0)           → 점수 70 * 1.5
직진(0~0.5)                 → 점수 50 * 1.0
좌측 방향(> 0.5)            → 점수 30 * 1.5
```

**Backward 상태 (dotProduct < 0) - 좌우 반전:**
```
우측(crossProduct > 0.5)     → 점수 100 * 1.5
약간 우측(0~0.5)            → 점수 70 * 1.5
역진(-0.5~0)                → 점수 50 * 2.0
좌측 방향(< -0.5)           → 점수 30 * 1.5
```

#### 방향 가중치 (DirectionWeights)

```csharp
public class DirectionWeights
{
    public float ForwardWeight { get; set; } = 1.0f;   // 직진
    public float LeftWeight { get; set; } = 1.5f;      // 좌측 (비직진이므로 높음)
    public float RightWeight { get; set; } = 1.5f;     // 우측 (비직진이므로 높음)
    public float BackwardWeight { get; set; } = 2.0f;  // 후진 (거리 페널티)
}
```

---

### 2. AGVDirectionCalculator.cs (Utils/)

**목적**: VirtualAGV 또는 실제 AGV와의 통합 인터페이스

```csharp
public class AGVDirectionCalculator
{
    public string GetNextNodeId(
        Point previousRfidPos,    // 이전 RFID 위치
        MapNode currentNode,      // 현재 노드
        Point currentRfidPos,     // 현재 RFID 위치
        AgvDirection direction,   // 이동 방향
        List<MapNode> allNodes    // 모든 노드
    )
    {
        return _pathfinder.GetNextNodeId(
            previousRfidPos,
            currentNode,
            currentRfidPos,
            direction,
            allNodes
        );
    }

    // 추가: 선택된 방향 역추적
    public AgvDirection AnalyzeSelectedDirection(
        Point previousPos,
        Point currentPos,
        MapNode selectedNextNode,
        List<MapNode> connectedNodes
    )
    {
        // 벡터 비교로 실제 선택된 방향 분석
    }
}
```

---

### 3. DirectionalPathfinderTest.cs (Utils/)

**목적**: NewMap.agvmap 파일 기반 테스트

#### 기능

```csharp
public class DirectionalPathfinderTest
{
    // 맵 파일 로드
    public bool LoadMapFile(string filePath)

    // 테스트 실행
    public void TestDirectionalMovement(
        string previousRfidId,
        string currentRfidId,
        AgvDirection direction
    )

    // 정보 출력
    public void PrintAllNodes()
    public void PrintNodeInfo(string rfidId)
}
```

---

## 📊 테스트 시나리오

### 테스트 케이스 1: 직선 경로 전진
```
001 → 002 → Forward
예상: 003
이유: 직진 이동 (직진 가중치 1.0)
```

### 테스트 케이스 2: 역진
```
002 → 001 → Backward
예상: 이전 노드 또는 null (001이 002의 유일한 연결)
이유: 역진 방향 (후진 가중치 2.0)
```

### 테스트 케이스 3: 좌회전
```
002 → 003 → Forward
예상: 004
이유: 직진 계속
```

### 테스트 케이스 4: 분기점에서 우회전
```
003 → 004 → Right
예상: 030 (또는 N022)
이유: 우측 방향 가중치 1.5
```

### 테스트 케이스 5: Backward 시 좌우 반전
```
004 → 003 → Backward → Left
예상: 002
이유: Backward 상태에서 Left = 반전되어 원래는 우측 방향
      (Backward 기준 좌측 = Forward 기준 우측)
```

---

## 🔍 벡터 연산 예시

### 예시 1: 001 → 002 → Forward

```
이동 벡터: (206-65, 244-229) = (141, 15)
정규화: (141/142, 15/142) ≈ (0.993, 0.106)

003 위치: (278, 278)
002→003 벡터: (278-206, 278-244) = (72, 34)
정규화: (72/80, 34/80) ≈ (0.9, 0.425)

내적 = 0.993*0.9 + 0.106*0.425 ≈ 0.939 (매우 유사)
→ Forward 점수: 100 * 1.0 = 100.0 ✓ 최고 점수
→ 결과: 003 반환
```

### 예시 2: 003 → 004 → Right

```
이동 벡터: (380-278, 340-278) = (102, 62)
정규화: (102/119, 62/119) ≈ (0.857, 0.521)

N022 위치: (?, ?)
N031 위치: (?, ?)
(실제 맵 데이터에 따라 계산)

Right 선택 → crossProduct 음수 선호
→ N031 선택 가능성 높음
```

---

## 📁 파일 구조

```
AGVNavigationCore/
├── PathFinding/
│   └── Planning/
│       └── DirectionalPathfinder.cs          ← 핵심 알고리즘
├── Utils/
│   ├── AGVDirectionCalculator.cs             ← 통합 인터페이스
│   ├── DirectionalPathfinderTest.cs          ← 테스트 클래스
│   └── TestRunner.cs                         ← 실행 프로그램
└── AGVNavigationCore.csproj                  ← 프로젝트 파일 (수정됨)
```

---

## 🚀 사용 방법

### 기본 사용

```csharp
// 1. 계산기 생성
var calculator = new AGVDirectionCalculator();

// 2. 맵 노드 로드
List<MapNode> allNodes = LoadMapFromFile("NewMap.agvmap");

// 3. 다음 노드 계산
string nextNodeId = calculator.GetNextNodeId(
    previousRfidPos: new Point(65, 229),      // 001 위치
    currentNode: node002,                      // 현재 노드
    currentRfidPos: new Point(206, 244),      // 002 위치
    direction: AgvDirection.Forward,           // 전진
    allNodes: allNodes
);

Console.WriteLine($"다음 노드: {nextNodeId}"); // 003
```

### VirtualAGV 통합

```csharp
public class VirtualAGV
{
    private AGVDirectionCalculator _directionCalc;

    public void OnPositionChanged()
    {
        // SetPosition() 호출 후
        string nextNodeId = _directionCalc.GetNextNodeId(
            _targetPosition,       // 이전 위치
            _currentNode,         // 현재 노드
            _currentPosition,     // 현재 위치
            _currentDirection,    // 현재 방향
            _allNodes
        );
    }
}
```

### 테스트 실행

```csharp
var tester = new DirectionalPathfinderTest();
tester.LoadMapFile(@"C:\Data\...\NewMap.agvmap");
tester.TestDirectionalMovement("001", "002", AgvDirection.Forward);
```

---

## ✅ 검증 체크리스트

- [x] 2개 위치 히스토리 검증 로직
- [x] Forward/Backward/Left/Right 방향 처리
- [x] Backward 시 좌우 반전 구현
- [x] 방향별 가중치 시스템
- [x] 벡터 기반 방향 계산
- [x] NewMap.agvmap 파일 로드 지원
- [x] 테스트 프레임워크

---

## 🔗 관련 클래스

| 클래스 | 파일 | 용도 |
|--------|------|------|
| DirectionalPathfinder | PathFinding/Planning/ | 핵심 알고리즘 |
| AGVDirectionCalculator | Utils/ | 통합 인터페이스 |
| DirectionalPathfinderTest | Utils/ | 테스트 |
| TestRunner | Utils/ | 실행 프로그램 |
| MapNode | Models/ | 노드 데이터 |
| AgvDirection | Models/Enums.cs | 방향 열거형 |

---

## 📝 주의사항

⚠️ **2개 위치 히스토리 필수**
- previousPos가 Point.Empty이면 null 반환
- VirtualAGV.SetPosition() 호출 시 이전 위치를 _targetPosition에 저장

⚠️ **벡터 정규화**
- 매우 작은 이동(< 0.001)은 거리 0으로 처리

⚠️ **방향 가중치**
- 기본값: Forward=1.0, Left/Right=1.5, Backward=2.0
- 프로젝트별로 조정 가능

⚠️ **점수 시스템**
- 100점 = 완벽한 방향
- 0점 = 방향 불가
- 낮은 점수도 반환됨 (대안 경로)

---

## 🎯 향후 개선사항

1. **A* 알고리즘 통합**
   - 현재는 직접 연결된 노드만 고려
   - A* 알고리즘으로 확장 가능

2. **경로 캐싱**
   - 자주 이동하는 경로 캐시
   - 성능 향상

3. **동적 가중치 조정**
   - AGV 상태(배터리, 속도)에 따라 가중치 변경

4. **3D 좌표 지원**
   - 현재 2D Point만 지원
   - 3D 좌표 추가 가능

---

**작성일**: 2025-10-23
**상태**: 구현 완료, 테스트 대기