# AI Teammate Device Bridge v2 — Configuration & shared state
import logging
import os
import threading
from pathlib import Path

logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s")
log = logging.getLogger("bridge")

# Load .env — search CWD first, then package parent (editable install)
_env_candidates = [
    Path.cwd() / ".env",
    Path(__file__).resolve().parent.parent / ".env",
]
for env_file in _env_candidates:
    if env_file.exists():
        for line in env_file.read_text().splitlines():
            if "=" in line and not line.startswith("#"):
                k, v = line.split("=", 1)
                os.environ.setdefault(k.strip(), v.strip())
        break

DEVICE_ID = os.environ.get("DEVICE_ID", "")
GATEWAY_URL = os.environ.get("GATEWAY_URL", "")
API_KEY = os.environ.get("API_KEY", "")
MODE = os.environ.get("CONNECTION_MODE", "rclpy")
INTERVAL = float(os.environ.get("STATUS_REPORT_INTERVAL", "1.0"))
CMD_VEL_TOPIC = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel")
IMU_TOPIC = os.environ.get("IMU_TOPIC", "/imu/data")
ODOM_TOPIC = os.environ.get("ODOM_TOPIC", "/odom")
SCAN_TOPIC = os.environ.get("SCAN_TOPIC", "/scan")

# Obstacle
OBSTACLE_DISTANCE = float(os.environ.get("OBSTACLE_DISTANCE", "0.4"))
OBSTACLE_WAIT_TIMEOUT = float(os.environ.get("OBSTACLE_WAIT_TIMEOUT", "30"))

# Detection cooldown
DETECT_COOLDOWN = float(os.environ.get("DETECT_COOLDOWN_SEC", "2.0"))

# Keyframe capture
KF_MAX_BUFFER = int(os.environ.get("KF_MAX_BUFFER", "20"))
KF_BATCH_SIZE = int(os.environ.get("KF_BATCH_SIZE", "10"))
KF_MIN_DISPLACEMENT = float(os.environ.get("KF_MIN_DISPLACEMENT", "0.3"))
KF_MIN_ROTATION = float(os.environ.get("KF_MIN_ROTATION", "0.26"))
KF_MIN_INTERVAL = float(os.environ.get("KF_MIN_INTERVAL", "2.0"))
KF_WIFI_SCAN_INTERVAL = float(os.environ.get("KF_WIFI_SCAN_INTERVAL", "10.0"))
KF_THUMB_WIDTH = 160
KF_THUMB_HEIGHT = 120
KF_THUMB_QUALITY = 40

# Locations / fingerprints file paths
LOCATIONS_FILE = os.path.join(os.path.dirname(os.path.dirname(__file__)) or ".", "locations.json")
FINGERPRINTS_FILE = os.path.join(os.path.dirname(os.path.dirname(__file__)) or ".", "fingerprints.json")

# ── Shared mutable state ──

_ros2_cache: dict = {}
_ros2_lock = threading.Lock()
_ros2_node = None
_cmd_vel_pub = None
_estop_event = threading.Event()

# Gateway context (populated from welcome message)
_gw_context: dict = {}

# WS reference for service requests (set in ws_client.run())
_ws_ref = None
_service_futures: dict = {}

# Subprocess management
_subprocesses: dict[str, object] = {"slam": None, "nav2": None}

# Keyframe state
_keyframe_buffer: list = []
_last_kf_pose: dict | None = None
_last_kf_ts: float = 0.0
_kf_capture_active: bool = False
_kf_total_captured: int = 0
_kf_last_upload_ts: float = 0.0
_kf_wifi_cache: dict | None = None
_kf_wifi_cache_ts: float = 0.0

# Detection rate limit
_detect_last_ts: float = 0.0
