//

// Copyright (c) 2014 CNRS

// Authors: Mylene Campana

//

// This file is part of hpp-core

// hpp-core 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-core 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/>.

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

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

#include <hpp/pinocchio/configuration.hh>

#include <hpp/pinocchio/device.hh>

#include <hpp/pinocchio/joint.hh>

#include <hpp/rbprm/planner/parabola-path.hh>

#include <hpp/util/debug.hh>

namespace hpp {

namespace rbprm {

using core::interval_t;

using core::size_type;

using core::value_type;

using core::vector_t;

using pinocchio::displayConfig;

                           core::ConfigurationIn_t init,

                           core::ConfigurationIn_t end, value_type length,

               device->numberDof()),

      device_(device),

      initial_(init),

      end_(end),

{

}

                           core::ConfigurationIn_t init,

                           core::ConfigurationIn_t end, value_type length,

                           vector_t coefs, vector_t V0, vector_t Vimp,

                           std::vector<std::string> initialROMnames,

               device->numberDof()),

      V0_(V0),

      Vimp_(Vimp),

      initialROMnames_(initialROMnames),

      endROMnames_(endROMnames),

      device_(device),

      initial_(init),

      end_(end),

  hppDout(info, "V0_= " << V0_.transpose() << " Vimp_= " << Vimp_.transpose());

  hppDout(info, "initialROMnames size= " << initialROMnames_.size());

}

    : parent_t(path),

  hppDout(info, "V0_= " << V0_.transpose() << " Vimp_= " << Vimp_.transpose());

  hppDout(info, "initialROMnames size= " << initialROMnames_.size());

}

                                value_type param) const {

  }

  }

  // param = x_theta

  /* const value_type x_theta_max = - 0.5 *

       coefficients_ (1) / coefficients_ (0);

   const value_type x_theta_initial = cos(theta)*initial_ (0) +

       sin(theta)*initial_ (1);

   const value_type x_theta_end = cos(theta)*end_ (0) +

       sin(theta)*end_ (1);

   const bool tanThetaNotDefined = (theta < M_PI/2 + 1e-2 && theta > M_PI/2 -

   1e-2) || (theta > -M_PI/2 - 1e-2 && theta < -M_PI/2 + 1e-2);

*/

  /* Quaternions interpolation */

  /*const core::JointPtr_t SO3joint = device_->getJointByName

  ("base_joint_SO3"); const std::size_t rank = SO3joint->rankInConfiguration ();

  const core::size_type dimSO3 = SO3joint->configSize ();

  SO3joint->configuration ()->interpolate

      (initial_, end_, u, rank, result);

  // if robot-trunk has internal DoF (except freeflyer ones)

  // then linear interpolation on them

  const std::size_t freeflyerDim = 3 + dimSO3;

  const bool hasInternalDof = nbConfig > ecsDim + freeflyerDim;

  if (hasInternalDof) {

    for (core::size_type i = freeflyerDim; i<nbConfig-ecsDim; i++) {

      result (i) = (1 - u) * initial_ (i) + u * end_ (i);

    }

  }*/

  /* Set to zero extra-configs (only on path, not on extremities */

}

  hppDout(error, "path extract is not recommended on parabola path");

  bool success;

  ParabolaPathPtr_t result = rbprm::ParabolaPath::create(

  hppDout(info, "initialROMnames size= " << (*result).initialROMnames_.size());

}

  hppDout(notice, " ~ reverse path parabola !!!!!!!!!!!!!!!!!!!!!!");

  bool success;

  ParabolaPathPtr_t result =

  hppDout(info, "V0_= " << V0_.transpose() << " Vimp_= " << Vimp_.transpose());

  hppDout(info, "result V0_= " << (*result).V0_.transpose() << " result Vimp_= "

                               << (*result).Vimp_.transpose());

  hppDout(info,

          "path->initialROMnames size= " << (*result).initialROMnames_.size());

  hppDout(info,

          "this->initialROMnames size= " << (*this).initialROMnames_.size());

  hppDout(info, "result->initialROMnames size= "

                    << (*result).initialROMnames_.size());

}

/*  value_type ParabolaPath::computeLength

  (const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const {

    const int N = 6; // number -1 of interval sub-divisions

    // for N = 4, computation error ~= 1e-5.

    // for N = 20, computation error ~= 1e-11.

    value_type length = 0;

    const value_type theta = coefficients_ (3);

    value_type x1 = cos(theta) * q1 (0)  + sin(theta) * q1 (1); // x_theta_0

    value_type x2 = cos(theta) * q2 (0) + sin(theta) * q2 (1); // x_theta_imp

    hppDout(notice,"xTheta0 = "<<x1 << " , "<<coefficients_[6]<<"   xThetaImp =

  "<<x2);

    // Define integration bounds

    if (x1 > x2) { // re-order integration bounds

      const value_type xtmp = x1;

      x1 = x2;

      x2 = xtmp;

    }

    const value_type dx = (x2 - x1) / N; // integration step size

    for (int i=0; i<N; i++) {

      length += dx*( 0.166666667*lengthFunction (x1 + i*dx)

                     + 0.666666667*lengthFunction (x1 + (i+0.5)*dx )

                     + 0.166666667*lengthFunction (x1 + (i+1)*dx ));

      // apparently, 1/6 and 2/3 are not recognized as floats ...

    }

    hppDout (notice, "length = " << length);

    return length;

  }*/

// test (pierre) :

                                       const core::ConfigurationIn_t q2) const {

  ;

  // theta = coef[3]

}

// Function equivalent to sqrt( 1 + f'(x)^2 )

  const value_type y =

}

  bool success;

}

}  //   namespace rbprm

}  // namespace hpp