/*
 * Copyright (c) 2011-2019, 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.
 */

#include "dart/collision/CollisionGroup.hpp"

#include <cassert>

#include "dart/collision/CollisionObject.hpp"
#include "dart/collision/CollisionDetector.hpp"
#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/Skeleton.hpp"

namespace dart {
namespace collision {

//==============================================================================
CollisionGroup::CollisionGroup(const CollisionDetectorPtr& collisionDetector)
  : mCollisionDetector(collisionDetector),
    mUpdateAutomatically(true)
{
  assert(mCollisionDetector);
}

//==============================================================================
CollisionDetectorPtr CollisionGroup::getCollisionDetector()
{
  return mCollisionDetector;
}

//==============================================================================
ConstCollisionDetectorPtr CollisionGroup::getCollisionDetector() const
{
  return mCollisionDetector;
}

//==============================================================================
void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame)
{
  addShapeFrameImpl(shapeFrame, nullptr);
}

//==============================================================================
void CollisionGroup::addShapeFrames(
    const std::vector<const dynamics::ShapeFrame*>& shapeFrames)
{
  for (const auto& shapeFrame : shapeFrames)
    addShapeFrame(shapeFrame);
}

//==============================================================================
void CollisionGroup::addShapeFramesOf()
{
  // Do nothing
}

//==============================================================================
void CollisionGroup::subscribeTo()
{
  // Do nothing
}

//==============================================================================
void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame)
{
  if (!shapeFrame)
    return;

  const auto search
      = std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(),
                     [&](const std::unique_ptr<ObjectInfo>& info)
                     { return info->mFrame == shapeFrame; });

  if (mObjectInfoList.end() == search)
    return;

  removeCollisionObjectFromEngine((*search)->mObject.get());

  // Since the user is explicitly telling us to remove this ShapeFrame, we can
  // no longer remain subscribed to any sources that were providing this
  // ShapeFrame. Otherwise, this ShapeFrame would just reappear instantly the
  // next time an update is performed.
  for(const void* source : (*search)->mSources)
  {
    if(nullptr == source)
      continue;

    // We don't know which container this source is kept in, so try erasing it
    // from each.
    if(mSkeletonSources.erase(
         static_cast<const dynamics::MetaSkeleton*>(source)) > 0)
      continue;

    mBodyNodeSources.erase(static_cast<const dynamics::BodyNode*>(source));
  }

  mObjectInfoList.erase(search);
  mObserver.removeShapeFrame(shapeFrame);
}

//==============================================================================
void CollisionGroup::removeShapeFrames(
    const std::vector<const dynamics::ShapeFrame*>& shapeFrames)
{
  for (const auto& shapeFrame : shapeFrames)
    removeShapeFrame(shapeFrame);
}

//==============================================================================
void CollisionGroup::removeShapeFramesOf()
{
  // Do nothing
}

//==============================================================================
void CollisionGroup::removeAllShapeFrames()
{
  removeAllCollisionObjectsFromEngine();

  mObjectInfoList.clear();
  mObserver.removeAllShapeFrames();
}

//==============================================================================
bool CollisionGroup::hasShapeFrame(
    const dynamics::ShapeFrame* shapeFrame) const
{
  return std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(),
                      [&](const std::unique_ptr<ObjectInfo>& info)
                      { return info->mFrame == shapeFrame; })
      != mObjectInfoList.end();
}

//==============================================================================
std::size_t CollisionGroup::getNumShapeFrames() const
{
  return mObjectInfoList.size();
}

//==============================================================================
const dynamics::ShapeFrame* CollisionGroup::getShapeFrame(
    std::size_t index) const
{
  assert(index < mObjectInfoList.size());
  if(index < mObjectInfoList.size())
    return mObjectInfoList[index]->mFrame;

  return nullptr;
}

//==============================================================================
bool CollisionGroup::collide(
    const CollisionOption& option, CollisionResult* result)
{
  if(mUpdateAutomatically)
    update();

  return mCollisionDetector->collide(this, option, result);
}

//==============================================================================
bool CollisionGroup::collide(
    CollisionGroup* otherGroup,
    const CollisionOption& option,
    CollisionResult* result)
{
  if(mUpdateAutomatically)
    update();

  return mCollisionDetector->collide(this, otherGroup, option, result);
}

//==============================================================================
double CollisionGroup::distance(
    const DistanceOption& option, DistanceResult* result)
{
  if(mUpdateAutomatically)
    update();

  return mCollisionDetector->distance(this, option, result);
}

//==============================================================================
double CollisionGroup::distance(
    CollisionGroup* otherGroup,
    const DistanceOption& option,
    DistanceResult* result)
{
  if(mUpdateAutomatically)
    update();

  return mCollisionDetector->distance(this, otherGroup, option, result);
}

//==============================================================================
bool CollisionGroup::raycast(
    const Eigen::Vector3d& from,
    const Eigen::Vector3d& to,
    const RaycastOption& option,
    RaycastResult* result)
{
  if(mUpdateAutomatically)
    update();

  return mCollisionDetector->raycast(this, from, to, option, result);
}

//==============================================================================
void CollisionGroup::setAutomaticUpdate(const bool automatic)
{
  mUpdateAutomatically = automatic;
}

//==============================================================================
bool CollisionGroup::getAutomaticUpdate() const
{
  return mUpdateAutomatically;
}

//==============================================================================
void CollisionGroup::update()
{
  removeDeletedShapeFrames();

  for(auto& entry : mSkeletonSources)
    updateSkeletonSource(entry);

  for(auto& entry : mBodyNodeSources)
    updateBodyNodeSource(entry);
}

//==============================================================================
void CollisionGroup::removeDeletedShapeFrames()
{
  for(auto shapeFrame : mObserver.mDeletedFrames)
  {
    const auto search =
        std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(),
                     [&](const std::unique_ptr<ObjectInfo>& info)
                     { return info->mFrame == shapeFrame; });

    if(mObjectInfoList.end() == search)
      continue;

    // Clear out the ShapeFrame from any subscriber that it might have
    for(const void* source : (*search)->mSources)
    {
      if(nullptr == source)
        continue;

      auto skelSearch = mSkeletonSources.find(
            static_cast<const dynamics::MetaSkeleton*>(source));
      if(skelSearch != mSkeletonSources.end())
      {
        skelSearch->second.mObjects.erase(shapeFrame);
        for(auto& child : skelSearch->second.mChildren)
          child.second.mFrames.erase(shapeFrame);
      }

      auto bodySearch = mBodyNodeSources.find(
            static_cast<const dynamics::BodyNode*>(source));
      if(bodySearch != mBodyNodeSources.end())
        bodySearch->second.mObjects.erase(shapeFrame);
    }

    removeCollisionObjectFromEngine((*search)->mObject.get());
    mObjectInfoList.erase(search);
  }

  mObserver.mDeletedFrames.clear();
}

//==============================================================================
void CollisionGroup::updateEngineData()
{
  for (const auto& info : mObjectInfoList)
    info->mObject->updateEngineData();

  updateCollisionGroupEngineData();
}

//==============================================================================
void CollisionGroup::ShapeFrameObserver::addShapeFrame(
    const dynamics::ShapeFrame* shapeFrame)
{
  addSubject(shapeFrame);
  mMap.insert(std::make_pair(
                static_cast<const common::Subject*>(shapeFrame), shapeFrame));
}

//==============================================================================
void CollisionGroup::ShapeFrameObserver::removeShapeFrame(
    const dynamics::ShapeFrame* shapeFrame)
{
  removeSubject(shapeFrame);
  mMap.erase(shapeFrame);
}

//==============================================================================
void CollisionGroup::ShapeFrameObserver::removeAllShapeFrames()
{
  removeAllSubjects();
}

//==============================================================================
void CollisionGroup::ShapeFrameObserver::handleDestructionNotification(
    const common::Subject* subject)
{
  mDeletedFrames.insert(mMap[subject]);
  mMap.erase(subject);
}

//==============================================================================
auto CollisionGroup::addShapeFrameImpl(
    const dynamics::ShapeFrame* shapeFrame,
    const void* source) -> ObjectInfo*
{
  if (!shapeFrame)
    return nullptr;

  auto it = std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(),
              [&](const std::unique_ptr<ObjectInfo>& info)
              { return info->mFrame == shapeFrame; });

  if (it == mObjectInfoList.end())
  {
    auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame);

    addCollisionObjectToEngine(collObj.get());

    const dynamics::ConstShapePtr& shape = shapeFrame->getShape();

    // If shape is a nullptr, use improbable values for ID and version. ID is
    // not likely to ever be 0 because Shape IDs start at
    // PRIMITIVE_MAGIC_NUMBER (defined as 1000 in Shape.cpp). Version is not
    // likely to be 0 because the Shape class starts its version counter at 1,
    // and the version count should monotonically increase.
    mObjectInfoList.emplace_back(
          new ObjectInfo{shapeFrame, collObj,
                         shape? shape->getID() : 0,
                         shape? shape->getVersion() : 0, {}});
    mObserver.addShapeFrame(shapeFrame);

    it = --mObjectInfoList.end();
  }

  (*it)->mSources.insert(source);

  return it->get();
}

//==============================================================================
void CollisionGroup::removeShapeFrameInternal(
    const dynamics::ShapeFrame* shapeFrame,
    const void* source)
{
  if(!shapeFrame)
    return;

  const auto search =
      std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(),
                   [&](const std::unique_ptr<ObjectInfo>& info)
                   { return info->mFrame == shapeFrame; });

  if(mObjectInfoList.end() == search)
    return;

  std::unordered_set<const void*>& objectSources = (*search)->mSources;
  objectSources.erase(source);

  if(objectSources.empty())
  {
    removeCollisionObjectFromEngine((*search)->mObject.get());
    mObjectInfoList.erase(search);
    mObserver.removeShapeFrame(shapeFrame);
  }
}

//==============================================================================
bool CollisionGroup::updateSkeletonSource(SkeletonSources::value_type& entry)
{
  SkeletonSource& source = entry.second;

  const dynamics::ConstMetaSkeletonPtr& meta = source.mSource.lock();
  if(!meta)
  {
    // This skeleton no longer exists, so we should remove all its contents from
    // the CollisionGroup.
    for(const auto& object : source.mObjects)
      removeShapeFrameInternal(object.second->mFrame, object.first);

    return true;
  }

  // Note: Right now we can only subscribe to Skeletons, not MetaSkeletons, so
  // we can safely do a static_cast here. Eventually this static_cast should not
  // be needed, if we can figure out a versioning system for MetaSkeleton.
  const dynamics::Skeleton* skeleton =
      dynamic_cast<const dynamics::Skeleton*>(meta.get());

  const std::size_t currentSkeletonVersion = skeleton->getVersion();
  // If the version hasn't changed, then there will be nothing to update.
  if(currentSkeletonVersion == source.mLastKnownVersion)
    return false;

  source.mLastKnownVersion = currentSkeletonVersion;
  bool updateNeeded = false;

  auto unusedChildren = source.mChildren;

  // Check each child to see if its version number has changed. If it has, then
  // check to see if any of its ShapeFrames need to be updated, added, or
  // removed from the CollisionGroup.
  for(std::size_t i = 0; i < skeleton->getNumBodyNodes(); ++i)
  {
    const dynamics::BodyNode* bn = skeleton->getBodyNode(i);
    const std::size_t currentVersion = bn->getVersion();

    unusedChildren.erase(bn);

    // Attempt an insertion, in case this BodyNode was not already accounted for
    auto insertion = source.mChildren.insert(
          std::make_pair(bn, SkeletonSource::ChildInfo(currentVersion)));

    const auto child = insertion.first;

    if(insertion.second)
    {
      // This child was newly introduced, so we need to add it to the
      // CollisionGroup.
      updateNeeded = true;

      const auto& collisionShapeNodes =
          bn->getShapeNodesWith<dynamics::CollisionAspect>();

      for(const auto& shapeNode : collisionShapeNodes)
      {
        source.mObjects.insert(
            {shapeNode, addShapeFrameImpl(shapeNode, meta.get())});
        child->second.mFrames.insert(shapeNode);
      }

      // Skip the rest of the for-loop, because it's only for updating old
      // children
      continue;
    }

    // If the version of this BodyNode has not changed, then move on to the next
    // one.
    if(child->second.mLastKnownVersion == currentVersion)
      continue;

    child->second.mLastKnownVersion = currentVersion;

    std::unordered_set<const dynamics::ShapeFrame*> unusedFrames =
        child->second.mFrames;

    const std::vector<const dynamics::ShapeNode*> nodes =
        child->first->getShapeNodesWith<dynamics::CollisionAspect>();

    for(const dynamics::ShapeNode* node : nodes)
    {
      unusedFrames.erase(node);

      auto frameInsertion =
          source.mObjects.insert(std::make_pair(node, nullptr));

      const auto& it = frameInsertion.first;
      if(frameInsertion.second)
      {
        // If the insertion occurred, then this is a new ShapeFrame, and we
        // need to create a collision object for it.
        updateNeeded = true;

        it->second = addShapeFrameImpl(node, meta.get());
        child->second.mFrames.insert(node);
      }
      else
      {
        // If the insertion did not occur, then this is an old ShapeFrame, and
        // we should check if it needs an update.
        updateNeeded |= updateShapeFrame(it->second);
      }
    }

    for(const dynamics::ShapeFrame* unused : unusedFrames)
    {
      updateNeeded = true;
      removeShapeFrameInternal(unused, meta.get());
      child->second.mFrames.erase(unused);
    }
  }

  // Remove from this group any BodyNodes that no longer belong to the skeleton
  for(const auto& unusedChild : unusedChildren)
  {
    for(const dynamics::ShapeFrame* unusedFrame : unusedChild.second.mFrames)
    {
      updateNeeded = true;
      removeShapeFrameInternal(unusedFrame, meta.get());
      source.mObjects.erase(unusedFrame);
    }

    source.mChildren.erase(unusedChild.first);
  }

  return updateNeeded;
}

//==============================================================================
bool CollisionGroup::updateBodyNodeSource(BodyNodeSources::value_type& entry)
{
  BodyNodeSource& source = entry.second;

  const dynamics::ConstBodyNodePtr bn = source.mSource.lock();
  if(!bn)
  {
    // This BodyNode no longer exists, so we should remove all i ts contents
    // from the CollisionGroup.
    for(const auto& object : source.mObjects)
      removeShapeFrameInternal(object.second->mFrame, object.first);

    return true;
  }

  const std::size_t currentBodyNodeVersion = bn->getVersion();

  // If the version hasn't changed, then tehre will be nothing to update.
  if(currentBodyNodeVersion == source.mLastKnownVersion)
    return false;

  source.mLastKnownVersion = currentBodyNodeVersion;
  bool updateNeeded = false;

  std::unordered_map<const dynamics::ShapeFrame*, ObjectInfo*> unusedFrames =
      source.mObjects;

  const std::vector<const dynamics::ShapeNode*> nodes =
      bn->getShapeNodesWith<dynamics::CollisionAspect>();

  for(const dynamics::ShapeNode* node : nodes)
  {
    unusedFrames.erase(node);

    auto frameInsertion = source.mObjects.insert(std::make_pair(node, nullptr));

    const auto& it = frameInsertion.first;
    if(frameInsertion.second)
    {
      updateNeeded = true;
      // If the insertion occurred, then this is a new ShapeFrame, and we need
      // to create a collision object for it.
      it->second = addShapeFrameImpl(node, bn.get());
    }
    else
    {
      // If the insertion did not occur, then this is an old ShapeFrame, and
      // we should check if it needs an update.
      updateNeeded |= updateShapeFrame(it->second);
    }
  }

  // Remove from this group and ShapeFrames that no longer belong to the
  // BodyNode
  for(const auto& unusedFrame : unusedFrames)
  {
    updateNeeded = true;
    removeShapeFrameInternal(unusedFrame.first, bn.get());
    source.mObjects.erase(unusedFrame.first);
  }

  return updateNeeded;
}

//==============================================================================
bool CollisionGroup::updateShapeFrame(ObjectInfo* object)
{
  const dynamics::ConstShapePtr& shape = object->mFrame->getShape();
  const std::size_t currentID = shape? shape->getID() : 0;
  const std::size_t currentVersion = shape? shape->getVersion() : 0;

  if(currentID != object->mLastKnownShapeID
     || currentVersion != object->mLastKnownVersion)
  {
    removeCollisionObjectFromEngine(object->mObject.get());
    mCollisionDetector->refreshCollisionObject(object->mObject.get());
    addCollisionObjectToEngine(object->mObject.get());

    object->mLastKnownShapeID = currentID;
    object->mLastKnownVersion = currentVersion;

    return true;
  }

  return false;
}

}  // namespace collision
}  // namespace dart
