"""
executor.py — Execute LLaVA movement decisions
With LiDAR obstacle interrupt — stops immediately if obstacle detected.
"""
import time
import threading
from API.zmq_api import send_vel, gradual_stop, MOVE_MAP, STEP_PAUSE
from API.arm_api import ALL_ARM_NAMES, do_arm


def _obstacle_check():
    """Check LiDAR safety — returns True if obstacle detected. Safe if LiDAR unavailable."""
    try:
        from API.lidar_api import obstacle_ahead
        return obstacle_ahead()
    except ImportError:
        return False


def execute_action(move: str, duration: float):
    """Execute a single movement step. Stops if LiDAR detects obstacle."""
    if move in ALL_ARM_NAMES:
        do_arm(move)
        return
    if move == "stop" or move is None:
        gradual_stop()
        return
    if move in MOVE_MAP:
        vx, vy, vyaw = MOVE_MAP[move]
        t0 = time.time()
        while time.time() - t0 < duration:
            if _obstacle_check():
                gradual_stop()
                print("  [Safety] LiDAR obstacle — stopping")
                return
            send_vel(vx, vy, vyaw)
            time.sleep(0.05)
        gradual_stop()
        time.sleep(STEP_PAUSE)
    else:
        print(f"  Unknown move: '{move}' — skipping")


def move_step(move: str, duration: float):
    """Lightweight step for goal/patrol loops. Stops if LiDAR detects obstacle."""
    if move in MOVE_MAP:
        vx, vy, vyaw = MOVE_MAP[move]
        t0 = time.time()
        while time.time() - t0 < duration:
            if _obstacle_check():
                send_vel(0.0, 0.0, 0.0)
                print("  [Safety] LiDAR obstacle — pausing step")
                return
            send_vel(vx, vy, vyaw)
            time.sleep(0.05)
    send_vel(0.0, 0.0, 0.0)
    time.sleep(0.1)


def merge_actions(actions: list) -> list:
    """Merge consecutive same-direction steps into one smooth movement."""
    if not actions:
        return actions
    merged = [dict(actions[0])]
    for action in actions[1:]:
        if action.get("move") == merged[-1].get("move"):
            merged[-1]["duration"] = merged[-1].get("duration", 0) + action.get("duration", 0)
        else:
            merged.append(dict(action))
    return merged


def execute(d: dict):
    """Run full LLaVA decision — movements in sequence, then arm in background."""
    if d.get("abort"):
        print(f"  ABORT: {d['abort']}")
        gradual_stop()
        return

    speak = d.get("speak", "")
    actions = merge_actions(d.get("actions", []))
    arm_cmd = d.get("arm", None)

    print(f"Marcus: {speak}")

    if not actions:
        gradual_stop()
    else:
        for i, action in enumerate(actions):
            move = action.get("move")
            dur = float(action.get("duration", 2.0))
            print(f"  Step {i+1}/{len(actions)}: {move} for {dur}s")
            execute_action(move, dur)

    if arm_cmd:
        print(f"  Arm: {arm_cmd}")
        threading.Thread(target=do_arm, args=(arm_cmd,), daemon=True).start()

    return speak