//

// Copyright (c) 2017 CNRS

// Authors: Fernbach Pierre

//

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

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

#include <hpp/pinocchio/configuration.hh>

#include <hpp/rbprm/planner/rbprm-node.hh>

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

#include <hpp/util/debug.hh>

#include <hpp/util/timer.hh>

#include "hpp/rbprm/utils/algorithms.h"

namespace hpp {

namespace core {

typedef centroidal_dynamics::MatrixXX MatrixXX;

typedef centroidal_dynamics::Matrix6X Matrix6X;

typedef centroidal_dynamics::Vector3 Vector3;

typedef centroidal_dynamics::Matrix3 Matrix3;

typedef centroidal_dynamics::Matrix63 Matrix63;

typedef centroidal_dynamics::Vector6 Vector6;

typedef centroidal_dynamics::VectorX VectorX;

}

    const core::CollisionValidationReportPtr_t report, geom::T_Point& inter,

    geom::Point& pn, pinocchio::DeviceData& deviceData) {

  // hppDout(notice,"~~ compute intersection between : "<<obj_rom->name() << "

  // and "<<obj_env->name()); convert the two objects  :

  hppStartBenchmark(COMPUTE_INTERSECTION);

  // plane contains a list of points : the intersections between model_rom and

  // the infinite plane defined by model_env. but they may not be contained

  // inside the shape defined by model_env

  hppStopBenchmark(COMPUTE_INTERSECTION);

  hppDisplayBenchmark(COMPUTE_INTERSECTION);

        plane,

  else

  else

}

                             geom::Point& pn, geom::Point& center,

                             pinocchio::DeviceData& deviceData) {

  // compute center point of the hull

  hppDout(notice, "Center : " << center.transpose());

  hppDout(notice, "Normal : " << pn.transpose());

}

/**

 * @brief approximateContactPoint Compute the approximation of the contact

 * Point. Current implementation : closest point of the reference config inside

 * the contact surface.

 * @param report the collision report

 * @param pn output the normal of the contact

 * @param result output the contact point

 * @param config configuration of the robot (only the root's configuration is

 * used here)

 * @param device

 * @return bool success

 */

                             const core::CollisionValidationReportPtr_t report,

                             geom::Point& pn, geom::Point& result,

                             core::ConfigurationPtr_t config,

                             pinocchio::RbPrmDevicePtr_t device) {

  hppDout(notice, "Approximate contact point for rom " << romName);

  // hppDout(notice,"Number of points in the intersection : "<<hull.size());

      // hppDout(notice,"Reference position for rom"<<romName<<" =

      // "<<reference.transpose());

      // hppDout(notice,"Reference after root transform =

      // "<<refPoint.transpose());

      hppDout(notice,

              "Approximate contact point found : " << result.transpose());

    } else {

      hppDout(notice,

              "No reference end effector position defined, use center of "

              "intersection as approximation");

    }

  } else {

    hppDout(notice, "Error in the approximation of the ocntact point");

  }

}

    const geom::Point pn, const geom::Point center, const double sizeFootX,

    const double sizeFootY, const bool rectangularContact, const double mu,

    const Eigen::Quaterniond quat, size_t& indexRom, MatrixXX& V,

    Matrix6X& IP_hat) {

  // hppDout(notice,"normal for this contact : "<<getNormal());

  hppDout(notice, "mu used in computeNodeMatrix : " << mu);

  // compute tangent vector :

  // tProj is the the direction of the head of the robot projected in plan (x,y)

  // hppDout(info,"t"<<indexRom<<"1 : "<<ti1.transpose());

  // hppDout(info,"t"<<indexRom<<"2 : "<<ti2.transpose());

  // fill V with generating ray ([ n_i + \mu t_{i1} & n_i - \mu t_{i1} & n_i +

  // \mu t_{i2} & n_i - \mu t_{i2}]

    // hppDout(notice,"shift x = "<<shiftX.transpose());

    // hppDout(notice,"shift y = "<<shiftY.transpose());

    hppDout(notice, "Center of rom collision :  ["

                        << center[0] << " , " << center[1] << " , " << center[2]

                        << "]");

      // make a rectangle around center :

      else

      else

      // fill IP_hat with position : [I_3  pi_hat] ^T

      hppDout(notice, "position of rom collision :  [" << pContact[0] << " , "

                                                       << pContact[1] << " , "

                                                       << pContact[2] << "]");

      // ssContacts<<"["<<pContact[0]<<" , "<<pContact[1]<<" ,

      // "<<pContact[2]<<"],"; hppDout(info,"p"<<(indexRom+i)<<"^T =

      // "<<pContact.transpose()); hppDout(info,"IP_hat at iter "<<indexRom<< "

      // = \n"<<IP_hat);

      // hppDout(notice,"V"<<indexRom<<" = \n"<<Vi);

      // hppDout(info,"V at iter "<<indexRom<<" : \n"<<V);

    }

  } else {

    // fill IP_hat with position : [I_3  pi_hat] ^T

    // hppDout(notice,"Center of rom collision :  ["<<center[0]<<" ,

    // "<<center[1]<<" , "<<center[2]<<"]");

    hppDout(info, "p" << indexRom << "^T = " << center.transpose());

    // hppDout(info,"IP_hat at iter "<<indexRom<< " = \n"<<IP_hat);

    // hppDout(notice,"V"<<indexRom<<" = \n"<<Vi);

    // hppDout(info,"V at iter "<<indexRom<<" : \n"<<V);

  }

                                 bool rectangularContact, double sizeFootX,

                                 double sizeFootY, double m, double mu,

                                 pinocchio::RbPrmDevicePtr_t device) {

  hppStartBenchmark(FILL_NODE_MATRICE);

  core::RbprmValidationReportPtr_t rbReport =

  // checks : (use assert ? )

    hppDout(error, "~~ Validation Report cannot be cast");

  }

    hppDout(error,

            "~~ ComputeGIWC : trunk is in collision");  // shouldn't happen

  }

    hppDout(error,

            "~~ ComputeGIWC : roms filter not respected");  // shouldn't happen

  }

  hppDout(notice, "Robot mass used to compute matrices : " << m);

  // FIX ME : position of contact is in center of the collision surface

  size_type numContactpoints =

  hppDout(notice, "number of contact points = " << numContactpoints);

  // get the 2 object in contact for each ROM :

  hppDout(info,

          "~~ Number of roms in collision : " << rbReport->ROMReports.size());

  bool pointExist;

    hppDout(info, "~~ for rom : " << it.first);

      hppDout(error,

              "Unable to compute the approximation of the contact point");

      // save infos needed for LP problem in node structure

      // FIXME : Or retry with another obstacle ???

      // compute other LP values : (constant for each nodes)

    }

                                 indexRom, V, IP_hat);

  }  // for each ROMS

  // save infos needed for LP problem in node structure

  // compute other LP values : (constant for each nodes)

  // debug output :

  /*hppDout(info,"G = \n"<<getG());

    hppDout(info,"c^T = "<<c.transpose());

    hppDout(info,"m = "<<m);

    hppDout(info,"h^T = "<<geth().transpose());

    hppDout(info,"H = \n"<<getH());

*/

  hppDout(notice, "list of all centers = " << ssContact.str() << "]");

  hppStopBenchmark(FILL_NODE_MATRICE);

  hppDisplayBenchmark(FILL_NODE_MATRICE);

}

                                         pinocchio::RbPrmDevicePtr_t device) {

  core::RbprmValidationReportPtr_t rbReport =

    core::AllCollisionsValidationReportPtr_t romReports =

      hppDout(

          warning,

          "For rom : "

              << it.first

              << " unable to cast in a AllCollisionsValidationReport, did you "

                 "correctly call "

                 "computeAllContacts(true) before generating the report ? ");

    }

      // for each rom , for each different environnement surface in collision

      // with the rom: 1) compute the projection of the reference position

      // inside the intersection of the rom and the environnement surface 2)

      // find the surface with the minimal distance between the reference and

      // the projection 3) modify the report such that this surface is at the

      // top of the list

      hppDout(notice,

              "Reference position for rom" << it.first << " = " << reference);

      // fcl::Matrix3f rot = quat.matrix();

      hppDout(notice, "Reference after root transform = " << reference);

      double distance;

      CollisionValidationReportPtr_t bestReport(

      bool successInter;

      hppDout(notice, "Number of possible surfaces for rom : "

                          << romReports->collisionReports.size());

                                                  deviceSync.d());

          hppDout(notice, "Distance found : " << distance);

          }

        }

      }

      // 3)

    }

  }  // end for all rom

}

}  // namespace core

}  // namespace hpp