//

// Copyright (c) 2014 CNRS

// Authors: Steve Tonneau (steve.tonneau@laas.fr)

//

// This file is part of hpp-rbprm.

// hpp-rbprm is free software: you can redistribute it

// and/or modify it under the terms of the GNU Lesser General Public

// License as published by the Free Software Foundation, either version

// 3 of the License, or (at your option) any later version.

//

// hpp-rbprm is distributed in the hope that it will be

// useful, but WITHOUT ANY WARRANTY; without even the implied warranty

// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

// General Lesser Public License for more details.  You should have

// received a copy of the GNU Lesser General Public License along with

// hpp-core  If not, see

// <http://www.gnu.org/licenses/>.

#ifndef HPP_RBPRM_TIME_CONSTRAINT_HELPER_UTILS_HH

#define HPP_RBPRM_TIME_CONSTRAINT_HELPER_UTILS_HH

#include <hpp/constraints/generic-transformation.hh>

#include <hpp/core/bi-rrt-planner.hh>

#include <hpp/core/constraint-set.hh>

#include <hpp/core/path-optimization/partial-shortcut.hh>

#include <hpp/core/path-optimization/random-shortcut.hh>

#include <hpp/core/problem-target/goal-configurations.hh>

#include <hpp/core/roadmap.hh>

#include <hpp/core/steering-method/straight.hh>

#include <hpp/rbprm/interpolation/limb-rrt-shooter.hh>

#include <hpp/rbprm/interpolation/time-constraint-path-validation.hh>

#include <hpp/rbprm/interpolation/time-constraint-utils.hh>

#include <hpp/rbprm/interpolation/time-dependant.hh>

// #include <hpp/constraints/position.hh>

// #include <hpp/constraints/orientation.hh>

#include <hpp/constraints/locked-joint.hh>

#include <hpp/core/config-projector.hh>

#include <hpp/core/path-vector.hh>

#include <hpp/core/subchain-path.hh>

#include <hpp/pinocchio/joint.hh>

// #include <hpp/pinocchio/object-factory.hh>

#include <hpp/constraints/relative-com.hh>

#include <hpp/constraints/symbolic-calculus.hh>

#include <hpp/constraints/symbolic-function.hh>

#include <hpp/pinocchio/configuration.hh>

#include <hpp/rbprm/tools.hh>

#include <vector>

namespace hpp {

using namespace core;

namespace rbprm {

namespace interpolation {

namespace {

}

}  // namespace

template <class Path_T, class ShooterFactory_T, typename ConstraintFactory_T>

                          ConstraintFactory_T>::InitConstraints() {

}

template <class Path_T, class ShooterFactory_T, typename ConstraintFactory_T>

    SetConfigShooter(const hpp::rbprm::State& from,

                     const hpp::rbprm::State& to) {

}

namespace {

    const rbprm::T_Limb& limbs) {

  }

}

inline void DisableUnNecessaryCollisions(core::Problem& problem,

                                         rbprm::RbPrmLimbPtr_t limb) {

  // TODO should we really disable collisions for other bodies ?

  tools::RemoveNonLimbCollisionRec<core::Problem>(

      problem.robot()->rootJoint(),

      //"all",

      limb->limb_->name(), problem.collisionObstacles(), problem);

  if (limb->disableEndEffectorCollision_) {

    hpp::tools::RemoveEffectorCollision<core::Problem>(

        problem, problem.robot()->getJointByName(limb->effector_.name()),

        problem.collisionObstacles());

  }

}

    core::Problem& problem, const std::vector<std::string>& variations,

    const rbprm::T_Limb& limbs) {

          problem.collisionObstacles());

    }

  }

  // TODO should we really disable collisions for other bodies ?

      problem.collisionObstacles(), problem);

}

                                        const State& from, const State& to) {

}

}  // namespace

template <class Path_T, class Shooter_T, typename ConstraintFactory_T>

PathVectorPtr_t

    const State& from, const State& to, const size_t maxIterations) {

  // get limbs that moved

      to.allVariations(from, extractEffectorsName(limbs));

  }

  // TODO IS THIS REDUNDANT ?

  /*for(rbprm::CIT_Limb lit = limbs.begin(); lit != limbs.end(); ++lit)

  {

      if(lit->second->disableEndEffectorCollision_ &&

  std::find(variations.begin(), variations.end(), lit->second->limb_->name()) ==

  variations.end())

      {

          hpp::tools::RemoveEffectorCollision<core::Problem>(rootProblem_,

                                                             rootProblem_->robot()->getJointByName(lit->second->effector_->name()),

                                                             rootProblem_->collisionObstacles());

      }

  }*/

  hppDout(notice, "Run : start config = r([" << pinocchio::displayConfig(*start)

                                             << "])");

  hppDout(notice,

          "Run : end   config = r([" << pinocchio::displayConfig(*end) << "])");

  // ProblemTargetPtr_t target = problemTarget::GoalConfigurations::create

  // (planner);

  hppDout(notice, "Max iterations : " << maxIterations);

  boost::posix_time::ptime timeStart(

  try {

    hppDout(notice,

            "TimeConstraintHelper : solved in "

                << ((boost::posix_time::microsec_clock::universal_time() -

                     timeStart)

                        .total_milliseconds())

                << " ms");

    hppDout(notice, "With a roadmap of " << planner->roadmap()->nodes().size()

                                         << " nodes");

    hppDout(notice,

            "TimeConstraintHelper : planner returned an error : " << e.what());

    hppDout(notice, "TimeConstraintHelper : Projection error : " << e.what());

  }

}

namespace {

template <class Helper_T>

                         const std::size_t numOptimizations) {

    hppDout(notice, "Optimize random shortucut, iter : " << j);

  }

    hppDout(notice, "Optimize partial shortcut, iter : " << j);

  }

}

    }

  }

  // if (numValid==0)

  //     throw std::runtime_error("No path found at state 0");

  }

}

                                          std::size_t numValid,

                                          const bool keepExtraDof,

                                          hpp::pinocchio::DevicePtr_t device) {

  }

  PathVectorPtr_t flat = core::PathVector::create(

      flat);  // assure that there is no pathVector inside a pathVector

  hppDout(notice,

          "end of interpolateStatesFromPath, number of paths in pathVector : "

              << flat->numberPaths());

  } else {

    // reducing path (remove last value of the configuration)

    core::segments_t velIntervals(1,

    PathVectorPtr_t reducedPV = core::PathVector::create(

    }

  }

}

}  // namespace

struct GenPath {

  }

  const core::Problem& problem_;

};

struct ExtractPath {

  }

  PathPtr_t refPath_;

};

template <class Helper_T, class StateIterator_T, class ShooterFactory_T,

          typename ConstraintFactory_T, class PathGetter_T>

    RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,

    const ShooterFactory_T& shooterFactory,

    const ConstraintFactory_T& constraintFactory,

    const PathGetter_T& pathGetter, const StateIterator_T& startState,

    const StateIterator_T& endState, const std::size_t numOptimizations,

    const bool keepExtraDof = false,

    const pinocchio::value_type error_treshold = 0.001,

    const size_t maxIterations = 0) {

  hppDout(notice, "Begin interpolateStatesFromPathGetter :");

      pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM);

  bool valid[100];

  // treat each interpolation between two states separatly

  // in a different thread

  hppDout(notice, "InterpolateStates from path : distance = " << distance);

  std::size_t numValid;

    for (std::size_t i = 0; i < distance; ++i) {

      StateIterator_T a, b;

      a = (startState + i);

      b = (startState + i + 1);

      Helper_T helper(fullbody, shooterFactory, constraintFactory,

                      referenceProblem, pathGetter(a, b),

                      error_treshold);  // 0.1 : error

      helper.SetConstraints(get(a), get(b));

      hppDout(notice, "Start helper.run :");

      PathVectorPtr_t partialPath = helper.Run(get(a), get(b), maxIterations);

      hppDout(notice, "helper.run done.");

      if (partialPath) {

        res[i] = optimize(helper, partialPath, numOptimizations);

        valid[i] = true;

      } else {

        hppDout(notice,

                "InterpolateStates : helper.run returned an empty path");

        valid[i] = false;

      }

    }

  return ConcatenateAndResizePath(res, numValid, keepExtraDof,

}

template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T>

    RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,

    const ShooterFactory_T& shooterFactory,

    const ConstraintFactory_T& constraintFactory, const PathPtr_t refPath,

    const CIT_StateFrame& startState, const CIT_StateFrame& endState,

    const std::size_t numOptimizations, const bool keepExtraDof,

    const value_type error_treshold, const size_t maxIterations) {

  return interpolateStatesFromPathGetter<Helper_T, CIT_StateFrame,

                                         ShooterFactory_T, ConstraintFactory_T,

                                         ExtractPath>(

      fullbody, referenceProblem, shooterFactory, constraintFactory,

      extractPath, startState, endState, numOptimizations, keepExtraDof,

}

template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T,

          typename StateConstIterator>

    RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,

    const ShooterFactory_T& shooterFactory,

    const ConstraintFactory_T& constraintFactory,

    const StateConstIterator& startState, const StateConstIterator& endState,

    const std::size_t numOptimizations, const bool keepExtraDof,

    const value_type error_treshold, const size_t maxIterations) {

  return interpolateStatesFromPathGetter<Helper_T, StateConstIterator,

                                         ShooterFactory_T, ConstraintFactory_T,

                                         GenPath>(

      fullbody, referenceProblem, shooterFactory, constraintFactory, genPath,

      startState, endState, numOptimizations, keepExtraDof, error_treshold,

}

}  // namespace interpolation

}  // namespace rbprm

}  // namespace hpp

#endif  // HPP_RBPRM_TIME_CONSTRAINT_HELPER_UTILS_HH