/// Copyright (c) 2015 CNRS

/// Authors: Joseph Mirabel

///

///

// This file is part of hpp-wholebody-step.

// hpp-wholebody-step-planner 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-wholebody-step-planner 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-wholebody-step-planner. If not, see

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

#ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH

#define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH

#include <hpp/constraints/configuration-constraint.hh>

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

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

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

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

#include <hpp/core/path.hh>

#include <hpp/core/problem.hh>

#include <hpp/pinocchio/configuration.hh>

#include <hpp/pinocchio/frame.hh>

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

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

#include <pinocchio/fwd.hpp>

#include <pinocchio/multibody/frame.hpp>

namespace hpp {

namespace rbprm {

namespace interpolation {

using constraints::ComparisonTypes_t;

// declaration of available constraints

template <class Helper_T>

void CreateContactConstraints(const State& from, const State& to);

// template<class Helper_T, typename Reference>

// void CreateComConstraint(Helper_T& helper, const Reference& ref, const

// fcl::Vec3f& initTarget=fcl::Vec3f());

template <class Helper_T, typename Reference>

void CreateEffectorConstraint(Helper_T& helper, const Reference& ref,

                              const pinocchio::Frame effectorJoint,

                              const fcl::Vec3f& initTarget = fcl::Vec3f());

template <class Helper_T, typename Reference>

void Create6DEffectorConstraint(

    Helper_T& helper, const Reference& ref,

    const pinocchio::Frame effectorJoint,

    const fcl::Transform3f& initTarget = fcl::Transform3f());

template <class Helper_T, typename Reference>

void CreateOrientationConstraint(

    Helper_T& helper, const Reference& ref, const pinocchio::Frame effector,

    const pinocchio::DevicePtr_t endEffectorDevice,

    const fcl::Transform3f& initTarget = fcl::Transform3f());

// Implementation

/// Time varying right hand side of constraint

/// \tparam Reference type of shared pointer to Path.

///

/// This class compute the right hand side of a

/// constraints::Implicit instance as a mapping from interval

/// [0,1] to a vector space called reference.

template <class Reference>

struct VecRightSide : public RightHandSideFunctor {

  /// Constructor

  /// \param ref mapping defining the varying right hand side,

  ///        the definition interval may not be [0,1] and will be

  ///        normalized at evaluation.

  /// \param dim dimension of the right hand side. If the dimension of

  ///        the output of ref is bigger than dim, only the first

  ///        coefficients will be used.

               const bool times_ten = false)

  /// Compute and set right hand side of constraint.

  /// \param eq Implicit constraint,

  /// \param normalized_input real valued parameter between 0 and 1,

  ///        mapped in an affine way to a value in the definition interval

  ///        of the reference path.

  ///

  /// If \f$u\in[0,1]\f$ is the normalized input and \f$[a,b]\f$ the

  /// definition interval of \f$\mathbf{ref}\f$, then

  /// \f[

  /// \mathbf{rhs} = \mathbf{ref} (a + u (b-a))

  /// \f]

                          const constraints::value_type& normalized_input,

                          pinocchio::ConfigurationOut_t /*conf*/) const {

    bool success;

    } else {

    }

  }

  /// Reference path of the right hand side of the constraint

  const Reference ref_;

  /// Dimension of the right hand side of the constraint

  const int dim_;

  const bool times_ten_;

};

typedef constraints::PointCom PointCom;

typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t,

                                          PointCom::JacobianType_t>

    s_t;

typedef constraints::SymbolicFunction<s_t> PointComFunction;

typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t;

template <class Helper_T, typename Reference>

                         const fcl::Vec3f& initTarget = fcl::Vec3f()) {

  // constraints::ComparisonType equals = constraints::Equality;

      pinocchio::CenterOfMassComputation::create(device);

      "COM-constraint", device, /*10000 **/ PointCom::create(comComp));

      constraints::Implicit::create(comFunc, equals);

      TimeDependant(comEq, shared_ptr<VecRightSide<Reference> >(

}

template <class Helper_T, typename Reference>

void CreatePosturalTaskConstraint(Helper_T& helper, const Reference& ref) {

  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();

  // core::ComparisonTypePtr_t equals = core::Equality::create ();

  core::ConfigProjectorPtr_t& proj = helper.proj_;

  hppDout(notice, "create postural task, ref config = "

                      << pinocchio::displayConfig(ref));

  std::vector<bool> mask(device->numberDof(), false);

  Configuration_t weight(device->numberDof());

  // mask : 0 for the freeflyer and the extraDoFs :

  for (size_t i = 0; i < 3; i++) {

    mask[i] = false;

    weight[i] = 0.;

  }

  for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {

    mask[i] = false;

    weight[i] = 0.;

  }

  std::ostringstream oss;

  for (std::size_t i = 0; i < mask.size(); ++i) {

    oss << mask[i] << ",";

  }

  hppDout(notice, "mask = " << oss.str());

  // create a weight vector

  for (size_type i = 3; i < device->numberDof() - 7; ++i) {

    weight[i] = 1.;

  }

  // TODO : retrieve it from somewhere, store it in fullbody ?

  // value here for hrp2, from Justin

  // : chest

  weight[6] = 10.;

  // head :

  for (size_type i = 7; i <= 9; i++) weight[i] = 1.;

  // root's orientation

  for (size_type i = 3; i < 6; i++) {

    weight[i] = 50.;

  }

  for (size_t i = 3; i <= 9; i++) {

    mask[i] = true;

  }

  //  mask[5] = false; // z root rotation ????

  // normalize weight array :

  double moy = 0;

  int num_active = 0;

  for (size_type i = 0; i < weight.size(); i++) {

    if (mask[i]) {

      moy += weight[i];

      num_active++;

    }

  }

  moy = moy / num_active;

  for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;

  constraints::ConfigurationConstraintPtr_t postFunc =

      constraints::ConfigurationConstraint::create("Postural_Task", device, ref,

                                                   weight);

  ComparisonTypes_t comps;

  comps.push_back(constraints::Equality);

  const constraints::ImplicitPtr_t posturalTask =

      constraints::Implicit::create(postFunc, comps);

  proj->add(posturalTask, 1);

  // proj->updateRightHandSide();

}

    pinocchio::DevicePtr_t device, const fcl::Vec3f& initTarget,

    const pinocchio::Frame effectorFrame) {

  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false);

  // mask.push_back(true);

  return constraints::Position::create(

      "", device, effectorJoint,

}

    pinocchio::DevicePtr_t device, const fcl::Transform3f& initTarget,

    const pinocchio::Frame effectorFrame) {

  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false);

  // mask.push_back(true);

  return constraints::Orientation::create("", device, effectorJoint, rotation,

}

template <class Helper_T, typename Reference>

                              const pinocchio::Frame effectorFr,

                              const fcl::Vec3f& initTarget) {

      createPositionMethod(device, initTarget, effectorFrame), equals);

      TimeDependant(effEq, shared_ptr<VecRightSide<Reference> >(

}

/// Time varying right hand side of constraint

/// \tparam Reference type of shared pointer to Path.

/// \tparam method modifying the right hand side

///

/// This class compute the right hand side of a

/// constraints::Implicit instance as the composition of a mapping called

/// method with a mapping from interval [0,1] to a vector space called

/// reference.

template <typename Reference, typename fun>

struct funEvaluator : public RightHandSideFunctor {

  /// Constructor

  /// \param ref Reference path of right hand side.

  /// \param method mapping from the output space of ref to a vector

  ///        space of dimension the right hand side of the constraint.

  /// Time range of reference path of right hand side

  const std::pair<core::value_type, core::value_type> timeRange() {

    return ref_->timeRange();

  }

  /// Compute and set right hand side of constraint

  /// \param eq Implicit constraint,

  /// \param normalized_input real valued parameter between 0 and 1,

  ///        mapped in an affine way to a value in the definition interval

  ///        of the reference path.

  ///

  /// If \f$u\in[0,1]\f$ is the normalized input and \f$[a,b]\f$ the

  /// definition interval of \f$\mathbf{ref}\f$, then

  /// \f[

  /// \mathbf{rhs} = \mathbf{M} \left(\mathbf{ref} (a + u (b-a))\right)

  /// \f]

  /// where \f$\mathbf{M}\f$ is the method provided to the constructor.

                  const constraints::value_type& normalized_input,

                  pinocchio::ConfigurationOut_t /*conf*/) const {

    bool success;

    // maps from interval [0,1] to definition interval.

            .vector());

  }

  const Reference ref_;

  const fun method_;

  const std::size_t dim_;

};

template <class Helper_T, typename Reference>

                                 const pinocchio::Frame effectorFr,

                                 const pinocchio::DevicePtr_t endEffectorDevice,

                                 const fcl::Transform3f& initTarget) {

      createOrientationMethod(device, initTarget, effectorFrame);

      endEffectorDevice, initTarget,

      endEffectorDevice->getFrameByName(

          endEffectorDevice->rootJoint()

              ->childJoint(0)

              ->name()));  // same orientation constraint but for a freeflyer

                           // device that represent the end effector (same dim

                           // as the ref path)

      constraints::Implicit::create(orCons, equals);

  // proj->updateRightHandSide();

                                                                 orConsRef));

}

template <class Helper_T, typename Reference>

                                const pinocchio::Frame effectorJoint,

                                const fcl::Transform3f& initTarget) {

  // CreateEffectorConstraint(helper, ref, effectorJoint,

  // initTarget.getTranslation());

  // reduce dof if reference path is of lower dimension

  bool success;

  }

      createOrientationMethod(device, initTarget, effector);

      constraints::Implicit::create(orCons, equals);

  // proj->updateRightHandSide();

}

void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody,

                           pinocchio::DevicePtr_t device,

                           core::ConfigProjectorPtr_t projector,

                           const State& state,

                           const std::vector<std::string> active);

template <class Helper_T>

                              const State& to) {

}

std::string getEffectorLimb(const State& startState, const State& nextState);

fcl::Vec3f getNormal(const std::string& effector, const State& state,

                     bool& found);

pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody,

                             const State& startState, const State& nextState);

DevicePtr_t createFreeFlyerDevice();

}  // namespace interpolation

}  // namespace rbprm

}  // namespace hpp

#endif  // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH