/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef GAZEBO_PHYSICS_CONTACTMANAGER_HH_
#define GAZEBO_PHYSICS_CONTACTMANAGER_HH_

#include <vector>
#include <string>
#include <map>
#include <ignition/transport/Node.hh>

#include <boost/unordered/unordered_set.hpp>
#include <boost/unordered/unordered_map.hpp>
#include <boost/thread/recursive_mutex.hpp>

#include "gazebo/transport/TransportTypes.hh"

#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/Contact.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace physics
  {
    /// \brief A custom contact publisher created for each contact filter
    /// in the Contact Manager.
    class GZ_PHYSICS_VISIBLE ContactPublisher
    {
      /// \brief Contact message publisher
      public: transport::PublisherPtr publisher;

      /// \brief Pointers of collisions monitored by contact manager for
      /// contacts.
      public: boost::unordered_set<Collision *> collisions;

      /// \internal
      /// \brief Names of collisions passed in by CreateFilter. Cleared
      /// once converted to pointers.
      public: std::vector<std::string> collisionNames;

      /// \brief A list of contacts associated to the collisions.
      public: std::vector<Contact *> contacts;

      // Place ignition::transport objects at the end of this file to
      // guarantee they are destructed first.

      /// \brief Ignition contact message publisher
      public: ignition::transport::Node::Publisher publisherIgn;
    };

    /// \addtogroup gazebo_physics
    /// \{

    /// \class ContactManager ContactManager.hh physics/physics.hh
    /// \brief Aggregates all the contact information generated by the
    /// collision detection engine.
    class GZ_PHYSICS_VISIBLE ContactManager
    {
      /// \brief Constructor.
      public: ContactManager();

      /// \brief Destructor.
      public: virtual ~ContactManager();

      /// \brief Initialize the ContactManager. This is required in order to
      /// publish contact messages via the ContactManager::PublishContacts
      /// method.
      /// \param[in] _world Pointer to the world that is initializing the
      /// contact manager.
      public: void Init(WorldPtr _world);

      /// \brief Add a new contact.
      ///
      /// Normally this is only used by a Physics/Collision engine when
      /// a new contact is generated. All other users should just make use
      /// of the accessor functions.
      ///
      /// If no one is listening, then the return value will be NULL (unless
      /// SetNeverDropContacts(true) was called).
      /// This is then a signal to the Physics engine that it can skip the
      /// extra processing necessary to get back contact information.
      ///
      /// \param[in] _collision1 the first collision object
      /// \param[in] _collision2 the second collision object
      /// \param[in] _time the time of the contact
      ///
      /// \return The new contact. The physics engine should populate the
      /// contact's parameters. NULL will be returned if there are no
      /// subscribers to the contact topic and NeverDropContacts()
      /// returns false (default).
      public: Contact *NewContact(Collision *_collision1,
                                  Collision *_collision2,
                                  const common::Time &_time);

      /// \brief If set to true, NewContact() will always add contacts
      /// even if there are no subscribers.
      /// \param[in] _neverDrop if true, never drop contact computation
      public: void SetNeverDropContacts(const bool _neverDrop);

      /// \brief returns the value last set with SetNeverDropContacts().
      /// \return the value last set with SetNeverDropContacts().
      /// If SetNeverDropContacts() was never called, this will return false.
      public: bool NeverDropContacts() const;

      /// \brief Returns true if any subscribers are connected
      /// which would be interested in contact details of either collision
      /// \e _collision1 or \e collision2, given that they have been loaded
      /// into the world already.
      /// This is the same test which NewContact() uses to determine whether
      /// there are any subscribers for the contacts, but the test here is
      /// optimized because it returns as soon as one subscriber is found which
      /// listens to either of the collisions.
      /// Note that this function needs to go through the list of custom
      /// publishers and check whether \e _collsion1 or \e _collision2
      /// are part of the custom publishers and whether they have been loaded
      /// into the world, which comes at a cost.
      /// Unless there are any benefits in calling this function ahead of
      /// NewContact, it may be better to just use NewContact() directly.
      /// Also note that in order to exclude that NewContact() returns NULL,
      /// it is advisable to check NeverDropContacts() first (if it returns
      /// true, NewContacts() never returns NULL).
      /// \param[in] _collision1 the first collision object
      /// \param[in] _collision2 the second collision object
      /// \return true if any subscribers are connected for this pair
      public: bool SubscribersConnected(Collision *_collision1,
                                        Collision *_collision2) const;

      /// \brief Return the number of valid contacts.
      public: unsigned int GetContactCount() const;

      /// \brief Get a single contact by index. The index must be between
      /// 0 and ContactManager::GetContactCount.
      /// \param[in] _index Index of the Contact to return.
      /// \return Pointer to a contact, NULL If index is invalid.
      public: Contact *GetContact(unsigned int _index) const;

      /// \brief Get all the contacts.
      ///
      /// The return vector may have invalid contacts. Only use contents of
      /// the vector between 0 and ContactManager::GetContactCount
      /// \return Vector of contact pointers.
      public: const std::vector<Contact *> &GetContacts() const;

      /// \brief Clear all stored contacts.
      public: void Clear();

      /// \brief Publish all contacts in a msgs::Contacts message.
      public: void PublishContacts();

      /// \brief Set the contact count to zero.
      public: void ResetCount();

      /// \brief Create a filter for contacts. A new publisher will be created
      /// that publishes contacts associated to the input collisions.
      /// param[in] _name Filter name.
      /// param[in] _collisions A list of collision names used for filtering.
      /// \return New topic where filtered messages will be published to.
      public: std::string CreateFilter(const std::string &_topic,
                  const std::vector<std::string> &_collisions);

      /// \brief Create a filter for contacts. A new publisher will be created
      /// that publishes contacts associated to the input collision.
      /// param[in] _name Filter name.
      /// param[in] _collision A collision name used for filtering.
      /// \return New topic where filtered messages will be published to.
      public: std::string CreateFilter(const std::string &_topic,
                  const std::string &_collision);

      /// \brief Create a filter for contacts. A new publisher will be created
      /// that publishes contacts associated to the input collision.
      /// param[in] _name Filter name.
      /// param[in] _collisions A map of collision name to collision
      /// object.
      /// \return New topic where filtered messages will be published to.
      public: std::string CreateFilter(const std::string &_name,
                  const std::map<std::string, physics::CollisionPtr>
                  &_collisions);

      /// \brief Remove a contacts filter and the associated custom publisher
      /// param[in] _name Filter name.
      public: void RemoveFilter(const std::string &_name);

      /// \brief Get the number of filters in the contact manager.
      /// return Number of filters
      public: unsigned int GetFilterCount();

      /// \brief Check if a filter with the specified name exists.
      /// param[in] _name Name of filter.
      /// return True if the filter exists.
      public: bool HasFilter(const std::string &_name);

      /// \brief Helper function which gets the custom publishers which publish
      ///   contacts of either \e _collision1 or \e _collision2.
      /// \param[in] _collision1 the first collision object
      /// \param[in] _collision2 the second collision object
      /// \param[in] _getOnlyConnected return only publishers which currently
      ///   have subscribers
      /// \param[out] _publishers the resulting publishers.
      private: void GetCustomPublishers(Collision *_collision1,
                       Collision *_collision2, const bool _getOnlyConnected,
                       std::vector<ContactPublisher*> &_publishers);

      private: std::vector<Contact*> contacts;

      private: unsigned int contactIndex;

      /// \brief Node for communication.
      private: transport::NodePtr node;

      /// \brief Contact publisher.
      private: transport::PublisherPtr contactPub;

      /// \brief Pointer to the world.
      private: WorldPtr world;

      /// \brief A list of custom publishers that publish filtered contact
      /// messages to the specified topic
      private: boost::unordered_map<std::string, ContactPublisher *>
          customContactPublishers;

      /// \brief Mutex to protect the list of custom publishers.
      private: boost::recursive_mutex *customMutex;

      // Place ignition::transport objects at the end of this file to
      // guarantee they are destructed first.

      /// \brief Ignition node for communication.
      private: ignition::transport::Node nodeIgn;

      /// \brief Contact publisher.
      private: ignition::transport::Node::Publisher contactPubIgn;

      /// \brief Addition of new contacts happens also with no subscribers.
      /// This takes effect if NewContact() is called if there
      /// are no subscribers. Default is false.
      private: bool neverDropContacts;
    };
    /// \}
  }
}
#endif
