#!/usr/bin/env python
# Copyright (c) 2019 - The Procedural Generation for Gazebo authors
# For information on the respective copyright owner see the NOTICE file
#
# 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.

from __future__ import print_function
import rospy
import sys
import argparse
from time import time, sleep
from rosgraph_msgs.msg import Clock
from std_srvs.srv import Empty


CLOCK_TIME = 0


def clock_callback(msg):
    global CLOCK_TIME
    CLOCK_TIME = rospy.Time(
        secs=msg.clock.secs, nsecs=msg.clock.nsecs).to_sec()


def main():
    parser = argparse.ArgumentParser(description="Simulation timer")
    parser.add_argument(
        '--timeout', '-t', type=float, help='Timeout in seconds')
    args = parser.parse_args()

    print('Simulation timer')

    if rospy.is_shutdown():
        ros_timeout = 30
        start_ros_timeout = time()
        while time() - start_ros_timeout < ros_timeout and rospy.is_shutdown():
            print('Waiting for ROS master...')
            sleep(0.1)
        if rospy.is_shutdown():
            raise rospy.ROSException('ROS master is not running!')

    rospy.init_node('simulation_timer', anonymous=True)

    timeout = args.timeout
    if args.timeout is None:
        print('No timeout provided')
        return

    if args.timeout < 0:
        raise rospy.ROSException(
            'Termination time must be a positive floating point value')

    rospy.wait_for_service('/gazebo/pause_physics', 10)
    pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)

    print('Starting simulation timer - Timeout = %2.f s' % timeout)

    start_process_timeout = time()

    while CLOCK_TIME < timeout:
        sleep(0.001)
        if CLOCK_TIME == 0:
            if time() - start_process_timeout > 60:
                print('Clock was not initialized for 60 seconds')
                break
        if rospy.is_shutdown():
            print('ROS master was killed!')
            break

    print('Pausing simulation...')
    pause()
    print('Simulation paused!')

    print('Simulation timeout!')


if __name__ == '__main__':
    main()
    sys.exit(0)
