#!/usr/bin/env python3
"""
waypoint_logger.py — GPS waypoint logger and route planner for SaltyBot (Issue #617)

Interactive CLI for recording, managing, and publishing GPS waypoints from
an Android/Termux phone.  Uses termux-location for GPS fixes (same provider
as sensor_dashboard.py) and paho-mqtt to publish routes to the broker.

Features
────────
  • Record GPS waypoint at current position (with name and optional tags)
  • List all saved waypoints with distance/bearing from previous
  • Delete a waypoint by index
  • Clear all waypoints in the active route
  • Publish the route as a Nav2-compatible waypoint list to MQTT
  • Subscribe to saltybot/phone/gps for live GPS if sensor_dashboard.py
    is already running (avoids double GPS call)
  • Load/save route to JSON file (persistent between sessions)

MQTT topics
───────────
  Publish:   saltybot/phone/route
  Subscribe: saltybot/phone/gps  (live GPS from sensor_dashboard.py)

Route JSON published to MQTT
─────────────────────────────
  {
    "route_name": "patrol_loop",
    "ts": 1710000000.0,
    "waypoints": [
      {
        "index": 0,
        "name": "start",
        "tags": ["patrol", "home"],
        "lat": 45.123, "lon": -73.456, "alt_m": 12.3,
        "accuracy_m": 2.1,
        "recorded_at": 1710000000.0
      }, ...
    ]
  }

Nav2 PoseStamped list format (also included under "nav2_poses" key):
  Each entry has frame_id, position {x,y,z}, orientation {x,y,z,w}
  using a flat-earth approximation relative to the first waypoint.

Usage
─────
  python3 phone/waypoint_logger.py [OPTIONS]

  --broker   HOST    MQTT broker IP        (default: 192.168.1.100)
  --port     PORT    MQTT port             (default: 1883)
  --file     PATH    Route JSON file       (default: ~/saltybot_route.json)
  --route    NAME    Route name            (default: route)
  --provider PROV    GPS provider: gps|network|passive (default: gps)
  --live-gps         Subscribe to sensor_dashboard GPS instead of polling
  --no-mqtt          Disable MQTT (log-only mode)
  --debug            Verbose logging

Dependencies (Termux)
──────────────────────
  pkg install termux-api python
  pip install paho-mqtt
"""

import argparse
import json
import logging
import math
import os
import subprocess
import sys
import threading
import time
from dataclasses import dataclass, field, asdict
from datetime import datetime, timezone
from typing import Optional

try:
    import paho.mqtt.client as mqtt
    MQTT_AVAILABLE = True
except ImportError:
    MQTT_AVAILABLE = False

# ── Constants ─────────────────────────────────────────────────────────────────

TOPIC_ROUTE    = "saltybot/phone/route"
TOPIC_GPS_LIVE = "saltybot/phone/gps"

EARTH_RADIUS_M = 6_371_000.0   # mean Earth radius in metres

DEFAULT_FILE   = os.path.expanduser("~/saltybot_route.json")

# ── Data model ────────────────────────────────────────────────────────────────

@dataclass
class Waypoint:
    index:       int
    name:        str
    lat:         float
    lon:         float
    alt_m:       float        = 0.0
    accuracy_m:  float        = -1.0
    tags:        list[str]    = field(default_factory=list)
    recorded_at: float        = field(default_factory=time.time)

    def to_dict(self) -> dict:
        return asdict(self)

    @staticmethod
    def from_dict(d: dict) -> "Waypoint":
        return Waypoint(
            index       = int(d.get("index", 0)),
            name        = str(d.get("name", "")),
            lat         = float(d["lat"]),
            lon         = float(d["lon"]),
            alt_m       = float(d.get("alt_m", 0.0)),
            accuracy_m  = float(d.get("accuracy_m", -1.0)),
            tags        = list(d.get("tags", [])),
            recorded_at = float(d.get("recorded_at", 0.0)),
        )


# ── Geo maths ─────────────────────────────────────────────────────────────────

def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Great-circle distance between two WGS84 points (metres)."""
    φ1, φ2 = math.radians(lat1), math.radians(lat2)
    Δφ = math.radians(lat2 - lat1)
    Δλ = math.radians(lon2 - lon1)
    a = math.sin(Δφ / 2) ** 2 + math.cos(φ1) * math.cos(φ2) * math.sin(Δλ / 2) ** 2
    return 2 * EARTH_RADIUS_M * math.asin(math.sqrt(a))


def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Initial bearing from point-1 to point-2 (degrees, 0=N, 90=E)."""
    φ1, φ2 = math.radians(lat1), math.radians(lat2)
    Δλ = math.radians(lon2 - lon1)
    x = math.sin(Δλ) * math.cos(φ2)
    y = math.cos(φ1) * math.sin(φ2) - math.sin(φ1) * math.cos(φ2) * math.cos(Δλ)
    return (math.degrees(math.atan2(x, y)) + 360) % 360


def _compass(deg: float) -> str:
    """Convert bearing degrees to 8-point compass label."""
    dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]
    return dirs[round(deg / 45) % 8]


def flat_earth_xy(origin_lat: float, origin_lon: float,
                  lat: float, lon: float) -> tuple[float, float]:
    """
    Simple flat-earth approximation — metres east/north from origin.
    Accurate within ~1% for distances < 100 km.
    """
    lat_rad = math.radians((origin_lat + lat) / 2.0)
    x = math.radians(lon - origin_lon) * EARTH_RADIUS_M * math.cos(lat_rad)
    y = math.radians(lat - origin_lat) * EARTH_RADIUS_M
    return x, y


# ── GPS acquisition ───────────────────────────────────────────────────────────

def get_gps_fix(provider: str = "gps", timeout: float = 12.0) -> Optional[dict]:
    """
    Acquire a GPS fix via termux-location.
    Falls back to 'network' provider if 'gps' times out.
    Returns a dict with lat, lon, alt_m, accuracy_m or None on failure.
    """
    def _try(prov: str, t: float) -> Optional[dict]:
        try:
            r = subprocess.run(
                ["termux-location", "-p", prov, "-r", "once"],
                capture_output=True, text=True, timeout=t,
            )
            if r.returncode != 0 or not r.stdout.strip():
                return None
            raw = json.loads(r.stdout)
            lat = float(raw.get("latitude",  0.0))
            lon = float(raw.get("longitude", 0.0))
            if lat == 0.0 and lon == 0.0:
                return None
            return {
                "lat":        lat,
                "lon":        lon,
                "alt_m":      float(raw.get("altitude",  0.0)),
                "accuracy_m": float(raw.get("accuracy",  -1.0)),
                "provider":   prov,
            }
        except (subprocess.TimeoutExpired, json.JSONDecodeError,
                FileNotFoundError, KeyError):
            return None

    fix = _try(provider, timeout)
    if fix is None and provider == "gps":
        logging.debug("GPS timeout — retrying with network provider")
        fix = _try("network", 6.0)
    return fix


# ── Live GPS subscriber (optional, uses sensor_dashboard.py stream) ──────────

class LiveGPS:
    """
    Subscribe to saltybot/phone/gps on MQTT and cache the latest fix.
    Used when sensor_dashboard.py is already running to avoid double GPS calls.
    """

    def __init__(self, broker: str, port: int):
        self._lock   = threading.Lock()
        self._latest: Optional[dict] = None

        if not MQTT_AVAILABLE:
            return

        self._client = mqtt.Client(client_id="wp-live-gps", clean_session=True)
        self._client.on_connect = lambda c, u, f, rc: c.subscribe(TOPIC_GPS_LIVE)
        self._client.on_message = self._on_msg
        try:
            self._client.connect(broker, port, keepalive=30)
            self._client.loop_start()
        except Exception as e:
            logging.warning("LiveGPS connect failed: %s", e)

    def _on_msg(self, client, userdata, message) -> None:
        try:
            data = json.loads(message.payload)
            with self._lock:
                self._latest = data
        except json.JSONDecodeError:
            pass

    def get(self, max_age_s: float = 3.0) -> Optional[dict]:
        with self._lock:
            d = self._latest
        if d is None:
            return None
        if time.time() - d.get("ts", 0.0) > max_age_s:
            return None
        return {
            "lat":        d.get("lat",       0.0),
            "lon":        d.get("lon",       0.0),
            "alt_m":      d.get("alt_m",     0.0),
            "accuracy_m": d.get("accuracy_m", -1.0),
            "provider":   d.get("provider",  "live"),
        }

    def stop(self) -> None:
        if MQTT_AVAILABLE and hasattr(self, "_client"):
            self._client.loop_stop()
            self._client.disconnect()


# ── Route ─────────────────────────────────────────────────────────────────────

class Route:
    """Ordered list of waypoints with persistence and Nav2 export."""

    def __init__(self, name: str, path: str):
        self.name       = name
        self._path      = path
        self._waypoints: list[Waypoint] = []
        self._load()

    # ── Persistence ───────────────────────────────────────────────────────────

    def _load(self) -> None:
        if not os.path.exists(self._path):
            return
        try:
            with open(self._path, "r") as f:
                data = json.load(f)
            self.name       = data.get("route_name", self.name)
            self._waypoints = [Waypoint.from_dict(w) for w in data.get("waypoints", [])]
            # Re-index to keep indices contiguous after deletes
            for i, wp in enumerate(self._waypoints):
                wp.index = i
            logging.info("Loaded %d waypoints from %s", len(self._waypoints), self._path)
        except (json.JSONDecodeError, KeyError, ValueError) as e:
            logging.warning("Could not load route file: %s", e)

    def save(self) -> None:
        with open(self._path, "w") as f:
            json.dump(self.to_dict(), f, indent=2)
        logging.debug("Saved route to %s", self._path)

    # ── Waypoint operations ───────────────────────────────────────────────────

    def add(self, name: str, lat: float, lon: float, alt_m: float,
            accuracy_m: float, tags: list[str]) -> Waypoint:
        wp = Waypoint(
            index      = len(self._waypoints),
            name       = name,
            lat        = lat,
            lon        = lon,
            alt_m      = alt_m,
            accuracy_m = accuracy_m,
            tags       = tags,
        )
        self._waypoints.append(wp)
        self.save()
        return wp

    def delete(self, index: int) -> bool:
        if not (0 <= index < len(self._waypoints)):
            return False
        self._waypoints.pop(index)
        for i, wp in enumerate(self._waypoints):
            wp.index = i
        self.save()
        return True

    def clear(self) -> int:
        n = len(self._waypoints)
        self._waypoints.clear()
        self.save()
        return n

    def __len__(self) -> int:
        return len(self._waypoints)

    def __iter__(self):
        return iter(self._waypoints)

    def get(self, index: int) -> Optional[Waypoint]:
        if 0 <= index < len(self._waypoints):
            return self._waypoints[index]
        return None

    # ── Serialisation ─────────────────────────────────────────────────────────

    def to_dict(self) -> dict:
        return {
            "route_name": self.name,
            "ts":         time.time(),
            "waypoints":  [wp.to_dict() for wp in self._waypoints],
            "nav2_poses": self._nav2_poses(),
        }

    def _nav2_poses(self) -> list[dict]:
        """
        Flat-earth Nav2 PoseStamped list (x east, y north) relative to
        the first waypoint.  Yaw faces the next waypoint; last faces prev.
        """
        if not self._waypoints:
            return []

        origin = self._waypoints[0]
        poses  = []

        for i, wp in enumerate(self._waypoints):
            x, y = flat_earth_xy(origin.lat, origin.lon, wp.lat, wp.lon)

            # Compute yaw: face toward next waypoint (or prev for last)
            if i + 1 < len(self._waypoints):
                nxt = self._waypoints[i + 1]
                nx, ny = flat_earth_xy(origin.lat, origin.lon, nxt.lat, nxt.lon)
                yaw = math.atan2(ny - y, nx - x)
            elif i > 0:
                prv = self._waypoints[i - 1]
                px, py = flat_earth_xy(origin.lat, origin.lon, prv.lat, prv.lon)
                yaw = math.atan2(y - py, x - px)
            else:
                yaw = 0.0

            poses.append({
                "frame_id": "map",
                "position":    {"x": round(x, 3), "y": round(y, 3), "z": round(wp.alt_m, 3)},
                "orientation": {
                    "x": 0.0, "y": 0.0,
                    "z": round(math.sin(yaw / 2), 6),
                    "w": round(math.cos(yaw / 2), 6),
                },
                "waypoint_name": wp.name,
            })

        return poses


# ── MQTT publisher ────────────────────────────────────────────────────────────

class MQTTClient:
    """Simple synchronous paho wrapper for route publishing."""

    def __init__(self, broker: str, port: int):
        self._broker = broker
        self._port   = port
        self._client = None
        self._connected = False

        if not MQTT_AVAILABLE:
            return
        self._client = mqtt.Client(client_id="saltybot-waypoint-logger", clean_session=True)
        self._client.on_connect    = self._on_connect
        self._client.on_disconnect = self._on_disconnect
        self._client.reconnect_delay_set(min_delay=3, max_delay=30)
        try:
            self._client.connect_async(broker, port, keepalive=60)
            self._client.loop_start()
        except Exception as e:
            logging.warning("MQTT connect failed: %s", e)

    def _on_connect(self, c, u, f, rc) -> None:
        self._connected = (rc == 0)
        if rc == 0:
            logging.debug("MQTT connected")

    def _on_disconnect(self, c, u, rc) -> None:
        self._connected = False

    def publish(self, topic: str, payload: dict, qos: int = 1) -> bool:
        if self._client is None:
            return False
        # Wait briefly for connection
        deadline = time.monotonic() + 5.0
        while not self._connected and time.monotonic() < deadline:
            time.sleep(0.1)
        if not self._connected:
            return False
        info = self._client.publish(
            topic, json.dumps(payload, separators=(",", ":")), qos=qos, retain=True
        )
        return info.rc == 0

    def stop(self) -> None:
        if self._client:
            self._client.loop_stop()
            self._client.disconnect()


# ── CLI helpers ───────────────────────────────────────────────────────────────

def _fmt_ts(epoch: float) -> str:
    return datetime.fromtimestamp(epoch, tz=timezone.utc).strftime("%Y-%m-%d %H:%M:%S UTC")


def _fmt_dist(metres: float) -> str:
    if metres < 1000:
        return f"{metres:.1f} m"
    return f"{metres / 1000:.3f} km"


def _print_waypoints(route: Route) -> None:
    if not len(route):
        print("  (no waypoints)")
        return
    print(f"\n  {'#':>3}  {'Name':<18}  {'Lat':>10}  {'Lon':>11}  {'Alt':>7}  {'Acc':>7}  {'Tags'}")
    print("  " + "─" * 75)
    prev = None
    for wp in route:
        dist_str = ""
        brg_str  = ""
        if prev is not None:
            d = haversine_m(prev.lat, prev.lon, wp.lat, wp.lon)
            b = bearing_deg(prev.lat, prev.lon, wp.lat, wp.lon)
            dist_str = f"  ↑ {_fmt_dist(d)} {_compass(b)}"
        tags = ",".join(wp.tags) if wp.tags else "—"
        print(f"  {wp.index:>3}  {wp.name:<18}  {wp.lat:>10.6f}  {wp.lon:>11.6f}"
              f"  {wp.alt_m:>6.1f}m  "
              f"{wp.accuracy_m:>5.1f}m  {tags}{dist_str}")
        prev = wp
    print()


def _acquire_fix(args, live_gps: Optional[LiveGPS]) -> Optional[dict]:
    """Get a GPS fix — from live MQTT stream if available, else termux-location."""
    if live_gps is not None:
        fix = live_gps.get(max_age_s=3.0)
        if fix is not None:
            return fix
        print("  (live GPS stale — falling back to termux-location)")
    print("  Acquiring GPS fix...", end="", flush=True)
    fix = get_gps_fix(provider=args.provider)
    if fix:
        print(f" {fix['lat']:.6f}, {fix['lon']:.6f}  ±{fix['accuracy_m']:.1f}m")
    else:
        print(" FAILED")
    return fix


# ── Interactive menu ──────────────────────────────────────────────────────────

def _menu_record(route: Route, args, live_gps: Optional[LiveGPS]) -> None:
    fix = _acquire_fix(args, live_gps)
    if fix is None:
        print("  Could not get GPS fix.")
        return

    default_name = f"wp{len(route)}"
    name = input(f"  Waypoint name [{default_name}]: ").strip() or default_name
    tags_raw = input("  Tags (comma-separated, or Enter to skip): ").strip()
    tags = [t.strip() for t in tags_raw.split(",") if t.strip()] if tags_raw else []

    wp = route.add(name, fix["lat"], fix["lon"], fix["alt_m"], fix["accuracy_m"], tags)
    print(f"  ✓ Recorded #{wp.index}: {wp.name}  ({wp.lat:.6f}, {wp.lon:.6f})")


def _menu_list(route: Route) -> None:
    print(f"\n  Route: '{route.name}'  ({len(route)} waypoints)")
    _print_waypoints(route)

    if len(route) >= 2:
        wps = list(route)
        total = sum(
            haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
            for i in range(len(wps) - 1)
        )
        print(f"  Total route distance: {_fmt_dist(total)}")


def _menu_delete(route: Route) -> None:
    if not len(route):
        print("  No waypoints to delete.")
        return
    _print_waypoints(route)
    raw = input("  Enter index to delete (or Enter to cancel): ").strip()
    if not raw:
        return
    try:
        idx = int(raw)
    except ValueError:
        print("  Invalid index.")
        return
    wp = route.get(idx)
    if wp is None:
        print(f"  No waypoint at index {idx}.")
        return
    confirm = input(f"  Delete '{wp.name}'? [y/N]: ").strip().lower()
    if confirm == "y":
        route.delete(idx)
        print(f"  ✓ Deleted '{wp.name}'.")
    else:
        print("  Cancelled.")


def _menu_publish(route: Route, mqtt_client: Optional[MQTTClient]) -> None:
    if not len(route):
        print("  No waypoints to publish.")
        return

    payload = route.to_dict()
    print(f"\n  Publishing {len(route)} waypoints to {TOPIC_ROUTE}...")

    if mqtt_client is None:
        print("  (MQTT disabled — printing payload)")
        print(json.dumps(payload, indent=2))
        return

    ok = mqtt_client.publish(TOPIC_ROUTE, payload, qos=1)
    if ok:
        print(f"  ✓ Published to {TOPIC_ROUTE}  (retained=true)")
    else:
        print("  ✗ MQTT publish failed — check broker connection.")

    # Also print Nav2 summary
    poses = payload.get("nav2_poses", [])
    if poses:
        print(f"\n  Nav2 poses ({len(poses)}):")
        for p in poses:
            pos = p["position"]
            print(f"    {p['waypoint_name']:<18}  x={pos['x']:>8.2f}m  y={pos['y']:>8.2f}m")


def _menu_clear(route: Route) -> None:
    if not len(route):
        print("  Route is already empty.")
        return
    confirm = input(f"  Clear all {len(route)} waypoints? [y/N]: ").strip().lower()
    if confirm == "y":
        n = route.clear()
        print(f"  ✓ Cleared {n} waypoints.")
    else:
        print("  Cancelled.")


def _menu_rename(route: Route) -> None:
    new_name = input(f"  New route name [{route.name}]: ").strip()
    if new_name:
        route.name = new_name
        route.save()
        print(f"  ✓ Route renamed to '{route.name}'.")


def _menu_info(route: Route) -> None:
    print(f"\n  Route file: {route._path}")
    print(f"  Route name: {route.name}")
    print(f"  Waypoints:  {len(route)}")
    if len(route):
        wps = list(route)
        print(f"  First wp:   {wps[0].name}  ({_fmt_ts(wps[0].recorded_at)})")
        print(f"  Last wp:    {wps[-1].name}  ({_fmt_ts(wps[-1].recorded_at)})")
        if len(route) >= 2:
            total = sum(
                haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
                for i in range(len(wps) - 1)
            )
            print(f"  Total dist: {_fmt_dist(total)}")
    print()


# ── Main loop ─────────────────────────────────────────────────────────────────

MENU = """
  ┌─ SaltyBot Waypoint Logger ──────────────────────────┐
  │  r  Record waypoint at current GPS position         │
  │  l  List all waypoints                              │
  │  d  Delete a waypoint                               │
  │  p  Publish route to MQTT                           │
  │  c  Clear all waypoints                             │
  │  n  Rename route                                    │
  │  i  Route info                                      │
  │  q  Quit                                            │
  └─────────────────────────────────────────────────────┘
"""


def main() -> None:
    parser = argparse.ArgumentParser(
        description="SaltyBot GPS waypoint logger (Issue #617)"
    )
    parser.add_argument("--broker",   default="192.168.1.100",
                        help="MQTT broker IP (default: 192.168.1.100)")
    parser.add_argument("--port",     type=int, default=1883,
                        help="MQTT port (default: 1883)")
    parser.add_argument("--file",     default=DEFAULT_FILE,
                        help=f"Route JSON file (default: {DEFAULT_FILE})")
    parser.add_argument("--route",    default="route",
                        help="Route name (default: route)")
    parser.add_argument("--provider", default="gps",
                        choices=["gps", "network", "passive"],
                        help="GPS provider (default: gps)")
    parser.add_argument("--live-gps", action="store_true",
                        help="Subscribe to saltybot/phone/gps for live GPS")
    parser.add_argument("--no-mqtt",  action="store_true",
                        help="Disable MQTT (log-only mode)")
    parser.add_argument("--debug",    action="store_true",
                        help="Verbose logging")
    args = parser.parse_args()

    logging.basicConfig(
        level=logging.DEBUG if args.debug else logging.WARNING,
        format="%(asctime)s [%(levelname)s] %(message)s",
    )

    # Load or create route
    route = Route(args.route, args.file)

    # MQTT client
    mqtt_client: Optional[MQTTClient] = None
    if not args.no_mqtt:
        if MQTT_AVAILABLE:
            mqtt_client = MQTTClient(args.broker, args.port)
        else:
            print("Warning: paho-mqtt not installed — MQTT disabled. Run: pip install paho-mqtt")

    # Optional live GPS subscription
    live_gps: Optional[LiveGPS] = None
    if args.live_gps and MQTT_AVAILABLE:
        live_gps = LiveGPS(args.broker, args.port)
        print(f"  Subscribed to live GPS on {TOPIC_GPS_LIVE}")

    print(MENU)
    print(f"  Route: '{route.name}'  |  {len(route)} waypoints loaded  |  file: {args.file}")
    print()

    try:
        while True:
            try:
                choice = input("  > ").strip().lower()
            except EOFError:
                break

            if choice in ("q", "quit", "exit"):
                break
            elif choice in ("r", "record"):
                _menu_record(route, args, live_gps)
            elif choice in ("l", "list"):
                _menu_list(route)
            elif choice in ("d", "delete"):
                _menu_delete(route)
            elif choice in ("p", "publish"):
                _menu_publish(route, mqtt_client)
            elif choice in ("c", "clear"):
                _menu_clear(route)
            elif choice in ("n", "rename"):
                _menu_rename(route)
            elif choice in ("i", "info"):
                _menu_info(route)
            elif choice == "":
                pass
            else:
                print("  Unknown command. Type r/l/d/p/c/n/i/q.")

    except KeyboardInterrupt:
        print("\n  Interrupted.")
    finally:
        if live_gps:
            live_gps.stop()
        if mqtt_client:
            mqtt_client.stop()

    print("  Bye.")


if __name__ == "__main__":
    main()