/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_DYNAMICS_DETAIL_NODE_HPP_
#define DART_DYNAMICS_DETAIL_NODE_HPP_

#include <cassert>

#include "dart/dynamics/Node.hpp"

#include "dart/common/StlHelpers.hpp"

namespace dart {
namespace dynamics {

//==============================================================================
template <class NodeType>
std::size_t AccessoryNode<NodeType>::getIndexInBodyNode() const
{
  return static_cast<const NodeType*>(this)->mIndexInBodyNode;
}

//==============================================================================
template <class NodeType>
std::size_t AccessoryNode<NodeType>::getIndexInSkeleton() const
{
  return static_cast<const NodeType*>(this)->mIndexInSkeleton;
}

//==============================================================================
template <class NodeType>
std::size_t AccessoryNode<NodeType>::getIndexInTree() const
{
  return static_cast<const NodeType*>(this)->mIndexInTree;
}

//==============================================================================
template <class NodeType>
std::size_t AccessoryNode<NodeType>::getTreeIndex() const
{
  return static_cast<const NodeType*>(this)->getBodyNodePtr()->getTreeIndex();
}

//==============================================================================
template <class NodeType>
void AccessoryNode<NodeType>::remove()
{
  return static_cast<NodeType*>(this)->stageForRemoval();
}

//==============================================================================
template <class NodeType>
void AccessoryNode<NodeType>::reattach()
{
  static_cast<NodeType*>(this)->attach();
}

} // namespace dynamics
} // namespace dart

//==============================================================================
// Macros for specializing Nodes within BodyNodes
//==============================================================================
#define DART_ENABLE_NODE_SPECIALIZATION()                                      \
public:                                                                        \
  template <class T>                                                           \
  std::size_t getNumNodes() const                                              \
  {                                                                            \
    return dart::dynamics::BodyNode::getNumNodes<T>();                         \
  }                                                                            \
  template <class T>                                                           \
  T* getNode(std::size_t index)                                                \
  {                                                                            \
    return dart::dynamics::BodyNode::getNode<T>(index);                        \
  }                                                                            \
  template <class T>                                                           \
  const T* getNode(std::size_t index) const                                    \
  {                                                                            \
    return dart::dynamics::BodyNode::getNode<T>(index);                        \
  }

//==============================================================================
#define DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE(NodeName, it)                 \
  mNodeMap[typeid(NodeName)] = std::vector<dart::dynamics::Node*>();           \
  it = mNodeMap.find(typeid(NodeName));

//==============================================================================
#define DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, PluralName)      \
  DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE(NodeName, m##PluralName##Iterator)

//==============================================================================
#define DART_INSTANTIATE_SPECALIZED_NODE(NodeName)                             \
  DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, NodeName##s)

//==============================================================================
#define DETAIL_DART_SPECIALIZED_NODE_INLINE(NodeName, PluralName, it)          \
private:                                                                       \
  dart::dynamics::BodyNode::NodeMap::iterator it;                              \
                                                                               \
public:                                                                        \
  inline std::size_t getNum##PluralName() const                                \
  {                                                                            \
    return it->second.size();                                                  \
  }                                                                            \
                                                                               \
  inline NodeName* get##NodeName(std::size_t index)                            \
  {                                                                            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(index, it->second));                        \
  }                                                                            \
                                                                               \
  inline const NodeName* get##NodeName(std::size_t index) const                \
  {                                                                            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(index, it->second));                        \
  }

//==============================================================================
#define DART_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, PluralName)           \
  DETAIL_DART_SPECIALIZED_NODE_INLINE(                                         \
      NodeName, PluralName, m##PluralName##Iterator)

//==============================================================================
#define DART_SPECIALIZED_NODE_INTERNAL(NodeName)                               \
  DART_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, NodeName##s)

//==============================================================================
#define DETAIL_DART_SPECIALIZED_NODE_DECLARE(NodeName, PluralName, it)         \
private:                                                                       \
  dart::dynamics::BodyNode::NodeMap::iterator it;                              \
                                                                               \
public:                                                                        \
  std::size_t getNum##PluralName() const;                                      \
  NodeName* get##NodeName(std::size_t index);                                  \
  const NodeName* get##NodeName(std::size_t index) const;

//==============================================================================
#define DART_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, PluralName)          \
  DETAIL_DART_SPECIALIZED_NODE_DECLARE(                                        \
      NodeName, PluralName, m##PluralName##Iterator)

//==============================================================================
#define DART_SPECIALIZED_NODE_DECLARE(NodeName)                                \
  DART_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, NodeName##s)

//==============================================================================
#define DETAIL_DART_SPECIALIZED_NODE_DEFINE(                                   \
    BodyNodeType, NodeName, PluralName, it)                                    \
  std::size_t BodyNodeType ::getNum##PluralName() const                        \
  {                                                                            \
    return it->second.size();                                                  \
  }                                                                            \
                                                                               \
  NodeName* BodyNodeType ::get##NodeName(std::size_t index)                    \
  {                                                                            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(index, it->second));                        \
  }                                                                            \
                                                                               \
  const NodeName* BodyNodeType ::get##NodeName(std::size_t index) const        \
  {                                                                            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(index, it->second));                        \
  }

//==============================================================================
#define DART_SPECIALIZED_NODE_DEFINE_IRREGULAR(                                \
    BodyNodeType, NodeName, PluralName)                                        \
  DETAIL_DART_SPECIALIZED_NODE_DEFINE(                                         \
      BodyNodeType, NodeName, PluralName, m##PluralName##Iterator)

//==============================================================================
#define DART_SPECIALIZED_NODE_DEFINE(BodyNodeType, NodeName)                   \
  DART_SPECIALIZED_NODE_DEFINE_IRREGULAR(BodyNodeType, NodeName, NodeName##s)

//==============================================================================
#define DETAIL_DART_SPECIALIZED_NODE_TEMPLATE(                                 \
    BodyNodeType, NodeName, PluralName)                                        \
  template <>                                                                  \
  inline std::size_t BodyNodeType ::getNumNodes<NodeName>() const              \
  {                                                                            \
    return getNum##PluralName();                                               \
  }                                                                            \
  template <>                                                                  \
  inline NodeName* BodyNodeType ::getNode<NodeName>(std::size_t index)         \
  {                                                                            \
    return get##NodeName(index);                                               \
  }                                                                            \
  template <>                                                                  \
  inline const NodeName* BodyNodeType ::getNode<NodeName>(std::size_t index)   \
      const                                                                    \
  {                                                                            \
    return get##NodeName(index);                                               \
  }

//==============================================================================
#define DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR(                              \
    BodyNodeType, NodeName, PluralName)                                        \
  DETAIL_DART_SPECIALIZED_NODE_TEMPLATE(BodyNodeType, NodeName, PluralName)

//==============================================================================
#define DART_SPECIALIZED_NODE_TEMPLATE(BodyNodeType, NodeName)                 \
  DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR(BodyNodeType, NodeName, NodeName##s)

//==============================================================================
// Macros for specializing Nodes within Skeletons
//==============================================================================
#define DART_SKEL_ENABLE_NODE_SPECIALIZATION()                                 \
public:                                                                        \
  template <class T>                                                           \
  std::size_t getNumNodes() const                                              \
  {                                                                            \
    return dart::dynamics::Skeleton::getNumNodes<T>();                         \
  }                                                                            \
  template <class T>                                                           \
  std::size_t getNumNodes(std::size_t treeIndex) const                         \
  {                                                                            \
    return dart::dynamics::Skeleton::getNumNodes<T>(treeIndex);                \
  }                                                                            \
  template <class T>                                                           \
  T* getNode(std::size_t index)                                                \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(index);                        \
  }                                                                            \
  template <class T>                                                           \
  T* getNode(std::size_t nodeIdx, std::size_t treeIndex)                       \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex);           \
  }                                                                            \
  template <class T>                                                           \
  const T* getNode(std::size_t index) const                                    \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(index);                        \
  }                                                                            \
  template <class T>                                                           \
  const T* getNode(std::size_t nodeIdx, std::size_t treeIndex) const           \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex);           \
  }                                                                            \
  template <class T>                                                           \
  T* getNode(const std::string& name)                                          \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(name);                         \
  }                                                                            \
  template <class T>                                                           \
  const T* getNode(const std::string& name) const                              \
  {                                                                            \
    return dart::dynamics::Skeleton::getNode<T>(name);                         \
  }

//==============================================================================
#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, PluralName) \
  mSpecializedTreeNodes[typeid(NodeName)] = &mTree##PluralName##Iterators;     \
  mSkelCache.mNodeMap[typeid(NodeName)]                                        \
      = std::vector<dart::dynamics::Node*>();                                  \
  m##PluralName##Iterator = mSkelCache.mNodeMap.find(typeid(NodeName));        \
  mNodeNameMgrMap[typeid(NodeName)] = dart::common::NameManager<Node*>(        \
      std::string("Skeleton::") + #NodeName + " | "                            \
      + mAspectProperties.mName);                                              \
  mNameMgrFor##PluralName = &mNodeNameMgrMap.find(typeid(NodeName))->second;

//==============================================================================
#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE(NodeName)                       \
  DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, NodeName##s);

//==============================================================================
#define DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, func)                  \
  if (treeIndex >= treeIts.size())                                             \
  {                                                                            \
    dterr << "[" << #func << "] Requesting an invalid tree (" << treeIndex     \
          << "). "                                                             \
          << "The number of trees in this Skeleton is: " << treeIts.size()     \
          << "\n";                                                             \
    assert(false);                                                             \
    return 0;                                                                  \
  }

//==============================================================================
#define DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE(                              \
    NodeName, PluralName, skelIt, treeIts, NameMgr)                            \
private:                                                                       \
  dart::dynamics::Skeleton::NodeMap::iterator skelIt;                          \
  std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts;            \
  dart::common::NameManager<dart::dynamics::Node*>* NameMgr;                   \
                                                                               \
public:                                                                        \
  inline std::size_t getNum##PluralName() const                                \
  {                                                                            \
    return skelIt->second.size();                                              \
  }                                                                            \
  inline std::size_t getNum##PluralName(std::size_t treeIndex) const           \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum##PluralName);       \
    treeIts[treeIndex]->second.size();                                         \
  }                                                                            \
                                                                               \
  inline NodeName* get##NodeName(std::size_t index)                            \
  {                                                                            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(index, skelIt->second));                    \
  }                                                                            \
  inline NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex) \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName);            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second));    \
  }                                                                            \
                                                                               \
  inline const NodeName* get##NodeName(std::size_t index) const                \
  {                                                                            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(index, skelIt->second));                    \
  }                                                                            \
  inline const NodeName* get##NodeName(                                        \
      std::size_t treeIndex, std::size_t nodeIndex) const                      \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName);            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second));    \
  }                                                                            \
                                                                               \
  inline NodeName* get##NodeName(const std::string& name)                      \
  {                                                                            \
    return static_cast<NodeName*>(NameMgr->getObject(name));                   \
  }                                                                            \
  inline const NodeName* get##NodeName(const std::string& name) const          \
  {                                                                            \
    return static_cast<const NodeName*>(NameMgr->getObject(name));             \
  }

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, PluralName)      \
  DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE(                                    \
      NodeName,                                                                \
      PluralName,                                                              \
      m##PluralName##Iterator,                                                 \
      mTree##PluralName##Iterators,                                            \
      mNameMgrFor##PluralName);

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_INLINE(NodeName)                            \
  DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, NodeName##s);

//==============================================================================
#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE(                             \
    NodeName, PluralName, skelIt, treeIts, NameMgr)                            \
private:                                                                       \
  dart::dynamics::Skeleton::NodeMap::iterator skelIt;                          \
  std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts;            \
  dart::common::NameManager<dart::dynamics::Node*>* NameMgr;                   \
                                                                               \
public:                                                                        \
  std::size_t getNum##PluralName() const;                                      \
                                                                               \
  std::size_t getNum##PluralName(std::size_t treeIndex) const;                 \
                                                                               \
  NodeName* get##NodeName(std::size_t index);                                  \
                                                                               \
  NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex);       \
                                                                               \
  const NodeName* get##NodeName(std::size_t index) const;                      \
                                                                               \
  const NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex)  \
      const;                                                                   \
                                                                               \
  NodeName* get##NodeName(const std::string& name);                            \
                                                                               \
  const NodeName* get##NodeName(const std::string& name) const;

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, PluralName)     \
  DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE(                                   \
      NodeName,                                                                \
      PluralName,                                                              \
      m##PluralName##Iterator,                                                 \
      mTree##PluralName##Iterators,                                            \
      mNameMgrFor##PluralName);

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_DECLARE(NodeName)                           \
  DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, NodeName##s);

//==============================================================================
#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE(                              \
    SkeletonType, NodeName, PluralName, skelIt, treeIts, NameMgr)              \
  std::size_t SkeletonType ::getNum##PluralName() const                        \
  {                                                                            \
    return skelIt->second.size();                                              \
  }                                                                            \
  std::size_t SkeletonType ::getNum##PluralName(std::size_t treeIndex) const   \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum##PluralName);       \
    return treeIts[treeIndex]->second.size();                                  \
  }                                                                            \
                                                                               \
  NodeName* SkeletonType ::get##NodeName(std::size_t index)                    \
  {                                                                            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(index, skelIt->second));                    \
  }                                                                            \
  NodeName* SkeletonType ::get##NodeName(                                      \
      std::size_t treeIndex, std::size_t nodeIndex)                            \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName);            \
    return static_cast<NodeName*>(                                             \
        getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second));    \
  }                                                                            \
                                                                               \
  const NodeName* SkeletonType ::get##NodeName(std::size_t index) const        \
  {                                                                            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(index, skelIt->second));                    \
  }                                                                            \
  const NodeName* SkeletonType ::get##NodeName(                                \
      std::size_t treeIndex, std::size_t nodeIndex) const                      \
  {                                                                            \
    DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName);            \
    return static_cast<const NodeName*>(                                       \
        getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second));    \
  }                                                                            \
                                                                               \
  NodeName* SkeletonType ::get##NodeName(const std::string& name)              \
  {                                                                            \
    return static_cast<NodeName*>(NameMgr->getObject(name));                   \
  }                                                                            \
  const NodeName* SkeletonType ::get##NodeName(const std::string& name) const  \
  {                                                                            \
    return static_cast<const NodeName*>(NameMgr->getObject(name));             \
  }

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR(                          \
    SkeletonType, NodeName, PluralName)                                        \
  DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE(                                    \
      SkeletonType,                                                            \
      NodeName,                                                                \
      PluralName,                                                              \
      m##PluralName##Iterator,                                                 \
      mTree##PluralName##Iterators,                                            \
      mNameMgrFor##PluralName);

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_DEFINE(SkeletonType, NodeName)              \
  DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR(                                \
      SkeletonType, NodeName, NodeName##s);

//==============================================================================
#define DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE(                            \
    SkelType, NodeName, PluralName)                                            \
  template <>                                                                  \
  inline std::size_t SkelType ::getNumNodes<NodeName>() const                  \
  {                                                                            \
    return getNum##PluralName();                                               \
  }                                                                            \
  template <>                                                                  \
  inline std::size_t SkelType ::getNumNodes<NodeName>(std::size_t index) const \
  {                                                                            \
    return getNum##PluralName(index);                                          \
  }                                                                            \
  template <>                                                                  \
  inline NodeName* SkelType ::getNode<NodeName>(std::size_t index)             \
  {                                                                            \
    return get##NodeName(index);                                               \
  }                                                                            \
  template <>                                                                  \
  inline NodeName* SkelType ::getNode<NodeName>(                               \
      std::size_t treeIndex, std::size_t nodeIndex)                            \
  {                                                                            \
    return get##NodeName(treeIndex, nodeIndex);                                \
  }                                                                            \
  template <>                                                                  \
  inline const NodeName* SkelType ::getNode<NodeName>(std::size_t index) const \
  {                                                                            \
    return get##NodeName(index);                                               \
  }                                                                            \
  template <>                                                                  \
  inline const NodeName* SkelType ::getNode<NodeName>(                         \
      std::size_t treeIndex, std::size_t nodeIndex) const                      \
  {                                                                            \
    return get##NodeName(treeIndex, nodeIndex);                                \
  }                                                                            \
  template <>                                                                  \
  inline NodeName* SkelType::getNode<NodeName>(const std::string& name)        \
  {                                                                            \
    return get##NodeName(name);                                                \
  }                                                                            \
  template <>                                                                  \
  inline const NodeName* SkelType::getNode<NodeName>(const std::string& name)  \
      const                                                                    \
  {                                                                            \
    return get##NodeName(name);                                                \
  }

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR(                         \
    SkelType, NodeName, PluralName)                                            \
  DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE(SkelType, NodeName, PluralName)

//==============================================================================
#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE(SkelType, NodeName)                \
  DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR(                               \
      SkelType, NodeName, NodeName##s);

#endif // DART_DYNAMICS_DETAIL_NODE_HPP_
