"""Launch full teleop with webcam (no image server, with hand drivers).

Replicates start_teleop.sh but skips the image server that kills the UVC driver.
"""
import paramiko
import sys
import time

sys.stdout.reconfigure(encoding='utf-8', errors='replace')

ssh = paramiko.SSHClient()
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
ssh.connect('10.0.0.64', username='unitree', password='123',
            timeout=15, look_for_keys=False, allow_agent=False)

# ── Step 0: Kill any existing teleop / hand driver processes ──
print("Cleaning up old processes...")
_, o, _ = ssh.exec_command(
    'ps aux | grep -E "teleop_hand_and_arm|ModbusDataHandler|image_server" | grep -v grep',
    timeout=5)
procs = o.read().decode().strip()
if procs:
    print(f"  Found:\n{procs}")
    for line in procs.split('\n'):
        parts = line.split()
        if len(parts) > 1:
            ssh.exec_command(f'kill {parts[1]}', timeout=5)
    time.sleep(1)
    print("  Killed stale processes")
else:
    print("  No stale processes")

# ── Step 1: Start Inspire hand Modbus-to-DDS drivers ──
print("\nStarting Inspire hand drivers...")

TELEOP_DIR = "/home/unitree/xr_teleoperate/teleop"
CYCLONE_HOME = "/home/unitree/g1-control/repos/cyclonedds/install/cyclonedds"
CYCLONE_URI = "file:///home/unitree/g1-control/repos/cyclonedds/cyclonedds.xml"
PYTHON = "/home/unitree/miniforge3/envs/tv/bin/python"

env_prefix = (
    f'export CYCLONEDDS_HOME="{CYCLONE_HOME}" && '
    f'export CYCLONEDDS_URI="{CYCLONE_URI}" && '
)

# Right hand (also initializes DDS)
right_hand_cmd = (
    f'{env_prefix} '
    f'cd {TELEOP_DIR} && '
    f'nohup {PYTHON} -m inspire_hand.ModbusDataHandler '
    f'--ip 192.168.123.211 --initDDS True > /tmp/right_hand.log 2>&1 &'
)
ssh.exec_command(right_hand_cmd, timeout=10)
print("  Right hand driver started (192.168.123.211)")
time.sleep(2)

# Left hand (check reachability first)
_, o, _ = ssh.exec_command('ping -c 1 -W 1 192.168.123.210 2>&1 | grep "1 received"', timeout=5)
left_reachable = bool(o.read().decode().strip())
if left_reachable:
    left_hand_cmd = (
        f'{env_prefix} '
        f'cd {TELEOP_DIR} && '
        f'nohup {PYTHON} -m inspire_hand.ModbusDataHandler '
        f'--ip 192.168.123.210 > /tmp/left_hand.log 2>&1 &'
    )
    ssh.exec_command(left_hand_cmd, timeout=10)
    print("  Left hand driver started (192.168.123.210)")
else:
    print("  Left hand not reachable (skipped)")

time.sleep(2)

# ── Step 2: Launch teleop with webcam ──
print("\nStarting teleop with webcam...")
print("=" * 60)

teleop_cmd = (
    f'{env_prefix} '
    f'cd {TELEOP_DIR} && '
    f'{PYTHON} teleop_hand_and_arm.py '
    f'--arm=G1_29 --ee=inspire_ftp --webcam 0 --webcam-res 720p'
)

stdin, stdout, stderr = ssh.exec_command(teleop_cmd, timeout=300, get_pty=True)

# Stream output in real-time
start_time = time.time()
while time.time() - start_time < 120:
    if stdout.channel.recv_ready():
        data = stdout.channel.recv(4096).decode('utf-8', errors='replace')
        print(data, end='')
    if stderr.channel.recv_stderr_ready():
        data = stderr.channel.recv_stderr(4096).decode('utf-8', errors='replace')
        print(f"[err] {data}", end='')
    if stdout.channel.exit_status_ready():
        remaining = stdout.channel.recv(65536).decode('utf-8', errors='replace')
        if remaining:
            print(remaining, end='')
        remaining_err = stderr.channel.recv_stderr(65536).decode('utf-8', errors='replace')
        if remaining_err:
            print(f"[err] {remaining_err}", end='')
        exit_code = stdout.channel.recv_exit_status()
        print(f"\n\nProcess exited with code {exit_code}")
        break
    time.sleep(0.1)
else:
    print("\n\nTeleop is running! (2 min output window elapsed)")
    print("Connect Quest 3 to: https://10.0.0.64:8012")

ssh.close()