#!/usr/bin/ruby

# Copyright (C) 2019 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# We use 'dl' for Ruby <= 1.9.x and 'fiddle' for Ruby >= 2.0.x
if RUBY_VERSION.split('.')[0] < '2'
  require 'dl'
  require 'dl/import'
  include DL
else
  require 'fiddle'
  require 'fiddle/import'
  include Fiddle
end

require 'optparse'
require 'erb'

# Constants.
LIBRARY_NAME = '@library_location@'
LIBRARY_VERSION = '@PROJECT_VERSION_FULL@'

COMMON_OPTIONS =
               "  -h [--help]                Print this help message.\n"\
               "                                                    \n"        +
               "  --force-version <VERSION>  Use a specific library version.\n"\
               "                                                    \n"        +
               '  --versions                 Show the available versions.'

COMMANDS = { 'gazebo' =>
  "Run and manage Gazebo simulations.                                              \n"\
  "                                                                                \n"\
  "  ign gazebo [options] [file]                                                   \n"\
  "                                                                                \n"\
  "                                                                                \n"\
  "Available Options:                                                              \n"\
  "  -g                           Run only the GUI.                                \n"\
  "\n"\
  "  --iterations [arg]           Number of iterations to execute.                 \n"\
  "\n"\
  "  --levels                     Use the level system. The default is false,      \n"\
  "                               which loads all models. It's always true         \n"\
  "                               with --network-role.                             \n"\
  "\n"\
  "  --network-role [arg]         Participant role used in a distributed           \n"\
  "                               simulation environment. Role is one of           \n"\
  "                               [primary, secondary]. It implies --levels.       \n"\
  "\n"\
  "  --network-secondaries [arg]  Number of secondary participants expected        \n"\
  "                               to join a distributed simulation                 \n"\
  "                               environment. (Primary only).                     \n"\
  "\n"\
  "  --record                     Use logging system to record states and          \n"\
  "                               console messages to the default location,        \n"\
  "                               in ~/.ignition/gazebo/log.                       \n"\
  "\n"\
  "  --record-path [arg]          Implicitly invokes --record, and specifies       \n"\
  "                               custom path to put recorded files. Argument      \n"\
  "                               is path to record states and console             \n"\
  "                               messages. Specifying this argument will          \n"\
  "                               enable console logging to a console.log          \n"\
  "                               file in the specified path.                      \n"\
  "\n"\
  "  --record-resources           Implicitly invokes --record, and records         \n"\
  "                               meshes and material files, in addition to        \n"\
  "                               states and console messages.                     \n"\
  "\n"\
  "  --record-topic [arg]         Specify the name of an additional topic to       \n"\
  "                               record. Implicitly invokes --record.             \n"\
  "                               Zero or more topics can be specified by          \n"\
  "                               using multiple --record-topic options.           \n"\
  "                               Regular expressions can be used, which           \n"\
  "                               likely requires quotes. A default set of         \n"\
  "                               topics are also recorded, which support          \n"\
  "                               simulation state playback. Enable debug          \n"\
  "                               console output with the -v 4 option              \n"\
  "                               and look for 'Recording default topic' in        \n"\
  "                               order to determine the default set of            \n"\
  "                               topics.                                          \n"\
  "                               Examples:                                        \n"\
  "                                 1. Record all topics.                          \n"\
  "                                     --record-topic \".*\"                      \n"\
  "                                 2. Record only the /stats topic.               \n"\
  "                                     --record-topic /stats                      \n"\
  "                                 3. Record the /stats and /clock topics.        \n"\
  "                                     --record-topic /stats \                    \n"\
  "                                     --record-topic /clock                      \n"\
  "\n"\
  "  --log-overwrite              When recording, overwrite existing files.        \n"\
  "                               Only valid if recording is enabled.              \n"\
  "\n"\
  "  --log-compress               When recording, compress final log files.        \n"\
  "                               Only valid if recording is enabled.              \n"\
  "\n"\
  "  --playback [arg]             Use logging system to play back states.          \n"\
  "                               Argument is path to recorded states.             \n"\
  "\n"\
  "  -r                           Run simulation on start.                         \n"\
  "\n"\
  "  -s                           Run only the server (headless mode). This        \n"\
  "                               overrides -g, if it is also present.             \n"\
  "\n"\
  "  -v [ --verbose ] [arg]       Adjust the level of console output (0~4).        \n"\
  "                               The default verbosity is 1, use -v without       \n"\
  "                               arguments for level 3.                           \n"\
  "\n"\
  "  --gui-config [arg]           Ignition GUI configuration file to load.         \n"\
  "                               If no config is given, the configuration in      \n"\
  "                               the SDF file is used. And if that's not          \n"\
  "                               provided, the default installed config is        \n"\
  "                               used.                                            \n"\
  "\n"\
  "  --physics-engine [arg]       Ignition Physics engine plugin to load.          \n"\
  "                               Gazebo will use DART by default.                 \n"\
  "                               (ignition-physics-dartsim-plugin)                \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               IGN_GAZEBO_PHYSICS_ENGINE_PATH.                  \n"\
  "\n"\
  "  --render-engine [arg]        Ignition Rendering engine plugin to load for     \n"\
  "                               both the server and the GUI. Gazebo will use     \n"\
  "                               OGRE2 by default. (ogre2)                        \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               IGN_GAZEBO_RENDER_ENGINE_PATH.                   \n"\
  "\n"\
  "  --render-engine-gui [arg]    Ignition Rendering engine plugin to load for     \n"\
  "                               the GUI. Gazebo will use OGRE2 by default.       \n"\
  "                               (ogre2)                                          \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               IGN_GAZEBO_RENDER_ENGINE_PATH.                   \n"\
  "\n"\
  "  --render-engine-server [arg] Ignition Rendering engine plugin to load for     \n"\
  "                               the server. Gazebo will use OGRE2 by default.    \n"\
  "                               (ogre2)                                          \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               IGN_GAZEBO_RENDER_ENGINE_PATH.                   \n"\
  "\n"\
  "  --version                    Print Gazebo version information.                \n"\
  "\n"\
  "  -z [arg]                     Update rate in Hertz.                            \n"\
  "\n"+
  COMMON_OPTIONS + "\n\n" +
  "Environment variables:                                                  \n"\
  "  IGN_GAZEBO_RESOURCE_PATH    Colon separated paths used to locate      \n"\
  " resources such as worlds and models.                                 \n\n"\
  "  IGN_GAZEBO_SYSTEM_PLUGIN_PATH    Colon separated paths used to        \n"\
  " locate system plugins.                                               \n\n"\
  "  IGN_GAZEBO_SERVER_CONFIG_PATH    Path to server configuration file. \n\n"\
  "  IGN_GUI_PLUGIN_PATH    Colon separated paths used to locate GUI       \n"\
  " plugins.                                                             \n\n"\
  "  GZ_GUI_RESOURCE_PATH    Colon separated paths used to locate GUI      \n"\
  " resources such as configuration files.                               \n\n"\
  "  IGN_GAZEBO_NETWORK_ROLE     Participant role used in a distributed    \n"\
  " simulation environment. Role is one of [PRIMARY, SECONDARY]. This is   \n"\
  " deprecated in ign-gazebo2. Please use --network-role instead.        \n\n"\
  "  IGN_GAZEBO_NETWORK_SECONDARIES    Number of secondary participants    \n"\
  " expected to join a distributed simulation environment. (Primary only)  \n"\
  " This is deprecated in ign-gazebo2. Please use --network-role instead.  \n"
}

#
# Class for the Ignition Gazebo command line tools.
#
class Cmd

  def killProcess(pid, name, timeout)
    Process.kill("-INT", pid)

    sleepSecs = 0.001
    iterations = (timeout / sleepSecs).to_i
    i = 0
    killedPid = 0
    while killedPid != pid && i < iterations
      begin
        killedPid = Process.waitpid(pid, Process::WNOHANG)
      rescue
        # The process has exited, so return.
        return
      end

      break if killedPid == pid

      sleep sleepSecs
      i = i + 1
    end

    if killedPid != pid
      puts "Escalating to SIGKILL on [#{name}]"
      Process.kill("-KILL", pid)
    end
  end

  #
  # Return a structure describing the options.
  #
  def parse(args)
    options = {
      'file' => '',
      'gui' => 0,
      'hz' => -1,
      'iterations' => 0,
      'levels' => 0,
      'network_role' => '',
      'network_secondaries' => 0,
      'record' => 0,
      'record-path' => '',
      'record-resources' => 0,
      'record-topics' => [],
      'log-overwrite' => 0,
      'log-compress' => 0,
      'playback' => '',
      'run' => 0,
      'server' => 0,
      'verbose' => '1',
      'gui_config' => '',
      'physics_engine' => '',
      'rendering_engine_gui' => '',
      'rendering_engine_server' => '',
      'wait_gui' => 1
    }

    usage = COMMANDS[args[0]]

    opt_parser = OptionParser.new do |opts|
      opts.banner = usage

      opts.on('-h', '--help') do
        puts usage
        exit
      end
      opts.on('--iterations [arg]', Integer,
              'Number of iterations to execute') do |i|
        options['iterations'] = i
      end
      opts.on('--network-role [arg]', String) do |role|
        options['network_role'] = role
      end
      opts.on('--network-secondaries [arg]', Integer) do |i|
        options['network_secondaries'] = i
      end
      opts.on('-z [arg]', Float, 'Update rate in Hertz') do |h|
        options['hz'] = h
      end
      opts.on('-r') do
        options['run'] = 1
      end
      opts.on('-g') do
        options['gui'] = 1
        # Runing the Gui only, don't show world loading menu
        options['wait_gui'] = 0
      end
      opts.on('-s') do
        options['server'] = 1
        # Runing the server only, don't wait for starting world from Gui
        options['wait_gui'] = 0
      end
      opts.on('--levels') do
        options['levels'] = 1
      end
      opts.on('--record') do
        options['record'] = 1
      end
      opts.on('--record-path [arg]', String) do |r|
        options['record-path'] = r
      end
      opts.on('--record-resources') do
        options['record-resources'] = 1
      end
      opts.on('--record-topic [arg]', String) do |t|
        options['record-topics'].append(t)
      end
      opts.on('--log-overwrite') do
        options['log-overwrite'] = 1
      end
      opts.on('--log-compress') do
        options['log-compress'] = 1
      end
      opts.on('--playback [arg]', String) do |p|
        options['playback'] = p
      end
      opts.on('-v [verbose]', '--verbose [verbose]', String) do |v|
        options['verbose'] = v || '3'
      end
      opts.on('--gui-config [arg]', String) do |c|
        options['gui_config'] = c
      end
      opts.on('--physics-engine [arg]', String) do |e|
        options['physics_engine'] = e
      end
      opts.on('--render-engine-gui [arg]', String) do |g|
        options['render_engine_gui'] = g
      end
      opts.on('--render-engine-server [arg]', String) do |k|
        options['render_engine_server'] = k
      end
      opts.on('--render-engine [arg]', String) do |f|
        options['render_engine_gui'] = f
        options['render_engine_server'] = f
      end
      opts.on('--version') do
        options['version'] = '1'
      end

    end # opt_parser do

    opt_parser.parse!(args)

    # SDF file as positional argument
    filename = args.pop
    if filename and filename != 'gazebo'
      options['file'] = filename
    end

    options['command'] = args[0]

    options
  end # parse()

  def execute(args)
    options = parse(args)

    if LIBRARY_NAME[0] == '/'
      # If the first character is a slash, we'll assume that we've been given an
      # absolute path to the library. This is only used during test mode.
      plugin = LIBRARY_NAME
    else
      # We're assuming that the library path is relative to the current
      # location of this script.
      plugin = File.expand_path(File.join(File.dirname(__FILE__), LIBRARY_NAME))
    end
    conf_version = LIBRARY_VERSION

    begin
      Importer.dlload plugin
    rescue DLError => e
      puts "Library error for [#{plugin}]: #{e.to_s}"
      if plugin.end_with? ".dylib"
        puts "
If this script was executed with /usr/bin/ruby, this error may be caused by
macOS System Integrity Protection. One workaround is to use a different
version of ruby, for example:
    brew install ruby
and add the following line to your shell profile:
    export PATH=/usr/local/opt/ruby/bin:$PATH
If you are using a colcon workspace, please ensure that the setup script
has properly set the DYLD_LIBRARY_PATH environment variables."
      end
      exit(-1)
    end

    # Read the library version.
    Importer.extern 'char *ignitionGazeboVersion()'
    begin
      plugin_version = Importer.ignitionGazeboVersion.to_s
    rescue DLError
      puts "Library error: Problem running 'ignitionGazeboVersion()' from #{plugin}."
      exit(-1)
    end

    # Sanity check: Verify that the version of the yaml file matches the version
    # of the library that we are using.
    unless plugin_version.eql? conf_version
      puts "Error: Version mismatch. Your configuration file version is
            [#{conf_version}] but #{plugin} version is [#{plugin_version}]."
      exit(-1)
    end

    usage = COMMANDS[args[0]]

    begin

      # Import the findFuelResource function
      Importer.extern 'const char *findFuelResource(const char *)'

      if options.key?('version')
        Importer.extern 'char *gazeboVersionHeader()'
        puts Importer.gazeboVersionHeader.to_s
        exit
      end

      # Global configurations
      if options.key?('verbose')
        Importer.extern 'void cmdVerbosity(const char *)'
        Importer.cmdVerbosity(options['verbose'])
      end

      parsed = ''
      if options['file'] != ''
        # Check if the passed in file exists.
        if File.exists?(options['file'])
          path = options['file']
        # If not, then first check the IGN_GAZEBO_RESOURCE_PATH environment
        # variable, then the configuration path from the launch library.
        else
          resourcePathEnv = ENV['IGN_GAZEBO_RESOURCE_PATH']
          if !resourcePathEnv.nil?
            resourcePaths = resourcePathEnv.split(':')
            for resourcePath in resourcePaths
              filePath = File.join(resourcePath, options['file'])
              if File.exists?(filePath)
                path = filePath
                break
              end
            end
          end

          if path.nil?
            Importer.extern 'char *worldInstallDir()'
            path = File.join(Importer.worldInstallDir().to_s, options['file'])
            if !File.exists?(path)
              path = Importer.findFuelResource(options['file']).to_s
              options['file'] = path
              if path == ""
                puts "Unable to find or download file " + options['file']
                exit(-1)
              end
            end
          end
        end

        # ERB parse the file, and then run the result
        parsed = ERB.new(File.read(path)).result()
      end

      # Import the runServer function
      Importer.extern 'int runServer(const char *, int, int, float, int,
                               const char *, int, int, const char *,
                               int, int, int, const char *, const char *,
                               const char *, const char *, const char *,
                               const char *, int)'

      # Import the runGui function
      Importer.extern 'int runGui(const char *, const char *, int)'

      # If playback is specified, and the user has not specified a
      # custom gui config, set the gui config to load the playback
      # gui config
      if (options['playback'] != '' and options['gui_config'] == '')
        options['gui_config'] = "_playback_"
      end

      # Neither the -s nor -g options were used, so run both the server
      # and gui.
      if options['server'] == 0 && options['gui'] == 0

        if plugin.end_with? ".dylib"
          puts "`ign gazebo` currently only works with the -s argument on macOS.
See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info."
          exit(-1)
        end

        serverPid = Process.fork do
          ENV['RMT_PORT'] = '1500'
          Process.setpgid(0, 0)
          Process.setproctitle('ign gazebo server')
          Importer.runServer(parsed, options['iterations'], options['run'],
            options['hz'], options['levels'], options['network_role'],
            options['network_secondaries'], options['record'],
            options['record-path'], options['record-resources'],
            options['log-overwrite'], options['log-compress'],
            options['playback'], options['physics_engine'],
            options['render_engine_server'], options['render_engine_gui'],
            options['file'], options['record-topics'].join(':'), options['wait_gui'])
        end

        guiPid = Process.fork do
          ENV['RMT_PORT'] = '1501'
          Process.setpgid(0, 0)
          Process.setproctitle('ign gazebo gui')
          Importer.runGui(options['gui_config'], options['file'], options['wait_gui'])
        end

        Signal.trap("INT") {
          self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0)
          self.killProcess(serverPid, "Ignition Gazebo Server", 5.0)
          return 1
        }

        # Wait for a child process to end
        pid, status = Process.wait2

        if pid == serverPid
          self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0)
        else
          self.killProcess(serverPid, "Ignition Gazebo Server", 5.0)
        end

      # If the -s option was specified, then run only the server
      elsif options['server'] == 1
        ENV['RMT_PORT'] = '1500'
        Importer.runServer(parsed, options['iterations'], options['run'],
            options['hz'], options['levels'], options['network_role'],
            options['network_secondaries'], options['record'],
            options['record-path'], options['record-resources'],
            options['log-overwrite'], options['log-compress'],
            options['playback'], options['physics_engine'],
            options['render_engine_server'], options['render_engine_gui'],
            options['file'], options['record-topics'].join(':'), options['wait_gui'])
            # Otherwise run the gui
      else options['gui']
        if plugin.end_with? ".dylib"
          puts "`ign gazebo` currently only works with the -s argument on macOS.
See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info."
          exit(-1)
        end

        ENV['RMT_PORT'] = '1501'
        Importer.runGui(options['gui_config'], options['file'], options['wait_gui'])
      end
    rescue
      puts "Library error: Problem running [#{options['command']}]() "\
        "from #{plugin}."
    # begin
    end
  # execute
  end
# class
end
