//

// Copyright (c) 2014 CNRS

// Authors: Florent Lamiraux

//

// 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 <boost/tuple/tuple.hpp>

#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>

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

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

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

#include <hpp/core/edge.hh>

#include <hpp/core/kinodynamic-distance.hh>

#include <hpp/core/node.hh>

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

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

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

#include <hpp/core/path.hh>

#include <hpp/core/problem.hh>

#include <hpp/core/roadmap.hh>

#include <hpp/core/steering-method.hh>

#include <hpp/pinocchio/configuration.hh>

#include <hpp/pinocchio/device.hh>

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

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

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

#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh>

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

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

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

#include <hpp/util/debug.hh>

#include <hpp/util/timer.hh>

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

namespace hpp {

namespace rbprm {

using core::BiRRTPlanner;

using core::Configuration_t;

using core::ConfigurationPtr_t;

using core::Path;

using core::PathPtr_t;

using core::Problem;

using core::Roadmap;

using core::RoadmapPtr_t;

using core::size_type;

using pinocchio::displayConfig;

using pinocchio::value_type;

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;

    core::ProblemConstPtr_t problem, const RoadmapPtr_t& roadmap) {

}

}

    : BiRRTPlanner(problem),

      roadmap_(dynamic_pointer_cast<core::Roadmap>(

      sm_(dynamic_pointer_cast<SteeringMethodKinodynamic>(

      smParabola_(rbprm::SteeringMethodParabola::create(problem)),

      rbprmPathValidation_(dynamic_pointer_cast<RbPrmPathValidation>(

         "steering method should be a kinodynamic steering method for this "

         "solver");

      rbprmPathValidation_ &&

      "Path validation should be a RbPrmPathValidation class for this solver");

         "When using dynamic planner, the robot mass should be correctly "

         "defined.");

  hppDout(notice, "number of affordances objects : "

                      << problem->collisionObstacles().size());

               2.;

               2.;

  else

  hppDout(notice, "tryJump in dynamic planner = " << tryJump_);

  hppDout(notice, "mu define in python : " << mu_);

}

    : BiRRTPlanner(problem, roadmap),

      roadmap_(dynamic_pointer_cast<core::Roadmap>(

      sm_(dynamic_pointer_cast<SteeringMethodKinodynamic>(

      smParabola_(rbprm::SteeringMethodParabola::create(problem)),

      rbprmPathValidation_(dynamic_pointer_cast<RbPrmPathValidation>(

         "steering method should be a kinodynamic steering method for this "

         "solver");

      rbprmPathValidation_ &&

      "Path validation should be a RbPrmPathValidation class for this solver");

         "When using dynamic planner, the robot mass should be correctly "

         "defined.");

  hppDout(notice, "number of affordances objects : "

                      << problem->collisionObstacles().size());

               2.;

               2.;

  else

  hppDout(notice, "tryJump in dynamic planner = " << tryJump_);

  hppDout(notice, "mu define in python : " << mu_);

}

    core::ConfigurationPtr_t& qProj_, const core::NodePtr_t& near,

    const core::ConfigurationPtr_t& target, bool reverse) {

    } else {

    }

    } else {

    }

  }

}

    const core::ConfigurationPtr_t& from,

    const core::ConfigurationPtr_t& target, bool reverse) {

    } else {

    }

      else

    } else {

    }

  } else {

    else

  }

}

                                     core::ConfigurationPtr_t q_last,

                                     const core::ConfigurationPtr_t& target,

                                     bool reverse, core::NodePtr_t& x_jump,

                                     core::NodePtr_t& x_new,

                                     core::PathPtr_t& kinoPath,

                                     core::PathPtr_t& paraPath) {

  const size_type indexECS =

  hppDout(notice, "!! begin tryParabolaPath");

  // 1. compute a parabola between last configuration valid in contact, and

  // qrand (target)

    hppDout(notice, "!! ParaPath computed");

        0) {  // only add if the full path is valid (because we can't extract a

              // subpath of a parabola path)

      hppDout(notice, "!! parabola path valid !");

      ParabolaPathPtr_t parabolaPath =

      core::ConfigurationPtr_t q_new(

      core::ConfigurationPtr_t q_jump(

      // 2. update q_jump with the correct initial velocity needed for the

      // computed parabola

      // TODO : update q_jump with the right velocity from parabola

      }

      hppDout(notice, "q_last = " << displayConfig(*q_last));

      hppDout(notice, "q_jump = " << displayConfig(*q_jump));

      hppDout(notice, "q_target = " << displayConfig(*target));

      hppDout(notice, "q_new = " << displayConfig(*q_new));

      // 3. compute a kinodynamic path between near and q_jump

      hppStartBenchmark(EXTEND);

      hppStopBenchmark(EXTEND);

      hppDisplayBenchmark(EXTEND);

        hppDout(notice, "!! Kino path computed");

        kinoPathValid =

          hppDout(notice, "!! Kino path valid !");

            // 4. add both nodes and edges to the roadmap

            hppDout(notice, "add both nodes and edges to the roadmap");

          } else {

            hppDout(notice, "!! lenght of Kino path incorrect.");

          }

        } else {

          hppDout(notice, "!! Kino path not valid.");

        }

      } else {

        hppDout(notice, "!! Kino path doesn't exist.");

      }

    } else {

      hppDout(notice, "!! Parabola path not valid.");

    }

  } else {

    hppDout(notice, "!! parabola path doesn't exist.");

  }

}

  hppDout(info,

          "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ new Step "

          "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");

  value_type distance;

  core::NodePtr_t near, reachedNodeFromStart;

  hppStartBenchmark(SHOOT);

  hppDout(info, "Random configuration : " << displayConfig(*q_rand));

  hppStopBenchmark(SHOOT);

  hppDisplayBenchmark(SHOOT);

  // ######################## first , try to connect to start component

  // #################### //

  hppStartBenchmark(NEAREST);

  hppStopBenchmark(NEAREST);

  hppDisplayBenchmark(NEAREST);

  if (castNode)

    hppDout(notice, "Node casted correctly");

  else

    hppDout(notice, "Impossible to cast node to rbprmNode");

  hppStartBenchmark(EXTEND);

  hppStopBenchmark(EXTEND);

  hppDisplayBenchmark(EXTEND);

    pathValidFromStart =

    // Insert new path to q_near in roadmap

        hppDout(info,

                "~~~~~~~~~~~~~~~~~~~~ New node added to start component : "

                    << displayConfig(*q_new));

      } else {

      }

    }

  }

  hppDout(info,

          "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Try to connect end component "

          "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");

  // ######################## now try to connect qrand to end components (in

  // reverse )######################## //

    hppStartBenchmark(NEAREST);

    hppStopBenchmark(NEAREST);

    hppDisplayBenchmark(NEAREST);

    hppStartBenchmark(EXTEND);

    hppStopBenchmark(EXTEND);

    hppDisplayBenchmark(EXTEND);

      pathValidFromEnd =

      }

          pathValidFromEnd)  // qrand was successfully connected to both trees

      {

        // we won, a path is found

        hppDout(

            info,

            "~~~~~~~~~~~~~~~~~~~~ Start and goal component connected !!!!!! "

                << displayConfig(*q_new));

        hppDout(notice, "#### end of planning phase #### ");

          ConfigurationPtr_t q_newEnd =

          core::NodePtr_t newNode =

          hppDout(info,

                  "~~~~~~~~~~~~~~~~~~~~~~ New node added to end component : "

                      << displayConfig(*q_newEnd));

                                          // -> qnewEnd)

            hppStartBenchmark(EXTEND);

            path =

            hppStopBenchmark(EXTEND);

            hppDisplayBenchmark(EXTEND);

                hppDout(info,

                        "~~~~~~~~ both new nodes connected together !!!!!! "

                            << displayConfig(*q_new));

              }

            }

          }

        }

      }

    }

  }

}

  // randomnize the collision pair, in order to get a different surface of

  // contact each time (because only the first one in collision is considered by

  // fcl and put in the report)

                                     // collision pairs

  hppDout(notice, "Compute GIWC, call validate for configuration : "

                      << pinocchio::displayConfig(*(x->configuration())));

        report,

  }

                                 core::ValidationReportPtr_t report) {

  hppDout(notice, "## compute GIWC");

  // fil normal information in node

    hppDout(info, "~~ NODE cast correctly");

  } else {

    hppDout(error, "~~ NODE cannot be cast");

  }

  hppDout(info, "~~ q = " << displayConfig(*q));

}  // computeGIWC

// re implement virtual method, same as base class but without the symetric edge

// (goal -> start)

  // call steering method here to build a direct conexion

  core::NodePtr_t x_jump;

    hppStartBenchmark(EXTEND);

    hppStopBenchmark(EXTEND);

    hppDisplayBenchmark(EXTEND);

    hppDout(notice, "try direction path, after extendInternal");

    hppDout(notice, "try direction path, after continue");

    } else {

    }

      // roadmap ()->addEdge (initNode, itn, projPath);  // (TODO a

      // supprimer)display the path no matter if it's successful or not

      bool pathValid =

      // roadmap ()->addEdge (initNode, itn, validPath);  // (TODO a

      // supprimer)display the path no matter if it's successful or not

                                          // successfull, add the edge

          // check if the validation fail because of the ROM or because of the

          // trunk

          RbPrmPathValidationPtr_t rbprmPathValidation =

          bool successPathOperator;

              successPathOperator) {  // if it failed because of the ROM, we can

                                      // try a parabola

            core::ConfigurationPtr_t q_jump(

            core::NodePtr_t x_goal;

            bool parabolaSuccess =

                                kinoPath, paraPath);

            hppDout(notice, "parabola success = " << parabolaSuccess);

              hppDout(notice, "x_goal conf = "

                                  << displayConfig(*(x_goal->configuration())));

            }

          } else {

            hppDout(notice, "trunk in collision");

          }

                                               // reached to the roadmap

          core::ConfigurationPtr_t q_new(

          core::NodePtr_t x_new =

        }

      }

    }

  }

    const core::PathVectorPtr_t& path) {

  /*std::cout<<"total_path_computed = "<<sm_->totalTimeComputed_<<std::endl;

  std::cout<<"total_path_validated = "<<sm_->totalTimeValidated_<<std::endl;

  std::cout<<"percentage validated path

  ="<<((sm_->totalTimeValidated_)/(sm_->totalTimeComputed_))*100.<<std::endl;

  std::cout<<"rejected_paths = "<<sm_->rejectedPath_<<std::endl;

  */

  /*

  std::ofstream myfile;

  myfile.open ("/local/dev_hpp/benchs/benchHyq_darpa.txt", std::ios::out |

  std::ios::app ); myfile<<"total_path_computed =

  "<<sm_->totalTimeComputed_<<std::endl; myfile<<"total_path_validated =

  "<<sm_->totalTimeValidated_<<std::endl; myfile<<"percentage_validated_path

  ="<<(double)sm_->totalTimeValidated_/sm_->totalTimeValidated_<<std::endl;

  myfile<<"total_direction_computed = "<<sm_->dirTotal_<<std::endl;

  myfile<<"total_direction_valid = "<<sm_->dirValid_<<std::endl;

  myfile<<"percentage_valide_direction

  ="<<(double)(((double)sm_->dirValid_)/((double)sm_->dirTotal_))<<std::endl;

  myfile<<"rejected_paths = "<<sm_->rejectedPath_<<std::endl;

  myfile<<"num_nodes = "<<roadmap()->nodes().size()<<std::endl;

  myfile.close();

  */

}

    core::Parameter::FLOAT, "DynamicPlanner/sizeFootX",

    "The lenght of the feet along X axis (assuming rectangular feet).",

    core::Parameter::FLOAT, "DynamicPlanner/sizeFootY",

    "The lenght of the feet along Y axis (assuming rectangular feet).",

    core::Parameter::FLOAT, "DynamicPlanner/friction",

    "Value of the friction coefficient between the feet of the robot and the "

    "ground.",

    core::Parameter::BOOL, "DynamicPlanner/tryJump",

    "If True, when a trajectory is invalid because all the rom leave "

    "the contact, a ballistic motion is tried to connect both states",

}  // namespace rbprm

}  // namespace hpp