/*
 * Copyright (c) 2011-2022, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/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.
 *     * Neither the name of the Georgia Tech Research Corporation nor
 *       the names of its contributors may be used to endorse or
 *       promote products derived from this software without specific
 *       prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''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 GEORGIA
 * TECH RESEARCH CORPORATION 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.
 *
 */

/**
 * @file RRT.cpp
 * @author Tobias Kunz, Can Erdogan
 * @date Jan 31, 2013
 * @brief The generic RRT implementation. It can be inherited for modifications
 * to collision checking, sampling and etc.
 */

#include "dart/planning/RRT.hpp"

#include <flann/flann.hpp>

#include "dart/dynamics/Skeleton.hpp"
#include "dart/simulation/World.hpp"

using namespace std;
using namespace Eigen;
using namespace dart;
using namespace simulation;
using namespace dynamics;

namespace dart {
namespace planning {

/* *********************************************************************************************
 */
RRT::RRT(
    WorldPtr world,
    SkeletonPtr robot,
    const std::vector<std::size_t>& dofs,
    const VectorXd& root,
    double stepSize)
  : ndim(dofs.size()),
    stepSize(stepSize),
    world(world),
    robot(robot),
    dofs(dofs),
    index(
        new flann::Index<flann::L2<double> >(flann::KDTreeSingleIndexParams()))
{
  // Reset the random number generator and add the given start configuration to
  // the flann structure
  srand(time(nullptr));
  addNode(root, -1);
}

/* *********************************************************************************************
 */
RRT::RRT(
    WorldPtr world,
    SkeletonPtr robot,
    const std::vector<std::size_t>& dofs,
    const vector<VectorXd>& roots,
    double stepSize)
  : ndim(dofs.size()),
    stepSize(stepSize),
    world(world),
    robot(robot),
    dofs(dofs),
    index(
        new flann::Index<flann::L2<double> >(flann::KDTreeSingleIndexParams()))
{
  // Reset the random number generator and add the given start configurations to
  // the flann structure
  srand(time(nullptr));
  for (std::size_t i = 0; i < roots.size(); i++)
  {
    addNode(roots[i], -1);
  }
}

/* *********************************************************************************************
 */
bool RRT::connect()
{
  VectorXd qtry = getRandomConfig();
  return connect(qtry);
}

/* *********************************************************************************************
 */
bool RRT::connect(const VectorXd& target)
{

  // Get the index of the nearest neighbor in the tree to the given target
  int NNidx = getNearestNeighbor(target);

  // Keep taking steps towards the target until a collision happens
  StepResult result = STEP_PROGRESS;
  while (result == STEP_PROGRESS)
  {
    result = tryStepFromNode(target, NNidx);
    NNidx = configVector.size() - 1;
  }
  return (result == STEP_REACHED);
}

/* *********************************************************************************************
 */
RRT::StepResult RRT::tryStep()
{
  VectorXd qtry = getRandomConfig();
  return tryStep(qtry);
}

/* *********************************************************************************************
 */
RRT::StepResult RRT::tryStep(const VectorXd& qtry)
{
  int NNidx = getNearestNeighbor(qtry);
  return tryStepFromNode(qtry, NNidx);
}

/* *********************************************************************************************
 */
RRT::StepResult RRT::tryStepFromNode(const VectorXd& qtry, int NNidx)
{

  // Get the configuration of the nearest neighbor and check if already reached
  const VectorXd& qnear = *(configVector[NNidx]);
  if ((qtry - qnear).norm() < stepSize)
  {
    return STEP_REACHED;
  }

  // Create the new node: scale the direction vector to stepSize and add to
  // qnear
  VectorXd qnew = qnear + stepSize * (qtry - qnear).normalized();

  // Check for collision, make changes to the qNew and create intermediate
  // points if necessary NOTE: This is largely implementation dependent and in
  // default, no points are created.
  list<VectorXd> intermediatePoints;
  bool collisionClear = newConfig(intermediatePoints, qnew, qnear, qtry);
  if (!collisionClear)
    return STEP_COLLISION;

  // Add the intermediate nodes and the final new node to the tree
  list<VectorXd>::iterator it = intermediatePoints.begin();
  for (; it != intermediatePoints.end(); ++it)
    NNidx = addNode(*it, NNidx);
  addNode(qnew, NNidx);
  return STEP_PROGRESS;
}

/* *********************************************************************************************
 */
bool RRT::newConfig(
    list<VectorXd>& /*intermediatePoints*/,
    VectorXd& qnew,
    const VectorXd& /*qnear*/,
    const VectorXd& /*qtarget*/)
{
  return !checkCollisions(qnew);
}

/* *********************************************************************************************
 */
int RRT::addNode(const VectorXd& qnew, int parentId)
{

  // Update the graph vector
  VectorXd* temp = new VectorXd(qnew);
  configVector.push_back(temp);
  parentVector.push_back(parentId);

  // Update the underlying flann structure (the kdtree)
  unsigned int id = configVector.size() - 1;
  if (id == 0)
    index->buildIndex(
        flann::Matrix<double>((double*)temp->data(), 1, temp->size()));
  else
    index->addPoints(
        flann::Matrix<double>((double*)temp->data(), 1, temp->size()));

  activeNode = id;
  return id;
}

/* *********************************************************************************************
 */
inline int RRT::getNearestNeighbor(const VectorXd& qsamp)
{
  int nearest;
  double distance;
  const flann::Matrix<double> queryMatrix(
      (double*)qsamp.data(), 1, qsamp.size());
  flann::Matrix<int> nearestMatrix(&nearest, 1, 1);
  flann::Matrix<double> distanceMatrix(flann::Matrix<double>(&distance, 1, 1));
  index->knnSearch(
      queryMatrix,
      nearestMatrix,
      distanceMatrix,
      1,
      flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
  activeNode = nearest;
  return nearest;
}

/* *********************************************************************************************
 */
// random # between min & max
inline double RRT::randomInRange(double min, double max)
{
  assert(max - min >= 0.0);
  assert(max - min < numeric_limits<double>::infinity());

  if (min == max)
    return min;
  return min + ((max - min) * ((double)rand() / ((double)RAND_MAX + 1)));
}

/* *********************************************************************************************
 */
VectorXd RRT::getRandomConfig()
{
  // Samples a random point for qtmp in the configuration space, bounded by the
  // provided configuration vectors (and returns ref to it)
  VectorXd config(ndim);
  for (int i = 0; i < ndim; ++i)
  {
    config[i] = randomInRange(
        robot->getPositionLowerLimit(dofs[i]),
        robot->getPositionUpperLimit(dofs[i]));
  }
  return config;
}

/* *********************************************************************************************
 */
double RRT::getGap(const VectorXd& target)
{
  return (target - *(configVector[activeNode])).norm();
}

/* *********************************************************************************************
 */
void RRT::tracePath(int node, std::list<VectorXd>& path, bool reverse)
{

  // Keep following the "linked list" in the given direction
  int x = node;
  while (x != -1)
  {
    if (!reverse)
      path.push_front(*(configVector[x]));
    else
      path.push_back(*(configVector[x]));
    x = parentVector[x];
  }
}

/* *********************************************************************************************
 */
bool RRT::checkCollisions(const VectorXd& c)
{
  robot->setPositions(dofs, c);
  return world->checkCollision();
}

/* *********************************************************************************************
 */
std::size_t RRT::getSize()
{
  return configVector.size();
}

} // namespace planning
} // namespace dart
