#!/usr/bin/env bash

# 1-Click Launch Script for C++ 4WD Motor Control with Mid-360S LiDAR Avoidance
# 현재 고정 배선: 모터 드라이버 = ttyUSB1, RC 수신기(XR1) = ttyUSB0
PORT="${1:-/dev/ttyUSB1}"
RC_PORT="${RC_PORT:-/dev/ttyUSB0}"
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 수신기 포트 둘 다)
if [ -f "./set_low_latency.sh" ]; then
    echo "[1/2] USB 시리얼 포트 지연 시간 1ms 최적화 적용 중... (모터: $PORT / RC: $RC_PORT)"
    ./set_low_latency.sh "$PORT"
    ./set_low_latency.sh "$RC_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) + 라이다 장애물 회피 시작"
echo "  - 모터 포트: $PORT / RC 수신기 포트: $RC_PORT"
echo "  - 라이다 기능 비활성화: --no_lidar 옵션"
echo "  - 비상 정지: CH5 브레이크 스위치 또는 Ctrl+C"
echo "=================================================================="

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