// 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/fcl/BVH/BVH_model.h>

#include <hpp/fcl/collision_object.h>

#include <Eigen/Geometry>

#include <hpp/core/collision-validation-report.hh>

#include <hpp/core/collision-validation.hh>

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

#include <hpp/core/problem.hh>

#include <hpp/pinocchio/configuration.hh>

#include <hpp/pinocchio/liegroup-element.hh>

#include <hpp/pinocchio/liegroup-space.hh>

#include <hpp/rbprm/rbprm-shooter.hh>

#include <hpp/rbprm/rbprm-validation-report.hh>

#include <hpp/util/timer.hh>

#include <pinocchio/algorithm/joint-configuration.hpp>

namespace hpp {

using namespace core;

using namespace fcl;

namespace {

static const int SIZE_EULER = 6;

typedef fcl::BVHModel<OBBRSS> BVHModelOB;

typedef fcl::shared_ptr<const BVHModelOB> BVHModelOBConst_Ptr_t;

    const pinocchio::FclConstCollisionObjectPtr_t object) {

    hppDout(warning,

            "Collision geometry in shooter is not a BV_OBBRSS, cannot get the "

            "model.");

  }

  // assert(object->collisionGeometry()->getNodeType() == BV_OBBRSS);

  const BVHModelOBConst_Ptr_t model =

  // assert(model->getModelType() == BVH_MODEL_TRIANGLES);

    hppDout(warning, "Collision model is not of type BVH_MODEL_TRIANGLES.");

  }

}

  double a, b, c;

}

    const pinocchio::RbPrmDevicePtr_t robot) {

    } else {

    }

  }

}

                          Configuration_t& config, const Vec3f& translation) {

  }

               const Vec3f& translation) {

  // bound to positions limits

  }

/*void SampleRotationRec(ConfigurationPtr_t config, JointVector_t& jv,

std::size_t& current)

{

    JointPtr_t joint = jv[current++];

    std::size_t rank = joint->rankInConfiguration ();

    const hpp::pinocchio::LiegroupSpacePtr_t jm = joint->configurationSpace();

    jm->neutral().setNeutral(); setRandom ();

    hpp::pinocchio::LiegroupSpace toto;

    //toto/

    joint->jointModel()-> configuration ()->uniformlySample (rank, *config);

    if(current<jv.size())

        SampleRotationRec(config,jv,current);

}*/

    // std::cout << "rot i " << rot [i/2] << " i " << i/2 << std::endl;

  }

  /*for(std::size_t i = 0; i <4; ++i)

  {

      config(rank+i) = qt.coeffs()(i);

  }*/

}

/*void SampleRotation(pinocchio::DevicePtr_t so3, ConfigurationPtr_t config,

JointVector_t& jv)

{

    std::size_t id = 1;

    if(so3->rootJoint())

    {

        Eigen::Matrix <value_type, 3, 1> confso3;

        id+=1;

        pinocchio::JointPtr_t joint = so3->rootJoint();

        for(int i =0; i <3; ++i)

        {

            joint->configuration()->uniformlySample (i, confso3);

            joint = joint->numberChildJoints() > 0 ? joint->childJoint(0) : 0;

        }

        Eigen::Quaterniond qt = Eigen::AngleAxisd(confso3(0),

Eigen::Vector3d::UnitZ())

          * Eigen::AngleAxisd(confso3(1), Eigen::Vector3d::UnitY())

          * Eigen::AngleAxisd(confso3(2), Eigen::Vector3d::UnitX());

        std::size_t rank = 3;

        (*config)(rank+0) = qt.w();

        (*config)(rank+1) = qt.x();

        (*config)(rank+2) = qt.y();

        (*config)(rank+3) = qt.z();

    }

    if(id < jv.size())

        SampleRotationRec(config,jv,id);

}*/

/*std::vector<double> initSo3()

{

    std::vector<double> res;

    int sign = -1;

    for (int i =0; i<6; ++i)

    {

        res.push_back(sign * std::numeric_limits<double>::infinity());

        sign *= -1;

    }

    //DevicePtr_t so3Robot = pinocchio::Device::create("so3Robot");

    //so3Robot->rootJoint(0);

    return res;

}*/

                       const std::vector<double>& limitszyx) {

  /*pinocchio::Joint* previous = so3Robot->rootJoint();

  if(previous == 0)

  {

      // init joints

      previous = new pinocchio::jointRotation::Bounded(fcl::Transform3f());

      pinocchio::Joint * jy = new

  pinocchio::jointRotation::Bounded(fcl::Transform3f()); pinocchio::Joint * jx =

  new pinocchio::jointRotation::Bounded(fcl::Transform3f());

      so3Robot->rootJoint(previous);

      previous->addChildJoint (jy);

      jy->addChildJoint (jx);

      previous->name("so3z");

      jy->name("so3y");

      jx->name("so3x");

  }

  // set limits pinocchio::JointPtr_t

  assert(limitszyx.size() == 6);

  int i = 0;

  pinocchio::JointPtr_t current = previous;

  for(std::vector<double>::const_iterator cit = limitszyx.begin();

      cit != limitszyx.end(); ++cit, ++i)

  {

      if(i % 2 == 0)

      {

          current->lowerBound(0, *cit);

      }

      else

      {

          current->upperBound(0, *cit);

          current = current->numberChildJoints() > 0 ? current->childJoint(0) :

  0;

      }

  }*/

}  // namespace

namespace rbprm {

    const pinocchio::RbPrmDevicePtr_t& robot,

    const ObjectStdVector_t& geometries, const affMap_t& affordances,

    const std::vector<std::string>& filter,

    const std::map<std::string, std::vector<std::string> >& affFilters,

    const std::size_t shootLimit, const std::size_t displacementLimit) {

  hppDout(notice, "&&&&&& SEED = " << seed);

  RbPrmShooter* ptr =

      new RbPrmShooter(robot, geometries, affordances, filter, affFilters,

}

/**

 * @brief getUsedSurfaces produce a list of CollisionObject from the affordances

 * list : use all objects corresponding to at least one affordance filter set.

 * @param affordances

 * @param affFilters

 * @return

 */

    const affMap_t& affordances,

    const std::map<std::string, std::vector<std::string> >& affFilters) {

  hppDout(notice, "Begin getUsedSurfaces from affordances");

    hppDout(notice, "For rom : " << itFilter->first);

      hppDout(notice, "aff type : " << *itType);

        hppDout(notice,

                "new type of affordance, add corresponding collision objects "

                "to the list");

          }

        }

      }

    }

  }

  hppDout(notice, "final size of surfaces list : " << surfaces.size());

}

// TODO: outward

    const pinocchio::RbPrmDevicePtr_t& robot,

    const hpp::core::ObjectStdVector_t& geometries, const affMap_t& affordances,

    const std::vector<std::string>& filter,

    const std::map<std::string, std::vector<std::string> >& affFilters,

    : shootLimit_(shootLimit),

      displacementLimit_(displacementLimit),

      filter_(filter),

      weights_(),

      triangles_(),

      robot_(robot),

                                                affordances, geometries)),

      uniformShooter_(core::configurationShooter::Uniform::create(robot)),

  }

    const core::ObjectStdVector_t& geometries) {

      hppDout(notice, "Area of triangle = " << weight);

    }

  }

  hppDout(notice, "Sum of all areas of triangles : " << sum);

    hppDout(notice, "current weight = " << previousWeight);

  }

  hppDout(notice, "number of triangle for the shooter : " << triangles_.size());

    const {

}

  }

}

                                   const std::vector<double>& eulerSo3,

                                   Configuration_t& config,

                                   const Vec3f p) const {

    const Contact& c, hpp::core::CollisionObjectConstPtr_t colObj) {

  } else {

    hppDout(warning,

            "In shooter : cannot get contact normal, use z axis by default.");

  }

}

  hppDout(notice, "!!! Random shoot");

  HPP_DEFINE_TIMECOUNTER(SHOOT_COLLISION);

    // pick one triangle randomly

    else

    // http://stackoverflow.com/questions/4778147/sample-random-point-in-triangle

    double r1, r2;

    // set configuration position to sampled point

    // rotate and translate randomly until valid configuration found or

    // no obstacle is reachable

      HPP_START_TIMECOUNTER(SHOOT_COLLISION);

      RbprmValidationReportPtr_t report =

      HPP_STOP_TIMECOUNTER(SHOOT_COLLISION);

        // try to rotate to reach rom

          // SampleRotation(eulerSo3_, config);

          HPP_START_TIMECOUNTER(SHOOT_COLLISION);

          HPP_STOP_TIMECOUNTER(SHOOT_COLLISION);

          }

          HPP_START_TIMECOUNTER(SHOOT_COLLISION);

          // found = validator_->validate(config, filter_);

          HPP_STOP_TIMECOUNTER(SHOOT_COLLISION);

        }

      {

        // retrieve Contact information

                 0.03));

      }

    }

  }

  hppDout(info, "shoot : " << pinocchio::displayConfig(config));

  HPP_DISPLAY_TIMECOUNTER(SHOOT_COLLISION);

}

    core::Parameter::FLOAT, "RbprmShooter/ratioWeighted",

    "The ratio used to select a random triangle with a weight proportional to "

    "it's area. "

    "Otherwise the triangles are choosed uniformly. ",

}  // namespace rbprm

}  // namespace hpp