#!/usr/bin/env bash

function usage() {
  echo "Usage: rosrun [--prefix cmd] [--debug] PACKAGE EXECUTABLE [ARGS]"
  echo "  rosrun will locate PACKAGE and try to find"
  echo "  an executable named EXECUTABLE in the PACKAGE tree."
  echo "  If it finds it, it will run it with ARGS."
}

args=1
rosrun_prefix=""

function debug() {
  return
}

while [ $args == 1 ]
do
  case "$1" in
    "--help" | "-h")
      usage
      exit 0
      ;;
    "--prefix" | "-p")
      rosrun_prefix="$2"
      shift
      shift
      ;;
    "--debug" | "-d")
      shift
      function debug() { echo "[rosrun]" "$@" ; }
      ;;
    *) # default
      args=0
  esac
done

if [ $# -lt 2 ]; then
  usage
  exit 0
fi

# early check that filename does not end with '/'
case $2 in
  */) echo "Invalid filename: " $2
    exit 1
    ;;
esac

if [[ -n $CMAKE_PREFIX_PATH ]]; then
  IFS=$'\n'
  catkin_package_libexec_dirs=($(catkin_find --without-underlays --libexec --share "$1" 2> /dev/null))
  unset IFS
  debug "Looking in catkin libexec dirs: ${catkin_package_libexec_dirs[*]}"
fi
pkgdir=$(rospack find "$1")
debug "Looking in rospack dir: $pkgdir"
if [[ ${#catkin_package_libexec_dirs[@]} -eq 0 && -z $pkgdir ]]; then
  exit 2
fi
if [[ ! $2 == */* ]]; then
  # The -perm /mode usage is not available in find on the Mac
  #exepathlist=(`find $pkgdir -name $2 -type f -perm /u+x,g+x,o+x`)
  # -L: #3475
  if [[ $(uname) == Darwin ]]; then
    _perm="+111"
  else
    _perm="/111"
  fi
  debug "Searching for $2 with permissions $_perm"
  exepathlist="$(find -L "${catkin_package_libexec_dirs[@]}" "$pkgdir" -name "$2" -type f  -perm "$_perm" ! -regex ".*$pkgdir\/build\/.*" | uniq)"
  IFS=$'\n'
  exepathlist=($exepathlist)
  unset IFS
  unset _perm
  if [[ ${#exepathlist[@]} == 0 ]]; then
    echo "[rosrun] Couldn't find executable named $2 below $pkgdir"
    nonexepathlist=($(find -H "$pkgdir" -name "$2"))
    if [[ ${#nonexepathlist[@]} != 0 ]]; then
      echo "[rosrun] Found the following, but they're either not files,"
      echo "[rosrun] or not executable:"
      for p in "${nonexepathlist[@]}"; do
        echo "[rosrun]   ${p}"
      done
    fi
    exit 3

  elif [[ ${#exepathlist[@]} -gt 1 ]]; then
    echo "[rosrun] You have chosen a non-unique executable, please pick one of the following:"
    select opt in "${exepathlist[@]}"; do
      exepath="$opt"
      break
    done
  else
    exepath="${exepathlist[0]}"
  fi
else
  absname="$pkgdir/$2"
  debug "Path given. Looing for $absname"
  if [[ ! -f $absname || ! -x $absname ]]; then
    echo "[rosrun] Couldn't find executable named $absname"
    exit 3
  fi
  exepath="$absname"
fi
shift
shift
debug "Running $rosrun_prefix $exepath" "$@"
exec $rosrun_prefix "$exepath" "$@"
