/*
 * 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.
 */

/**
 * @file PathPlanner.h
 * @author Tobias Kunz (?), Can Erdogan
 * @date Jan 30, 2013
 * @brief Contains the path planner class definition which is templated on a RRT implementation and
 * creates an interface to generate trajectories with different RRT algorithms such as goal-biased,
 * bidirectional, connect and etc.
 */

#ifndef DART_PLANNING_PATHPLANNER_HPP_
#define DART_PLANNING_PATHPLANNER_HPP_

#include <Eigen/Core>
#include <iostream>
#include <limits>
#include <list>
#include <vector>
#include "dart/dynamics/Skeleton.hpp"
#include "dart/simulation/World.hpp"
#include "dart/planning/RRT.hpp"
#include <cstdio>

namespace dart {
namespace planning {

/* ********************************************************************************************* */
/// The path planner class - a common interface to motion planners
template <class R = RRT>
class PathPlanner {
public:

  bool connect;            ///< Whether we take a small step or shoot for the target node
  bool bidirectional;      ///< Whether two trees try to meet each other or one goes for the goal
  double stepSize;        ///< Step size from a node in the tree to the random/goal node
  double goalBias;        ///< Choose btw goal and random value (for goal-biased search)
  std::size_t maxNodes;        ///< Maximum number of iterations the sampling would continue
  simulation::WorldPtr world;  ///< The world that the robot is in (for obstacles and etc.)

  // NOTE: It is useful to keep the rrts around after planning for reuse, analysis, and etc.
  R* start_rrt;            ///< The rrt for unidirectional search
  R* goal_rrt;              ///< The second rrt if bidirectional search is executed

public:

  /// The default constructor
  PathPlanner() : world(nullptr) {}

  /// The desired constructor - you should use this one.
  PathPlanner(simulation::World& world, bool bidirectional_ = true, bool connect_ = true, double stepSize_ = 0.1,
    std::size_t maxNodes_ = 1e6, double goalBias_ = 0.3) : world(&world), bidirectional(bidirectional_),
    connect(connect_), stepSize(stepSize_), maxNodes(maxNodes_), goalBias(goalBias_) {
  }

  /// The destructor
  virtual ~PathPlanner() {}

  /// Plan a path from a single start configuration to a single goal
  bool planPath(dynamics::Skeleton* robot, const std::vector<int> &dofs, const Eigen::VectorXd &start,
      const Eigen::VectorXd &goal, std::list<Eigen::VectorXd> &path) {
    std::vector<Eigen::VectorXd> startVector, goalVector;
    startVector.push_back(start);
    goalVector.push_back(goal);
    return planPath(robot, dofs, startVector, goalVector, path);
  }

  /// Plan a path from a _set_ of start configurations to a _set_ of goals
  bool planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs, const std::vector<Eigen::VectorXd> &start,
    const std::vector<Eigen::VectorXd> &goal, std::list<Eigen::VectorXd> &path);

private:

  /// Performs a unidirectional RRT with the given options.
  bool planSingleTreeRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
    const std::vector<Eigen::VectorXd> &start, const Eigen::VectorXd &goal,
    std::list<Eigen::VectorXd> &path);

  /// Performs bidirectional RRT with the given options.
  /// NOTE This algorithm has several different popular implementations. The implementation in the
  /// kinodynamic paper (1999) by LaValle and Kuffner extend the two RRTs towards a common random
  /// configurations whereas here, first, start rrt extends towards a random node and creates
  /// some node N. Afterwards, the second rrt extends towards _the node N_ and they continue
  /// swapping roles.
  bool planBidirectionalRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
    const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
    std::list<Eigen::VectorXd> &path);
};

/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs,
    const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
    std::list<Eigen::VectorXd> &path) {

  Eigen::VectorXd savedConfiguration = robot->getPositions(dofs);

  // ====================================================================
  // Check for collisions in the start and goal configurations

  // Sift through the possible start configurations and eliminate those that are in collision
  std::vector<Eigen::VectorXd> feasibleStart;
  for(unsigned int i = 0; i < start.size(); i++) {
    robot->setPositions(dofs, start[i]);
    if(!world->checkCollision()) feasibleStart.push_back(start[i]);
  }

  // Return false if there are no feasible start configurations
  if(feasibleStart.empty()) {
    printf("WARNING: PathPlanner: Feasible start points are empty!\n");
    return false;
  }

  // Sift through the possible goal configurations and eliminate those that are in collision
  std::vector<Eigen::VectorXd> feasibleGoal;
  for(unsigned int i = 0; i < goal.size(); i++) {
    robot->setPositions(dofs, goal[i]);
    if(!world->checkCollision()) feasibleGoal.push_back(goal[i]);
  }

  // Return false if there are no feasible goal configurations
  if(feasibleGoal.empty()) {
    printf("WARNING: PathPlanner: Feasible goal points are empty!\n");
    return false;
  }

  // ====================================================================
  // Make the correct RRT algorithm for the given method

  // Direct the search towards single or bidirectional
  bool result = false;
  if(bidirectional)
    result = planBidirectionalRrt(robot, dofs, feasibleStart, feasibleGoal, path);
  else {
    if(feasibleGoal.size() > 1) fprintf(stderr, "WARNING: planPath is using ONLY the first goal!\n");
    result = planSingleTreeRrt(robot, dofs, feasibleStart, feasibleGoal.front(), path);
  }

  // Restore previous robot configuration
  robot->setPositions(dofs, savedConfiguration);

  return result;
}

/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::planSingleTreeRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
    const std::vector<Eigen::VectorXd> &start, const Eigen::VectorXd &goal,
    std::list<Eigen::VectorXd> &path) {

  const bool debug = false;

  // Initialize the RRT
  start_rrt = new R (world, robot, dofs, start, stepSize);

  // Expand the tree until the goal is reached or the max # nodes is passed
  typename R::StepResult result = R::STEP_PROGRESS;
  double smallestGap = std::numeric_limits<double>::infinity();
  std::size_t numNodes = start_rrt->getSize();
  while(numNodes <= maxNodes) {

    // Get the target node based on the bias
    Eigen::VectorXd target;
    double randomValue = ((double) rand()) / RAND_MAX;
    if(randomValue < goalBias) target = goal;
    else target = start_rrt->getRandomConfig();

    // Based on the method, either attempt to connect to the target directly or take a small step
    if(connect) start_rrt->connect(target);
    else start_rrt->tryStep(target);

    // Check if the goal is reached and create the path, if so
    double gap = start_rrt->getGap(goal);
    if(gap < stepSize) {
      if(debug) std::cout << "Returning true, reached the goal" << std::endl;
      start_rrt->tracePath(start_rrt->activeNode, path);
      return true;
    }

    // Update the number of nodes
    numNodes = start_rrt->getSize();
  }

  if(debug) printf("numNodes: %lu\n", numNodes);

  // Maximum # of iterations are reached and path is not found - failed.
  return false;
}

/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::planBidirectionalRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
    const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
    std::list<Eigen::VectorXd> &path) {

  const bool debug = false;

  // Initialize both the start and goal RRTs.
  // NOTE: We use the pointers for the RRTs to swap their roles in extending towards a target
  // (random or goal) node.
  start_rrt = new R(world, robot, dofs, start, stepSize);
  goal_rrt = new R(world, robot, dofs, goal, stepSize);
  R* rrt1 = start_rrt;
  R* rrt2 = goal_rrt;

  // Expand the tree until the trees meet or the max # nodes is passed
  double smallestGap = std::numeric_limits<double>::infinity();
  std::size_t numNodes = rrt1->getSize() + rrt2->getSize();
  while(numNodes < maxNodes) {

    // Swap the roles of the two RRTs. Remember, the first rrt reaches out to a target node and
    // creates a new node and the second rrt reaches to _the new node_.
    R* temp = rrt1;
    rrt1 = rrt2;
    rrt2 = temp;

     // Get the target node based on the bias
    Eigen::VectorXd target;
    double randomValue = ((double) rand()) / RAND_MAX;
    if(randomValue < goalBias) target = goal[0];
    else target = rrt1->getRandomConfig();

    // Based on the method, rrt1 either attempt to connect to the target directly or takes a step
    if(connect) rrt1->connect(target);
    else rrt1->tryStep(target);

    // rrt2 uses the last added node of rrt1 as a target and reaches out to it (connect or step)
    // NOTE: If a node was not added, the nearest neighbor to the random node in rrt1 is used.
    // NOTE: connect(x) and tryStep(x) functions return true if rrt2 can add the given node
    // in the tree. In this case, this would imply that the two trees meet.
    bool treesMet = false;
    const Eigen::VectorXd& rrt2target = *(rrt1->configVector[rrt1->activeNode]);
    if(connect) treesMet = rrt2->connect(rrt2target);
    else treesMet = (rrt2->tryStep(rrt2target) == R::STEP_REACHED);

    // Check if the trees have met and create the path, if so.
    if(treesMet) {
      start_rrt->tracePath(start_rrt->activeNode, path);
      goal_rrt->tracePath(goal_rrt->activeNode, path, true);
      return true;
    }

    // Update the number of nodes in the two trees
    numNodes = rrt1->getSize() + rrt2->getSize();

    // Print the gap between the trees in debug mode
    if(debug) {
      double gap = rrt2->getGap(*(rrt1->configVector[rrt1->activeNode]));
      if(gap < smallestGap) {
        smallestGap = gap;
        std::cout << "Gap: " << smallestGap << "  Sizes: " << start_rrt->configVector.size()
          << "/" << goal_rrt->configVector.size() << std::endl;
      }
    }
  }

  // Maximum # of iterations are reached and path is not found - failed.
  return false;
}

} // namespace planning
} // namespace dart

#endif  // DART_PLANNING_PATHPLANNER_HPP_
