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

#pragma once

// IWYU pragma: begin_keep
#include <Eigen/Core>

#include <coal/broadphase/broadphase_collision_manager.h>

#include "pinocchio/multibody.hpp"

#include "pinocchio/collision/broadphase-manager.hpp"
#include "pinocchio/collision/broadphase-callbacks.hpp"
// IWYU pragma: end_keep

namespace pinocchio
{

  ///
  /// \brief Calls computeCollision for every active pairs of GeometryData.
  /// This function assumes that \ref updateGeometryPlacements and  broadphase_manager.update() have
  /// been called first.
  ///
  /// \param[in] broadphase_manager broadphase instance for collision detection.
  /// \param[in] callback callback pointer used for collision detection.
  ///
  /// \warning if stopAtFirstcollision = true, then the collisions vector will
  /// not be entirely fulfilled (of course).
  ///
  template<typename BroadPhaseManagerDerived>
  bool computeCollisions(
    BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
    CollisionCallBackBase * callback);

  ///
  /// \brief Calls computeCollision for every active pairs of GeometryData.
  /// This function assumes that \ref updateGeometryPlacements and  broadphase_manager.update() have
  /// been called first.
  ///
  /// \param[in] broadphase_manager broadphase instance for collision detection.
  /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
  /// collision is detected.
  ///
  /// \warning if stopAtFirstcollision = true, then the collisions vector will
  /// not be entirely fulfilled (of course).
  ///
  template<typename BroadPhaseManagerDerived>
  bool computeCollisions(
    BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
    const bool stopAtFirstCollision = false);

  ///
  /// Compute the forward kinematics, update the geometry placements and run the collision detection
  /// using the broadphase manager.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  ///
  /// \param[in] model robot model (const)
  /// \param[out] data corresponding data (nonconst) where the forward kinematics results are stored
  /// \param[in] broadphase_manager broadphase manager for collision detection.
  /// \param[in] callback callback pointer used for collision detection.///
  /// \param[in] q robot configuration.
  ///
  /// \warning if stopAtFirstcollision = true, then the collisions vector will
  /// not be entirely fulfilled (of course).
  /// \note A similar function is available without model, data and q, not recomputing the forward
  /// kinematics.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename BroadPhaseManagerDerived,
    typename ConfigVectorType>
  inline bool computeCollisions(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
    CollisionCallBackBase * callback,
    const Eigen::MatrixBase<ConfigVectorType> & q);

  ///
  /// Compute the forward kinematics, update the geometry placements and run the collision detection
  /// using the broadphase manager.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  ///
  /// \param[in] model robot model (const)
  /// \param[out] data corresponding data (nonconst) where the forward kinematics results are stored
  /// \param[in] broadphase_manager broadphase manager for collision detection.
  /// \param[in] q robot configuration.
  /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
  /// collision is detected.
  ///
  /// \warning if stopAtFirstcollision = true, then the collisions vector will
  /// not be entirely fulfilled (of course).
  /// \note A similar function is available without model, data and q, not recomputing the forward
  /// kinematics.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename BroadPhaseManagerDerived,
    typename ConfigVectorType>
  inline bool computeCollisions(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const bool stopAtFirstCollision = false);

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/collision/broadphase.hxx"
// IWYU pragma: end_exports
