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

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

def test_015_only():
    """015 케이스만 테스트"""
    print("=== 015 Case Debug ===")

    # 맵 로드
    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)

    # 015 케이스
    print("\n--- Testing: 033->032(F) to 015 ---")
    result = pathfinder.find_path(start_rfid="032", target_rfid="015", current_direction=AgvDirection.FORWARD, came_from_rfid="033")

    if result.success:
        print(f"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})")

        actual_path = UniversalPathFormatter.format_path(result, agv_map)
        print(f"\n실제: {actual_path}")

        expected = "032 ->(F) 031 ->(R) -> 032 -> 033 -> 034 -> 035 -> 036 -> 037 -> 005 -> 004 -> 012 -> 013 ->(R) -> 012 -> 030 -> 029 -> 028 -> 027 -> 026 -> 025 -> 024 -> 023 -> 022 -> 021 -> 020 -> 014 -> 015"
        print(f"정답: {expected}")

        print(f"\n매치: {actual_path == expected}")
    else:
        print(f"FAILED: {result.error_message}")

if __name__ == "__main__":
    test_015_only()