#!/usr/bin/env python3
"""sanad_api_g1 — G1 fleet MAP uploader.

Scope (this build): upload the G1's navigation MAP to the YS Lootah fleet
server. Nothing else (no telemetry / commands / logs — those are separate
agents). It is the "Maps sync" row of the fleet spec:

    POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map      (Bearer device token)

WHAT IT UPLOADS
---------------
The G1's map is produced by the web_nav3 (Nav2 + RTAB-Map) stack and stored on
disk as a RTAB-Map SQLite ``.db`` file (per web_nav3/web/backend.py:
``MAPS_ROOT/<robot>/<name>.db`` + a ``maps_meta.json`` sidecar), with each map's
named places kept in ``web/data/<robot>/places/<name>.json``.

There is NO rendered PNG on disk — the dashboard draws the occupancy grid live
over rosbridge. By design choice this agent uploads the RAW ``.db`` (the actual
map artifact) plus its places, rather than rendering an image. That means the
FLEET SERVER must accept a ``rtabmap_db`` artifact on the map endpoint (see
MAP_UPLOAD_MODE for the two wire formats). The documented spec body
(image_base64 / resolution / origin) is for a rendered map; switch to the
telemetry-agent's live-rosbridge renderer if you need that instead.

NO ROS, NO DDS. Pure files + HTTPS, so it drops into any robot as its own
container. It reads the maps/places straight from mounted volumes; the web_nav3
HTTP API is only used (optionally) to learn which map is "active".

CHANGE DETECTION
----------------
Each map's ``.db`` is fingerprinted (size + mtime fast-path, then sha256). A map
is (re)uploaded only when its fingerprint changes — matching the spec's "send
the nav map on change". State persists in ``STATE_DIR/uploaded.json`` so a
restart doesn't re-push unchanged maps.

CONFIG — all via environment (see .env.example)
-----------------------------------------------
  SERVER_URL        base URL of the fleet server (required)   e.g. https://fleet.example.com
  DEVICE_TOKEN      per-robot bearer token (required)
  SN                this robot's fleet id (path key)           default g1_7892
  ROBOT             web_nav3 robot name (maps subdir + header) default sanad
  MAPS_DIR          mounted web_nav3 maps/ dir (contains <robot>/*.db and/or *.db)
  DATA_DIR          mounted web_nav3 web/data dir (per-map places live under <robot>/places/)
  LEGACY_PLACES     optional path to a legacy places.json (per-robot, no map scoping)
  WEB_NAV3_URL      optional http://127.0.0.1:8765 — used only to read the ACTIVE map
  MAP_SELECT        all | active | newest                      default all
  MAP_UPLOAD_MODE   multipart | base64json                     default multipart
  MAP_ENDPOINT      path template, {sn} substituted            default /api/v1/fleet/ingest/{sn}/map
  POLL_INTERVAL     seconds between scans (loop mode)           default 30
  STATE_DIR         writable dir for upload state               default /data/state
  VERIFY_TLS        1|0 verify server TLS                       default 1
  HTTP_TIMEOUT      per-request timeout seconds                 default 30

CLI
---
  python sanad_api_g1.py            # run the loop (default)
  python sanad_api_g1.py --once     # one scan+upload pass, then exit
  python sanad_api_g1.py --list     # list discovered maps (no upload)
  python sanad_api_g1.py --dry-run  # build payloads + report, never POST
  python sanad_api_g1.py --force    # ignore state; upload even if unchanged
"""
from __future__ import annotations

import argparse
import base64
import hashlib
import json
import logging
import math
import os
import sys
import time
from dataclasses import dataclass, field
from pathlib import Path
from typing import Any, Dict, List, Optional

import requests

log = logging.getLogger("sanad_api_g1")


# --------------------------------------------------------------------------- #
# tiny .env loader (so it also runs bare, outside docker) — no dependency
# --------------------------------------------------------------------------- #
def _load_dotenv(path: str = ".env") -> None:
    p = Path(path)
    if not p.exists():
        return
    for line in p.read_text().splitlines():
        line = line.strip()
        if not line or line.startswith("#") or "=" not in line:
            continue
        k, _, v = line.partition("=")
        k, v = k.strip(), v.strip().strip('"').strip("'")
        os.environ.setdefault(k, v)


def _env(name: str, default: str = "") -> str:
    return os.environ.get(name, default).strip()


def _env_bool(name: str, default: bool) -> bool:
    v = _env(name, "1" if default else "0").lower()
    return v in ("1", "true", "yes", "on")


# --------------------------------------------------------------------------- #
# config
# --------------------------------------------------------------------------- #
@dataclass
class Config:
    server_url: str
    device_token: str
    sn: str
    robot: str
    maps_dir: Path
    data_dir: Optional[Path]
    legacy_places: Optional[Path]
    web_nav3_url: str
    map_select: str
    upload_mode: str
    endpoint_tmpl: str
    poll_interval: float
    state_dir: Path
    verify_tls: bool
    http_timeout: float

    @classmethod
    def from_env(cls) -> "Config":
        server = _env("SERVER_URL").rstrip("/")
        token = _env("DEVICE_TOKEN")
        missing = [n for n, v in (("SERVER_URL", server), ("DEVICE_TOKEN", token)) if not v]
        if missing:
            raise SystemExit(f"[config] missing required env: {', '.join(missing)}")
        data_dir = _env("DATA_DIR")
        legacy = _env("LEGACY_PLACES")
        return cls(
            server_url=server,
            device_token=token,
            sn=_env("SN", "g1_7892"),
            robot=_env("ROBOT", "sanad"),
            maps_dir=Path(_env("MAPS_DIR", "/data/maps")),
            data_dir=Path(data_dir) if data_dir else None,
            legacy_places=Path(legacy) if legacy else None,
            web_nav3_url=_env("WEB_NAV3_URL", "").rstrip("/"),
            map_select=_env("MAP_SELECT", "all").lower(),
            upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(),
            endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"),
            poll_interval=float(_env("POLL_INTERVAL", "30")),
            state_dir=Path(_env("STATE_DIR", "/data/state")),
            verify_tls=_env_bool("VERIFY_TLS", True),
            http_timeout=float(_env("HTTP_TIMEOUT", "30")),
        )

    def map_url(self) -> str:
        return self.server_url + self.endpoint_tmpl.format(sn=self.sn)

    def auth_headers(self) -> Dict[str, str]:
        return {"Authorization": f"Bearer {self.device_token}"}


# --------------------------------------------------------------------------- #
# map artifact
# --------------------------------------------------------------------------- #
@dataclass
class MapArtifact:
    path: Path            # absolute path to the .db
    name: str             # file name, e.g. floor-1.db
    stem: str             # name without .db, e.g. floor-1
    size: int
    mtime: int
    description: str = ""
    sha256: str = ""
    points: List[Dict[str, Any]] = field(default_factory=list)

    def fingerprint(self) -> str:
        # cheap identity for the change check before we hash the whole file
        return f"{self.size}:{self.mtime}"


def _sha256(path: Path) -> str:
    h = hashlib.sha256()
    with path.open("rb") as f:
        for chunk in iter(lambda: f.read(1024 * 1024), b""):
            h.update(chunk)
    return h.hexdigest()


# --------------------------------------------------------------------------- #
# discovery — read maps + places straight from the mounted web_nav3 files
# --------------------------------------------------------------------------- #
def _map_key(stem: str) -> str:
    """Mirror backend._map_key: safe filename stem for the per-map places file."""
    stem = Path(stem).name
    if stem.endswith(".db"):
        stem = stem[:-3]
    return "".join(c for c in stem if c.isalnum() or c in "_-.")


def _read_json(path: Path, default: Any) -> Any:
    try:
        return json.loads(path.read_text() or "")
    except Exception:
        return default


def _yaw_from_pose(pose: Dict[str, Any]) -> float:
    """Yaw (rad) from a places-pose dict. Supports full quaternion, planar
    (qz,qw), or an explicit yaw field."""
    if "qw" in pose or "qz" in pose:
        qx = float(pose.get("qx", 0.0)); qy = float(pose.get("qy", 0.0))
        qz = float(pose.get("qz", 0.0)); qw = float(pose.get("qw", 1.0))
        return math.atan2(2.0 * (qw * qz + qx * qy),
                          1.0 - 2.0 * (qy * qy + qz * qz))
    return float(pose.get("yaw", 0.0))


def _places_files_for(cfg: Config, stem: str) -> List[Path]:
    """Candidate on-disk places files for a map, most-specific first."""
    out: List[Path] = []
    key = _map_key(stem)
    if cfg.data_dir:
        out.append(cfg.data_dir / cfg.robot / "places" / f"{key}.json")
    if cfg.legacy_places:
        out.append(cfg.legacy_places)
    return out


def load_points(cfg: Config, stem: str) -> List[Dict[str, Any]]:
    """Return the map's saved places as fleet points: {name, type, x, y, yaw}.

    Places store shape (web_nav3): {"<name>": {x,y,z,qx,qy,qz,qw}}.
    """
    for pf in _places_files_for(cfg, stem):
        data = _read_json(pf, None) if pf.exists() else None
        if isinstance(data, dict) and data:
            pts: List[Dict[str, Any]] = []
            for name, pose in data.items():
                if not isinstance(pose, dict):
                    continue
                try:
                    pts.append({
                        "name": name,
                        "type": str(pose.get("type", "waypoint")),
                        "x": float(pose["x"]),
                        "y": float(pose["y"]),
                        "yaw": round(_yaw_from_pose(pose), 4),
                    })
                except (KeyError, TypeError, ValueError):
                    continue
            log.debug("points for %s: %d (from %s)", stem, len(pts), pf)
            return pts
    return []


def discover_maps(cfg: Config) -> List[MapArtifact]:
    """Find every .db under MAPS_DIR/<robot>/ and MAPS_DIR/ (legacy root)."""
    roots = [cfg.maps_dir / cfg.robot, cfg.maps_dir]
    meta: Dict[str, Any] = {}
    meta_file = cfg.maps_dir / cfg.robot / "maps_meta.json"
    if meta_file.exists():
        meta = _read_json(meta_file, {}) or {}

    seen: set[str] = set()
    out: List[MapArtifact] = []
    for root in roots:
        if not root.exists():
            continue
        for p in sorted(root.glob("*.db")):
            rp = str(p.resolve())
            if rp in seen:
                continue
            seen.add(rp)
            st = p.stat()
            out.append(MapArtifact(
                path=p,
                name=p.name,
                stem=p.stem,
                size=st.st_size,
                mtime=int(st.st_mtime),
                description=(meta.get(p.name) or {}).get("description", ""),
            ))
    out.sort(key=lambda m: m.mtime, reverse=True)
    return out


def _active_map_name(cfg: Config) -> Optional[str]:
    """Ask web_nav3 which map is currently loaded (optional; None if not set/up)."""
    if not cfg.web_nav3_url:
        return None
    try:
        r = requests.get(cfg.web_nav3_url + "/api/status",
                         headers={"X-Robot-Name": cfg.robot},
                         timeout=min(cfg.http_timeout, 5))
        r.raise_for_status()
        am = (r.json() or {}).get("active_map")
        return _map_key(am) if am else None
    except requests.RequestException as e:
        log.debug("active-map query failed: %s", e)
        return None


def select_maps(cfg: Config, maps: List[MapArtifact]) -> List[MapArtifact]:
    if not maps:
        return []
    if cfg.map_select == "newest":
        return maps[:1]
    if cfg.map_select == "active":
        active = _active_map_name(cfg)
        if active:
            picked = [m for m in maps if _map_key(m.stem) == active]
            if picked:
                return picked
            log.warning("active map %r not found on disk; falling back to newest", active)
        return maps[:1]
    return maps  # "all"


# --------------------------------------------------------------------------- #
# state (which fingerprints already uploaded)
# --------------------------------------------------------------------------- #
def _state_file(cfg: Config) -> Path:
    return cfg.state_dir / "uploaded.json"


def load_state(cfg: Config) -> Dict[str, str]:
    return _read_json(_state_file(cfg), {}) if _state_file(cfg).exists() else {}


def save_state(cfg: Config, state: Dict[str, str]) -> None:
    try:
        cfg.state_dir.mkdir(parents=True, exist_ok=True)
        _state_file(cfg).write_text(json.dumps(state, indent=2))
    except Exception as e:
        log.warning("could not persist state: %s", e)


# --------------------------------------------------------------------------- #
# upload
# --------------------------------------------------------------------------- #
def build_meta(cfg: Config, m: MapArtifact) -> Dict[str, Any]:
    return {
        "sn": cfg.sn,
        "name": m.stem,
        "file": m.name,
        "format": "rtabmap_db",
        "size_bytes": m.size,
        "sha256": m.sha256,
        "mtime": m.mtime,
        "description": m.description,
        "points": m.points,
    }


def upload_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool:
    url = cfg.map_url()
    meta = build_meta(cfg, m)
    try:
        if cfg.upload_mode == "base64json":
            body = dict(meta)
            body["db_base64"] = base64.b64encode(m.path.read_bytes()).decode("ascii")
            resp = session.post(url, json=body, headers=cfg.auth_headers(),
                                timeout=cfg.http_timeout, verify=cfg.verify_tls)
        else:  # multipart (default)
            with m.path.open("rb") as fh:
                files = {"db": (m.name, fh, "application/octet-stream")}
                data = {"meta": json.dumps(meta)}
                resp = session.post(url, files=files, data=data,
                                    headers=cfg.auth_headers(),
                                    timeout=cfg.http_timeout, verify=cfg.verify_tls)
    except requests.RequestException as e:
        log.error("upload %s FAILED (transport): %s", m.name, e)
        return False

    if not resp.ok:
        detail = resp.text[:300]
        log.error("upload %s FAILED: HTTP %s %s", m.name, resp.status_code, detail)
        return False
    log.info("uploaded %s (%.2f MB, %d points) -> HTTP %s",
             m.name, m.size / 1024 / 1024, len(m.points), resp.status_code)
    return True


# --------------------------------------------------------------------------- #
# one pass
# --------------------------------------------------------------------------- #
def run_once(cfg: Config, *, force: bool, dry_run: bool,
             session: requests.Session) -> int:
    maps = select_maps(cfg, discover_maps(cfg))
    if not maps:
        log.info("no .db maps found under %s (robot=%s)", cfg.maps_dir, cfg.robot)
        return 0

    state = load_state(cfg)
    uploaded = 0
    for m in maps:
        prev = state.get(str(m.path.resolve()))
        if not force and prev == m.fingerprint():
            log.debug("unchanged, skip: %s", m.name)
            continue
        # confirm change with a real content hash (mtime can shift without edits)
        m.sha256 = _sha256(m.path)
        m.points = load_points(cfg, m.stem)

        if dry_run:
            log.info("[dry-run] would upload %s (%.2f MB, sha=%s…, %d points)",
                     m.name, m.size / 1024 / 1024, m.sha256[:12], len(m.points))
            uploaded += 1
            continue

        if upload_map(cfg, m, session):
            state[str(m.path.resolve())] = m.fingerprint()
            save_state(cfg, state)
            uploaded += 1

    if uploaded == 0:
        log.info("nothing to upload (%d map(s) already current)", len(maps))
    return uploaded


def cmd_list(cfg: Config) -> None:
    maps = discover_maps(cfg)
    active = _active_map_name(cfg)
    if not maps:
        print(f"(no .db maps under {cfg.maps_dir} for robot '{cfg.robot}')")
        return
    print(f"{len(maps)} map(s) under {cfg.maps_dir} (robot={cfg.robot}):")
    for m in maps:
        pts = load_points(cfg, m.stem)
        flag = "  <-- active" if active and _map_key(m.stem) == active else ""
        print(f"  {m.name:<28} {m.size/1024/1024:6.2f} MB  {len(pts):>3} points"
              f"  {m.description}{flag}")


# --------------------------------------------------------------------------- #
# main
# --------------------------------------------------------------------------- #
def main(argv: Optional[List[str]] = None) -> int:
    ap = argparse.ArgumentParser(description="G1 fleet map uploader")
    ap.add_argument("--once", action="store_true", help="one scan+upload pass, then exit")
    ap.add_argument("--list", action="store_true", help="list discovered maps and exit")
    ap.add_argument("--dry-run", action="store_true", help="build payloads but never POST")
    ap.add_argument("--force", action="store_true", help="upload even if unchanged")
    ap.add_argument("--interval", type=float, default=None, help="override POLL_INTERVAL seconds")
    ap.add_argument("-v", "--verbose", action="store_true")
    args = ap.parse_args(argv)

    logging.basicConfig(
        level=logging.DEBUG if args.verbose else logging.INFO,
        format="%(asctime)s %(levelname)s %(name)s: %(message)s",
    )
    _load_dotenv()
    cfg = Config.from_env()
    if args.interval is not None:
        cfg.poll_interval = args.interval

    log.info("sanad_api_g1 map uploader — sn=%s robot=%s server=%s mode=%s select=%s",
             cfg.sn, cfg.robot, cfg.server_url, cfg.upload_mode, cfg.map_select)
    log.info("maps_dir=%s data_dir=%s web_nav3=%s",
             cfg.maps_dir, cfg.data_dir, cfg.web_nav3_url or "(disabled)")

    if args.list:
        cmd_list(cfg)
        return 0

    session = requests.Session()
    if args.once or args.dry_run:
        run_once(cfg, force=args.force, dry_run=args.dry_run, session=session)
        return 0

    log.info("loop every %.0fs (Ctrl-C to stop)", cfg.poll_interval)
    while True:
        try:
            run_once(cfg, force=args.force, dry_run=False, session=session)
        except Exception as e:  # never let the loop die
            log.exception("pass failed: %s", e)
        try:
            time.sleep(cfg.poll_interval)
        except KeyboardInterrupt:
            log.info("stopped")
            return 0


if __name__ == "__main__":
    sys.exit(main())