#include <fstream>

#include <hpp/bezier-com-traj/common_solve_methods.hh>

#include <hpp/bezier-com-traj/solve.hh>

#include <hpp/rbprm/contact_generation/reachability.hh>

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

#include <hpp/rbprm/stability/stability.hh>

#include <hpp/util/timer.hh>

#include <iostream>

#ifndef QHULL

#define QHULL 0

#endif

#ifndef STAT_TIMINGS

#define STAT_TIMINGS 0

#endif

#ifndef FULL_TIME_SAMPLING

#define FULL_TIME_SAMPLING 0

#endif

namespace hpp {

namespace rbprm {

namespace reachability {

using centroidal_dynamics::Matrix3;

using centroidal_dynamics::Vector3;

// helper method used to print vectors of string

                         const std::vector<std::string>& vect) {

  }

}

                    const std::string& fileName, bool clipZ = false) {

  using std::endl;

  hppDout(notice, "print to file : " << path);

  }

  }

}

// Method to print (in hppDout) the inequalities express as halfspace, in a

// format readable by qHull use qHalf FP | qConvex Ft      to use them

                fcl::Vec3f intPoint = fcl::Vec3f::Zero(),

                const std::string& fileName = std::string(),

                bool clipZ = false) {

  using std::endl;

  }

  hppDout(notice, ss.str());

}

    const std::pair<MatrixXX, VectorX>& Ab,

    const std::pair<MatrixXX, VectorX>& Cd) {

}

/**

 * @brief computeDistanceCost cost that minimize || x - c ||

 * @param c0 target point (the point that we want to be the closest from)

 * @return the matrices H and g that express the cost

 */

}

                       const fcl::Vec3f& c, fcl::Vec3f& c_out) {

  hppDout(notice, "Call solveur solveIntersection");

  hppDout(notice, "init = " << c);

  bezier_com_traj::ResultData res =

  hppDout(notice, "success Solveur solveIntersection = " << res.success_);

  hppDout(notice,

          "x = [" << c_out[0] << "," << c_out[1] << "," << c_out[2] << "]");

}

    const centroidal_dynamics::Equilibrium& contactPhase,

    const fcl::Vec3f& int_point, const fcl::Vec3f& acc) {

  // gravity vector

  hppDout(notice, "Compute stability constraints");

  hppDout(notice, "With acceleration = " << acc);

  // compute GIWC

  centroidal_dynamics::LP_status status =

    hppDout(warning, "getPolytopeInequalities failed, lp status : " + status);

  }

  // hppDout(notice,"Hrow : \n"<<Hrow);

  hppDout(notice, "Dim H rows : " << dimH << " ; col : " << H.cols());

  // hppDout(notice,"H : \n"<<H);

  // constraints : mH[:,3:6] g^  x <= h + mH[:,0:3]g

  // A = mH g^

  // b = h + mHg

  /* hppDout(notice,"Stability constraints matrices : ");

   hppDout(notice,"Interior point : \n"<<int_point);

   hppDout(notice,"A = \n"<<A);

   hppDout(notice,"b = \n"<<b);*/

#if QHULL

  hppDout(notice, "Stability constraints qHull : ");

  printQHull(std::make_pair(A, b), int_point);

#else

  (void)int_point;  // silent the unused parameter warning in case QHULL 0

#endif

}

    const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success) {

  hppStartBenchmark(REACHABLE_CALL_CENTROIDAL);

  centroidal_dynamics::Equilibrium contactCone(

      centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;

  try {

        fullbody, state, contactCone, alg, fullbody->getFriction(), 0.05,

        0.05);  // 0.01 : 'safe' support zone, under the flexibility

    hppDout(notice, "Error in setupLibrary : " << e.what());

  }

  hppStopBenchmark(REACHABLE_CALL_CENTROIDAL);

  hppDisplayBenchmark(REACHABLE_CALL_CENTROIDAL);

}

    const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success,

    const fcl::Vec3f& acc) {

  hppDout(notice, "contact order : ");

  hppDout(notice, "  " << state.contactOrder_.front());

  centroidal_dynamics::Equilibrium cone(

  } else {

  }

}

    const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success,

    const fcl::Vec3f& acc) {

  std::pair<MatrixXX, VectorX> Ab =

    return stackConstraints(

  else

}

                               State& previous, State& intermediate,

                               State& next) {

  // TODO

  hppDout(notice, "isReachableIntermadiate :");

  hppDout(notice, "Contact variations : " << contactsNames);

  hppDout(notice, "isReachableIntermediate : ");

  hppDout(notice, "resBreak status    : " << resBreak.status);

  hppDout(notice, "resCreation status : " << resCreate.status);

#if QHULL

  hppDout(notice, "constraint for contact break : ");

  printQHull(resBreak.constraints_, resBreak.x, "constraints_break.txt");

  hppDout(notice, "constraint for contact creation : ");

  printQHull(resCreate.constraints_, resCreate.x, "constraints_create.txt");

#endif

    hppDout(notice, "reachable intermediate success, x = " << res.x);

// only for test, it take time to compute :

#if QHULL

    hppDout(notice, "constraint for intersection : ");

    printQHull(stackConstraints(resBreak.constraints_, resCreate.constraints_),

               res.x, "constraints.txt");

#endif

  }

}

                   State& next, const fcl::Vec3f& acc,

                   bool useIntermediateState) {

  hppStartBenchmark(IS_REACHABLE);

         "Reachability : previous state have less than 1 contact.");

         "Reachability : next state have less than 1 contact.");

  hppDout(notice, "IsReachable called : for configuration :  r(["

                      << pinocchio::displayConfig(previous.configuration_)

                      << "])");

  hppDout(notice, "contact for previous : " << previous.nbContacts);

  hppDout(notice, "contact for next     : " << next.nbContacts);

  hppDout(notice, "Contacts break : " << contactsBreak);

  hppDout(notice, "contacts creation : " << contactsCreation);

#if QHULL == 0

    hppDout(notice, "No contact variation, abort.");

  }

#endif

    hppDout(notice, "Too many contact variation, abort.");

  }

        1) {  // there is 1 contact repositionning between previous and next

      // we need to create the intermediate state, and call is reachable for the

      // 3 states.

      hppDout(notice,

              "Contact repositionning between the 2 states, create "

              "intermediate state");

        hppDout(notice, "call isReachableIntermediate.");

      }

    } else {

      hppDout(notice,

              "Contact break and creation are different. You need to call "

              "isReachable with 2 adjacent states");

    }

  }

  bool success;

  bool successCone;

      hppDout(warning,

              "Unable to compute computeStabilityConstraintsForState.");

    }

      hppDout(warning,

              "Unable to compute computeStabilityConstraintsForState.");

    }

  }

  // there is only one contact creation OR (exclusive) break between the two

  // states test C_p \inter C_n (ie : A_p \inter K_p \inter A_n \inter K_n),

  // with simplifications du to relations between the constraints :

           0) {  // next have one less contact than previous

    hppDout(notice, "Contact break between previous and next state");

    // A_n \inside A_p, thus  A_p is redunbdant

    // K_p \inside K_n, thus  K_n is redunbdant

    // So, we only need to test A_n \inter K_p

    // TODO : K_p can be given as parameter to avoid re-computation

    // std::pair<MatrixXX,VectorX> A_n =

    // computeStabilityConstraintsForState(fullbody,next);

    // std::pair<MatrixXX,VectorX> K_p =

    // computeKinematicsConstraintsForState(fullbody,previous); Ab =

    // stackConstraints(A_n,K_p); develloped computation, needed to display the

    // differents constraints :

    hppStartBenchmark(REACHABLE_STABILITY);

      hppDout(warning,

              "Unable to compute computeStabilityConstraintsForState.");

    }

    hppStopBenchmark(REACHABLE_STABILITY);

    hppDisplayBenchmark(REACHABLE_STABILITY);

    hppStartBenchmark(REACHABLE_KINEMATIC);

    hppStopBenchmark(REACHABLE_KINEMATIC);

    hppDisplayBenchmark(REACHABLE_KINEMATIC);

    hppStartBenchmark(REACHABLE_STACK);

    hppStopBenchmark(REACHABLE_STACK);

    hppDisplayBenchmark(REACHABLE_STACK);

  } else {  // next have one more contact than previous

    hppDout(notice, "Contact creation between previous and next");

    // A_p \inside A_n, thus A_n is redunbdant

    // K_n \inside K_p ; and K_n = K_p \inter K_n^m (where K_n^m is the

    // kinematic constraint for the moving limb at state n) we use K_n^m,

    // because C_p can be given as parameter to avoid re-computation So, we only

    // need to test C_n \inter K_p_m std::pair<MatrixXX,VectorX> C_p   =

    // computeConstraintsForState(fullbody,previous);

    // std::pair<MatrixXX,VectorX> K_n_m =

    // computeKinematicsConstraintsForLimb(fullbody,previous,contactsCreation[0]);

    // // kinematic constraint only for the moving contact for state previous Ab

    // = stackConstraints(C_p,K_n_m);

    hppStartBenchmark(REACHABLE_STABILITY);

      hppDout(warning,

              "Unable to compute computeStabilityConstraintsForState.");

    }

    hppStopBenchmark(REACHABLE_STABILITY);

    hppDisplayBenchmark(REACHABLE_STABILITY);

    // K_p = computeKinematicsConstraintsForState(fullbody,previous);

    hppStartBenchmark(REACHABLE_KINEMATIC);

    hppStopBenchmark(REACHABLE_KINEMATIC);

    hppDisplayBenchmark(REACHABLE_KINEMATIC);

    hppStartBenchmark(REACHABLE_STACK);

    hppStopBenchmark(REACHABLE_STACK);

    hppDisplayBenchmark(REACHABLE_STACK);

  }

  // compute COM positions :

      pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM);

  // compute the position in the middle of the most constrained support polygon

  // (used for the cost function):

             0) {  // next have the smaller support polygone

  } else {  // previous have the smaller support polygon

  }

    fcl::Vec3f position =

  }

  hppStartBenchmark(QP_REACHABLE);

  hppStopBenchmark(QP_REACHABLE);

  hppDisplayBenchmark(QP_REACHABLE);

    hppDout(notice, "REACHABLE");

  } else {

    hppDout(notice, "UNREACHABLE");

  }

  hppStopBenchmark(IS_REACHABLE);

  hppDisplayBenchmark(IS_REACHABLE);

// compute interior point to display with qHull (only required for display and

// debug)

#if QHULL

  Vector3 int_pt_kin;

  fcl::Vec3f int_pt_stab;

  if (res.success()) {

    int_pt_stab = x;

    int_pt_kin = x;

  } else {

    if (contactsBreak.size() > 0) {

      int_pt_kin = com_previous;

      intersectionExist(A_n, (com_previous + com_next) / 2., int_pt_stab);

    } else {

      int_pt_kin = com_next;

      intersectionExist(A_p, (com_previous + com_next) / 2., int_pt_stab);

    }

  }

  if (contactsBreak.size() > 0) {

    hppDout(notice, "Stability constraint for state i+1 :");

    printQHull(A_n, int_pt_stab, "stability.txt", true);

    hppDout(notice, "Kinematics constraint for state i :");

    printQHull(K_p, int_pt_kin, "kinematics.txt");

  } else {

    hppDout(notice, "Stability constraint for state i :");

    printQHull(A_p, int_pt_stab, "stability.txt", true);

    hppDout(notice, "Kinematics constraint for state i+1 :");

    printQHull(K_n, int_pt_kin, "kinematics.txt");

  }

  hppDout(notice, "Intersection of constraints :");

  printQHull(Ab, int_pt_stab, "constraints.txt");

  if (contactsCreation.size() <= 0 && contactsBreak.size() <= 0) {

    hppDout(notice, "No contact variation, abort.");

    return Result(NO_CONTACT_VARIATION);

  }

#endif

}

                     bool quasiStaticSuccess) {

  using std::endl;

}

                          State& next, bool tryQuasiStatic,

                          std::vector<double> timings, int numPointsPerPhases) {

  hppDout(notice, "IsReachableDynamic called : ");

  hppDout(notice, "Between configuration : "

                      << pinocchio::displayConfig(previous.configuration_));

  hppDout(notice, "and     configuration : "

                      << pinocchio::displayConfig(next.configuration_));

      fullbody->device_->extraConfigSpace().dimension() >= 6 &&

      "You need to set at least 6 extraDOF to use the reachability methods.");

    hppDout(notice, "Same root position, unable to compute");

  }

  hppDout(notice, "Contacts break : " << contactsBreak);

  hppDout(notice, "contacts creation : " << contactsCreation);

    hppDout(notice, "No contact variation, abort.");

  }

    hppDout(notice, "More than 2 contact change");

  }

    hppDout(notice,

            "Only contact creation. State Next have more contact than state "

            "Previous");

      hppDout(

          notice,

          "Previous is in single support. Unable to compute");  // FIXME : maybe

                                                                // compute in

                                                                // quasiStatic

    }

  }

    hppDout(

        notice,

        "Only contact break. State Next have less contact than state Previous");

      hppDout(notice,

              "Next is in single support. Unable to compute");  // FIXME : maybe

                                                                // compute in

                                                                // quasiStatic

    }

  }

  // build intermediate state :

  }

#if STAT_TIMINGS

  // outputs timings results in file :

  std::ofstream file;

  std::string path("/local/fernbac/bench_iros18/bench/timing_hrp2_darpa.log");

  hppDout(notice, "print to file : " << path);

  file.open(path.c_str(), std::ios_base::app);

  file << "# new pair of states" << std::endl;

#endif

  // compute COM positions :

      pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM);

    hppDout(notice, "Try quasi-static reachability : ");

    Result quasiStaticResult =

      hppDout(notice, "REACHABLE in quasi-static");

      // build a Bezier curve of order 2 :

#if !STAT_TIMINGS

#endif

    } else {

      hppDout(notice, "UNREACHABLE in quasi-static");

      // return Result(REACHABLE); // testing

    }

  }

  hppStartBenchmark(IS_REACHABLE_DYNAMIC);

  hppStartBenchmark(COMPUTE_DOUBLE_DESCRIPTION);

  // build ProblemData from states object and call solveOneStep()

  // build contactPhases :

  std::pair<MatrixX3, VectorX> Ab =

  centroidal_dynamics::Equilibrium conePrevious =

  }

  centroidal_dynamics::Equilibrium coneMid =

  centroidal_dynamics::Equilibrium coneNext =

  hppStopBenchmark(COMPUTE_DOUBLE_DESCRIPTION);

  hppDisplayBenchmark(COMPUTE_DOUBLE_DESCRIPTION);

  }

    hppDout(notice, "Contact break AND creation, create intermediate state");

  }

      bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL |

      bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC |

  // set init/goal values :

  pData.ddc0_ =

  // pData.dc0_ = fcl::Vec3f::Zero();

  // pData.dc1_ = fcl::Vec3f::Zero();

  hppDout(notice, "Build pData : ");

  hppDout(notice, "c0 = " << pData.c0_.transpose());

  hppDout(notice, "c1 = " << pData.c1_.transpose());

  hppDout(notice, "dc0 = " << pData.dc0_.transpose());

  hppDout(notice, "dc1 = " << pData.dc1_.transpose());

  hppDout(notice, "ddc0 = " << pData.ddc0_.transpose());

  hppDout(notice, "ddc1 = " << pData.ddc1_.transpose());

         "Error in computation of contact constraints, must be only 2 or 3 "

         "phases.");

  // compute initial guess :

  // average of all contact point for the state with the less contacts, z =

  // average of the CoM heigh between previous and next

  else

#if !FULL_TIME_SAMPLING

#endif

  hppDout(notice, " timings provided size :  " << timings.size());

    // build timing vector

    // TODO : retrieve timing found by planning ?? how ?? (pass it as argument

    // or store it inside the states ?)

    hppDout(notice, "Contact break and creation. Use hardcoded timing matrice");

#if FULL_TIME_SAMPLING

    const double time_increment = 0.05;

    const double min_SS = 0.6;

    const double max_SS = 1.6;

    const double min_DS = 0.3;

    const double max_DS = 1.5;

    if (contactsBreak.size() == 1 && contactsCreation.size() == 1) {

      current_timings = VectorX(3);

      // current_timings<<0.6,0.4,0.6; //hrp2

      // total_time = 1.6;

      current_timings << min_DS, min_SS, min_DS;  // hrp2

    } else {

      hppDout(notice, "Only two phases.");

      current_timings = VectorX(2);

      current_timings << 1., 1.;

    }

#else

    timings_matrix =

                          // total time and the discretization step

    current_timings =

#endif

    }

  } else {

    hppDout(notice, "Timing vector is provided");

    }

  }

  // pData.representation_ = bezier_com_traj::FORCE;

  // loop over all possible timings :

#if STAT_TIMINGS

  while (!no_timings_left) {

#else

#endif

    // call solveur :

    hppDout(notice, "Try with timings : " << current_timings.transpose());

    hppDout(notice, "Call solveOneStep");

    hppStartBenchmark(SOLVE_TRANSITION_ONE_STEP);

      hppDout(notice, "Call computeCOMTraj discretized");

    } else {

      hppDout(notice, "Call computeCOMTraj continuous");

    }

    hppStopBenchmark(SOLVE_TRANSITION_ONE_STEP);

    hppDisplayBenchmark(SOLVE_TRANSITION_ONE_STEP);

    // wrap the result :

      hppDout(notice, "REACHABLE");

      hppDout(notice,

              "Resulting bezier curve have timing : " << bezierCurve->max());

        hppDout(notice, "with waypoint : " << (*wpit).transpose());

      }

      // replace extra dof in next.configuration to fit the final velocity and

      // acceleration found :

      hppDout(notice, "new final configuration : "

                          << pinocchio::displayConfig(next.configuration_));

      hppDout(notice, "position of the waypoint : " << resBezier.x.transpose());

      hppDout(notice, "With timings : " << current_timings.transpose());

      hppDout(notice, "total time = " << total_time);

      hppDout(notice, "new final velocity : " << resBezier.dc1_.transpose());

                             total_time)));

    } else {

      hppDout(notice, "UNREACHABLE");

    }

#if STAT_TIMINGS

    // print result in file :

    printTimingFile(file, current_timings, res.success(), quasiStaticSucces);

#else

    (void)quasiStaticSucces;  // silent warning

#endif

    // build the new timing vector :

#if FULL_TIME_SAMPLING

      current_timings[0] += time_increment;

      if (current_timings[0] > max_DS) {

        current_timings[0] = min_DS;

        current_timings[1] += time_increment;

        if (current_timings[1] > max_SS) {

          if (current_timings.size() == 3) {

            current_timings[1] = min_SS;

            current_timings[2] += time_increment;

            if (current_timings[2] > max_DS) {

              no_timings_left = true;

            }

          } else {

            no_timings_left = true;

          }

        }

      }

#else

      current_timings =

#endif

      }

    } else {

    }

  }

  hppStopBenchmark(IS_REACHABLE_DYNAMIC);

  hppDisplayBenchmark(IS_REACHABLE_DYNAMIC);

#if STAT_TIMINGS

  file.close();

  return Result(REACHABLE);

#endif

    hppDout(notice, "No valid timings found, always UNREACHABLE");

  }

  if (success && tryQuasiStatic) {

    hppDout(notice, "ONLY REACHABLE IN DYNAMIC !!!");

  }

  hppDout(notice, "Between configuration : r(["

                      << pinocchio::displayConfig(previous.configuration_)

                      << "])");

  hppDout(notice, "and     configuration : r(["

                      << pinocchio::displayConfig(next.configuration_) << "])");

}

}  // namespace reachability

}  // namespace rbprm

}  // namespace hpp