#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
"""
This file launches an ORK actionlib server according to a config file
"""
from ecto.opts import scheduler_options
from object_recognition_core.utils.training_detection_args import create_parser, read_arguments
from object_recognition_ros.server import RecognitionServer
import ecto_ros
import rospy
import sys

if __name__ == '__main__':

    # make a copy for the args we pass to ROS later on
    ros_argv = sys.argv[:]
    # make a copy for the args we pass to ecto later on
    ecto_argv = sys.argv[:]

    # create an ORK parser (it is special as it can read from option files)
    parser = create_parser()
    parser.description = ' This file executes an actionlib server that executes the ORK plasm contained in the ' \
                        'configuration file'

    # add a node name option
    def filter_node_name(node_name):
        return node_name
    ros_group = parser.add_argument_group('ROS parameters')
    ros_group.add_argument('--node_name', help='The name for the node. If "", it is not run in a ROS node',
                       default='object_recognition', type=filter_node_name)

    # cleanup the arguments
    clean_args = sys.argv
    ecto_ros.strip_ros_args(clean_args)

    args = parser.parse_args(args=clean_args[1:])
    if args.node_name and args.node_name != '""':
        rospy.loginfo('ORK server started with name: %s' % args.node_name)
        ecto_ros.init([s for s in ecto_argv if not s.startswith('__name:=')], args.node_name, False)

    # add ecto options
    scheduler_options(parser)
    ork_params, _args = read_arguments(args)
    rospy.init_node('recognize_objects_server', argv=ros_argv)
    server = RecognitionServer(ork_params)
    rospy.spin()
