/*
 * 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_TRANSPORT_TOPICMANAGER_HH_
#define GAZEBO_TRANSPORT_TOPICMANAGER_HH_

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <map>
#include <list>
#include <string>
#include <vector>
#include <boost/unordered/unordered_set.hpp>

#include "gazebo/common/Assert.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/common/SingletonT.hh"

#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/transport/SubscribeOptions.hh"
#include "gazebo/transport/SubscriptionTransport.hh"
#include "gazebo/transport/PublicationTransport.hh"
#include "gazebo/transport/ConnectionManager.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/Publication.hh"
#include "gazebo/transport/Subscriber.hh"
#include "gazebo/util/system.hh"

/// \brief Explicit instantiation for typed SingletonT.
GZ_SINGLETON_DECLARE(GZ_TRANSPORT_VISIBLE, gazebo, transport, TopicManager)

namespace gazebo
{
  namespace transport
  {
    /// \addtogroup gazebo_transport
    /// \{

    /// \class TopicManager TopicManager.hh transport/transport.hh
    /// \brief Manages topics and their subscriptions
    class GZ_TRANSPORT_VISIBLE TopicManager : public SingletonT<TopicManager>
    {
      private: TopicManager();
      private: virtual ~TopicManager();

      /// \brief Initialize the manager
      public: void Init();

      /// \brief Finalize the manager
      public: void Fini();

      /// \brief Find a publication object by topic
      /// \param[in] _topic The topic to search for
      /// \return Pointer to the publication object, if found (can be null)
      public: PublicationPtr FindPublication(const std::string &_topic);

      /// \brief Add a node to the manager
      /// \param[in,out] _node The node to be added
      public: void AddNode(NodePtr _node);

      /// \brief Remove a node by its id
      /// \param[in] _id The ID of the node to be removed
      public: void RemoveNode(unsigned int _id);

      /// \brief Process all nodes under management
      /// \param[in] _onlyOut True means only outbound messages on nodes will be
      /// sent. False means nodes process both outbound and inbound messages
      public: void ProcessNodes(bool _onlyOut = false);

      /// \brief Subscribe to a topic
      /// \param[in] _options The options to use for the subscription
      /// \return Pointer to the newly created subscriber
      public: SubscriberPtr Subscribe(const SubscribeOptions &_options);

      /// \brief Unsubscribe from a topic. Use a Subscriber rather than
      ///        calling this function directly
      /// \param[in] _topic The topic to unsubscribe from
      /// \param[in] _sub The node to unsubscribe
      public: void Unsubscribe(const std::string &_topic, const NodePtr &_sub);

      /// \brief Advertise on a topic
      /// \param[in] _topic The name of the topic
      /// \param[in] _queueLimit The maximum number of outgoing messages
      /// to queue
      /// \param[in] _hz Update rate for the publisher. Units are
      /// 1.0/seconds.
      /// \return Pointer to the newly created Publisher
      public: template<typename M>
              PublisherPtr Advertise(const std::string &_topic,
                                     unsigned int _queueLimit,
                                     double _hzRate)
              {
                google::protobuf::Message *msg = NULL;
                M msgtype;
                msg = dynamic_cast<google::protobuf::Message *>(&msgtype);
                if (!msg)
                  gzthrow("Advertise requires a google protobuf type");

                this->UpdatePublications(_topic, msg->GetTypeName());

                PublisherPtr pub = PublisherPtr(new Publisher(_topic,
                      msg->GetTypeName(), _queueLimit, _hzRate));

                std::string msgTypename;
                PublicationPtr publication;

                // Connect all local subscription to the publisher
                msgTypename = msg->GetTypeName();

                publication = this->FindPublication(_topic);
                GZ_ASSERT(publication != NULL, "FindPublication returned NULL");

                publication->AddPublisher(pub);
                if (!publication->GetLocallyAdvertised())
                {
                  ConnectionManager::Instance()->Advertise(_topic, msgTypename);
                }

                publication->SetLocallyAdvertised(true);
                pub->SetPublication(publication);


                SubNodeMap::iterator iter2;
                SubNodeMap::iterator stEnd2 = this->subscribedNodes.end();
                for (iter2 = this->subscribedNodes.begin();
                     iter2 != stEnd2; ++iter2)
                {
                  if (iter2->first == _topic)
                  {
                    std::list<NodePtr>::iterator liter;
                    std::list<NodePtr>::iterator lEnd = iter2->second.end();
                    for (liter = iter2->second.begin();
                        liter != lEnd; ++liter)
                    {
                      publication->AddSubscription(*liter);
                    }
                  }
                }

                return pub;
              }

      /// \brief Unadvertise a topic
      /// \param[in] _topic The topic to be unadvertised
      public: void Unadvertise(const std::string &_topic);

      /// \brief Unadvertise a publisher.
      /// \param[in] _pub Publisher to unadvertise.
      public: void Unadvertise(PublisherPtr _pub);

      /// \brief Unadvertise a publisher, based on a publisher id.
      /// \param[in] _topic The publisher's topic. It will also be unadvertised
      /// if there are no advertised publishers left.
      /// \param[in] _id ID of the publisher to unadvertise.
      public: void Unadvertise(const std::string &_topic, const uint32_t _id);

      /// \brief Send a message. Use a Publisher instead of calling this
      ///        function directly.
      /// \param [in] _topic Name of the topic
      /// \param [in] _message The message to send.
      /// \param [in] _cb Callback, used when the publish is completed.
      /// \param [in] _id ID associated with the message.
      public: void Publish(const std::string &_topic, MessagePtr _message,
                  boost::function<void(uint32_t)> _cb, uint32_t _id);

      /// \brief Connection a local Publisher to a remote Subscriber
      /// \param[in] _topic The topic to use
      /// \param[in] _sublink The subscription transport object to use
      public: void ConnectPubToSub(const std::string &_topic,
                                    const SubscriptionTransportPtr _sublink);

      /// \brief Connect a local Subscriber to a remote Publisher
      /// \param[in] _pub The publish object to use
      public: void ConnectSubToPub(const msgs::Publish &_pub);

      /// \brief Disconnect a local publisher from a remote subscriber
      /// \param[in] _topic The topic to be disconnected
      /// \param[in] _host The host to be disconnected
      /// \param[in] _port The port to be disconnected
      public: void DisconnectPubFromSub(const std::string &_topic,
                                         const std::string &_host,
                                         unsigned int _port);

      /// \brief Disconnect all local subscribers from a remote publisher
      /// \param[in] _topic The topic to be disconnected
      /// \param[in] _host The host to be disconnected
      /// \param[in] _port The port to be disconnected
      public: void DisconnectSubFromPub(const std::string &_topic,
                                         const std::string &_host,
                                         unsigned int _port);

      /// \brief Connect all subscribers on a topic to known publishers
      /// \param[in] _topic The topic to be connected
      public: void ConnectSubscribers(const std::string &_topic);

      /// \brief Update our list of advertised topics
      /// \param[in] _topic The topic to be updated
      /// \param[in] _msgType The type of the topic to be updated
      /// \return True if the provided params define a new publisher,
      /// false otherwise
      public: PublicationPtr UpdatePublications(const std::string &_topic,
                                                const std::string &_msgType);

      /// \brief Register a new topic namespace
      /// \param[in] _name The name of the new namespace
      public: void RegisterTopicNamespace(const std::string &_name);

      /// \brief Get all the topic namespaces
      /// \param[out] _namespaces The list of namespaces will be written here
      public: void GetTopicNamespaces(std::list<std::string> &_namespaces);

      /// \brief Clear all buffers
      public: void ClearBuffers();

      /// \brief Pause or unpause processing of incoming messages
      /// \param[in] _pause If true pause processing; otherwse unpause
      public: void PauseIncoming(bool _pause);

      /// \brief Add a node to the list of nodes that requires processing.
      /// \param[in] _ptr Node to process.
      public: void AddNodeToProcess(NodePtr _ptr);

      /// \brief A map of string->list of Node pointers
      typedef std::map<std::string, std::list<NodePtr> > SubNodeMap;

      private: typedef std::map<std::string, PublicationPtr> PublicationPtr_M;
      private: PublicationPtr_M advertisedTopics;
      private: PublicationPtr_M::iterator advertisedTopicsEnd;
      private: SubNodeMap subscribedNodes;
      private: std::vector<NodePtr> nodes;

      /// \brief Nodes that require processing.
      private: boost::unordered_set<NodePtr> nodesToProcess;

      private: boost::recursive_mutex nodeMutex;

      /// \brief Used to protect subscription connection creation.
      private: boost::mutex subscriberMutex;

      /// \brief Mutex to protect node processing
      private: boost::mutex processNodesMutex;

      private: bool pauseIncoming;

      // Singleton implementation
      private: friend class SingletonT<TopicManager>;
    };
    /// \}
  }
}
#endif
