#!/usr/bin/env bash

# 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
# udev 규칙(/etc/udev/rules.d/99-fori-robot-serial.rules)으로 고정된 심볼릭
# 링크 사용 — ttyUSB 번호는 꽂는 순서에 따라 바뀌지만 이 이름들은 고정이다.
PORT="${1:-/dev/ttyMOTOR}"
RC_PORT="${RC_PORT:-/dev/ttyRC}"
IMU_PORT="${IMU_PORT:-/dev/ttyIMU}"
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

cd "$SCRIPT_DIR" || exit 1

echo "=================================================================="
echo " 🚀 ZLAC8015D 4WD + Livox Mid-360S 라이다 원클릭 런치 시스템"
echo "=================================================================="

# 1. USB Latency 1ms 단축 (모터/RC/IMU 포트 전부)
if [ -f "./set_low_latency.sh" ]; then
    echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT / IMU: $IMU_PORT)"
    ./set_low_latency.sh "$PORT"
    ./set_low_latency.sh "$RC_PORT"
    ./set_low_latency.sh "$IMU_PORT"
fi

# 2. C++ 바이너리 존재 여부 확인 및 컴파일
if [ ! -f "./xbox_motor_control_cpp" ]; then
    echo "[2/2] C++ 바이너리 컴파일 진행 중 (make)..."
    make
fi

echo "=================================================================="
echo " [시작] C++ 100Hz 초저지연 4WD RC(RadioMaster Pocket + XR1) + IMU(HWT905) + 라이다 장애물 회피 시작"
echo "  - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT / IMU 포트: $IMU_PORT"
echo "  - 라이다 기능 비활성화: --no_lidar 옵션 / IMU 비활성화: --no_imu 옵션"
echo "  - 매 주행마다 logs/에 CSV 로그 자동 저장 (끄려면 --no_log)"
echo "  - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C"
echo "=================================================================="

# 3. C++ 4WD 프로그램 실행 (추가 인자 전달 가능. --rc_port/--imu_port를 다시
# 넘기면 아래 기본값을 덮어쓸 수 있다 — 인자 파싱은 뒤에 온 값이 우선 적용됨)
if [ $# -gt 1 ]; then
    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT" "${@:2}"
else
    ./xbox_motor_control_cpp --port "$PORT" --rc_port "$RC_PORT" --imu_port "$IMU_PORT"
fi