#!/usr/bin/env python
from __future__ import print_function
import numpy as np
import pyexotica as exo
PKG = 'exotica_examples'
import roslib; roslib.load_manifest(PKG)  # This line is not needed with Catkin.

import unittest

class TestClass(unittest.TestCase):
    def setUp(self):
        self.problem = exo.Setup.load_problem('{exotica_examples}/test/resources/test_com_valkyrie.xml')
        self.scene = self.problem.get_scene()

    def test_controlled_joint_to_collision_link_map(self):
        true_output = {u'torsoRoll': [u'torso_collision_0'], u'upperNeckPitch': [u'upperNeckPitchLink_collision_0', u'head_collision_0', u'head_collision_1', u'hokuyo_link_collision_0', u'hokuyo_link_collision_1'], u'rightWristRoll': [u'rightWristRollLink_collision_0'], u'leftHipYaw': [u'leftHipYawLink_collision_0'], u'rightElbowPitch': [u'rightElbowPitchLink_collision_0'], u'leftHipRoll': [u'leftHipRollLink_collision_0'], u'torsoYaw': [u'torsoYawLink_collision_0'], u'world_joint/rot_x': [u'pelvis_collision_0'], u'rightWristPitch': [u'rightPalm_collision_0', u'rightIndexFingerPitch1Link_collision_0', u'rightIndexFingerPitch2Link_collision_0', u'rightIndexFingerPitch3Link_collision_0', u'rightMiddleFingerPitch1Link_collision_0', u'rightMiddleFingerPitch2Link_collision_0', u'rightMiddleFingerPitch3Link_collision_0', u'rightPinkyPitch1Link_collision_0', u'rightPinkyPitch2Link_collision_0', u'rightPinkyPitch3Link_collision_0', u'rightThumbRollLink_collision_0', u'rightThumbPitch1Link_collision_0', u'rightThumbPitch2Link_collision_0', u'rightThumbPitch3Link_collision_0'], u'leftAnkleRoll': [u'leftFoot_collision_0', u'leftFoot_collision_1', u'leftFoot_collision_2', u'leftFoot_collision_3', u'leftFoot_collision_4', u'leftFoot_collision_5', u'leftFoot_collision_6', u'leftFoot_collision_7', u'leftFoot_collision_8'], u'leftElbowPitch': [u'leftElbowPitchLink_collision_0'], u'leftAnklePitch': [u'leftAnklePitchLink_collision_0'], u'leftWristRoll': [u'leftWristRollLink_collision_0'], u'leftForearmYaw': [u'leftForearmLink_collision_0'], u'leftShoulderRoll': [u'leftShoulderRollLink_collision_0'], u'leftHipPitch': [u'leftHipPitchLink_collision_0'], u'lowerNeckPitch': [u'lowerNeckPitchLink_collision_0'], u'rightShoulderRoll': [u'rightShoulderRollLink_collision_0'], u'rightHipYaw': [u'rightHipYawLink_collision_0'], u'leftKneePitch': [u'leftKneePitchLink_collision_0'], u'rightForearmYaw': [u'rightForearmLink_collision_0'], u'neckYaw': [u'neckYawLink_collision_0'], u'rightShoulderPitch': [u'rightShoulderPitchLink_collision_0'], u'leftWristPitch': [u'leftPalm_collision_0',     u'leftIndexFingerPitch1Link_collision_0', u'leftIndexFingerPitch2Link_collision_0', u'leftIndexFingerPitch3Link_collision_0', u'leftMiddleFingerPitch1Link_collision_0', u'leftMiddleFingerPitch2Link_collision_0', u'leftMiddleFingerPitch3Link_collision_0', u'leftPinkyPitch1Link_collision_0', u'leftPinkyPitch2Link_collision_0', u'leftPinkyPitch3Link_collision_0', u'leftThumbRollLink_collision_0', u'leftThumbPitch1Link_collision_0', u'leftThumbPitch2Link_collision_0', u'leftThumbPitch3Link_collision_0'], u'rightShoulderYaw': [u'rightShoulderYawLink_collision_0'], u'rightAnklePitch': [u'rightAnklePitchLink_collision_0'], u'torsoPitch': [u'torsoPitchLink_collision_0'], u'rightHipRoll': [u'rightHipRollLink_collision_0'], u'leftShoulderPitch': [u'leftShoulderPitchLink_collision_0'], u'rightKneePitch': [u'rightKneePitchLink_collision_0'], u'leftShoulderYaw': [u'leftShoulderYawLink_collision_0'], u'rightHipPitch': [u'rightHipPitchLink_collision_0'], u'rightAnkleRoll': [u'rightFoot_collision_0', u'rightFoot_collision_1', u'rightFoot_collision_2', u'rightFoot_collision_3', u'rightFoot_collision_4', u'rightFoot_collision_5', u'rightFoot_collision_6', u'rightFoot_collision_7', u'rightFoot_collision_8']}
        for key in true_output:
            if self.scene.controlled_joint_to_collision_link_map[key] != true_output[key]:
                print('Different output for', key, 'True:', true_output[key], 'vs', 'Returned:', self.scene.controlled_joint_to_collision_link_map[key])
                raise AssertionError("Different output.")


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