"""Launch teleop server from correct directory + ensure cert server running."""
import paramiko, sys, 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)

# Kill any existing teleop screen
ssh.exec_command('screen -S teleop -X quit 2>/dev/null; sleep 1', timeout=5)
time.sleep(2)

# Ensure cert server is running on 9090
_, o, _ = ssh.exec_command('ss -tlnp | grep 9090', timeout=5)
if not o.read().decode().strip():
    print('Starting cert server on port 9090...')
    ssh.exec_command(
        'cd ~/.config/xr_teleoperate && nohup python3 -m http.server 9090 --bind 0.0.0.0 > /dev/null 2>&1 &',
        timeout=5)
    time.sleep(2)
else:
    print('Cert server already running on 9090')

# Launch teleop from ~/xr_teleoperate/teleop (correct dir for URDF paths)
cmd = (
    'screen -dmS teleop bash -c "'
    'source ~/miniforge3/etc/profile.d/conda.sh && conda activate tv && '
    'cd ~/xr_teleoperate/teleop && '
    'python3 teleop_hand_and_arm.py --arm=G1_29 --input-mode hand --motion --display-mode pass-through '
    '2>&1 | tee /tmp/teleop_v60.log'
    '"'
)
print('Starting teleop server...')
ssh.exec_command(cmd, timeout=10)
time.sleep(1)

# Wait for server to start
for i in range(10):
    time.sleep(2)
    _, o, _ = ssh.exec_command('ss -tlnp | grep 8012', timeout=5)
    port = o.read().decode().strip()
    if port:
        print(f'Server listening on port 8012! ({(i+1)*2}s)')
        break
    # Check if it crashed
    _, o, _ = ssh.exec_command('tail -3 /tmp/teleop_v60.log', timeout=5)
    log = o.read().decode('utf-8', errors='replace').strip()
    if 'exiting program' in log.lower() or 'error' in log.lower():
        print(f'Server crashed! Log:')
        _, o, _ = ssh.exec_command('tail -20 /tmp/teleop_v60.log', timeout=5)
        print(o.read().decode('utf-8', errors='replace'))
        ssh.close()
        sys.exit(1)
    print(f'  waiting... ({(i+1)*2}s)')
else:
    print('Server did not start in 20s. Log:')
    _, o, _ = ssh.exec_command('tail -20 /tmp/teleop_v60.log', timeout=5)
    print(o.read().decode('utf-8', errors='replace'))
    ssh.close()
    sys.exit(1)

# Show final state
_, o, _ = ssh.exec_command('tail -5 /tmp/teleop_v60.log', timeout=5)
print('\nLog (last 5 lines):')
print(o.read().decode('utf-8', errors='replace'))

# Verify cert server accessible
_, o, _ = ssh.exec_command('curl -s http://localhost:9090/rootCA.pem | head -1', timeout=5)
cert = o.read().decode().strip()
print(f'\nCert server: {"OK" if "BEGIN" in cert else "NOT WORKING"}')

print('\n=== READY ===')
print('Cert install:  http://10.0.0.64:9090/rootCA.pem')
print('Teleop page:   https://10.0.0.64:8012')

ssh.close()