#!/bin/bash

# -----------------------------------------------------------------
# 1. 自动识别路径
# -----------------------------------------------------------------
REPO_PATH=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
echo "检测到仓库根目录: $REPO_PATH"

ROS1_WS_PATH="${REPO_PATH}/ros1_ws/dog_msgs_ws"
ROS2_WS_PATH="${REPO_PATH}/ros2_ws"

if [ ! -d "$ROS2_WS_PATH" ]; then
    echo "错误: 找不到 ros2_ws 目录。请确保脚本位于 ledog_ros2 根目录下。"
    exit 1
fi

# -----------------------------------------------------------------
# 2. 自动识别 Shell 类型
# -----------------------------------------------------------------
CURRENT_SHELL=$(basename "$SHELL")
if [[ "$CURRENT_SHELL" == "zsh" ]]; then
    SETUP_EXT="zsh"
    echo "检测到当前 Shell 为 zsh，将使用 setup.zsh"
else
    SETUP_EXT="bash"
    echo "检测到当前 Shell 为 bash (或未知)，将使用 setup.bash"
fi

# 定义会话名称
SESSION_NAME="ledog"

# 检查tmux是否已安装
if ! command -v tmux &> /dev/null
then
    echo "错误: tmux 未安装。请先运行 'sudo dnf install tmux'"
    exit 1
fi

# 检查会话是否已存在，如果存在则先杀死它
tmux has-session -t $SESSION_NAME 2>/dev/null
if [ $? = 0 ]; then
    echo "检测到已存在的会话，正在关闭..."
    tmux kill-session -t $SESSION_NAME
fi

echo "正在启动新的 tmux 会话: $SESSION_NAME (2x2 网格)"

# -----------------------------------------------------------------
# 第一步：构建 2x2 网格布局
# -----------------------------------------------------------------
# 1. 创建会话 (只有 Pane 0)
tmux new-session -d -s $SESSION_NAME -n "RobotControl" -c $REPO_PATH

# 2. 连续拆分出 3 个新窗格 (现在总共4个)
tmux split-window -t $SESSION_NAME:0 -c $REPO_PATH
tmux split-window -t $SESSION_NAME:0 -c $REPO_PATH
tmux split-window -t $SESSION_NAME:0 -c $REPO_PATH

# 将 4 个窗格强制排列成 2x2，并重置编号顺序为 Z 字形：
# 0(左上)  1(右上)
# 2(左下)  3(右下)
tmux select-layout -t $SESSION_NAME:0 tiled

# -----------------------------------------------------------------
# 第二步：向指定物理位置发送命令
# -----------------------------------------------------------------

# --- [Pane 0: 左上] 硬件接口 ---
tmux send-keys -t $SESSION_NAME:0.0 "source ${ROS2_WS_PATH}/install/setup.${SETUP_EXT}" C-m
tmux send-keys -t $SESSION_NAME:0.0 "echo '>>> 启动硬件接口...'" C-m
tmux send-keys -t $SESSION_NAME:0.0 "ros2 launch so101_hw_interface so101_hw.launch.py" C-m

# --- [Pane 1: 右上] 手柄控制器 ---
tmux send-keys -t $SESSION_NAME:0.1 "source ${ROS2_WS_PATH}/install/setup.${SETUP_EXT}" C-m
tmux send-keys -t $SESSION_NAME:0.1 "echo '>>> 启动手柄控制器...'" C-m
tmux send-keys -t $SESSION_NAME:0.1 "ros2 launch joystick_alphadog_with_so101_servo unified_control.launch.py use_sim:=False" C-m

# --- [Pane 2: 左下] MoveIt ---
tmux send-keys -t $SESSION_NAME:0.2 "source ${ROS2_WS_PATH}/install/setup.${SETUP_EXT}" C-m
tmux send-keys -t $SESSION_NAME:0.2 "echo '>>> 启动 MoveIt...'" C-m
tmux send-keys -t $SESSION_NAME:0.2 "sleep 3" C-m
tmux send-keys -t $SESSION_NAME:0.2 "ros2 launch lerobot_moveit so101_moveit.launch.py display:=False" C-m

# --- [Pane 3: 右下] 网桥 ---
tmux send-keys -t $SESSION_NAME:0.3 "echo '>>> 启动 ROS 1 网桥...'" C-m
tmux send-keys -t $SESSION_NAME:0.3 "sleep 5" C-m
# 设置混合环境
tmux send-keys -t $SESSION_NAME:0.3 "source ${ROS1_WS_PATH}/devel/setup.${SETUP_EXT}" C-m
tmux send-keys -t $SESSION_NAME:0.3 "source ${ROS2_WS_PATH}/install/setup.${SETUP_EXT}" C-m
tmux send-keys -t $SESSION_NAME:0.3 "ros2 launch joystick_alphadog_with_so101_servo start_static_bridge.launch.py" C-m

# 聚焦回左上角
tmux select-pane -t $SESSION_NAME:0.0

echo "所有节点已在 tmux 会话 '$SESSION_NAME' 中启动。"
echo "请运行 'tmux attach -t $SESSION_NAME' 来查看。"