//
// Copyright (c) 2022 INRIA
//

#include "pinocchio/collision/collision.hpp"
#include "pinocchio/collision/broadphase.hpp"
#include "pinocchio/collision/tree-broadphase-manager.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#include <coal/broadphase/broadphase_dynamic_AABB_tree.h>

#include <vector>
#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_tree_broadphase_with_empty_models)
{
  Model model;
  GeometryModel geom_model;
  GeometryData geom_data(geom_model);

  TreeBroadPhaseManagerTpl<coal::DynamicAABBTreeCollisionManager> broadphase_manager(
    &model, &geom_model, &geom_data);

  BOOST_CHECK(broadphase_manager.check());
}

BOOST_AUTO_TEST_CASE(test_collisions)
{
  const std::string filename =
    EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/romeo_description/urdf/romeo_small.urdf");
  std::vector<std::string> packageDirs;
  const std::string meshDir =
    boost::filesystem::path(EXAMPLE_ROBOT_DATA_MODEL_DIR).parent_path().parent_path().string();
  packageDirs.push_back(meshDir);
  const std::string srdf_filename =
    EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/romeo_description/srdf/romeo.srdf");

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
  GeometryModel geom_model;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
  geom_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false);

  Data data(model), data_broadphase(model);
  GeometryData geom_data(geom_model), geom_data_broadphase(geom_model);

  pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false);
  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];

  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
  pinocchio::updateGeometryPlacements(model, data_broadphase, geom_model, geom_data_broadphase, q);

  BOOST_CHECK(computeCollisions(geom_model, geom_data) == false);
  BOOST_CHECK(computeCollisions(geom_model, geom_data, false) == false);

  TreeBroadPhaseManagerTpl<coal::DynamicAABBTreeCollisionManager> broadphase_manager(
    &model, &geom_model, &geom_data_broadphase);
  BOOST_CHECK(computeCollisions(broadphase_manager) == false);
  BOOST_CHECK(computeCollisions(broadphase_manager, false) == false);
  BOOST_CHECK(computeCollisions(model, data_broadphase, broadphase_manager, q) == false);
  BOOST_CHECK(computeCollisions(model, data_broadphase, broadphase_manager, q, false) == false);

  const int num_configs = 1000;
  for (int i = 0; i < num_configs; ++i)
  {
    Eigen::VectorXd q_rand = randomConfiguration(model);
    q_rand.head<7>() = q.head<7>();

    BOOST_CHECK(
      computeCollisions(model, data_broadphase, broadphase_manager, q_rand)
      == computeCollisions(model, data, geom_model, geom_data, q_rand));
  }
}

BOOST_AUTO_TEST_SUITE_END()
