#!/usr/bin/env python
import roslib
import unittest
import numpy as np
import pyexotica as exo

PKG = 'exotica_examples'
roslib.load_manifest(PKG)  # This line is not needed with Catkin.

class TestClass(unittest.TestCase):
    def test_collision(self):
        # Latest FCL
        (_, ik_prob) = exo.Initializers.load_xml_full(
            '{exotica_examples}/test/resources/test_valkyrie_collisionscene_fcl_latest.xml')
        ik_problem = exo.Setup.create_problem(ik_prob)
        ik_problem.update(np.zeros(ik_problem.N,))
        ik_scene = ik_problem.get_scene()

        # Check number of collision robot links
        np.testing.assert_equal(len(ik_scene.get_collision_robot_links()), 79)

        # Check number of collision world links
        np.testing.assert_equal(len(ik_scene.get_collision_world_links()), 0)

        # Get links in self-collision
        exo.tools.get_colliding_links(ik_scene, debug=True)

        # Testing is_state_valid(True, 0.0) - with self-collision
        np.testing.assert_equal(ik_scene.is_state_valid(True, 0.0), True)

        # Testing is_state_valid(False, 0.0) - without self-collision
        np.testing.assert_equal(ik_scene.is_state_valid(False, 0.0), True)


if __name__ == '__main__':
    import rostest
    rostest.rosrun(PKG, 'TestValkyrieCollisionLatest', TestClass)
