#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from universal_pathfinder import UniversalAGVPathfinder, UniversalPathFormatter
from agv_pathfinder import AGVMap, AgvDirection

def test_q1_1_all():
    """Q1-1 전체 케이스 테스트"""
    print("=== Q1-1 All Cases Test ===")

    # 맵 로드
    agv_map = AGVMap()
    agv_map.load_from_file(r"C:\Data\Source\(5613#) ENIG AGV\Source\Cs_HMI\Data\NewMap.agvmap")

    pathfinder = UniversalAGVPathfinder(agv_map)

    # Q1-1 정답들 (실제 사용자가 제공한 정답)
    expected_answers = {
        "040": "032 ->(F) 031 ->(R) 032 -> 040",
        "041": "032 ->(F) 040 ->(R) 032 -> 031 -> 041",
        "008": "032 ->(B) 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 006 -> 007 -> 008",
        "001": "032 ->(B) 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 004 -> 003 -> 002 -> 001",
        "011": "032 ->(B) 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 004 -> 030 -> 009 -> 010 -> 011",
        "019": "032 ->(B) 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 004 -> 012 -> 013 ->(F) -> 012 -> 016 -> 017 -> 018 -> 019",
        "015": "032 ->(B) 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 004 -> 012 -> 016 ->(F) -> 012 -> 013 -> 014 -> 015"
    }

    print(f"\nQ1-1 시나리오: 033->032(F) 상황에서의 목적지별 경로")
    success_count = 0

    for target, expected in expected_answers.items():
        print(f"\n--- Target: {target} ---")
        result = pathfinder.find_path(start_rfid="032", target_rfid=target, current_direction=AgvDirection.FORWARD, came_from_rfid="033")

        if target == "015":  # 015 케이스 디버깅
            print(f"DEBUG - Steps count: {len(result.path_steps)}")
            for i, step in enumerate(result.path_steps):
                from_rfid = agv_map.get_node(step.from_node).rfid_id
                to_rfid = agv_map.get_node(step.to_node).rfid_id
                print(f"  Step {i}: {from_rfid} -> {to_rfid} ({step.motor_direction.value})")
            print()

        if result.success:
            actual_path = UniversalPathFormatter.format_path(result, agv_map)
            print(f"실제: {actual_path}")
            print(f"정답: {expected}")

            if actual_path == expected:
                print("[SUCCESS]")
                success_count += 1
            else:
                print("[FAILED] - Path mismatch")
        else:
            print(f"[FAILED]: {result.error_message}")

    print(f"\n=== Q1-1 결과: {success_count}/7 성공 ===")
    return success_count == 7

if __name__ == "__main__":
    test_q1_1_all()