#!/usr/bin/env python3

import argparse
import os
import sys
from threading import Event

import rospy
from gazebo_msgs.msg import ModelStates
from std_srvs.srv import Empty


class WaitForModel:
    def __init__(self, model, service, delay):
        self._model = model
        self._delay = rospy.Duration.from_sec(delay)
        self._model_exists = False
        self._call_time = None
        self._ready_event = Event()
        rospy.on_shutdown(self.on_shutdown)

        rospy.loginfo(f"Waiting for model {model}")
        self._init_srv_client = rospy.ServiceProxy(service, Empty)
        self._model_states_sub = rospy.Subscriber("/gazebo/model_states", ModelStates,
                                                  self._model_states_cb)

    def _model_states_cb(self, msg):
        if self._model not in msg.name:
            return
        if not self._model_exists:
            self._call_time = rospy.Time.now() + self._delay
            self._model_exists = True
        if rospy.Time.now() < self._call_time:
            return
        try:
            self._init_srv_client.wait_for_service(1)
        except rospy.ROSException:
            return
        rospy.loginfo(f"Detected model {self._model}")
        self._init_srv_client.call()
        self._ready_event.set()

    def on_shutdown(self):
        self._ready_event.set()

    def run(self):
        self._ready_event.wait()
        return os.EX_OK if self._model_exists else os.EX_SOFTWARE


if __name__ == "__main__":
    parser = argparse.ArgumentParser(
        description="Waits for a model to exist and then calls a service")
    parser.add_argument("--model", type=str, metavar="MODEL", required=True,
                        help="Model for which to wait")
    parser.add_argument("--service", type=str, metavar="SRV", required=True,
                        help="Service to call when the model appears")
    parser.add_argument("--delay", type=float, metavar="SEC", default=0.0,
                        help="Extra time to wait after the model has appeared "
                             "and before calling the service")
    args = parser.parse_known_args()[0]

    rospy.init_node("wait_for_model")
    wait_for_model = WaitForModel(args.model, args.service, args.delay)
    sys.exit(wait_for_model.run())
