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

repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
expected_omnisocket_commit="de3f5c96779dbe1571c10feb22fc7f2331b6b222"

for executable in \
  "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
  "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
  "$repo_dir/tg3_local_teleop/data_recorder_node.py" \
  "$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
  "$repo_dir/tg3_local_teleop/run.sh" \
  "$repo_dir/tg3_local_teleop/run_data_recorder.sh" \
  "$repo_dir/tg3_local_teleop/wait_ros_ready.sh" \
  "$repo_dir/tg3_local_teleop/home.sh" \
  "$repo_dir/tg3_local_teleop/status.sh" \
  "$repo_dir/tg3_data_collection/data_get_sync.py" \
  "$repo_dir/tg3_data_collection/decode_joint_data.py"; do
  if [[ ! -x "$executable" ]]; then
    echo "Required executable bit is missing: $executable" >&2
    exit 1
  fi
done

python3 -m py_compile \
  "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
  "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
  "$repo_dir/tg3_local_teleop/gesture_toggle.py" \
  "$repo_dir/tg3_local_teleop/head_control.py" \
  "$repo_dir/tg3_local_teleop/data_collection.py" \
  "$repo_dir/tg3_local_teleop/data_recorder_protocol.py" \
  "$repo_dir/tg3_local_teleop/data_recorder_node.py" \
  "$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
  "$repo_dir/tg3_data_collection/data_get_sync.py" \
  "$repo_dir/tg3_data_collection/decode_joint_data.py"
python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py"
python3 "$repo_dir/tg3_local_teleop/test_session_gate.py"
python3 "$repo_dir/tg3_local_teleop/test_gesture_toggle.py"
python3 "$repo_dir/tg3_local_teleop/test_head_control.py"
python3 "$repo_dir/tg3_local_teleop/test_idle_session_refresh.py"
python3 "$repo_dir/tg3_local_teleop/test_data_collection.py"
python3 "$repo_dir/tg3_local_teleop/test_data_recorder_protocol.py"
python3 "$repo_dir/tg3_local_teleop/test_delete_ready_episode.py"
python3 "$repo_dir/tg3_data_collection/test_data_get_sync.py"
python3 - "$repo_dir/tg3_local_teleop/config.toml" <<'PY'
import sys
import tomllib

with open(sys.argv[1], "rb") as stream:
    document = tomllib.load(stream)
section = document["data_collection"]
topics = set(section["topics"])
required = set(section["required_topics"])
head_rgbd = {
    "/ob_camera_head/color/image_raw/compressed",
    "/ob_camera_head/color/camera_info",
    "/ob_camera_head/color/metadata",
    "/ob_camera_head/depth/image_raw/compressedDepth",
    "/ob_camera_head/depth/camera_info",
    "/ob_camera_head/depth/metadata",
}
assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist"
assert head_rgbd.isdisjoint(required), "head RGB-D must not block recording"
assert not any(topic.startswith("/ob_camera_waist/") for topic in topics), (
    "waist camera topics must stay out of the recording whitelist"
)
assert "/tf_static" in topics and "/tf_static" not in required
assert section.get("minimum_topic_rates_hz", {}) == {}
groups = section["optional_topic_groups"]
assert set(groups) == {"head_rgbd"}
assert set(groups["head_rgbd"]["topics"]) == head_rgbd
assert groups["head_rgbd"]["minimum_topic_rates_hz"] == {
    "/ob_camera_head/color/image_raw/compressed": 20.0,
    "/ob_camera_head/depth/image_raw/compressedDepth": 20.0,
}
assert section["minimum_free_gib"] >= 100.0
assert section["retain_failed_episodes"] is False
head = document["head"]
assert head["enabled"] is True
assert head["command_topic"] == "/head/cmd"
assert head["state_topic"] == "/robot_state"
assert head["pitch_motor_id"] == 2
assert head["mode"] == 0
assert 0.0 < head["max_current_a"] <= 1.0
assert -0.2617994 <= head["min_pitch_rad"] < head["max_pitch_rad"] <= 1.012291
assert 0.0 < head["max_pitch_speed_rad_s"] <= 0.2
assert 0.0 < head["max_pitch_accel_rad_s2"] <= 0.5
assert 0.0 <= head["joystick_deadzone"] < 1.0
assert "/head/cmd" in topics and "/head/cmd" not in required
print("config.toml parse passed")
PY

if [[ -n "${OMNISOCKETGO_DIR:-}" ]]; then
  actual_omnisocket_commit="$(git -C "$OMNISOCKETGO_DIR" rev-parse HEAD)"
  if [[ "$actual_omnisocket_commit" != "$expected_omnisocket_commit" ]]; then
    echo "OmniSocketGo commit mismatch: $actual_omnisocket_commit" >&2
    exit 1
  fi
  echo "OmniSocketGo commit verified: $actual_omnisocket_commit"
else
  echo "OmniSocketGo is external and was not checked; expected commit: $expected_omnisocket_commit"
fi

echo "All offline TG3 checks passed"