#!/usr/bin/env python
import roslib
import unittest
PKG = 'exotica_examples'
roslib.load_manifest(PKG)  # This line is not needed with Catkin.


def test_import():
    global exo
    import pyexotica as exo


def test_setup():
    global exo
    exo.Setup()


def test_getters():
    global exo
    print(exo.Setup().get_solvers())
    print(exo.Setup().get_problems())
    print(exo.Setup().get_maps())
    print(exo.Setup().get_collision_scenes())
    print(exo.Setup().get_dynamics_solvers())
    exo.Setup().get_initializers()
    print(exo.Setup().get_package_path('exotica_python'))


def test_ros():
    global exo
    exo.Setup().init_ros()


def test_load_xml():
    global exo
    (sol, prob) = exo.Initializers.load_xml_full(exo.Setup.get_package_path(
        'exotica_examples')+'/resources/configs/example_ik.xml')
    (sol, prob) = exo.Initializers.load_xml_full(
        '{exotica_examples}/resources/configs/example_ik.xml')
    problem = exo.Setup.create_problem(prob)
    solver = exo.Setup.create_solver(sol)
    solver.specify_problem(problem)
    solver.solve()


class TestClass(unittest.TestCase):
    def test_1_import(self):
        test_import()

    def test_2_setup(self):
        test_setup()

    def test_3_getters(self):
        test_getters()

    def test_4_ros(self):
        test_ros()

    def test_5_xml(self):
        test_load_xml()


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