//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#include <iostream>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE UrdfTest
#include <boost/test/unit_test.hpp>

using namespace se3;
using namespace std;

BOOST_AUTO_TEST_SUITE(SRDF)

BOOST_AUTO_TEST_CASE(removeCollisionPairs)
{
  using namespace se3::urdf;
  using namespace se3::srdf;
  const string model_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
  const string model_dir = PINOCCHIO_SOURCE_DIR"/models/romeo";
  const string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/srdf/romeo_collision.srdf";
  
  Model model;
  buildModel(model_filename, model);
  
  GeometryModel geom_model;
  vector<string> paths; paths.push_back(model_dir);
  buildGeom(model,model_filename,COLLISION,geom_model,paths);
  
  geom_model.addAllCollisionPairs();
  
  const size_t num_init_col_pairs = geom_model.collisionPairs.size();
  
  removeCollisionPairsFromSrdf(model,geom_model,srdf_filename,false);
  const size_t num_col_pairs = geom_model.collisionPairs.size();
  
  BOOST_CHECK(num_init_col_pairs > num_col_pairs);
}
  
BOOST_AUTO_TEST_CASE(readNeutralConfig)
{
  using namespace se3::urdf;
  using namespace se3::srdf;
  const string model_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/urdf/romeo.urdf";
  const string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/srdf/romeo_collision.srdf";
  
  Model model;
  buildModel(model_filename, model);

  Eigen::VectorXd q = getNeutralConfigurationFromSrdf(model,srdf_filename,false);
  
  BOOST_CHECK(q.size() == model.nq);
  BOOST_CHECK(!q.isZero());
}
  
BOOST_AUTO_TEST_SUITE_END()