#!/usr/bin/env bash
set -Eeuo pipefail

# ============================================
# 配置项
# ============================================
PROJECT_DIR="${PROJECT_DIR:-$HOME/release/ctag2f-90d-sdk}"
WARMUP_DIR="${WARMUP_DIR:-$HOME/release/orisys-sdk}"
CONDA_ENV="${CONDA_ENV:-py311}"
RENDER_MODE="${RENDER_MODE:-auto}"
GRIPPER_CONFIG="${GRIPPER_CONFIG:-$PROJECT_DIR/config/gripper.json}"

# ============================================
# 检测 USB 摄像头并返回主设备索引（整数）
# ============================================
detect_cameras() {
    declare -A cam_nodes
    declare -A cam_paths

    for dev in /dev/video*; do
        [[ -e "$dev" ]] || continue
        local dev_num="${dev##*/video}"
        local info
        info=$(udevadm info -n "$dev" 2>/dev/null || true)

        # 检查是否为 USB 摄像头（根据 ID_MODEL 或 ID_VENDOR）
        if echo "$info" | grep -qi "ID_MODEL.*USB Camera" || \
           echo "$info" | grep -qi "ID_VENDOR.*1bcf" ; then
            local id_path
            id_path=$(echo "$info" | grep -i "ID_PATH=" | cut -d'=' -f2 || true)
            if [[ -z "$id_path" ]]; then
                id_path=$(udevadm info -q path -n "$dev" 2>/dev/null || true)
            fi
            local key="$id_path"
            if [[ -z "${cam_nodes[$key]:-}" ]]; then
                cam_nodes[$key]="$dev_num"
            else
                local current="${cam_nodes[$key]}"
                if (( dev_num < current )); then
                    cam_nodes[$key]="$dev_num"
                fi
            fi
            cam_paths[$key]="$id_path"
        fi
    done

    if [[ ${#cam_nodes[@]} -lt 2 ]]; then
        echo "错误：找到 ${#cam_nodes[@]} 个 USB 摄像头，需要 2 个。" >&2
        return 1
    fi

    local cam_left=""
    local cam_right=""
    for key in "${!cam_nodes[@]}"; do
        local path="$key"
        local num="${cam_nodes[$key]}"
        if [[ "$path" == *"xhci-hcd.0"* ]] || [[ "$path" == *"usb1"* ]]; then
            cam_left="$num"
        elif [[ "$path" == *"xhci-hcd.1"* ]] || [[ "$path" == *"usb2"* ]]; then
            cam_right="$num"
        else
            if [[ -z "$cam_left" ]] || (( num < cam_left )); then
                cam_right="$cam_left"
                cam_left="$num"
            else
                cam_right="$num"
            fi
        fi
    done

    if [[ -z "$cam_left" ]] || [[ -z "$cam_right" ]]; then
        local nums=()
        for n in "${cam_nodes[@]}"; do nums+=("$n"); done
        IFS=$'\n' sorted=($(sort -n <<<"${nums[*]}"))
        cam_left="${sorted[0]}"
        cam_right="${sorted[1]}"
    fi

    echo "$cam_left $cam_right"
}

# ============================================
# 更新 gripper.json 中的 video 路径
# ============================================
update_config() {
    local cam_right_idx="$1"
    local cam_left_idx="$2"
    local config_file="$GRIPPER_CONFIG"

    # 检查 jq 是否安装
    if ! command -v jq &> /dev/null; then
        echo "jq 未安装，正在安装..."
        sudo apt update && sudo apt install -y jq
    fi

    # 构建设备路径
    local video1="/dev/video$cam_left_idx"
    local video2="/dev/video$cam_right_idx"

    echo "更新配置文件: $config_file"
    echo "  video1 = $video1"
    echo "  video2 = $video2"

    # 使用 jq 更新字段
    jq --arg v1 "$video1" --arg v2 "$video2" \
        '.video1 = $v1 | .video2 = $v2' "$config_file" > "$config_file.tmp"
    mv "$config_file.tmp" "$config_file"
    echo "✅ 配置文件已更新。"
}

# ============================================
# 加载 conda 环境
# ============================================
load_conda() {
    for conda_sh in \
        "$HOME/miniconda3/etc/profile.d/conda.sh" \
        "$HOME/anaconda3/etc/profile.d/conda.sh" \
        "/opt/conda/etc/profile.d/conda.sh"; do
        if [[ -f "$conda_sh" ]]; then
            # shellcheck disable=SC1090
            source "$conda_sh"
            break
        fi
    done

    if ! command -v conda >/dev/null 2>&1; then
        echo "Conda not found." >&2
        exit 1
    fi

    conda activate "$CONDA_ENV"
    echo "✅ Conda environment '$CONDA_ENV' activated."
}

# ============================================
# 主逻辑
# ============================================
main() {
    # 检查项目目录
    if [[ ! -d "$PROJECT_DIR" ]]; then
        echo "错误：项目目录不存在: $PROJECT_DIR" >&2
        exit 1
    fi
    if [[ ! -d "$WARMUP_DIR" ]]; then
        echo "警告：warmup 目录不存在: $WARMUP_DIR，跳过预热。" >&2
        WARMUP_DIR=""
    fi

    load_conda

    # 移除不兼容的 PyOpenGL-accelerate
    if python -c 'import OpenGL_accelerate' >/dev/null 2>&1; then
        echo "移除不兼容的 PyOpenGL-accelerate ..."
        python -m pip uninstall -y PyOpenGL-accelerate
    fi

    # 检测摄像头
    echo "正在检测 USB 摄像头..."
    CAMERAS=$(detect_cameras)
    if [[ -z "$CAMERAS" ]]; then
        echo "错误：无法检测到两个 USB 摄像头。" >&2
        exit 1
    fi
    CAM_LEFT=$(echo "$CAMERAS" | awk '{print $1}')
    CAM_RIGHT=$(echo "$CAMERAS" | awk '{print $2}')
    echo "左摄像头索引: $CAM_LEFT"
    echo "右摄像头索引: $CAM_RIGHT"

    # --- 更新配置文件 ---
    update_config "$CAM_LEFT" "$CAM_RIGHT"

    # ---------- 执行 warmup ----------
    if [[ -n "$WARMUP_DIR" ]]; then
        echo "============================================="
        echo "🧪 开始传感器预热 (sensor warmup)"
        echo "   目录: $WARMUP_DIR"
        echo "============================================="
        cd "$WARMUP_DIR"

        echo ">>> 执行: python warmup.py -v $CAM_LEFT"
        python warmup.py -v "$CAM_LEFT"
        echo "✅ warmup 左摄像头完成"

        echo ">>> 执行: python warmup.py -v $CAM_RIGHT"
        python warmup.py -v "$CAM_RIGHT"
        echo "✅ warmup 右摄像头完成"

        cd "$PROJECT_DIR"
        echo "============================================="
        echo "✅ 传感器预热全部完成"
        echo "============================================="
    fi

    # ---------- 设置渲染模式 ----------
    if [[ "$RENDER_MODE" == "auto" ]]; then
        wayland_plugin="$(find "${CONDA_PREFIX:-/nonexistent}" -type f \
            -path '*/platforms/libqwayland*.so' -print -quit 2>/dev/null || true)"
        if [[ -n "$wayland_plugin" && -n "${WAYLAND_DISPLAY:-}" ]]; then
            RENDER_MODE=wayland
        else
            echo "Conda Qt 没有 Wayland 插件；使用 XWayland 和软件 OpenGL。"
            RENDER_MODE=software
        fi
    fi

    case "$RENDER_MODE" in
        wayland)
            if [[ -z "${WAYLAND_DISPLAY:-}" ]]; then
                echo "WAYLAND_DISPLAY 为空。请在 Ubuntu 桌面终端中运行此脚本。" >&2
                exit 1
            fi
            export QT_QPA_PLATFORM=wayland
            export PYOPENGL_PLATFORM=egl
            unset QT_OPENGL LIBGL_ALWAYS_SOFTWARE QT_XCB_GL_INTEGRATION
            ;;
        software)
            export QT_QPA_PLATFORM=xcb
            export QT_OPENGL=software
            export LIBGL_ALWAYS_SOFTWARE=1
            export PYOPENGL_PLATFORM=glx
            export QT_XCB_GL_INTEGRATION=xcb_glx
            ;;
        xcb)
            export QT_QPA_PLATFORM=xcb
            export PYOPENGL_PLATFORM=glx
            export QT_XCB_GL_INTEGRATION=xcb_glx
            unset QT_OPENGL LIBGL_ALWAYS_SOFTWARE
            ;;
        *)
            echo "未知的 RENDER_MODE: $RENDER_MODE (可用: auto, wayland, software, xcb)" >&2
            exit 2
            ;;
    esac

    # ---------- 启动主程序 ----------
    echo "🚀 启动 gripper visualizer: mode=$RENDER_MODE, conda=$CONDA_ENV"
    exec python -m gripper.visualizer "$@"
}

# ============================================
# 执行主函数
# ============================================
main "$@"