Marcus/Brain/command_parser.py

"""
command_parser.py — Local command regex patterns + dispatcher
Handles place memory, odometry, session recall, help, examples
"""
import re
import time
from API.zmq_api import send_vel, gradual_stop, MOVE_MAP, STEP_DURATION_SEC
from API.memory_api import mem, place_save, place_goto, places_list_str
from API.odometry_api import odom, ODOM_AVAILABLE
from API.camera_api import get_frame
from API.llava_api import ask
from Brain.executor import execute

# ── Compiled patterns ────────────────────────────────────────────────────────

_RE_REMEMBER = re.compile(
    r"^(?:remember|save|mark|call|name|label)\s+(?:this|here|current|position)?\s*as\s+(.+)$", re.I)
_RE_GOTO = re.compile(
    r"^(?:go\s+to|navigate\s+to|take\s+me\s+to|move\s+to|return\s+to|head\s+to)\s+(.+)$", re.I)
_RE_FORGET = re.compile(
    r"^(?:forget|delete|remove)\s+(?:place\s+)?(.+)$", re.I)
_RE_RENAME = re.compile(
    r"^rename\s+(.+?)\s+(?:to|as)\s+(.+)$", re.I)
_RE_WALK_DIST = re.compile(
    r"^(?:walk|go|move)\s+(?:forward\s+)?(\d+(?:\.\d+)?)\s*m(?:eter(?:s)?)?$", re.I)
_RE_WALK_BACK = re.compile(
    r"^(?:walk|go|move)\s+backward?\s+(\d+(?:\.\d+)?)\s*m(?:eter(?:s)?)?$", re.I)
_RE_TURN_DEG = re.compile(
    r"^turn\s+(?:(left|right)\s+)?(\d+(?:\.\d+)?)\s*deg(?:ree(?:s)?)?$", re.I)
# Step-based motion: "walk 1 step", "walk forward 2 steps", "move back 1 step",
# "turn left", "turn right 2 steps". Kept local so these never fall through to
# the VLM — on the Jetson the cold-load is 60-90 s and we don't want to pay
# that for a trivial two-second motion. One step = 2 s of motion at the default
# velocity, matching the undo-loop duration already used below.
_RE_WALK_STEP = re.compile(
    r"^(?:walk|go|move|step)(?:\s+(forward|back(?:ward)?))?\s+(\d+)\s*steps?$", re.I)
_RE_TURN_STEP = re.compile(
    r"^turn\s+(left|right)(?:\s+(\d+)\s*steps?)?$", re.I)

# Simple one-shot motion — one word or verb+direction, no counts/units.
# All default to a ~2 s motion at the normal velocity. Kept local so the
# user doesn't eat a 5 s Qwen round-trip for a trivial "go back".
#
# Matches:
#     "left" / "right" / "forward" / "back" / "backward"
#     "go back" / "step back" / "move back" / "walk back" / "run back"
#     "go forward" / "step forward" / "move forward" / "walk forward"
#     "go left" / "move right" / "step left" / etc.
#     "head forward" / "head back"
# Does NOT match multi-word phrases like "walk to the chair" — those
# still fall through to Qwen where they belong.
_RE_SIMPLE_DIR = re.compile(
    r"^(?:(?:walk|go|move|step|run|head)\s+)?"
    r"(forward|back(?:ward)?|left|right)$",
    re.I,
)

# Bare stop / pause words — no need to ask Qwen what "stop" means.
_RE_STOP_SIMPLE = re.compile(
    r"^(?:stop|halt|wait|pause|stay|freeze|hold|stand\s+still|don'?t\s+move)$",
    re.I,
)
_RE_PATROL_RT = re.compile(
    r"^patrol[/:]\s*(.+)$", re.I)
_RE_LAST_CMD = re.compile(
    r"^(?:last\s+command|what\s+did\s+i\s+(?:say|type)\s+last|repeat\s+last)$", re.I)
_RE_DO_AGAIN = re.compile(
    r"^(?:do\s+that\s+again|repeat|again|redo)$", re.I)
_RE_UNDO = re.compile(
    r"^(?:undo|go\s+back\s+(?:to\s+)?(?:where|from\s+where)\s+(?:you|i)\s+(?:started|were|came)|reverse\s+last|turn\s+back\s+from).*$", re.I)
_RE_LAST_SESS = re.compile(
    r"^(?:last\s+session|what\s+(?:did\s+you\s+do|happened)\s+last\s+(?:session|time)|previous\s+session)$", re.I)
_RE_WHERE = re.compile(
    r"^(?:where\s+am\s+i|current\s+position|my\s+(?:location|position)|position)$", re.I)
_RE_GO_HOME = re.compile(
    r"^(?:go\s+home|return\s+to\s+start|come\s+back\s+home|go\s+back\s+to\s+start)$", re.I)
_RE_SESSION_SUMMARY = re.compile(
    r"^(?:session\s+summary|what\s+happened\s+today|session\s+report)$", re.I)
_RE_AUTO = re.compile(
    r"^auto(?:nomous)?\s+(on|off|status|save|summary)$", re.I)

# Autonomous mode instance — set by init_autonomous()
_auto = None


def init_autonomous(auto_instance):
    """Wire in the AutonomousMode instance from marcus_brain."""
    global _auto
    _auto = auto_instance


def try_local_command(cmd: str) -> bool:
    """
    Handle local commands (place, odom, memory, help).
    Returns True if handled, False if not matched (send to LLaVA).
    """
    # ── PLACE MEMORY ─────────────────────────────────────────────────────
    m = _RE_REMEMBER.match(cmd)
    if m:
        place_save(m.group(1).strip())
        return True

    m = _RE_GOTO.match(cmd)
    if m:
        name = m.group(1).strip()
        if name.lower() in ("start", "home", "beginning"):
            if odom and ODOM_AVAILABLE:
                odom.return_to_start()
            else:
                print("  [Places] Odometry not running — cannot return to start")
        else:
            place_goto(name)
        return True

    m = _RE_FORGET.match(cmd)
    if m:
        if mem:
            mem.delete_place(m.group(1).strip())
        else:
            print("  [Places] Memory not available")
        return True

    m = _RE_RENAME.match(cmd)
    if m:
        if mem:
            mem.rename_place(m.group(1).strip(), m.group(2).strip())
        else:
            print("  [Places] Memory not available")
        return True

    if re.match(r"^(?:places|list\s+places|what\s+places|show\s+(?:places|locations)|known\s+places)$", cmd, re.I):
        print(places_list_str())
        return True

    # ── ODOMETRY MOVEMENT ────────────────────────────────────────────────
    m = _RE_WALK_DIST.match(cmd)
    if m:
        meters = float(m.group(1))
        if odom:
            odom.walk_distance(meters)
        else:
            vx, _, _ = MOVE_MAP["forward"]
            t0 = time.time()
            while time.time() - t0 < meters / abs(vx):
                send_vel(vx=vx)
                time.sleep(0.05)
            gradual_stop()
        return True

    m = _RE_WALK_BACK.match(cmd)
    if m:
        meters = float(m.group(1))
        if odom:
            odom.walk_distance(meters, direction="backward")
        else:
            vx, _, _ = MOVE_MAP["backward"]
            t0 = time.time()
            while time.time() - t0 < meters / abs(vx):
                send_vel(vx=vx)
                time.sleep(0.05)
            gradual_stop()
        return True

    m = _RE_TURN_DEG.match(cmd)
    if m:
        direction = m.group(1)
        degrees = float(m.group(2))
        if direction and direction.lower() == "right":
            degrees = -degrees
        if odom:
            odom.turn_degrees(degrees)
        else:
            # vyaw magnitude comes from MOVE_MAP["left"]; duration is
            # abs(degrees)/(vyaw_deg_per_sec). vyaw in config is rad/s.
            _, _, vyaw_mag = MOVE_MAP["left"]
            vyaw_deg_per_sec = abs(vyaw_mag) * 180.0 / 3.14159265
            vyaw = vyaw_mag if degrees > 0 else -vyaw_mag
            duration = abs(degrees) / vyaw_deg_per_sec
            t0 = time.time()
            while time.time() - t0 < duration:
                send_vel(vyaw=vyaw)
                time.sleep(0.05)
            gradual_stop()
        return True

    m = _RE_WALK_STEP.match(cmd)
    if m:
        direction = (m.group(1) or "forward").lower()
        if direction.startswith("back"):
            direction = "backward"
        steps = int(m.group(2))
        vx, _, _ = MOVE_MAP[direction]
        duration = STEP_DURATION_SEC * steps
        t0 = time.time()
        while time.time() - t0 < duration:
            send_vel(vx=vx)
            time.sleep(0.05)
        gradual_stop()
        return True

    m = _RE_TURN_STEP.match(cmd)
    if m:
        direction = m.group(1).lower()
        steps = int(m.group(2)) if m.group(2) else 1
        _, _, vyaw = MOVE_MAP[direction]
        duration = STEP_DURATION_SEC * steps
        t0 = time.time()
        while time.time() - t0 < duration:
            send_vel(vyaw=vyaw)
            time.sleep(0.05)
        gradual_stop()
        return True

    # ── BARE / SIMPLE DIRECTIONAL COMMANDS ───────────────────────────────
    # "left", "right", "forward", "back", "go back", "move forward",
    # "step right", "walk back", "head forward" — any one-word direction
    # or verb+direction with no explicit count. Duration and velocities
    # come entirely from config_Navigation.json (MOVE_MAP +
    # step_duration_sec) — no magic numbers here.
    m = _RE_SIMPLE_DIR.match(cmd)
    if m:
        direction = m.group(1).lower()
        if direction.startswith("back"):
            direction = "backward"
        vx, vy, vyaw = MOVE_MAP[direction]
        t0 = time.time()
        while time.time() - t0 < STEP_DURATION_SEC:
            send_vel(vx=vx, vy=vy, vyaw=vyaw)
            time.sleep(0.05)
        gradual_stop()
        return True

    # ── BARE STOP / PAUSE ─────────────────────────────────────────────────
    m = _RE_STOP_SIMPLE.match(cmd)
    if m:
        gradual_stop()
        return True

    # ── NAMED PATROL ROUTE ───────────────────────────────────────────────
    m = _RE_PATROL_RT.match(cmd)
    if m:
        raw_route = m.group(1)
        names = re.split(r"[→,;]+|\s{2,}", raw_route)
        names = [n.strip() for n in names if n.strip()]
        if not names:
            print("  Usage: patrol: door → desk → exit")
            return True
        if not mem:
            print("  [Places] Memory not available")
            return True
        waypoints, missing = [], []
        for name in names:
            place = mem.get_place(name)
            if place is None:
                missing.append(name)
            elif not place.get("has_odom"):
                print(f"  [Places] '{name}' has no coordinates — skipping")
            else:
                waypoints.append({"x": place["x"], "y": place["y"], "heading": place["heading"], "name": name})
        if missing:
            print(f"  [Places] Unknown places: {', '.join(missing)}")
        if not waypoints:
            print("  [Places] No valid waypoints — patrol cancelled")
            return True
        if odom:
            print(f"  [Places] Named patrol: {' → '.join(n['name'] for n in waypoints)}")
            odom.patrol_route(waypoints)
        else:
            print("  [Places] Odometry not running")
        return True

    # ── SESSION MEMORY RECALL ────────────────────────────────────────────
    if _RE_LAST_CMD.match(cmd):
        if mem:
            last = mem.get_last_command()
            print(f"  Last command: '{last}'" if last else "  No commands logged yet")
        else:
            print("  Memory not available")
        return True

    if _RE_UNDO.match(cmd):
        if not mem:
            print("  Memory not available — cannot undo")
            return True
        recent = mem.get_last_n_commands(5)
        move_words = {"turn right": ("left", 1), "turn left": ("right", -1),
                      "walk forward": ("backward", 1), "move forward": ("backward", 1),
                      "move back": ("forward", 1), "walk backward": ("forward", 1)}
        for c in reversed(recent):
            cl = c.lower()
            for phrase, (reverse_dir, _) in move_words.items():
                if phrase in cl:
                    print(f"  Undoing: '{c}' → reversing with '{reverse_dir}'")
                    vx, vy, vyaw = MOVE_MAP[reverse_dir]
                    t0 = time.time()
                    while time.time() - t0 < STEP_DURATION_SEC:
                        send_vel(vx=vx, vy=vy, vyaw=vyaw)
                        time.sleep(0.05)
                    gradual_stop()
                    return True
        print("  No movement command to undo")
        return True

    if _RE_DO_AGAIN.match(cmd):
        if not mem:
            print("  Memory not available — cannot repeat")
            return True
        recent = mem.get_last_n_commands(5)
        repeat = None
        for c in reversed(recent):
            if not _RE_DO_AGAIN.match(c) and not _RE_LAST_CMD.match(c):
                repeat = c
                break
        if repeat:
            print(f"  Repeating: '{repeat}'")
            if try_local_command(repeat):
                return True
            # Not a local command — send directly to LLaVA
            print("Thinking...")
            img = get_frame()
            if img:
                d = ask(repeat, img)
                execute(d)
            return True
        else:
            print("  No previous command to repeat")
        return True

    if _RE_LAST_SESS.match(cmd):
        if mem:
            print(mem.last_session_summary())
        else:
            print("  Memory not available")
        return True

    if _RE_SESSION_SUMMARY.match(cmd):
        if mem:
            print(f"  Session: {mem._session_id}")
            print(f"  Duration: {mem.session_duration_str()}")
            print(f"  Commands: {mem.commands_count()}")
            print(f"  Places: {mem.places_count()}")
            detections = mem.get_session_detections()
            classes = {d.get("class") for d in detections}
            print(f"  Detected: {', '.join(classes) if classes else 'nothing yet'}")
        else:
            print("  Memory not available")
        return True

    if _RE_WHERE.match(cmd):
        if odom and ODOM_AVAILABLE:
            print(f"  Position: {odom.status_str()}")
            print(f"  Distance from start: {odom.get_distance_from_start():.2f}m")
        else:
            print("  Odometry not running — position unknown")
        return True

    if _RE_GO_HOME.match(cmd):
        if odom and ODOM_AVAILABLE:
            odom.return_to_start()
        else:
            print("  Odometry not running — cannot navigate home")
        return True

    # ── AUTONOMOUS MODE ──────────────────────────────────────────────────
    m = _RE_AUTO.match(cmd)
    if m:
        subcmd = m.group(1).lower()
        if _auto is None:
            print("  [Auto] Autonomous mode not initialized")
            return True
        if subcmd == "on":
            _auto.enable()
        elif subcmd == "off":
            _auto.disable()
        elif subcmd == "status":
            _auto.status()
        elif subcmd == "save":
            _auto.save_snapshot()
        elif subcmd == "summary":
            if _auto.is_enabled():
                _auto.status()
            else:
                print("  [Auto] Not running — use 'auto on' to start")
        return True

    # ── LIDAR STATUS ─────────────────────────────────────────────────────
    if re.match(r"^(?:lidar|lidar\s+status|slam\s+status)$", cmd, re.I):
        try:
            from API.lidar_api import LIDAR_AVAILABLE, get_lidar_status
            if not LIDAR_AVAILABLE:
                print("  LiDAR: not available")
            else:
                s = get_lidar_status()
                print(f"  LiDAR: {s['mode']} | loc: {s['loc_state']} | "
                      f"frame age: {s['last_frame_age']}s")
                if s.get("pose"):
                    p = s["pose"]
                    print(f"  SLAM pose: x={p['x']:.2f} y={p['y']:.2f} h={p['heading']:.1f}")
                safety = s.get("safety", {})
                if safety.get("emergency"):
                    print(f"  EMERGENCY: {safety.get('reasons', [])}")
                perf = s.get("perf", {})
                if perf:
                    print(f"  FPS: {perf.get('input_fps', 0):.0f} in / "
                          f"ICP: {perf.get('icp_ms', 0):.1f}ms / "
                          f"CPU: {perf.get('cpu_percent', 0):.0f}%")
        except ImportError:
            print("  LiDAR: module not loaded")
        return True

    # ── HELP / EXAMPLES ──────────────────────────────────────────────────
    if re.match(r"^(?:help[/]|help|commands|menu|[?][/]|[?])$", cmd, re.I):
        _print_help()
        return True

    if re.match(r"^(?:example[/]|examples[/]|ex[/]|example|examples|ex|show examples)$", cmd, re.I):
        _print_examples()
        return True

    return False


def _print_help():
    print("""
  MARCUS — COMMAND HELP
  Movement: turn left/right, walk forward/back, walk 1 meter, turn 90 degrees
  Vision:   what do you see, yolo
  Goals:    goal/ stop when you see a person
  Places:   remember this as door, go to door, places, forget door
  Patrol:   patrol, patrol: door → desk → exit
  Session:  last command, do that again, last session, session summary
  Search:   search/ /path/to/photo.jpg [hint], search/ person in blue
  Auto:     auto on, auto off, auto status, auto save, auto summary
  LiDAR:    lidar, lidar status
  System:   help, example, yolo, q""")


def _print_examples():
    print("""
  MARCUS — USAGE EXAMPLES
  turn left | turn right 90 degrees | walk forward | walk 1 meter
  what do you see | describe what is in front of you
  goal/ stop when you see a person | goal/ stop when you see a laptop
  remember this as door | go to door | places | forget door
  patrol | patrol: door → desk → window
  last command | do that again | last session | session summary""")