module Orocos::ROS

# File lib/orocos/ros.rb, line 5
def self.available?
    if @available.nil?
        @available = defined? TRANSPORT_ROS
    end
    @available
end

# File lib/orocos/ros/base.rb, line 33
def self.clear
    default_loader.clear
    @loaded = false
end

# File lib/orocos/ros/base.rb, line 21
def self.default_loader
    Orocos.default_loader
    @default_loader ||= DefaultLoader.new(Orocos.default_loader)
end

# File lib/orocos/ros.rb, line 39
def self.default_ros_master_uri
    ENV['ROS_MASTER_URI']
end

# File lib/orocos/ros.rb, line 11
def self.disable
    @enabled = false
end

static VALUE ros_init(int argc, VALUE* _argv, VALUE mod)
{
    VALUE name, rest;
    rb_scan_args(argc, _argv, "1*", &name, &rest);

    size_t size = RARRAY_LEN(rest);
    std::vector<char const*> argv;
    argv.resize(size + 1);
    argv[0] = "";
    for (int i = 0; i < size; ++i)
    {
        VALUE element = RARRAY_PTR(rest)[i];
        argv[i + 1] = StringValuePtr(element);
    }

    if(!ros::isInitialized()){
        int argc = 0;
        ros::init(argc,NULL,StringValuePtr(name), ros::init_options::NoSigintHandler | ros::init_options::NoRosout);
      if(ros::master::check())
          ros::start();
      else{
          rb_raise(eROSComError, "cannot communicate with ROS master");
      }   
    }

    static ros::AsyncSpinner spinner(1); // Use 1 threads
    spinner.start();
    return Qnil;
}

# File lib/orocos/ros.rb, line 14
def self.enabled?
    if @enabled == false
        return false
    elsif available? && (ENV['ROCK_ROS_INTEGRATION'] != '0')
        return false if !ENV['ROS_MASTER_URI']

        if @enabled.nil?
            # This is getting automatically enabled, check if it is
            # actually available
            begin
                Orocos::ROS.name_service
                Orocos.default_cmdline_arguments = Orocos.default_cmdline_arguments.merge('with-ros' => true)
                Orocos.debug "ROS integration was enabled, passing default arguments: #{Orocos.default_cmdline_arguments}"
                @enabled = true
            rescue Orocos::ROS::ComError
                Orocos.warn "ROS integration was enabled, but I cannot contact the ROS master at #{Orocos::ROS.default_ros_master_uri}, disabling"
                Orocos::ROS.disable
                Orocos.default_cmdline_arguments = Orocos.default_cmdline_arguments.delete('with-ros')
                @enabled = false
            end
        end
        @enabled
    end
end

# File lib/orocos/ros/rpc.rb, line 21
def self.initialize(name = ROS.caller_id[1..-1])
    if initialized?
        raise RuntimeError, "cannot initialize the ROS layer multiple times"
    end

    ROS.load

    do_initialize(name)

    at_exit do
        if ROS.initialized?
            ROS.shutdown
        end
    end
end

static VALUE ros_is_initialized(VALUE mod)
{
    return ros::isInitialized() ? Qtrue : Qfalse;
}

# File lib/orocos/ros/base.rb, line 27
def self.load
    @loaded = true
end

# File lib/orocos/ros/base.rb, line 31
def self.loaded?; !!@loaded end

# File lib/orocos/ros/base.rb, line 7
def self.map_message_type_to_orogen(message_type)
    default_loader.map_message_type_to_orogen(message_type)
end

# File lib/orocos/ros.rb, line 48
def self.name_service
    if @name_service
        return @name_service
    else
        ns = Orocos::ROS::NameService.new
        ns.validate
        @name_service = ns
    end
end

# File lib/orocos/ros/rpc.rb, line 15
def self.ros_master
    @ros_master ||= XMLRPC::Client::Proxy.new(ros_master_uri, '')
end

# File lib/orocos/ros/rpc.rb, line 7
def self.ros_master_uri
    ENV['ROS_MASTER_URI']
end

# File lib/orocos/ros/base.rb, line 47
def self.roscore_available?
    begin
        !rosnode_list.empty?
    rescue InternalError => e
        false
    end
end

# File lib/orocos/ros/base.rb, line 41
def self.roscore_pid
    @roscore_pid || 0
end

# File lib/orocos/ros/base.rb, line 86
def self.roscore_shutdown
    begin
        if @roscore_pid
            info "roscore will be shutdown"
            status = ::Process.kill('INT',@roscore_pid)
            @roscore_pid = nil
            return status
        end
    rescue Errno::ESRCH
        raise ArgumentError, "trying to shutdown roscore, which is not running anymore with pid '#{@roscore_pid}'"
    end

    warn "roscore is not controlled by this process; no shutdown will be performed"
    false
end

# File lib/orocos/ros/base.rb, line 57
def self.roscore_start(*args)
    options = args.last.kind_of?(Hash) ? args.pop : Hash.new
    options, unknown_options = Kernel.filter_options options,
        :redirect => File.join("/var/tmp/roscore.log")

    args << options

    if !roscore_available?
        @roscore_pid = Utilrb.spawn "roscore", *args
        ::Process.detach(@roscore_pid)
        @roscore_pid
    elsif !@roscore_pid
        warn "roscore is already running, but is not controlled by this process"
    else
        info "roscore is already running, pid '#{@roscore_pid}'"
    end

    if unknown_options[:wait]
        while !roscore_available?
            sleep 0.1
        end
    end
end

# File lib/orocos/ros/base.rb, line 108
def self.roslaunch(package_name, launch_name, options = Hash.new)
    launch_name = launch_name.gsub(/\.launch/,'')
    launch_name = launch_name + ".launch"
    arguments = [package_name, launch_name]
    arguments += [options]

    pid = Utilrb.spawn "roslaunch", "__name:=#{launch_name}", *arguments
    pid
end

static VALUE ros_shutdown()
{
    ros::shutdown();
    return Qnil;
}

# File lib/orocos/ros/topic.rb, line 183
def self.topic(name)
    Orocos.name_service.each do |ns|
        if ns.respond_to?(:find_topic_by_name)
            if topic = ns.find_topic_by_name(name)
                return topic
            end
        end
    end
    raise NotFound, "topic #{name} does not seem to exist"
end