#!/usr/bin/env bash
# Run OpenVLA-Micro on the Nova Carter diff-drive robot.
# Starts: Kinect driver → VLA inference → instruction CLI
set -e

SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
source /opt/ros/lyrical/setup.bash

# Kill existing pipeline
echo 1121 | sudo -S pkill -9 -f kinect_driver     2>/dev/null || true
echo 1121 | sudo -S pkill -9 -f vla_micro_ros2_node 2>/dev/null || true
echo 1121 | sudo -S pkill -9 -f vla_waypoint_follower 2>/dev/null || true
echo 1121 | sudo -S pkill -9 -f instruction_cli    2>/dev/null || true
echo 1121 | sudo -S pkill -9 -f freenect            2>/dev/null || true
sleep 2

echo "Starting OpenVLA-Micro pipeline..."

python3 /home/robotai/kinect_driver.py &
PID_KINECT=$!
sleep 3

python3 "$SCRIPT_DIR/vla_micro_ros2_node.py" &
PID_VLA=$!
sleep 5

python3 /home/robotai/instruction_cli.py &
PID_CLI=$!

trap "echo 1121 | sudo -S kill $PID_KINECT $PID_VLA $PID_CLI 2>/dev/null; echo Stopped" EXIT INT TERM
echo "All nodes running. Instructions via instruction_cli."
wait