#!/usr/bin/env python
from __future__ import print_function
import pyexotica as exo
import math
import time
import numpy as np

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

import unittest

PENETRATING_DISTANCE_ATOL = 1e-6
AFAR_DISTANCE_ATOL = 1e-3
CLOSE_DISTANCE_ATOL = 1e-6
PUBLISH_PROXIES = False

if PUBLISH_PROXIES:
    exo.Setup.init_ros()

def get_problem_initializer(collision_scene, URDF):
    return ('exotica/UnconstrainedEndPoseProblem',
            {'Name': 'TestProblem',
             'PlanningScene': [('exotica/Scene',
                                {'CollisionScene': [(collision_scene, {'Name': 'MyCollisionScene'})],
                                 'JointGroup': 'group1',
                                 'Name': 'TestScene',
                                 'Debug': '0',
                                 'SRDF': '{exotica_examples}/test/resources/a_vs_b.srdf',
                                 'SetRobotDescriptionRosParams': '1',
                                 'URDF': URDF})]})


def debug_publish(p, scene):
    if PUBLISH_PROXIES:
        scene.get_kinematic_tree().publish_frames()
        scene.publish_proxies(p)
        time.sleep(.5)


def test_sphere_vs_sphere_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_sphere_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_sphere_vs_primitive_sphere_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_almost_equal(p[0].distance, 1.)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_sphere_vs_primitive_sphere_distance: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_sphere_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_sphere_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_sphere_vs_primitive_sphere_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -.4, atol=PENETRATING_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_sphere_penetrating: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_box_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_box_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_sphere_vs_primitive_box_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.5)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([1, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_box_distance: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_box_touching(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_box_touching.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_sphere_vs_primitive_box_touching: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 0.)
    expected_contact_1 = np.array([0, 0, 0])
    expected_contact_2 = np.array([0, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_box_touching: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_box_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_box_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_sphere_vs_primitive_box_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -0.4, atol=PENETRATING_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_box_penetrating: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_cylinder_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_cylinder_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_sphere_vs_primitive_cylinder_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.5)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([1, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_cylinder_distance: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_cylinder_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_primitive_cylinder_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_sphere_vs_primitive_cylinder_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -0.4, atol=PENETRATING_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_sphere_vs_primitive_cylinder_penetrating: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_mesh_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_mesh_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_sphere_vs_mesh_distance: is_state_valid(True): PASSED')

    expected_distance = 1
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])

    p = scene.get_collision_distance("A", "B")
    print(p)
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, expected_distance)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_sphere_vs_mesh_distance: _distance, Contact Points, Normals: PASSED')

    # mesh_vs_Sphere Test
    p = scene.get_collision_distance("B", "A")
    print(p)
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, expected_distance)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_sphere_distance: _distance, Contact Points, Normals: PASSED')


def test_sphere_vs_mesh_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_sphere_vs_mesh_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_sphere_vs_mesh_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    print(p)
    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -.4, atol=PENETRATING_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_sphere_vs_mesh_penetrating: _distance, Contact Points, Normals: PASSED')

    # mesh_vs_Sphere Test
    p = scene.get_collision_distance("B", "A")
    print(p)
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_sphere_penetrating: _distance, Contact Points, Normals: PASSED')


def test_box_vs_box_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_primitive_box_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_box_vs_primitive_box_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    print(p)
    np.testing.assert_allclose(p[0].contact_1[0], expected_contact_1[0], atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2[0], expected_contact_2[0], atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=CLOSE_DISTANCE_ATOL)
    print('primitive_box_vs_primitive_box_distance: _distance, Contact Points, Normals: PASSED')


def test_box_vs_box_touching(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_primitive_box_touching.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_box_vs_primitive_box_touching: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 0.)
    expected_contact_1 = np.array([0, 0, 0])
    expected_contact_2 = np.array([0, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1-p[0].contact_2, np.zeros((3,)))
    # Only check x-axis as this is face to face contact and thus undefined where y and z are.
    np.testing.assert_allclose(p[0].contact_1[0], expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2[0], expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_box_vs_primitive_box_touching: _distance, Contact Points, Normals: PASSED')


def test_box_vs_box_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_primitive_box_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_box_vs_primitive_box_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -.2)
    expected_contact_1 = np.array([0.1, 0, 0])
    expected_contact_2 = np.array([-0.1, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    # Only test the x-component for the contacts as the faces are parallel.
    np.testing.assert_allclose(p[0].contact_1[0], expected_contact_1[0])
    np.testing.assert_allclose(p[0].contact_2[0], expected_contact_2[0])
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_box_vs_primitive_box_penetrating: _distance, Contact Points, Normals: PASSED')


def test_box_vs_cylinder_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_primitive_cylinder_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_box_vs_primitive_cylinder_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1[0], expected_contact_1[0]) # TODO: face
    np.testing.assert_allclose(p[0].contact_2[0], expected_contact_2[0]) # TODO: face
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_box_vs_primitive_cylinder_distance: _distance, Contact Points, Normals: PASSED')


def test_box_vs_cylinder_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_primitive_cylinder_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_box_vs_primitive_cylinder_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -.2, atol=CLOSE_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.1, 0, 0])
    expected_contact_2 = np.array([-0.1, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_box_vs_primitive_cylinder_penetrating: _distance, Contact Points, Normals: PASSED')


def test_box_vs_mesh_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_mesh_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_box_vs_mesh_distance: is_state_valid(True): PASSED')

    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    print(p)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_box_vs_mesh_distance: _distance, Contact Points, Normals: PASSED')

    # mesh_vs_box
    p = scene.get_collision_distance("B", "A")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_box_distance: _distance, Contact Points, Normals: PASSED')


def test_box_vs_mesh_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_box_vs_mesh_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_box_vs_mesh_penetrating: is_state_valid(True): PASSED')

    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    print(p)
    np.testing.assert_allclose(p[0].distance, -.2, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_box_vs_mesh_penetrating: _distance, Contact Points, Normals: PASSED')
    
    # mesh_vs_box
    p = scene.get_collision_distance("B", "A")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    print(p)
    np.testing.assert_allclose(p[0].distance, -.2, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_box_penetrating: _distance, Contact Points, Normals: PASSED')


def test_cylinder_vs_cylinder_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_cylinder_vs_primitive_cylinder_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_cylinder_vs_primitive_cylinder_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1[0], expected_contact_1[0]) # TODO: face
    np.testing.assert_allclose(p[0].contact_2[0], expected_contact_2[0]) # TODO: face
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_cylinder_vs_primitive_cylinder_distance: _distance, Contact Points, Normals: PASSED')


def test_cylinder_vs_cylinder_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_cylinder_vs_primitive_cylinder_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_cylinder_vs_primitive_cylinder_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, -.2, atol=CLOSE_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.1, 0, 0])
    expected_contact_2 = np.array([-0.1, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=AFAR_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=AFAR_DISTANCE_ATOL)
    print('primitive_cylinder_vs_primitive_cylinder_penetrating: _distance, Contact Points, Normals: PASSED')


def test_cylinder_vs_mesh_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_cylinder_vs_mesh_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('primitive_cylinder_vs_mesh_distance: is_state_valid(True): PASSED')

    expected_contact_1 = np.array([-1, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    print(p)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.5)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_cylinder_vs_mesh_distance: _distance, Contact Points, Normals: PASSED')

    # mesh vs cylinder
    p = scene.get_collision_distance("B", "A")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1.5)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_cylinder_vs_distance: _distance, Contact Points, Normals: PASSED')


def test_cylinder_vs_mesh_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/primitive_cylinder_vs_mesh_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('primitive_cylinder_vs_mesh_penetrating: is_state_valid(True): PASSED')

    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    print(p)
    np.testing.assert_allclose(p[0].distance, -.1, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('primitive_cylinder_vs_mesh_penetrating: _distance, Contact Points, Normals: PASSED')

    # mesh vs cylinder
    p = scene.get_collision_distance("B", "A")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    print(p)
    np.testing.assert_allclose(p[0].distance, -.1, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_1, expected_contact_2)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_1)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_2)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_1)
    print('mesh_vs_primitive_cylinder_penetrating: _distance, Contact Points, Normals: PASSED')


def test_mesh_vs_mesh_distance(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/mesh_vs_mesh_distance.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), True)
    print('mesh_vs_mesh_distance: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    np.testing.assert_allclose(p[0].distance, 1., atol=CLOSE_DISTANCE_ATOL)
    expected_contact_1 = np.array([-0.5, 0, 0])
    expected_contact_2 = np.array([0.5, 0, 0])
    expected_normal_1 = np.array([1, 0, 0])
    expected_normal_2 = np.array([-1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1, atol=CLOSE_DISTANCE_ATOL)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2, atol=CLOSE_DISTANCE_ATOL)
    print('mesh_vs_mesh_distance: _distance, Contact Points, Normals: PASSED')


def test_mesh_vs_mesh_penetrating(collision_scene):
    problem_initializer = get_problem_initializer(collision_scene, '{exotica_examples}/test/resources/mesh_vs_mesh_penetrating.urdf')
    prob = exo.Setup.create_problem(problem_initializer)
    prob.update(np.zeros(prob.N,))
    scene = prob.get_scene()

    np.testing.assert_equal(scene.is_state_valid(True), False)
    print('mesh_vs_mesh_penetrating: is_state_valid(True): PASSED')

    p = scene.get_collision_distance("A", "B")
    debug_publish(p, scene)
    np.testing.assert_equal(len(p), 1)
    print(p)
    np.testing.assert_allclose(p[0].distance, -.1, atol=CLOSE_DISTANCE_ATOL)
    expected_contact_1 = np.array([0.2, 0, 0])
    expected_contact_2 = np.array([-0.2, 0, 0])
    expected_normal_1 = np.array([-1, 0, 0])
    expected_normal_2 = np.array([1, 0, 0])
    np.testing.assert_allclose(p[0].contact_1, expected_contact_1)
    np.testing.assert_allclose(p[0].contact_2, expected_contact_2)
    np.testing.assert_allclose(p[0].normal_1, expected_normal_1)
    np.testing.assert_allclose(p[0].normal_2, expected_normal_2)
    print('mesh_vs_mesh_penetrating: _distance, Contact Points, Normals: PASSED')


#########################################

# Cf. Issue #364 for tracking deactivated tests.
class TestClass(unittest.TestCase):
    collision_scene = 'exotica/CollisionSceneFCLLatest'
    def test_sphere_vs_sphere_distance(self):
        test_sphere_vs_sphere_distance(TestClass.collision_scene)

    def test_sphere_vs_sphere_penetrating(self):
        test_sphere_vs_sphere_penetrating(TestClass.collision_scene)

    def test_sphere_vs_box_distance(self):
        test_sphere_vs_box_distance(TestClass.collision_scene)

    def test_sphere_vs_box_touching(self):
        test_sphere_vs_box_touching(TestClass.collision_scene)

    def test_sphere_vs_box_penetrating(self):
        test_sphere_vs_box_penetrating(TestClass.collision_scene)

    def test_sphere_vs_cylinder_distance(self):
        test_sphere_vs_cylinder_distance(TestClass.collision_scene)

    def test_sphere_vs_cylinder_penetrating(self):
        test_sphere_vs_cylinder_penetrating(TestClass.collision_scene)

#     def test_sphere_vs_mesh_distance(self):
#         test_sphere_vs_mesh_distance(TestClass.collision_scene)  # includes mesh vs sphere (distance OK, points not)

#     def test_sphere_vs_mesh_penetrating(self):
#         test_sphere_vs_mesh_penetrating(TestClass.collision_scene)   # BROKEN with libccd (not implemented)

    def test_box_vs_box_distance(self):
        test_box_vs_box_distance(TestClass.collision_scene)

    def test_box_vs_box_touching(self):
        test_box_vs_box_touching(TestClass.collision_scene)

    def test_box_vs_box_penetrating(self):
        test_box_vs_box_penetrating(TestClass.collision_scene)

    def test_box_vs_cylinder_distance(self):
        test_box_vs_cylinder_distance(TestClass.collision_scene)

    def test_box_vs_cylinder_penetrating(self):
        test_box_vs_cylinder_penetrating(TestClass.collision_scene)

    def test_box_vs_mesh_distance(self):
        test_box_vs_mesh_distance(TestClass.collision_scene)  # includes mesh vs box

#     def test_box_vs_mesh_penetrating(self):
#         test_box_vs_mesh_penetrating(collision_scene)   # BROKEN with libccd (not implemented)

    def test_cylinder_vs_cylinder_distance(self):
        test_cylinder_vs_cylinder_distance(TestClass.collision_scene)

    def test_cylinder_vs_cylinder_penetrating(self):
        test_cylinder_vs_cylinder_penetrating(TestClass.collision_scene)

    def test_cylinder_vs_mesh_distance(self):
        test_cylinder_vs_mesh_distance(TestClass.collision_scene)  # includes mesh vs cylinder

#     def test_cylinder_vs_mesh_penetrating(self):
#         test_cylinder_vs_mesh_penetrating(collision_scene)   # BROKEN with libccd (not implemented)

    def test_mesh_vs_mesh_distance(self):
        test_mesh_vs_mesh_distance(TestClass.collision_scene)

#     def test_mesh_vs_mesh_penetrating(self):
#         test_mesh_vs_mesh_penetrating(collision_scene)    # BROKEN with libccd (very inaccurate distance)

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