#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, 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 the Willow Garage 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.
#

## Gazebo test arm controller
##   sends posision
##   checks to see if P3D returns corresponding ground truth within TARGET_TOL of TARGET_VW
##          for a duration of TARGET_DURATION seconds

PKG = 'pr2_gazebo'
NAME = 'hztest'

import math
import roslib
roslib.load_manifest(PKG)
roslib.load_manifest('rostest')

import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from std_msgs.msg import *
from tf.msg import tfMessage
from pr2_mechanism_msgs.msg import MechanismStatistics

MIN_MSGS        = 1
TEST_TIMEOUT    = 360.0

class MyHzTest(unittest.TestCase):
    def __init__(self, *args):
        super(MyHzTest, self).__init__(*args)
        self.success = False
        self.count = 0

        #for time tracking
        self.started = False
        self.start_time = 0
        self.end_time = 0
        self.finished = False

        #some default values
        self.topic_name = '';
        self.hz_target = 10;
        self.initial_cycles = 1;
        self.hz_tol = 1;
        self.test_duration = 0;

    def Input(self, msg):
        self.count += 1;

        if self.count < self.initial_cycles:
          # will start after self.initial_cycles messages
          return;

        if not self.started:
          self.start_time = rospy.get_rostime().to_sec()
          print " got first ",self.initial_cycles," messages at: ",self.start_time, " sec"
          self.started = True

        self.end_time = rospy.get_rostime().to_sec()

        if (self.end_time - self.start_time) > self.test_duration:
          self.finished = True

        if (self.end_time - self.start_time) > 0.0:
          cur_hz = (self.count - self.initial_cycles) / (self.end_time - self.start_time)
        else:
          cur_hz = 0
        print "current rate: ", cur_hz

        if abs(cur_hz - self.hz_target) < self.hz_tol:
          self.success = True
        else:
          self.success = False

        #if self.finished:
        #  print " got ",self.count," messages at ",self.count / (rospy.get_rostime().to_sec() - self.start_time), " Hz"

    def test_hz(self):
        print "LNK\n"
        rospy.init_node(NAME, anonymous=True)

        self.topic_name = rospy.get_param('~topic','mechanism_state');
        self.hz_target = rospy.get_param('~hz',100);
        self.initial_cycles = rospy.get_param('~initial_cycles',1);
        self.hz_tol = rospy.get_param('~hzerror',1);
        self.test_duration = rospy.get_param('~test_duration',1);

        rospy.Subscriber(self.topic_name, rospy.AnyMsg, self.Input)

        print "topic name ",self.topic_name

        timeout_t = time.time() + self.test_duration + TEST_TIMEOUT
        while not rospy.is_shutdown() and not self.success and not self.finished and time.time() < timeout_t:
            # wait for first message
            if not self.started:
                print " waiting for first message realtime:",time.time()," simtime:",rospy.get_rostime().to_sec()
            else:
                print " realtime:",time.time()," simtime:",rospy.get_rostime().to_sec()," time elapsed:",(self.end_time - self.start_time)," msg count:",self.count-self.initial_cycles
            time.sleep(0.5)

        self.assert_(self.success)

if __name__ == '__main__':
    rostest.run(PKG, sys.argv[0], MyHzTest, sys.argv) #, text_mode=True)

