#!/usr/bin/env python
###############################################################################
# Software License Agreement (BSD License)
#
# Copyright (c) 2016, Kentaro Wada.
# 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.
###############################################################################

"""
Integration test node that subscribes to
topic (/pddl_ner/result) and verifies it's contents

below parameters must be set:

<test name="pddl_result_test"
      test-name="pddl_result_test"
      pkg="pddl_planner" type="pddlresulttest">
  <rosparam>
    topics:
      - name: /pddl_planner/result
        timeout: 10
        sequence_action: ['(HOLD CUP LARM BAR)', '(MOVE-TO BAR SINK)', '(OPEN-TAP)', '(WASH-CUP)', '(PLACE CUP LARM SINK)']
  </rosparam>
</test>

Author: Kentaro Wada <www.kentaro.wada@gmail.com>
Modified for PddlResultTest by <kei.okada@gmail.com>
"""
from __future__ import print_function

import sys
import time
import unittest

import rospy
import rostopic

from pddl_msgs.msg import PDDLPlannerActionResult
from pddl_msgs.msg import PDDLPlannerResult
from pddl_msgs.msg import PDDLStep

PKG = 'pddl_planner'
NAME = 'pddlresulttest'

class PddlResultChecker(object):
    def __init__(self, topic_name, timeout, result):
        self.topic_name = topic_name
        rospy.loginfo("subscribing {}".format(topic_name))
        self.targets = []
        self.deadline = None
        self.add_callback(timeout, result)
        self.sub = rospy.Subscriber(topic_name, PDDLPlannerActionResult, self._callback)

    def _callback(self, msg):

        rospy.loginfo("received msg")
        rospy.loginfo(" - use_durative_action: {}".format(msg.result.use_durative_action))
        rospy.loginfo(" - sequence: {}".format([x.action + " ".join(x.args) for x in msg.result.sequence]))
        rospy.loginfo("             {} {}".format([x.start_time for x in msg.result.sequence],
                                                  [x.action_duration for x in msg.result.sequence]))
        for target in self.targets:
            message_matched = True
            result = target['result']
            
            if target['msg_matched']:
                rospy.loginfo(" msg already passed {}".format([x.action + " ".join(x.args) for x in result.sequence]))
                continue

            if not result.use_durative_action == None:
                if not msg.result.use_durative_action == result.use_durative_action:
                    rospy.loginfo("  expecting: {}".format(result.use_durative_action))
                    message_matched = False
            if not result.sequence == None:
                if not msg.result.sequence == result.sequence:
                    rospy.loginfo("       expecting    : {}".format([x.action + " ".join(x.args) for x in result.sequence]))
                    rospy.loginfo("                      {} {}".format([x.start_time for x in result.sequence],
                                                                   [x.action_duration for x in result.sequence]))
                    message_matched = False

            if message_matched:
                target['msg_matched'] = msg
                rospy.loginfo("message matched to {}".format([x.action + " ".join(x.args) for x in result.sequence]))
                rospy.loginfo("                   {} {}".format([x.start_time for x in result.sequence],
                                                            [x.action_duration for x in result.sequence]))

    def add_callback(self, timeout, result):
        rospy.loginfo("register to check  {}".format([x.action + " ".join(x.args) for x in result.sequence]))
        rospy.loginfo("                   {} {}".format([x.start_time for x in result.sequence],
                                                            [x.action_duration for x in result.sequence]))
        rospy.loginfo("            within {} sec".format(timeout))

        deadline_ = rospy.Time.now() + rospy.Duration(timeout)
        self.targets.append({'deadline': deadline_ , 'result': result, 'msg_matched': None})
        if self.deadline:
            self.deadline = max(self.deadline, deadline_)
        else:
            self.deadline = deadline_

    def assert_published(self):
        if all(target['msg_matched'] for target in self.targets):
            return True
        if rospy.Time.now() > self.deadline:
            return False
        return None


class PddlResultTest(unittest.TestCase):
    def __init__(self, *args):
        super(self.__class__, self).__init__(*args)
        rospy.init_node(NAME)
        # scrape rosparam
        self.topics = []
        params = rospy.get_param('~topics', [])
        for param in params:
            if 'name' not in param:
                self.fail("'name' field in rosparam is required but not specified.")
            topic = {'timeout': 10, 'use_durative_action': False, 'sequence_action': [], 'sequence_start_time': [], 'sequence_duration': []}
            topic.update(param)
            self.topics.append(topic)
        # check if there is at least one topic
        if not self.topics:
            self.fail('No topic is specified in rosparam.')

    def test_publish(self):
        """Test topics are published and messages come"""
        use_sim_time = rospy.get_param('/use_sim_time', False)
        t_start = time.time()
        while not rospy.is_shutdown() and \
                use_sim_time and (rospy.Time.now() == rospy.Time(0)):
            rospy.logwarn_throttle(
                1, '/use_sim_time is specified and rostime is 0, /clock is published?')
            if time.time() - t_start > 10:
                self.fail('Timed out (10s) of /clock publication.')
            # must use time.sleep because /clock isn't yet published, so rospy.sleep hangs.
            time.sleep(0.1)
        # subscribe topics
        checkers = []
        for topic in self.topics:
            topic_name = topic['name']
            timeout = topic['timeout']
            result = PDDLPlannerResult()
            result.use_durative_action = topic['use_durative_action']
            #  https://stackoverflow.com/questions/60532658/python-2-vs-python-3-difference-in-map-behavior-with-three-arguments
            try:
                from itertools import zip_longest
                result.sequence = list(map(lambda x: PDDLStep(action = x[0].split()[0],
                                                              args = x[0].split()[1:],
                                                              start_time = x[1] or '',
                                                              action_duration = x[2] or ''),
                                           zip_longest(topic['sequence_action'],
                                                       topic['sequence_start_time'],
                                                       topic['sequence_duration'])))
            except:
                result.sequence = map(lambda a, s, d: PDDLStep(action = a.split()[0],
                                                               args = a.split()[1:],
                                                               start_time = s or '',
                                                               action_duration = d or ''),
                                      topic['sequence_action'],
                                      topic['sequence_start_time'],
                                      topic['sequence_duration'])

            # check if already subscribe topic
            checker = next((x for x in checkers if x.topic_name == topic_name), None)
            if checker == None:
                checkers.append(PddlResultChecker(topic_name, timeout, result))
            else:
                checker.add_callback(timeout, result)

        deadline = max(checker.deadline for checker in checkers)
        # assert
        finished_topics = []
        while not rospy.is_shutdown():
            if len(set([topic['name'] for topic in self.topics])) == len(finished_topics):
                assert True, 'Check all topics [%s] successfully' % (fnished_topics)
                break
            for checker in checkers:
                if checker.topic_name in finished_topics:
                    continue  # skip topic testing has finished
                ret = checker.assert_published()
                if ret is None:
                    continue  # skip if there is no test result
                finished_topics.append(checker.topic_name)
                assert ret, 'Topic [%s] is not published' % (checker.topic_name)
            rospy.sleep(0.01)
        self.assertTrue(True)


if __name__ == '__main__':
    import rostest
    try:
        rostest.run(PKG, NAME, PddlResultTest, sys.argv)
    except KeyboardInterrupt:
        pass
    print("{} exiting".format(PKG))
