#!/usr/bin/env bash
# Launch SO101 teleoperation with this machine's calibrated arms.
set -eo pipefail

script_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
source "$script_dir/../env/activate_lerobot.sh"
set -u

check_device() {
  if [[ ! -e "$1" ]]; then
    echo "设备未连接：$1" >&2
    exit 1
  fi
  if [[ ! -r "$1" || ! -w "$1" ]]; then
    echo "设备缺少读写权限：$1，请检查串口 dialout / 摄像头 video 权限。" >&2
    exit 1
  fi
}
check_device "$FOLLOWER_PORT"
check_device "$LEADER_PORT"
check_device "/dev/v4l/by-id/usb-ZSKJ-260319-K_Integrated_Webcam_HD-video-index0"
check_device "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_CAM1_USB2.0_CAM1-video-index0"

echo "启动遥操作；按 Ctrl+C 停止。"
exec lerobot-teleoperate \
  --robot.type=so101_follower \
  --robot.port="$FOLLOWER_PORT" \
  --robot.id=zihao_follower_arm \
  --robot.cameras='{"wrist": {"type": "opencv", "index_or_path": "/dev/v4l/by-id/usb-ZSKJ-260319-K_Integrated_Webcam_HD-video-index0", "width": 640, "height": 480, "fps": 30, "fourcc": "MJPG"}, "front": {"type": "opencv", "index_or_path": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_CAM1_USB2.0_CAM1-video-index0", "width": 640, "height": 480, "fps": 25, "fourcc": "MJPG"}}' \
  --teleop.type=so101_leader \
  --teleop.port="$LEADER_PORT" \
  --teleop.id=zihao_leader_arm \
  --display_data=true \
  "$@"