// Copyright (c) 2014, LAAS-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-rbprm. If not, see <http://www.gnu.org/licenses/>.

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

#include <hpp/core/configuration-shooter/uniform.hh>

#include <hpp/core/path-validation/discretized.hh>

#include <hpp/core/problem-solver.hh>

#include <hpp/pinocchio/configuration.hh>

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

#include <hpp/rbprm/interpolation/interpolation-constraints.hh>

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

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

#include <hpp/rbprm/tools.hh>

#ifdef PROFILE

#include "hpp/rbprm/rbprm-profiler.hh"

#endif

namespace hpp {

using namespace core;

namespace rbprm {

namespace interpolation {

                                      const State &to) const {

  // CreateComConstraint<ComRRTHelper,core::PathPtr_t>(helper, helper.refPath_);

  // add postural task to respect the constraint during all the motion but

  // greatly increase computation time

  // FIXME : can lead to discontinuities ...

  /*  Configuration_t refConfig = helper.fullbody_->referenceConfig();

    CreatePosturalTaskConstraint<ComRRTHelper,Configuration_t>(helper,

    refConfig); helper.proj_->lastIsOptional(true);

    helper.proj_->numOptimize(500);

    helper.proj_->lastAsCost(true);

    helper.proj_->errorThreshold(1e-3);*/

}

                       ProblemSolverPtr_t problemSolver,

                       const PathPtr_t comPath, const State &startState,

                       const State &nextState,

                       const std::size_t numOptimizations,

                       const bool keepExtraDof) {

}

                       ProblemPtr_t referenceProblem, const PathPtr_t comPath,

                       const State &startState, const State &nextState,

                       const std::size_t numOptimizations,

                       const bool keepExtraDof) {

  hppDout(notice, "Begin interpolation::comRRT.");

  // check whether there is a contact variations

  std::vector<std::string> variations = nextState.allVariations(

  hppDout(notice, "comRRT : comPath length : " << comPath->length());

  hppDout(notice, "comRRT : startState : r(["

                      << pinocchio::displayConfig(startState.configuration_)

                      << "])");

  hppDout(notice, "comRRT : nextState  : r(["

                      << pinocchio::displayConfig(nextState.configuration_)

                      << "])");

    hppDout(notice, "No contact variation, use comRRT.");

    /*if(keepExtraDof)

    {

        device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension()+1);

    }*/

    core::ConfigProjectorPtr_t proj =

    }

    core::ConstraintSetPtr_t cSet =

    ConfigurationPtr_t start =

    ConfigurationPtr_t end =

    ProblemTargetPtr_t target =

    hppDout(notice, "Start solve");

    hppDout(notice, "Solve success");

    // return guidePath;

  } else {

    hppDout(notice, "Contact variations, use limbRRT.");

    hppDout(notice, "LimbRRT done.");

  }

  SetComRRTConstraints constraintFactory;

#ifdef PROFILE

  RbPrmProfiler &watch = getRbPrmProfiler();

  watch.start("com_traj");

#endif

  hppDout(notice, "Start interpolateStatesFromPath");

  core::PathPtr_t resPath =

      interpolateStatesFromPath<ComRRTHelper, ComRRTShooterFactory,

                                SetComRRTConstraints>(

          fullbody, referenceProblem, shooterFactory, constraintFactory,

  hppDout(notice, "interpolateStatesFromPath end.");

    hppDout(notice, "end of com-rrt, number of paths in pathVector : "

                        << pv->numberPaths());

#ifdef PROFILE

  watch.stop("com_traj");

#endif

}

                               core::ProblemSolverPtr_t problemSolver,

                               const PathPtr_t comPath,

                               const PathPtr_t guidePath,

                               const CIT_StateFrame &startState,

                               const CIT_StateFrame &endState,

                               const std::size_t numOptimizations) {

  SetComRRTConstraints constraintFactory;

  return interpolateStatesFromPath<ComRRTHelper, ComRRTShooterFactory,

                                   SetComRRTConstraints>(

}

                                   core::ProblemPtr_t referenceProblem,

                                   const State &model,

                                   const fcl::Vec3f &targetCom, bool &success) {

  SetComRRTConstraints constraintFactory;

  ComRRTHelper helper(fullbody, unusedFactory, constraintFactory,

                                                     targetCom);

  // Configuration_t refConfig = fullbody->referenceConfig();

  // CreatePosturalTaskConstraint<ComRRTHelper,Configuration_t>(helper,

  // refConfig);

  //  helper.proj_->lastIsOptional(true);

  // helper.proj_->lastAsCost(true);

    /*hppDout(notice,"projection successfull, trying to optimize : ");

    CreatePosturalTaskConstraint<ComRRTHelper,ConfigurationPtr_t>(helper,

    refConfig); helper.proj_->lastIsOptional(true);

    helper.proj_->errorThreshold(1e-3);

    hppDout(notice,"before : "<<pinocchio::displayConfig(res));

    bool opSuccess = helper.proj_->optimize(res);

    hppDout(notice,"optimize successfull : "<<opSuccess);

    hppDout(notice,"after : "<<pinocchio::displayConfig(res));*/

  } else {

  }

  // copy extraDoF from original config :

  hppDout(notice, "projectOnCom end : ");

  hppDout(notice, "original config : "

                      << pinocchio::displayConfig(model.configuration_));

  hppDout(notice, "project  config : " << pinocchio::displayConfig(res));

  hppDout(notice, "project  config : "

                      << pinocchio::displayConfig(res.head(res.rows() - 1)));

}

/*void generateOneComPath(const pinocchio::ConfigurationIn_t & from, const

pinocchio::ConfigurationIn_t & to, const pinocchio::ConfigurationIn_t &

initSpeed, const double& acceleration)

{

}

typedef

std::vector<pinocchio::vector_t,Eigen::aligned_allocator<pinocchio::vector_t> >

T_Configuration; core::PathPtr_t generateComPath(pinocchio::DevicePtr_t device,

const T_Configuration& configurations, const std::vector<double>& accelerations,

const pinocchio::value_type dt, const pinocchio::ConfigurationIn_t & initSpeed)

{

    assert(configurations.size() == accelerations.size() +1);

    core::PathVectorPtr_t res = core::PathVector::create(device->configSize(),

device->numberDof()); pinocchio::value_type size_step = 1

/(pinocchio::value_type)(positions.size()); pinocchio::value_type u = 0.;

    CIT_Configuration pit = positions.begin();

    pinocchio::Configuration_t previous = addRotation(pit, 0., q1, q2, ref),

current;

    ++pit;

    for(;pit != positions.end()-1; ++pit, u+=size_step)

    {

        current = addRotation(pit, u, q1, q2, ref);

        res->appendPath(makePath(device,problem, previous, current));

        previous = current;

    }

    // last configuration is exactly q2

    current = addRotation(pit, 1., q1, q2, ref);

    res->appendPath(makePath(device,problem, previous, current));

    return res;

}*/

}  // namespace interpolation

}  // namespace rbprm

}  // namespace hpp