#include "QPReactiveRecoveryPlan.hpp"
#include <unsupported/Eigen/Polynomials>
#include <Eigen/StdVector>
#include "drake/util/drakeGeometryUtil.h"
#include "drake/solvers/qpSpline/splineGeneration.h"
#include "drake/util/lcmUtil.h"
#include "lcmtypes/drc/reactive_recovery_debug_t.hpp"
extern "C" {
  #include "iris/solver.h"
}

#define CVXGEN_MAX_ROWS 3
#define CVXGEN_MAX_PTS 8

#define DEBUG

using namespace Eigen;

Vars vars;
Params params;
Workspace work;
Settings settings;

VectorXd QPReactiveRecoveryPlan::closestPointInConvexHull(const Ref<const VectorXd> &x, const Ref<const MatrixXd> &V) {

  int dim = x.size();

  Matrix<double, CVXGEN_MAX_ROWS, 1> x_resized;
  Matrix<double, CVXGEN_MAX_ROWS, CVXGEN_MAX_PTS> V_resized;

  if (dim > CVXGEN_MAX_ROWS) {
    fprintf(stderr, "x can not be more than %d elements\n", CVXGEN_MAX_ROWS);
    exit(1);
  }
  if (V.rows() != dim) {
    fprintf(stderr, "V must have same number of rows as x\n");
    exit(1);
  }
  if (V.cols() > CVXGEN_MAX_PTS) {
    fprintf(stderr, "V can not be larger than %d x %d\n", CVXGEN_MAX_ROWS, CVXGEN_MAX_PTS);
    exit(1);
  }

  x_resized.head(dim) = x;
  V_resized.block(0, 0, dim, V.cols()) = V;

  if (dim < CVXGEN_MAX_ROWS) {
    x_resized.tail(CVXGEN_MAX_ROWS - dim) = VectorXd::Zero(CVXGEN_MAX_ROWS-dim);
    V_resized.block(dim, 0, CVXGEN_MAX_ROWS - dim, V_resized.cols()) = MatrixXd::Zero(CVXGEN_MAX_ROWS-dim, V_resized.cols());
  }

  for (int i=V.cols(); i < CVXGEN_MAX_PTS; i++) {
    for (int j=0; j < CVXGEN_MAX_ROWS; j++) {
      V_resized(j,i) = V_resized(j,i-1);
    }
  }

  for (int j=0; j < CVXGEN_MAX_PTS; j++) {
    V_resized.col(j) = V_resized.col(j) - x_resized;
  }

  set_defaults();
  setup_indexing();
  settings.verbose = 0;

  double *src = V_resized.data();
  double *dest = params.Y;
  for (int i=0; i < CVXGEN_MAX_ROWS * CVXGEN_MAX_PTS; i++) {
    *dest++ = *src++;
  }

  solve();

  Map<VectorXd>y(vars.v, CVXGEN_MAX_ROWS);
  Map<VectorXd>w(vars.w, CVXGEN_MAX_PTS);

  y.head(dim) = y.head(dim) + x.head(dim);
  return y.head(dim);
}

Isometry3d QPReactiveRecoveryPlan::closestPoseInConvexHull(const Isometry3d &pose, const Ref<const MatrixXd> &V) {
  if (V.rows() < 2 || V.rows() > 3) {
    fprintf(stderr, "Vertices should have dimension 2 or 3\n");
    exit(1);
  }
  const int dim = V.rows();
  Isometry3d new_pose = pose;
  new_pose.translation().head(dim) = QPReactiveRecoveryPlan::closestPointInConvexHull(pose.translation().head(dim), V);
  return new_pose;
}

Polynomial<double> QPReactiveRecoveryPlan::bangBangPolynomial(double x0, double xd0, double u) {
  VectorXd coefs(3);
  coefs << x0 - 0.25*xd0*xd0/u,
           0.5*xd0,
           0.25*u;
  Polynomial<double> p(coefs);
  return p;
}

std::vector<double> realRoots(Polynomial<double> p) {
  VectorXd coefs = p.getCoefficients();
  double order = p.getDegree();
  std::vector<double> roots;
  if (order == 1) {
    // c0 + c1*t = 0;
    // t = -c0/c1
    roots.push_back(-coefs(0) / coefs(1));
  } else if (order == 2) {
    // c0 + c1*t + c2*t^2 = 0;
    // t = (-c1 +- sqrt(c1^2 - 4*c2*c0)) / (2*c2)
    double discriminant = pow(coefs(1), 2) - 4*coefs(2)*coefs(0);
    if (discriminant >= 0) {
      roots.push_back((-coefs(1) + sqrt(discriminant)) / (2*coefs(2)));
      roots.push_back((-coefs(1) - sqrt(discriminant)) / (2*coefs(2)));
    }
  } else {
    PolynomialSolver<double, Dynamic> poly_solver(coefs);
    poly_solver.realRoots(roots);
  }
  return roots;
}

std::vector<double> QPReactiveRecoveryPlan::expIntercept(const ExponentialForm &expform, double l0, double ld0, double u, int degree) {
  // Find the t >= 0 solutions to a*e^(b*t) + c == l0 + 1/2*ld0*t + 1/4*u*t^2 - 1/4*ld0^2/u
  // using a taylor expansion up to power [degree]

  Polynomial<double> p_taylor = expform.taylorExpand(degree);
  Polynomial<double> p_bang = QPReactiveRecoveryPlan::bangBangPolynomial(l0, ld0, u);
  Polynomial<double> p_int = p_taylor - p_bang;

  std::vector<double> roots = realRoots(p_int);
  std::vector<double> nonneg_roots;

  for (std::vector<double>::iterator it = roots.begin(); it != roots.end(); ++it) {
    if (*it > 0) {
      nonneg_roots.push_back(*it);
    }
  }

  return nonneg_roots;
}

std::vector<BangBangIntercept> QPReactiveRecoveryPlan::bangBangIntercept(double x0, double xd0, double xf, double u_max) {
  std::vector<BangBangIntercept> intercepts;

  double us[2] = {u_max, -u_max};
  for (int i=0; i<2; i++) {
    double u = us[i];
    Polynomial<double> p = QPReactiveRecoveryPlan::bangBangPolynomial(x0 - xf, xd0, u);
    std::vector<double> roots = realRoots(p);
    for (std::vector<double>::iterator it = roots.begin(); it != roots.end(); ++it) {
      double t = *it;
      if (t >= std::abs(xd0 / u)) {
        BangBangIntercept inter;
        inter.tf = t;
        inter.tswitch = 0.5 * (t - xd0 / u);
        inter.u = u;
        intercepts.push_back(inter);
        break;
      }
    }
  }
  return intercepts;
}

double QPReactiveRecoveryPlan::icpError(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices) {
  if (foot_states.size() != 2) {
    throw std::runtime_error("isICPCaptured only supports 2 feet");
  }
  Matrix<double, 3, 8> all_vertices_in_world;

  int foot_count = 0;
  for (std::map<FootID, FootState>::const_iterator state = foot_states.begin(); state != foot_states.end(); ++state) {
    if (state->second.contact ||
        (state->second.pose.translation()(2) - state->second.terrain_height < this->capture_max_flyfoot_height)) {
      auto vert_it = foot_vertices.find(state->first);
      if (vert_it == foot_vertices.end()) {
        std::cout << footIDToName[state->first] << std::endl;
        throw std::runtime_error("Cannot find foot name in foot_vertices");
      }

      Matrix<double, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT> foot_vertices_in_world = state->second.pose * (this->capture_shrink_factor * vert_it->second);
      all_vertices_in_world.block(0, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT*foot_count, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT) = foot_vertices_in_world.block(0,0,3,QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT);
      ++foot_count;
    }
  }

  Matrix<double, 2, Dynamic> active_vertices_in_world = all_vertices_in_world.block(0, 0, 2, foot_count * QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT);

  VectorXd r_ic_near = QPReactiveRecoveryPlan::closestPointInConvexHull(r_ic, active_vertices_in_world);
  return (r_ic - r_ic_near).norm();
}

bool QPReactiveRecoveryPlan::isICPCaptured(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices) {
  if (foot_states.size() != 2) {
    throw std::runtime_error("isICPCaptured only supports 2 feet");
  }
  Matrix<double, 3, 8> all_vertices_in_world;

  int foot_count = 0;
  for (std::map<FootID, FootState>::const_iterator state = foot_states.begin(); state != foot_states.end(); ++state) {
    if (state->second.contact ||
        (state->second.pose.translation()(2) - state->second.terrain_height < this->capture_max_flyfoot_height)) {
      auto vert_it = foot_vertices.find(state->first);
      if (vert_it == foot_vertices.end()) {
        std::cout << footIDToName[state->first] << std::endl;
        throw std::runtime_error("Cannot find foot name in foot_vertices");
      }

      Matrix<double, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT> foot_vertices_in_world = state->second.pose * (this->capture_shrink_factor * vert_it->second);
      all_vertices_in_world.block(0, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT*foot_count, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT) = foot_vertices_in_world.block(0,0,3,QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT);
      ++foot_count;
    } else {
      return false;
    }
  }

  Matrix<double, 2, Dynamic> active_vertices_in_world = all_vertices_in_world.block(0, 0, 2, foot_count * QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT);

  VectorXd r_ic_near = QPReactiveRecoveryPlan::closestPointInConvexHull(r_ic, active_vertices_in_world);
  return (r_ic - r_ic_near).norm() < 1e-2; // threshold set by the accuracy of the cvxgen qp solver

}

ExponentialForm QPReactiveRecoveryPlan::icpTrajectory(double x_ic, double x_cop, double omega) {
  ExponentialForm icptraj((x_ic - x_cop), omega, x_cop);
  return icptraj;
}

bool tswitchComp(const BangBangIntercept int0, const BangBangIntercept int1) {
  return int0.tswitch < int1.tswitch;
}

bool errorCompare(const InterceptPlan plan0, const InterceptPlan plan1) {
  return plan0.error < plan1.error;
}

Isometry3d QPReactiveRecoveryPlan::getTWorldToLocal(const Isometry3d &icp, const Isometry3d &cop) {
  Isometry3d T_world_to_local = Isometry3d::Identity();
  T_world_to_local.rotate(AngleAxis<double>(-std::atan2(icp.translation().y() - cop.translation().y(), icp.translation().x() - cop.translation().x()), Vector3d(0, 0, 1)));
  T_world_to_local.translate(-cop.translation());
  return T_world_to_local;
}

double QPReactiveRecoveryPlan::getMinTimeToXprimeAxis(const FootState foot_state, const BipedDescription &biped, Isometry3d &T_world_to_local) {
  std::vector<BangBangIntercept> xprime_axis_intercepts =
    QPReactiveRecoveryPlan::bangBangIntercept((T_world_to_local * foot_state.pose).translation().y(),
                                              (T_world_to_local.linear() * foot_state.velocity.head(3))(1),
                                              0,
                                              biped.u_max);

  VectorXd times_to_xprime_axis(xprime_axis_intercepts.size());
  for (int i=0; i < xprime_axis_intercepts.size(); i++) {
    times_to_xprime_axis(i) = xprime_axis_intercepts[i].tf;
  }
  return times_to_xprime_axis.minCoeff();
}

double interceptPlanError(const InterceptPlan &plan, const std::map<FootID, FootState> &foot_states, const BipedDescription &biped) {
  Matrix<double, 3, 2*QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT> foot_vertices_in_world;
  foot_vertices_in_world.block(0, 0, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT) = plan.pose_next * biped.foot_vertices.find(plan.swing_foot)->second;
  foot_vertices_in_world.block(0, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT) = foot_states.find(plan.stance_foot)->second.pose * biped.foot_vertices.find(plan.stance_foot)->second;

  Vector2d icp_projected_onto_support_polygon = QPReactiveRecoveryPlan::closestPointInConvexHull(plan.icp_next.translation().head(2),
                                                                                foot_vertices_in_world.topRows(2));
  return (icp_projected_onto_support_polygon - plan.icp_next.translation().head(2)).norm();

  // return (plan.pose_next.translation().head(2) - plan.icp_plus_offset_next.translation().head(2)).norm();
}

Isometry3d snapToTerrain(const Isometry3d &pose, const double terrain_height, const Ref<const Vector3d> &terrain_normal) {
  Isometry3d pose_snapped = Isometry3d(Translation<double, 3>(Vector3d(pose.translation().x(), pose.translation().y(), terrain_height)));
  pose_snapped.linear() = pose.linear();

  Vector3d axis = (pose.rotation() * Vector3d(0, 0, 1)).cross(terrain_normal);
  double sin_theta = axis.norm();
  if (sin_theta > 1e-14) {
    pose_snapped.rotate(AngleAxis<double>(std::asin(sin_theta), axis));
  }
  return pose_snapped;
}

std::vector<InterceptPlan> QPReactiveRecoveryPlan::getInterceptsWithCoP(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp, const Isometry3d &cop) {

  Isometry3d T_world_to_local = QPReactiveRecoveryPlan::getTWorldToLocal(icp, cop);
  FootID stance_foot = otherFoot[swing_foot];

  // std::cerr << "reach verts in stance: " << this->biped.reachable_vertices.find(swing_foot)->second << std::endl;
  Matrix<double, 3, 4> reachable_vertices_in_world = foot_states.find(stance_foot)->second.pose * this->biped.reachable_vertices.find(swing_foot)->second;
  // std::cerr << "stance pose: " << foot_states.find(stance_foot)->second.pose.translation() << std::endl;
  // std::cerr << "reach verts in world: " << reachable_vertices_in_world << std::endl;
  // std::cerr << "foot name: " << footIDToName[swing_foot] << std::endl;

  double t_min_to_xprime = QPReactiveRecoveryPlan::getMinTimeToXprimeAxis(foot_states.find(swing_foot)->second, this->biped, T_world_to_local);
  t_min_to_xprime = std::max(t_min_to_xprime, this->min_step_duration);

  double x0 = (T_world_to_local * foot_states.find(swing_foot)->second.pose).translation().x();
  double xd0 = (T_world_to_local.linear() * foot_states.find(swing_foot)->second.velocity.head(3))(0);

  std::vector<InterceptPlan> intercept_plans;

  double x_ic = (T_world_to_local * icp).translation()(0);
  double x_cop = 0; // by the definition of our local frame

  ExponentialForm icp_traj_in_local = QPReactiveRecoveryPlan::icpTrajectory(x_ic, x_cop, this->biped.omega);
  double x_ic_int = icp_traj_in_local.value(t_min_to_xprime) + this->desired_icp_offset;
  // Don't narrow our stance to intercept if possible
  // double x_ic_target = std::max(x_ic_int, x0);
  double x_ic_target = x_ic_int;

  Polynomial<double> x_foot_poly_plus = QPReactiveRecoveryPlan::bangBangPolynomial(x0, xd0, this->biped.u_max);
  Polynomial<double> x_foot_poly_minus = QPReactiveRecoveryPlan::bangBangPolynomial(x0, xd0, -this->biped.u_max);
  Vector2d x_foot_int(x_foot_poly_plus.value(t_min_to_xprime), x_foot_poly_minus.value(t_min_to_xprime));

  if ((x_ic_target >= x_foot_int.minCoeff()) && (x_ic_target <= x_foot_int.maxCoeff())) {
    // std::cerr << "xprime dominates" << std::endl;
    // The time to get onto the xprime axis dominates, and we can hit the ICP (plus offset) as soon as we get to that axis
    std::vector<BangBangIntercept> intercepts = QPReactiveRecoveryPlan::bangBangIntercept(x0, xd0, x_ic_target, this->biped.u_max);
    if (intercepts.size() > 0) {
      // std::cerr << "x_ic_target: " << x_ic_target << std::endl;
      // if there are multiple options, take the one that switches sooner
      std::vector<BangBangIntercept>::iterator it_min = std::min_element(intercepts.begin(), intercepts.end(), tswitchComp);
      Isometry3d intercept_pose_in_world = T_world_to_local.inverse() * Isometry3d(Translation<double, 3>(Vector3d(x_ic_target, 0, 0)));
      // std::cerr << "intercept before reach: " << intercept_pose_in_world.translation() << std::endl;
      intercept_pose_in_world = QPReactiveRecoveryPlan::closestPoseInConvexHull(intercept_pose_in_world, reachable_vertices_in_world.topRows(2));
      // std::cerr << "intercept after reach: " << intercept_pose_in_world.translation() << std::endl;
      InterceptPlan intercept_plan;
      intercept_plan.tf = t_min_to_xprime;
      intercept_plan.tswitch = it_min->tswitch;
      intercept_plan.pose_next = intercept_pose_in_world;
      intercept_plan.icp_next = T_world_to_local.inverse() * Isometry3d(Translation<double, 3>(Vector3d(x_ic_int, 0, 0)));
      intercept_plan.cop = cop;
      intercept_plan.swing_foot = swing_foot;
      intercept_plan.stance_foot = otherFoot[swing_foot];
      intercept_plan.error = 0; // to be filled in later
      intercept_plan.stance_pose = foot_states.at(stance_foot).pose;
      intercept_plans.push_back(intercept_plan);
    }
  } else {
    // std::cerr << "xprime does not dominate" << std::endl;
    std::vector<double> us = {this->biped.u_max, -this->biped.u_max};
    for (std::vector<double>::iterator u = us.begin(); u != us.end(); ++u) {
      std::vector<double> t_int = QPReactiveRecoveryPlan::expIntercept(icp_traj_in_local + this->desired_icp_offset, x0, xd0, *u, 7);
      std::vector<double> t_int_feasible;
      for (std::vector<double>::iterator it = t_int.begin(); it != t_int.end(); ++it) {
        if (*it >= t_min_to_xprime && *it >= std::abs(xd0 / (*u))) {
          t_int_feasible.push_back(*it);
        }
      }
      std::vector<Isometry3d, aligned_allocator<Isometry3d>> reachable_poses;
      if (t_int_feasible.size() == 0) {
        // If there are no intercepts, get as close to our desired capture as possible within the reachable set.
        // note: this might be off the xcop->xic line
        double x_ic_future = icp_traj_in_local.value(t_min_to_xprime);
        Isometry3d reachable_pose_in_world = T_world_to_local.inverse() * Isometry3d(Translation<double, 3>(Vector3d(x_ic_future + this->desired_icp_offset, 0, 0)));
          // std::cerr << "intercept before reach: " << reachable_pose_in_world.translation() << std::endl;
        reachable_pose_in_world = QPReactiveRecoveryPlan::closestPoseInConvexHull(reachable_pose_in_world, reachable_vertices_in_world.topRows(2));
          // std::cerr << "intercept after reach: " << reachable_pose_in_world.translation() << std::endl;
        reachable_poses.push_back(reachable_pose_in_world);
      } else {
        for (std::vector<double>::iterator t = t_int_feasible.begin(); t != t_int_feasible.end(); ++t) {
          Isometry3d reachable_pose_in_world = T_world_to_local.inverse() * Isometry3d(Translation<double, 3>(Vector3d(icp_traj_in_local.value(*t) + this->desired_icp_offset, 0, 0)));
          // std::cerr << "intercept before reach: " << reachable_pose_in_world.translation() << std::endl;
          reachable_pose_in_world = QPReactiveRecoveryPlan::closestPoseInConvexHull(reachable_pose_in_world, reachable_vertices_in_world.topRows(2));
          // std::cerr << "intercept after reach: " << reachable_pose_in_world.translation() << std::endl;
          reachable_poses.push_back(reachable_pose_in_world);
        }
      }
      for (std::vector<Isometry3d, aligned_allocator<Isometry3d>>::iterator reachable_pose = reachable_poses.begin(); reachable_pose != reachable_poses.end(); ++ reachable_pose) {
        Isometry3d reachable_pose_in_local = T_world_to_local * (*reachable_pose);

        std::vector<BangBangIntercept> intercepts = QPReactiveRecoveryPlan::bangBangIntercept(x0, xd0, reachable_pose_in_local.translation().x(), this->biped.u_max);
        if (intercepts.size() > 0) {
          // if there are multiple options, take the one that switches sooner
          std::vector<BangBangIntercept>::iterator it_min = std::min_element(intercepts.begin(), intercepts.end(), tswitchComp);

          InterceptPlan intercept_plan;
          intercept_plan.tf = std::max(it_min->tf, t_min_to_xprime);
          intercept_plan.tswitch = it_min->tswitch;
          intercept_plan.pose_next = *reachable_pose;
          intercept_plan.icp_next = T_world_to_local.inverse() * Isometry3d(Translation<double, 3>(Vector3d(icp_traj_in_local.value(it_min->tf), 0, 0)));
          intercept_plan.cop = cop;
          intercept_plan.swing_foot = swing_foot;
          intercept_plan.stance_foot = otherFoot[swing_foot];
          intercept_plan.error = 0; // to be filled in later
          intercept_plan.stance_pose = foot_states.at(stance_foot).pose;
          intercept_plans.push_back(intercept_plan);
        }
      }
    }
  }

  for (std::vector<InterceptPlan>::iterator it = intercept_plans.begin(); it != intercept_plans.end(); ++it) {
    it->pose_next.linear() = foot_states.find(stance_foot)->second.pose.linear();
    it->pose_next = snapToTerrain(it->pose_next, foot_states.find(stance_foot)->second.terrain_height, foot_states.find(stance_foot)->second.terrain_normal);
    it->error = interceptPlanError(*it, foot_states, this->biped);
  }

  return intercept_plans;
}

std::vector<InterceptPlan> QPReactiveRecoveryPlan::getInterceptPlansForFoot(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp) {
  FootID stance_foot = otherFoot.find(swing_foot)->second;

  // Find the center of pressure, which we'll place as close as possible to the ICP
  Isometry3d cop = QPReactiveRecoveryPlan::closestPoseInConvexHull(icp,
                       (foot_states.find(stance_foot)->second.pose * (this->foot_hull_cop_shrink_factor * this->biped.foot_vertices.find(stance_foot)->second)).topRows(2));
  return this->getInterceptsWithCoP(swing_foot, foot_states, icp, cop);
}

std::vector<InterceptPlan> QPReactiveRecoveryPlan::getInterceptPlans(const std::map<FootID, FootState> &foot_states, const Isometry3d &icp) {
  std::vector<InterceptPlan> all_intercept_plans;
  std::vector<FootID> available_swing_feet;

  if (foot_states.find(RIGHT)->second.contact && foot_states.find(LEFT)->second.contact) {
    available_swing_feet.push_back(LEFT);
    available_swing_feet.push_back(RIGHT);
  } else if (!foot_states.find(RIGHT)->second.contact) {
    available_swing_feet.push_back(RIGHT);
  } else {
    available_swing_feet.push_back(LEFT);
  }

  for (std::vector<FootID>::iterator swing_foot = available_swing_feet.begin(); swing_foot != available_swing_feet.end(); ++swing_foot) {
    if (foot_states.find(*swing_foot)->second.velocity.head(3).squaredNorm() / this->biped.u_max / 2.0 < this->max_considerable_foot_swing) {
      std::vector<InterceptPlan> foot_plans = this->getInterceptPlansForFoot(*swing_foot, foot_states, icp);
      all_intercept_plans.insert(all_intercept_plans.end(), foot_plans.begin(), foot_plans.end());
    }
  }
  return all_intercept_plans;
}

std::unique_ptr<PiecewisePolynomial<double>> QPReactiveRecoveryPlan::freeKnotTimesSpline(double t0, double tf, const Ref<const MatrixXd> &xs, const Ref<const VectorXd> xd0, const Ref<const VectorXd> xdf) {
  const int grid_steps = 10;

  const size_t num_segments = xs.cols() - 1;
  const size_t ndof = xs.rows();
  const size_t num_knots = num_segments - 1;

  if (xs.rows() != xd0.rows() || xs.rows() != xdf.rows()) {
    throw std::runtime_error("size of xs and xd0 (or xdf) don't match");
  }

  std::vector<double> segment_times;
  segment_times.resize(num_segments + 1);
  segment_times[0] = t0;
  segment_times[static_cast<size_t>(num_segments)] = tf;
  std::vector<double> best_segment_times = segment_times;
  double t_step = (tf - t0) / grid_steps;
  double min_objective_value = std::numeric_limits<double>::infinity();

  // assemble the knot point locations for input to nWaypointCubicSpline
  MatrixXd xi = xs.block(0, 1, ndof, num_knots);

  int t_indices[num_knots];
  if (grid_steps <= num_knots){
    // If we have have too few grid steps, then by pigeonhole it's
    // impossible to give each a unique time in our grid search.
    throw std::runtime_error("Drake:freeKnotTimesSpline:TooManyKnotsForNumGridSteps");
  }
  for (int i=0; i<num_knots; i++)
    t_indices[i] = i+1; // assume knot point won't be the same time as the
          // initial state, or previous knot point

  while (t_indices[0] < grid_steps-num_knots+1){
    for (int i=0; i<num_knots; i++)
      segment_times[i+1] = t0 + t_indices[i]*t_step;

    bool valid_solution = true;
    double objective_value = 0.0;
    for (int dof = 0; dof < ndof && valid_solution; dof++) {
      try {
        PiecewisePolynomial<double> spline = nWaypointCubicSpline(segment_times, xs(dof, 0), xd0(dof), xs(dof, num_segments), xdf(dof), xi.row(dof).transpose());
        PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
        acceleration_squared *= acceleration_squared;
        PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
        objective_value += acceleration_squared_integral.scalarValue(spline.getEndTime()) - acceleration_squared_integral.scalarValue(spline.getStartTime());
      }
      catch (ConstraintMatrixSingularError&) {
        valid_solution = false;
      }
    }

    if (valid_solution && objective_value < min_objective_value) {
      best_segment_times = segment_times;
      min_objective_value = objective_value;
    }

    // Advance grid search counter or terminate, counting from
    // the latest t_index, and on overflow carrying to the
    // next lowest t_index and resetting to the new value of that
    // next lowest t_index. (since times must always be in order!)
    t_indices[num_knots-1]++;
    // carry, except for the lowest place, which we
    // use to detect doneness.
    for (int i=num_knots-1; i>0; i--){
      if ((i==num_knots-1 && t_indices[i] >= grid_steps) || (i<num_knots-1 && t_indices[i] >= t_indices[i+1])){
        t_indices[i-1]++;
        t_indices[i] = t_indices[i-1]+1;
      }
    }
  }

  std::vector<Matrix<Polynomial<double>, Dynamic, Dynamic>> poly_matrix;
  poly_matrix.reserve(num_segments);
  for (int i=0; i < num_segments; ++i) {
    poly_matrix.push_back(Matrix<Polynomial<double>, Dynamic, Dynamic> (ndof, 1));
  }
  for (int dof=0; dof < ndof; ++dof) {
    PiecewisePolynomial<double> one_d_spline = nWaypointCubicSpline(best_segment_times, xs(dof, 0), xd0(dof), xs(dof, num_segments), xdf(dof), xi.row(dof).transpose());
    for (int i=0; i < num_segments; ++i) {
      poly_matrix[i](dof) = one_d_spline.getPolynomial(i);
    }
  }
  std::unique_ptr<PiecewisePolynomial<double>> spline(new PiecewisePolynomial<double>(poly_matrix, best_segment_times));
  return spline;
}

std::unique_ptr<PiecewisePolynomial<double>> QPReactiveRecoveryPlan::straightToGoalTrajectory(double t_global, const InterceptPlan &intercept_plan, const FootStateMap &foot_states) {
  const FootState state = foot_states.at(intercept_plan.swing_foot);

  std::cout << "case 1" << std::endl;

  const double fraction_first = 0.7;

  const double swing_height_first_in_world = state.terrain_height + (state.pose.translation().z() - state.terrain_height) * (1 - std::pow(fraction_first,2));

  Matrix<double, 6, 3> xs;
  Vector6d xd0 = Vector6d::Zero(); // don't try to continue current velocity (it just leads to unpredictable and weird splines)
  Vector6d xdf = Vector6d::Zero();

  Quaterniond quat;
  xs.block(0, 0, 3, 1) = state.pose.translation();
  quat = Quaterniond(state.pose.rotation());
  auto w = quat2expmap(Vector4d(quat.w(), quat.x(), quat.y(), quat.z()), 1);
  xs.block(3, 0, 3, 1) = w.value();

  xs.block(0, 2, 3, 1) = intercept_plan.pose_next.translation();
  quat = Quaterniond(intercept_plan.pose_next.rotation());
  xs.block(3, 2, 3, 1) = quat2expmap(Vector4d(quat.w(), quat.x(), quat.y(), quat.z()), 0).value();

  auto w_unwrap = closestExpmap(xs.block(3, 0, 3, 1), xs.block(3, 2, 3, 1), 1);
  xs.block(3, 2, 3, 1) = w_unwrap.value();
  xd0.tail<3>() = w_unwrap.gradient().value() * xd0.tail<3>();

  xs.block(0, 1, 6, 1) = (1 - fraction_first) * xs.block(0, 0, 6, 1) + fraction_first * xs.block(0, 2, 6, 1);
  xs(2, 1) = swing_height_first_in_world;

  return QPReactiveRecoveryPlan::freeKnotTimesSpline(t_global, intercept_plan.tf + t_global, xs, xd0, xdf);
}

std::unique_ptr<PiecewisePolynomial<double>> QPReactiveRecoveryPlan::upOverAndDownTrajectory(double t_global, const InterceptPlan &intercept_plan, const FootStateMap &foot_states) {
  const FootState state = foot_states.at(intercept_plan.swing_foot);

  std::cout << "case 2" << std::endl;

  const double fraction_first = 0.15;
  const double fraction_second = 1 - fraction_first;

  double swing_height_first_in_world = state.terrain_height + this->swing_height_above_terrain;
  double swing_height_second_in_world = state.terrain_height + this->swing_height_above_terrain;

  if (state.pose.translation().z() > swing_height_first_in_world) {
    swing_height_first_in_world = swing_height_second_in_world * (fraction_first / fraction_second) + state.pose.translation().z() * (1 - fraction_first / fraction_second);
  }

  Matrix<double, 6, 4> xs;
  Vector6d xd0 = Vector6d::Zero(); // don't try to continue current velocity (it just leads to unpredictable and weird splines)
  Vector6d xdf = Vector6d::Zero();

  Quaterniond quat;
  xs.block(0, 0, 3, 1) = state.pose.translation();
  quat = Quaterniond(state.pose.rotation());
  auto w = quat2expmap(Vector4d(quat.w(), quat.x(), quat.y(), quat.z()), 1);
  xs.block(3, 0, 3, 1) = w.value();

  xs.block(0, 3, 3, 1) = intercept_plan.pose_next.translation();
  quat = Quaterniond(intercept_plan.pose_next.rotation());
  xs.block(3, 3, 3, 1) = quat2expmap(Vector4d(quat.w(), quat.x(), quat.y(), quat.z()), 0).value();

  auto w_unwrap = closestExpmap(xs.block(3, 0, 3, 1), xs.block(3, 3, 3, 1), 1);
  xs.block(3, 3, 3, 1) = w_unwrap.value();
  xd0.tail<3>() = w_unwrap.gradient().value() * xd0.tail<3>();

  xs.block(0, 1, 2, 1) = (1 - fraction_first) * xs.block(0, 0, 2, 1) + fraction_first * xs.block(0, 3, 2, 1);
  xs(2, 1) = swing_height_first_in_world;
  xs.block(3, 1, 3, 1) = xs.block(3, 0, 3, 1);

  xs.block(0, 2, 2, 1) = (1 - fraction_second) * xs.block(0, 0, 2, 1) + fraction_second * xs.block(0, 3, 2, 1);
  xs(2, 2) = swing_height_second_in_world;
  xs.block(3, 2, 3, 1) = xs.block(3, 3, 3, 1);

  return QPReactiveRecoveryPlan::freeKnotTimesSpline(t_global, t_global + intercept_plan.tf, xs, xd0, xdf);
}

std::unique_ptr<PiecewisePolynomial<double>> QPReactiveRecoveryPlan::swingTrajectory(double t_global, const InterceptPlan &intercept_plan, const std::map<FootID, FootState> &foot_states) {
  const FootState state = foot_states.at(intercept_plan.swing_foot);
  const double dist_to_goal = (intercept_plan.pose_next.translation().head(2) - state.pose.translation().head(2)).norm();
  // TODO: name the magic numbers here
  const double descend_coeff = std::pow(1.0 / 0.15, 2);

  std::cout << "planning swing with tf = " << intercept_plan.tf << std::endl;
  if (descend_coeff * std::pow(state.pose.translation().z() - state.terrain_height, 2) >= dist_to_goal) {
    // We're within a quadratic bowl around our target, so let's just descend straight there
    return this->straightToGoalTrajectory(t_global, intercept_plan, foot_states);
  } else {
    // We'll need to go up and then back down to get to the goal
    return this->upOverAndDownTrajectory(t_global, intercept_plan, foot_states);
  }
}

void QPReactiveRecoveryPlan::setRobot(RigidBodyTree *robot) {
  this->robot = robot;
  this->findFootSoleFrames();
  this->q_des.resize(robot->num_positions);
}

QPReactiveRecoveryPlan::QPReactiveRecoveryPlan(RigidBodyTree *robot, const RobotPropertyCache &rpc) {
  this->robot = robot;
  this->biped = getAtlasDefaults();
  this->robot_property_cache = rpc;
  if (this->robot) {
    this->setRobot(robot);
  }
  this->initLCM();
}

QPReactiveRecoveryPlan::QPReactiveRecoveryPlan(RigidBodyTree *robot, const RobotPropertyCache &rpc, BipedDescription biped) {
  this->robot = robot;
  this->biped = biped;
  this->robot_property_cache = rpc;
  if (this->robot) {
    this->setRobot(robot);
  }
  this->initLCM();
}

void QPReactiveRecoveryPlan::findFootSoleFrames() {
  std::map<FootID, bool> has_frame;
  has_frame[RIGHT] = false;
  has_frame[LEFT] = false;
  for (int i=0; i < robot->frames.size(); ++i) {
    if (this->robot->frames[i]->name == "r_foot_sole") {
      has_frame[RIGHT] = true;
      // frame_ind0 = -frameID - 2
      // i = -frameID - 2;
      // frameID = -i - 2;
      this->foot_frame_ids[RIGHT] = -i - 2;
      Isometry3d Tframe;
      int body_id = this->robot->parseBodyOrFrameID( this->foot_frame_ids[RIGHT], &Tframe);
      if (!Tframe.isApprox(this->robot->frames[i]->transform_to_body)) {
        throw std::runtime_error("somehow I got the frame ID/index logic wrong");
      }
      this->foot_body_ids[RIGHT] = body_id;
    } else if (this->robot->frames[i]->name == "l_foot_sole") {
      has_frame[LEFT] = true;
      this->foot_frame_ids[LEFT] = -i - 2;
      Isometry3d Tframe;
      int body_id = this->robot->parseBodyOrFrameID(this->foot_frame_ids[LEFT], &Tframe);
      if (!Tframe.isApprox(this->robot->frames[i]->transform_to_body)) {
        throw std::runtime_error("somehow I got the frame ID/index logic wrong");
      }
      this->foot_body_ids[LEFT] = body_id;
    }
  }

  if (!has_frame[RIGHT]) {
    throw std::runtime_error("could not find r_foot_sole frame");
  }
  if (!has_frame[LEFT]) {
    throw std::runtime_error("could not find l_foot_sole frame");
  }
}

void QPReactiveRecoveryPlan::resetInitialization() {
  this->initialized = false;
  this->last_swing_plan.reset(NULL);
}

drake::lcmt_qp_controller_input QPReactiveRecoveryPlan::getQPControllerInput(double t_global, const VectorXd &q, const VectorXd &v, const std::vector<bool>& contact_force_detected) {
  if (!this->initialized) {
    for (int i=0; i < this->robot_property_cache.position_indices.at("arm").size(); ++i) {
      int j = this->robot_property_cache.position_indices.at("arm")(i);
      this->q_des(j) = q(j);
    }
    this->initialized = true;
    this->t_start = t_global;
  }

  KinematicsCache<double> cache = this->robot->doKinematics(q, v);

  Vector2d r_ic = this->getICP(cache, v);
  Isometry3d icp = Isometry3d(Translation<double, 3>(Vector3d(r_ic(0), r_ic(1), 0)));

  FootStateMap foot_states = this->getFootStates(cache, v, contact_force_detected);

  bool is_captured = this->isICPCaptured(icp.translation().head<2>(), foot_states, this->biped.foot_vertices);

  drake::lcmt_qp_controller_input qp_input;
  this->setupQPInputDefaults(t_global, qp_input);

  if (this->last_swing_plan && t_global < this->last_swing_plan->getEndTime()) {
    // std::cout << "continuing current plan" << std::endl;
    this->getInterceptInput(t_global, foot_states, qp_input);
  } else if (is_captured) {
    // std::cout << "is captured" << std::endl;
    this->getCaptureInput(t_global, foot_states, icp, qp_input);
  } else if (this->last_swing_plan && t_global < this->last_swing_plan->getEndTime() + this->post_execution_delay) {
    // std::cout << "in delay after plan end" << std::endl;
    this->getCaptureInput(t_global, foot_states, icp, qp_input);
  } else {
    std::cout << "replanning" << std::endl;
    std::vector<InterceptPlan> intercept_plans = this->getInterceptPlans(foot_states, icp);
    if (intercept_plans.size() == 0) {
      std::cout << "recovery is not possible" << std::endl;
      this->getCaptureInput(t_global, foot_states, icp, qp_input);
    } else {
      std::vector<InterceptPlan>::iterator best_plan = std::min_element(intercept_plans.begin(), intercept_plans.end(), errorCompare);
      this->last_intercept_plan = *best_plan;
      this->last_swing_plan.reset(this->swingTrajectory(t_global, this->last_intercept_plan, foot_states).release());
      this->t_start = t_global;
      this->getInterceptInput(t_global, foot_states, qp_input);
    }
  }

  this->publishForVisualization(cache, t_global, icp);
  verifySubtypeSizes(qp_input);
  return qp_input;
}

void QPReactiveRecoveryPlan::publishQPControllerInput(double t_global, const VectorXd &q, const VectorXd &v, const std::vector<bool>& contact_force_detected) {
  drake::lcmt_qp_controller_input qp_input = this->getQPControllerInput(t_global, q, v, contact_force_detected);
  this->LCMHandle->publish("QP_CONTROLLER_INPUT", &qp_input);
}

void QPReactiveRecoveryPlan::setupQPInputDefaults(double t_global, drake::lcmt_qp_controller_input &qp_input) {
  qp_input.be_silent = false;
  qp_input.timestamp = static_cast<int64_t> (t_global * 1e6);
  qp_input.num_support_data = 0;
  qp_input.num_tracked_bodies = 0;
  qp_input.num_external_wrenches = 0;
  qp_input.num_joint_pd_overrides = 0;

  qp_input.zmp_data.timestamp = 0;

  Matrix4d A = Matrix4d::Zero();
  A.block(0, 2, 2, 2) = Matrix2d::Identity();
  eigenToCArrayOfArrays(A, qp_input.zmp_data.A);

  Matrix<double, 4, 2> B = Matrix<double, 4, 2>::Zero();
  B.block(2, 0, 2, 2) = Matrix2d::Identity();
  eigenToCArrayOfArrays(B, qp_input.zmp_data.B);

  Matrix<double, 2, 4> C = Matrix<double, 2, 4>::Zero();
  C.block(0, 0, 2, 2) = Matrix2d::Identity();
  eigenToCArrayOfArrays(C, qp_input.zmp_data.C);

  Matrix2d D = Matrix2d::Identity();
  D *= -(1.0 / pow(this->biped.omega, 2));
  eigenToCArrayOfArrays(D, qp_input.zmp_data.D);

  // x0 and y0 will be filled in from the plan

  Vector2d u0 = Vector2d::Zero();
  eigenToCArrayOfArrays(u0, qp_input.zmp_data.u0);

  Matrix2d R = Matrix2d::Zero();
  eigenToCArrayOfArrays(R, qp_input.zmp_data.R);

  Matrix2d Qy = Matrix2d::Identity();
  Qy *= 0.8;
  eigenToCArrayOfArrays(Qy, qp_input.zmp_data.Qy);

  eigenToCArrayOfArrays(this->S, qp_input.zmp_data.S);

  Vector4d s1 = Vector4d::Zero();
  eigenToCArrayOfArrays(s1, qp_input.zmp_data.s1);

  Vector4d s1dot = Vector4d::Zero();
  eigenToCArrayOfArrays(s1dot, qp_input.zmp_data.s1dot);

  qp_input.zmp_data.s2 = 0;
  qp_input.zmp_data.s2dot = 0;

  qp_input.whole_body_data.num_positions = this->robot->num_positions;
  qp_input.whole_body_data.q_des.resize(this->robot->num_positions);
  for (int i=0; i < this->robot->num_positions; ++i) {
    qp_input.whole_body_data.q_des[i] = this->q_des(i);
  }
  for (int i=0; i < this->robot_property_cache.position_indices.at("arm").size(); i++) {
    qp_input.whole_body_data.constrained_dofs.push_back(this->robot_property_cache.position_indices.at("arm")[i] + 1);
  }
  for (int i=0; i < this->robot_property_cache.position_indices.at("neck").size(); i++) {
    qp_input.whole_body_data.constrained_dofs.push_back(this->robot_property_cache.position_indices.at("neck")[i] + 1);
  }
  for (int i=0; i < this->robot_property_cache.position_indices.at("back_bky").size(); i++) {
    qp_input.whole_body_data.constrained_dofs.push_back(this->robot_property_cache.position_indices.at("back_bky")[i] + 1);
  }
  for (int i=0; i < this->robot_property_cache.position_indices.at("back_bkz").size(); i++) {
    qp_input.whole_body_data.constrained_dofs.push_back(this->robot_property_cache.position_indices.at("back_bkz")[i] + 1);
  }
  qp_input.whole_body_data.num_constrained_dofs = qp_input.whole_body_data.constrained_dofs.size();

  qp_input.param_set_name = "recovery";

}

void QPReactiveRecoveryPlan::publishForVisualization(KinematicsCache<double>& cache, double t_global, const Isometry3d &icp) {
  std::shared_ptr<drc::reactive_recovery_debug_t> msg(new drc::reactive_recovery_debug_t());

  msg->utime = static_cast<int64_t> (t_global * 1e6);

  auto com = this->robot->centerOfMass<double>(cache);
  memcpy(msg->com, com.data(), 3*sizeof(double));
  memcpy(msg->icp, icp.translation().head<2>().data(), 2*sizeof(double));

  msg->num_spline_ts = 0;
  msg->num_spline_segments = 0;

  this->LCMHandle->publish("REACTIVE_RECOVERY_DEBUG", msg.get());
}

Matrix3Xd QPReactiveRecoveryPlan::heelToeContacts(int body_id) {
  Matrix3Xd toe_contacts = this->robot_property_cache.contact_groups[body_id].at("toe");
  Matrix3Xd heel_contacts = this->robot_property_cache.contact_groups[body_id].at("heel");
  Matrix3Xd all_contacts(3, toe_contacts.cols() + heel_contacts.cols());
  all_contacts.block(0, 0, 3, toe_contacts.cols()) = toe_contacts;
  all_contacts.block(0, toe_contacts.cols(), 3, heel_contacts.cols()) = heel_contacts;
  return all_contacts;
}

std::map<SupportLogicType, std::vector<bool>> createSupportLogicMaps() {
  std::map<SupportLogicType, std::vector<bool> > ret;
  ret[REQUIRE_SUPPORT] = { {true, true, true, true} };
  ret[ONLY_IF_FORCE_SENSED] = { {false, false, true, true} };
  ret[KINEMATIC_OR_SENSED] = { {false, true, true, true} };
  ret[PREVENT_SUPPORT] = { {false, false, false, false} };
  return ret;
}

void QPReactiveRecoveryPlan::encodeSupportData(const int body_id, const FootState &foot_state, const SupportLogicType &support_logic, drake::lcmt_support_data &support_data) {
  support_data.timestamp = 0;
  support_data.body_id = body_id + 1;
  support_data.contact_pts.resize(3);
  Matrix3Xd all_contacts = this->heelToeContacts(body_id);
  support_data.num_contact_pts = all_contacts.cols();
  for (int i=0; i < 3; i++) {
    support_data.contact_pts[i].resize(all_contacts.cols());
    for (int j=0; j < all_contacts.cols(); j++) {
      support_data.contact_pts[i][j] = all_contacts(i, j);
    }
  }
  std::map<SupportLogicType, std::vector<bool>> logic_maps = createSupportLogicMaps();
  std::vector<bool> logic = logic_maps.at(support_logic);
  for (int i=0; i < 4; ++i) {
    support_data.support_logic_map[i] = logic[i];
  }
  support_data.mu = this->mu;
  support_data.use_support_surface = true;
  for (int i=0; i < 3; ++i) {
    // 4-vector describing a support surface: [v; b] such that v' * [x;y;z] + b == 0
    // we have normal n and height h at x0,y0
    // n' * [x0;y0;h] + b == 0
    // b = -n' * [x0;y0;h]
    support_data.support_surface[i] = static_cast<float>(foot_state.terrain_normal(i));
  }
  support_data.support_surface[3] = static_cast<float> (-1 * foot_state.terrain_normal.transpose() * Vector3d(foot_state.pose.translation().x(), foot_state.pose.translation().y(), foot_state.terrain_height));
}

void QPReactiveRecoveryPlan::encodeBodyMotionData(int body_or_frame_id, PiecewisePolynomial<double> spline, drake::lcmt_body_motion_data &body_motion) {
  body_motion.timestamp = 0;
  body_motion.body_id = body_or_frame_id + 1;
  encodePiecewisePolynomial(spline, body_motion.spline);
  body_motion.in_floating_base_nullspace = false;
  body_motion.control_pose_when_in_contact = false;
  body_motion.quat_task_to_world[0] = 1;
  body_motion.quat_task_to_world[1] = 0;
  body_motion.quat_task_to_world[2] = 0;
  body_motion.quat_task_to_world[3] = 0;
  body_motion.translation_task_to_world[0] = 0;
  body_motion.translation_task_to_world[1] = 0;
  body_motion.translation_task_to_world[2] = 0;
  body_motion.xyz_kp_multiplier[0] = 1;
  body_motion.xyz_kp_multiplier[1] = 1;
  body_motion.xyz_kp_multiplier[2] = 1;
  body_motion.xyz_damping_ratio_multiplier[0] = 1;
  body_motion.xyz_damping_ratio_multiplier[1] = 1;
  body_motion.xyz_damping_ratio_multiplier[2] = 1;
  body_motion.expmap_kp_multiplier = 1;
  body_motion.expmap_damping_ratio_multiplier = 1;
  body_motion.weight_multiplier[0] = 1;
  body_motion.weight_multiplier[1] = 1;
  body_motion.weight_multiplier[2] = 1;
  body_motion.weight_multiplier[3] = 1;
  body_motion.weight_multiplier[4] = 1;
  body_motion.weight_multiplier[5] = 1;
}

double angleAverage(double theta1, double theta2) {
  // (Copied from drakeUtil.cpp to avoid a lot of extra dependencies)
  //
  // Computes the average between two angles by averaging points on the unit
  // circle and taking the arctan of the result.
  //   see: http://en.wikipedia.org/wiki/Mean_of_circular_quantities
  // theta1 is a scalar or column vector of angles (rad)
  // theta2 is a scalar or column vector of angles (rad)

  double x_mean = 0.5 * (std::cos(theta1) + std::cos(theta2));
  double y_mean = 0.5 * (std::sin(theta1) + std::sin(theta2));

  double angle_mean = atan2(y_mean, x_mean);

  return angle_mean;
}

PiecewisePolynomial<double> constantPoseCubicSpline(const Isometry3d &pose) {
  std::vector<Matrix<Polynomial<double>, Dynamic, Dynamic>> poly_matrix;
  poly_matrix.push_back(Matrix<Polynomial<double>, Dynamic, Dynamic>(6, 1));
  std::vector<double> ts = {0, 0};
  Matrix<double, 0, 1> xi;
  Vector6d xyzexp;
  xyzexp.head<3>() = pose.translation().head<3>();
  Quaterniond quat = Quaterniond(pose.rotation());
  xyzexp.tail<3>() = quat2expmap(Vector4d(quat.w(), quat.x(), quat.y(), quat.z()), 0).value();
  for (int i=0; i < 6; ++i) {
    poly_matrix[0](i) = Polynomial<double>(Vector4d(xyzexp(i), 0, 0, 0));
  }
  return PiecewisePolynomial<double>(poly_matrix, ts);
}

void QPReactiveRecoveryPlan::getInterceptInput(double t_global, const FootStateMap &foot_states, drake::lcmt_qp_controller_input &qp_input) {

  PiecewisePolynomial<double>::CoefficientMatrix plan_shift(6, 1);
  plan_shift.topRows<3>() = this->last_intercept_plan.stance_pose.translation() - foot_states.at(this->last_intercept_plan.stance_foot).pose.translation();
  plan_shift.bottomRows<3>().setZero();
  // plan_shift = desired - measured;
  // measured = desired - plan_shift

  Vector4d x0 = Vector4d::Zero();
  x0.head<2>() = 0.5 * this->last_intercept_plan.stance_pose.translation().head<2>() + 0.5 * this->last_intercept_plan.pose_next.translation().head<2>() - plan_shift.topRows<2>();
  eigenToCArrayOfArrays(x0, qp_input.zmp_data.x0);
  eigenToCArrayOfArrays(this->last_intercept_plan.cop.translation().head<2>() - plan_shift.topRows<2>(), qp_input.zmp_data.y0);

  drake::lcmt_support_data support_data_stance;
  int stance_foot_id = this->foot_body_ids.at(this->last_intercept_plan.stance_foot);
  this->encodeSupportData(stance_foot_id, foot_states.at(this->last_intercept_plan.stance_foot),
                          REQUIRE_SUPPORT, support_data_stance);
  qp_input.support_data.push_back(support_data_stance);

  drake::lcmt_support_data support_data_swing;
  if (t_global - this->t_start <= (this->last_swing_plan->getEndTime() - this->last_swing_plan->getStartTime()) / 2) {
    this->encodeSupportData(this->foot_body_ids.at(this->last_intercept_plan.swing_foot),
                            foot_states.at(this->last_intercept_plan.swing_foot),
                            PREVENT_SUPPORT, support_data_swing);
  } else {
    this->encodeSupportData(this->foot_body_ids.at(this->last_intercept_plan.swing_foot),
                            foot_states.at(this->last_intercept_plan.swing_foot),
                            ONLY_IF_FORCE_SENSED, support_data_swing);
  }
  qp_input.support_data.push_back(support_data_swing);
  qp_input.num_support_data = qp_input.support_data.size();

  drake::lcmt_body_motion_data body_motion;
  this->encodeBodyMotionData(this->foot_frame_ids.at(this->last_intercept_plan.swing_foot),
                             *this->last_swing_plan - plan_shift, body_motion);
  body_motion.in_floating_base_nullspace = true;
  qp_input.body_motion_data.push_back(body_motion);

  double pelvis_height = this->last_intercept_plan.stance_pose.translation().z() + this->pelvis_height_above_sole;
  // double pelvis_height = foot_states.at(this->last_intercept_plan.stance_foot).terrain_height + this->pelvis_height_above_sole;
  Quaterniond rfoot_quat = Quaterniond(foot_states.at(RIGHT).pose.rotation());
  Quaterniond lfoot_quat = Quaterniond(foot_states.at(LEFT).pose.rotation());
  double pelvis_yaw = angleAverage(quat2rpy(Vector4d(rfoot_quat.w(), rfoot_quat.x(), rfoot_quat.y(), rfoot_quat.z()))(2),
                                quat2rpy(Vector4d(lfoot_quat.w(), lfoot_quat.x(), lfoot_quat.y(), lfoot_quat.z()))(2));
  Isometry3d pelvis_pose = Isometry3d(Translation<double, 3>(Vector3d(0, 0, pelvis_height)));
  pelvis_pose.rotate(AngleAxis<double>(pelvis_yaw, Vector3d(0, 0, 1)));
  this->encodeBodyMotionData(this->robot_property_cache.body_ids.pelvis,
                             constantPoseCubicSpline(pelvis_pose) - plan_shift,
                             body_motion);
  body_motion.weight_multiplier[3] = 0; // don't try to control x and y
  body_motion.weight_multiplier[4] = 0;
  qp_input.body_motion_data.push_back(body_motion);
  qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
}

void QPReactiveRecoveryPlan::getCaptureInput(double t_global, const FootStateMap &foot_states, const Isometry3d &icp, drake::lcmt_qp_controller_input &qp_input) {

  Vector4d x0 = Vector4d::Zero();
  x0.head<2>() = 0.5 * foot_states.at(RIGHT).pose.translation().head<2>() + 0.5 * foot_states.at(LEFT).pose.translation().head<2>();
  eigenToCArrayOfArrays(x0, qp_input.zmp_data.x0);
  eigenToCArrayOfArrays(x0.head<2>(), qp_input.zmp_data.y0);

  std::vector<FootID> foot_ids = {RIGHT, LEFT};
  for (std::vector<FootID>::iterator foot = foot_ids.begin(); foot != foot_ids.end(); ++foot) {
    drake::lcmt_support_data support_data;
    drake::lcmt_body_motion_data body_motion;
    if ((foot_states.at(*foot).pose.translation().z() - foot_states.at(*foot).terrain_height) < this->capture_max_flyfoot_height) {
      this->encodeSupportData(this->foot_body_ids.at(*foot), foot_states.at(*foot),
                              REQUIRE_SUPPORT, support_data);
    } else {
      this->encodeSupportData(this->foot_body_ids.at(*foot), foot_states.at(*foot),
                              ONLY_IF_FORCE_SENSED, support_data);
    }
    qp_input.support_data.push_back(support_data);

    Isometry3d pose_on_terrain = snapToTerrain(foot_states.at(*foot).pose, foot_states.at(*foot).terrain_height, foot_states.at(*foot).terrain_normal);
    this->encodeBodyMotionData(this->foot_frame_ids.at(*foot),
                               constantPoseCubicSpline(pose_on_terrain),
                               body_motion);
    body_motion.in_floating_base_nullspace = true;
    qp_input.body_motion_data.push_back(body_motion);
  }
  qp_input.num_support_data = qp_input.support_data.size();

  drake::lcmt_body_motion_data body_motion;
  double pelvis_height = 0.5 * (foot_states.at(LEFT).terrain_height + foot_states.at(RIGHT).terrain_height) + this->pelvis_height_above_sole;
  Quaterniond rfoot_quat = Quaterniond(foot_states.at(RIGHT).pose.rotation());
  Quaterniond lfoot_quat = Quaterniond(foot_states.at(LEFT).pose.rotation());
  double pelvis_yaw = angleAverage(quat2rpy(Vector4d(rfoot_quat.w(), rfoot_quat.x(), rfoot_quat.y(), rfoot_quat.z()))(2),
                                quat2rpy(Vector4d(lfoot_quat.w(), lfoot_quat.x(), lfoot_quat.y(), lfoot_quat.z()))(2));
  Isometry3d pelvis_pose = Isometry3d(Translation<double, 3>(Vector3d(0, 0, pelvis_height)));
  pelvis_pose.rotate(AngleAxis<double>(pelvis_yaw, Vector3d(0, 0, 1)));
  this->encodeBodyMotionData(this->robot_property_cache.body_ids.pelvis,
                             constantPoseCubicSpline(pelvis_pose),
                             body_motion);
  body_motion.weight_multiplier[3] = 0; // don't try to control x and y
  body_motion.weight_multiplier[4] = 0;
  qp_input.body_motion_data.push_back(body_motion);
  qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
}

Vector2d QPReactiveRecoveryPlan::getICP(KinematicsCache<double>& cache, const VectorXd &v) {
  Vector3d com_position = this->robot->centerOfMass(cache);
  Vector3d com_velocity = this->robot->centerOfMassJacobian(cache) * v;
  Vector2d icp = com_position.head(2) + com_velocity.head(2) / this->biped.omega;
  return icp;
}

FootStateMap QPReactiveRecoveryPlan::getFootStates(const KinematicsCache<double>& cache, const VectorXd &v, const std::vector<bool>& contact_force_detected) {
  std::vector<FootID> foot_ids = {RIGHT, LEFT};
  FootStateMap foot_states;
  double min_foot_height = std::numeric_limits<double>::infinity();
  for (std::vector<FootID>::iterator id = foot_ids.begin(); id != foot_ids.end(); ++id) {
    const int frame_id = this->foot_frame_ids[*id];
    Vector3d origin = Vector3d::Zero();
    foot_states[*id].pose = robot->relativeTransform(cache, 0, frame_id);
    auto twist = robot->relativeTwist(cache, 0, frame_id, frame_id);
    auto quat = rotmat2quat(foot_states[*id].pose.linear());
    foot_states[*id].velocity.topRows<3>() = foot_states[*id].pose.linear() * twist.bottomRows<3>();
    Matrix<double, QUAT_SIZE, SPACE_DIMENSION> omega_to_quatdot;
    angularvel2quatdotMatrix(quat, omega_to_quatdot, static_cast<Gradient<decltype(omega_to_quatdot), Dynamic>::type*>(nullptr));
    foot_states[*id].velocity.bottomRows<4>() = omega_to_quatdot * foot_states[*id].pose.linear() * twist.topRows<3>();
    int body_id = this->robot->parseBodyOrFrameID(this->foot_frame_ids[*id]);
    foot_states[*id].contact = contact_force_detected[body_id];
    if (foot_states[*id].pose.translation().z() < min_foot_height) {
      min_foot_height = foot_states[*id].pose.translation().z();
    }
  }

  for (std::vector<FootID>::iterator id = foot_ids.begin(); id != foot_ids.end(); ++id) {
    // NOTE: not using terrain maps at all, so we're just assuming the ground is flat under the lower foot
    foot_states[*id].terrain_height = min_foot_height;
    foot_states[*id].terrain_normal = Vector3d(0,0,1);
  }

  return foot_states;
}

void QPReactiveRecoveryPlan::initLCM() {
  this->LCMHandle = std::shared_ptr<lcm::LCM>(new lcm::LCM);
  if (!this->LCMHandle->good()) {
    throw std::runtime_error("lcm is not good");
  }
}


////////////////////////////////////////////////////////////////////////////////
// DualLaplacianStencil.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  A configurable stencil for Laplacian regularization terms for face-based
//  (piecewise constant) quantities. We support a plain 1D graph Laplacian
//  (with optional scaling by inverse dual edge lengths to ensure proper
//  scaling under refinement) and a Laplacian based on arbitrarily
//  triangulating the dual mesh and then using an intrinsic delaunay
//  triangulation-based Laplacian to cope with bad mesh quality.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  01/01/2020 16:26:45
////////////////////////////////////////////////////////////////////////////////
#ifndef DUALLAPLACIANSTENCIL_HH
#define DUALLAPLACIANSTENCIL_HH

#include "DualMesh.hh"
#include <Eigen/Sparse>
#include <MeshFEM/Utilities/MeshConversion.hh>

// We need to provide a wrapper for this function instead of calling it
// directly: libigl's `Triplet` will clash with ours if we include
// `intrinsic_delaunay_cotmatrix.h` in this header.
void igl_intrinsic_delaunay_cotmatrix(const Eigen::MatrixX3d &V,
                                      const Eigen::MatrixXi  &F,
                                      Eigen::SparseMatrix<Real> &L);

template<class Mesh>
struct DualLaplacianStencil {
    enum class Type { DualGraph, DualMeshIDT };

    Type type = Type::DualGraph;
    bool useUniformGraphWeights = true;

    DualLaplacianStencil(const Mesh &m) : m_mesh(m) {
        m_graphEdgeInvLen.resize(m.numHalfEdges());
        Real totalEdgeLen = 0;
        size_t num = 0;
        for (const auto &he : m.halfEdges()) {
            if (he.isBoundary()) continue;
            Real l = (m.elementBarycenter(he.           tri().index())
                    - m.elementBarycenter(he.opposite().tri().index())).norm();
            m_graphEdgeInvLen[he.index()] = 1.0 / l;
            totalEdgeLen += l;
            ++num;
        }
        m_averageEdgeLen = totalEdgeLen / num;

        std::vector<MeshIO::IOVertex > dualVertices;
        std::vector<MeshIO::IOElement> dualTris;
        std::vector<size_t> originatingPolygon;
        triangulatedBarycentricDual(m, dualVertices, dualTris, originatingPolygon);
        // LibIGL constructs the negative semi-definite Laplacian (which has
        // positive weights in the off-diagonals, assuming non-obtuse angles).
        igl_intrinsic_delaunay_cotmatrix(getV(dualVertices), getF(dualTris), m_dualIDTLaplacian);
    }

    // Call visitor(i, j, w_ij) for each stencil edge e_ij incident face i.
    template<class F>
    void visit(size_t i, F &&visitor) const {
        if (type == Type::DualGraph) {
            for (const auto &he : m_mesh.element(i).halfEdges()) {
                if (he.isBoundary()) continue;
                const auto &tri_j = he.opposite().tri();
                if (useUniformGraphWeights) visitor(i, tri_j.index(), 1.0);
                else                        visitor(i, tri_j.index(), m_averageEdgeLen * m_graphEdgeInvLen[he.index()]);
            }
        }
        else if (type == Type::DualMeshIDT) {
            // Loop over the ith column of the sparse Laplacian matrix...
            for (Eigen::SparseMatrix<Real>::InnerIterator it(m_dualIDTLaplacian, i); it; ++it) {
                size_t j = it.index(); // inner index (could also use it.row(), but that is storage-order-dependent)
                if (j == i) continue;  // skip diagonal
                visitor(i, j, it.value());
            }
        }
        else {
            assert(false);
        }
    }

    // Visit each edge e_ij with (i < j) in the graph
    template<class F>
    void visit_edges(F &&visitor) const {
        if (type == Type::DualGraph) {
            for (const auto &tri_i : m_mesh.elements()) {
                const size_t i = tri_i.index();
                for (const auto &he : tri_i.halfEdges()) {
                    if (he.isBoundary()) continue;
                    const auto &tri_j = he.opposite().tri();
                    const size_t j = tri_j.index();
                    if (i >= j) continue;
                    if (useUniformGraphWeights) visitor(i, j, 1.0);
                    else                        visitor(i, j, m_averageEdgeLen * m_graphEdgeInvLen[he.index()]);
                }
            }
        }
        else if (type == Type::DualMeshIDT) {
            for (const auto &tri_i : m_mesh.elements()) {
                const size_t i = tri_i.index();
                // Loop over the ith column of the sparse Laplacian matrix's upper triangle in order
                for (Eigen::SparseMatrix<Real>::InnerIterator it(m_dualIDTLaplacian, i); it; ++it) {
                    const size_t j = it.index(); // inner index (could also use it.row(), but that is storage-order-dependent)
                    if (j == i) break; // skip diagonal/lower triangle
                    visitor(i, j, it.value());
                }
            }
        }
        else {
            assert(false);
        }
    }

private:
    const Mesh &m_mesh;
    Eigen::SparseMatrix<Real> m_dualIDTLaplacian;
    Real m_averageEdgeLen;
    std::vector<Real> m_graphEdgeInvLen;
};

#endif /* end of include guard: DUALLAPLACIANSTENCIL_HH */


/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */

#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(const Graph& G,const count k, const count CB) : Centrality(G, true),k(k),CB(CB){
        S.resize(k);
        CFGCC = 0.;
        n=G.upperNodeIdBound();
        vList.resize(n);
        TopMatch.resize(n);
        /*Laplacian*/
        L.set_size(n,n);
        L.zeros();

        ERD.M=L;
        Adj=L;

        for(count i=0;i<L.n_rows;i++){
          vList[i]=i;
          for(count j=0;j<L.n_rows;j++){
            if(G.hasEdge(i,j)){
              L(i,j)=-1.;
              L(i,i)+=1;
              TopMatch[i].push_back(j);
            }
          }
        }
    }

    void CurrentFlowGroupCloseness::run() {
      count ID;
      count nPeripheralMerges;
      count minDegree;
      auto start = std::chrono::high_resolution_clock::now();
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> diff;
      std::vector<std::pair<node,node>> Matching;
      std::vector<std::pair<count,count>> Indices;

      nPeripheralMerges=0;
      if(CB>1){
          minDegree=1;
          std::vector<std::pair<count,count>> c_indices;
          std::vector<std::pair<node,node>> Matching;
          ID=0;
          c_indices.resize(0);

          ERDLevel Level(ID,vList,Matching,TopMatch,Adj);
          LevelList.push_back (Level);
          /*update*/
          minDegree=updateMinDegree(minDegree);

          while(minDegree==1){
            c_indices=peripheralCoarsingIndices();
            Matching=updateMatching(c_indices);
            coarseLaplacian(c_indices);
            ID++;
            Level.set(ID,vList,Matching,TopMatch,Adj);
            LevelList.push_back (Level);
            minDegree=updateMinDegree(minDegree);
            TopMatch=updateTopMatch(minDegree);
          }
          /*
          while(minDegree<CB){
            c_indices=coarsingIndices(minDegree, false);
            Matching=updateMatching(c_indices);
            coarseLaplacian(c_indices);
            ID++;
            Level.set(ID,vList,Matching,TopMatch,Adj);
            LevelList.push_back (Level);
            minDegree=updateMinDegree(minDegree);
            TopMatch=updateTopMatch(minDegree);
          }
          */
        }
        L=arma::pinv(L, 0.01);
        ERD.computeFromPinvL(L,vList);
        if(CB>1){
          ID=LevelList[LevelList.size()-1].get_ID();
          while(ID>1){
            Matching=LevelList[ID].get_Matching();
            for(count i=0;i<Matching.size();i++){
              uncoarse(Matching[i].second, Matching[i].first, ID);
            }
            ID--;
            vList=LevelList[ID].get_vList();
          }
          nPeripheralMerges=mergePeripheralNodes();
        }
        end = std::chrono::high_resolution_clock::now();
        diff = end-start;
        std::cout << "Computation of EffectiveResistanceDistanceMatrice finished in " << diff.count() << "(s)" << "\n\n";

        greedy(nPeripheralMerges);
      }
      /*******************************************************************************************************************************************************/
      std::vector<node> CurrentFlowGroupCloseness::groupMaxCurrentFlowCloseness(){
          return S;
      }
      /*******************************************************************************************************************************************************/
      double CurrentFlowGroupCloseness::getCFGCC() {
          return CFGCC;
      }
      /**************************************************************************************************************/
        void CurrentFlowGroupCloseness::cleanNetwork(){
          count n2;
          n2=0;
          for(count i=0;i<n;i++){
            if(L(i,i)==0){
              vList.erase(vList.begin()+i-n2);
              n2++;
            }
          }
          if(n2>0){
            n-=n2;
            arma::uvec indices(n);
            for(count i=0;i<n;i++){
                indices(i)=vList[i];
            }
            L=L.submat(indices, indices);
          }
        }
        /**************************************************************************************************************/
        void CurrentFlowGroupCloseness::greedy(count n_peripheral_merges){
          count k_max;
          node s;
          node s_next;
          node v;
          double S_CFGCC;
          double S_currentCFGCC;
          double scaling_factor;
          std::vector<bool> V;
          std::vector<double> d;

          k_max=S.size();
          scaling_factor=(double)(n);
          if(k_max>vList.size()){
            k_max=vList.size()-1;
            scaling_factor=(double)((G.upperNodeIdBound()))/(double)(k_max);
          }
          S_CFGCC=0.;
          V.resize(vList.size(),true);
          d.resize(G.upperNodeIdBound(),n*n);
          for(count i=0;i<k_max;i++){
            /*Maximal Gain Loop*/
            for (count j=0; j<vList.size()-n_peripheral_merges;j++) {
                //use vector of bools
                if (V[j]){
                    s=vList[j];
                    /*Sample Loop*/
                    S_currentCFGCC = 0.;
                    for (count l = 0; l < vList.size(); l++) {
                        v=vList[l];
                        if (ERD.M(v,s)< d[v])
                            S_currentCFGCC = S_currentCFGCC + ERD.M(v,s);
                        else {
                            S_currentCFGCC = S_currentCFGCC + d[v];
                        }
                    }
                    S_currentCFGCC =  scaling_factor / S_currentCFGCC;
                    if (S_currentCFGCC > S_CFGCC) {
                        S_CFGCC = S_currentCFGCC;
                        s_next = s;
                    }
                }
            }
            for(count j= 0; j<vList.size(); j++) {
              v=vList[j];
                if (ERD.M(v,s_next)< d[v]) {
                    d[v] = ERD.M(v,s_next);
                }
            }
            S[i]=s_next;
            V[s_next]=false;
        }
        CFGCC=S_CFGCC;
    }

    void CurrentFlowGroupCloseness::computeInitialERD(count CB){
      count ID;
      count minDegree;

      minDegree=1;
      std::vector<std::pair<count,count>> c_indices;
      std::vector<std::pair<node,node>> Matching;
      ID=0;
      c_indices.resize(0);

      ERDLevel Level(ID,vList,Matching,TopMatch,Adj);
      LevelList.push_back (Level);
      /*update*/
      minDegree=updateMinDegree(minDegree);

      while(minDegree==1){
        c_indices=peripheralCoarsingIndices();
        Matching=updateMatching(c_indices);
        coarseLaplacian(c_indices);
        ID++;
        Level.set(ID,vList,Matching,TopMatch,Adj);
        LevelList.push_back (Level);
        minDegree=updateMinDegree(minDegree);
        TopMatch=updateTopMatch(minDegree);
      }
      /*
      while(minDegree<CB){
        c_indices=coarsingIndices(minDegree, false);
        Matching=updateMatching(c_indices);
        coarseLaplacian(c_indices);
        ID++;
        Level.set(ID,vList,Matching,TopMatch,Adj);
        LevelList.push_back (Level);
        minDegree=updateMinDegree(minDegree);
        TopMatch=updateTopMatch(minDegree);
      }
      */
    }
    /***************************************************************************/
    std::vector<std::vector<node>> CurrentFlowGroupCloseness::updateTopMatch(count minDegree){
      bool search;
      count j;

      node v;
      node w;
      std::vector<std::vector<node>> update_List;
      update_List.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        v=vList[i];
        if(L(i,i)==minDegree){
          search=true;
          j=0;
          while( (j<vList.size())&&(search)){
            if((L(i,j)!=0) && (i!=j)){
              w=vList[j];
              update_List[v].push_back(w);
              if(update_List[v].size()==minDegree){
                search=false;
              }
            }
            j++;
          }
        }
      }
      return update_List;
    }
    /***************************************************************************/
    count CurrentFlowGroupCloseness::updateMinDegree(count minDegree){
      bool search;
      count min;
      count i;

      search=true;
      min=n;
      i=0;

      while((i<L.n_rows)&&(search)){
        if(L(i,i)<min){
          min=L(i,i);
          if(min==minDegree){
            search=false;
          }
        }
        i++;
      }
      if(min==0){
        std::cout<<"NETWORK_ERROR: min= "<< min << "\n\n";
      }
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==0){
          std::cout<<"ERROR\n";
        }
      }
      return min;
    }
    /***************************************************************************/
    std::vector<std::pair<node,node>> CurrentFlowGroupCloseness::updateMatching(std::vector<std::pair<count,count>> indices){
      std::vector<std::pair<node,node>> Matching;
      Matching.resize(indices.size());
        for(count i=0;i<Matching.size();i++){
          Matching[i].first=vList[indices[i].first];
          Matching[i].second=vList[indices[i].second];
        }
      return Matching;
    }
    /***************************************************************************/
    std::vector<std::pair<count,count>> CurrentFlowGroupCloseness::peripheralCoarsingIndices(){
      count v;
      count s;

      std::vector<std::pair<count,count>> indices;
      std::vector<count> reverse;
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        reverse[vList[i]]=i;
      }
      indices.resize(0);
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==1){
          v=vList[i];
          s=reverse[TopMatch[v][0]];
          indices.push_back(std::make_pair(i,s));
        }
      }
      return indices;
    }
    /***************************************************************************/
    std::vector<std::pair<count,count>> CurrentFlowGroupCloseness::coarsingIndices(count cDegree, bool Random){
        bool s_found;

        count c_index;
        count s_index;
        count l;

        node c;
        /*free supernodes*/
        std::vector<bool> s_List;
        /*potential candidates*/
        std::vector<count> c_List;
        /*c_index-s_index mapping*/
        std::vector<count> reverse;
        reverse.resize(G.upperNodeIdBound());
        for(count i=0;i<vList.size();i++){
          reverse[vList[i]]=i;
        }
        std::vector<std::pair<count,count>> indices;

        c_List.resize(0);
        s_List.resize(G.upperNodeIdBound(),true);
        /*potential candidates*/
        for(count i=0;i<L.n_rows;i++){
          if(L(i,i)==cDegree){
            c_List.push_back(i);
          }
        }
        if(Random){
          std::random_shuffle (c_List.begin(), c_List.end());
        }
        for(count i=0;i<c_List.size();i++){
          c_index=c_List[i];
          c=vList[c_index];
          if(s_List[c_index]){
            s_found=false;
            l=0;
            while(!(s_found)&&(k<TopMatch[c].size())){
              s_index=reverse[TopMatch[c][l]];
              if(s_List[s_index]){
                s_List[s_index]=false;
                s_found=true;
                indices.push_back(std::make_pair(c_index,s_index));
              }
              else{
                l++;
              }
            }
          }
        }
        return indices;
      }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::uncoarse(node s,node v,count ID){
      node w;
      std::vector<node> v_TopMatch;
      arma::vec v_Adj;
      arma::vec s_Adj;

      v_TopMatch = LevelList[ID].get_vecofTopmatch(v);
      v_Adj = LevelList[ID-1].get_CalofAdj(v);
      s_Adj = LevelList[ID-1].get_CalofAdj(s);
      ERD.firstJoin(vList,s,v,1.);
      vList.push_back (v);
      LevelList[ID].set_i_j_ofAdj(s,v,s_Adj(v));

      for(count i=1;i<v_TopMatch.size();i++){
        w=v_TopMatch[i];
        if(v_Adj(w)!=0){
          if(s_Adj(w)!=0){
            ERD.edgeFire(vList,v,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,v_Adj(w));
            LevelList[ID].set_i_j_ofAdj(v,w,s_Adj(w)-v_Adj(w));
          }
          else{
            ERD.edgeFire(vList,v,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,v_Adj(w));
            ERD.nonBridgeDelete(vList,s,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,0.);
          }
        }
      }
    }
    /***************************************************************************/
    count CurrentFlowGroupCloseness::mergePeripheralNodes(){
      node s;
      node v;
      std::vector<std::pair<node,node>> merge;
      std::vector<std::pair<node,node>> Matching;
      std::vector<count> merge_value;

      Matching=LevelList[1].get_Matching();
      merge_value.resize(G.upperNodeIdBound(),0);
      for(count i=0;i<Matching.size();i++){
        v=Matching[i].first;
        s=Matching[i].second;
        merge_value[s]++;
        if(merge_value[s]==1){
          merge.push_back (std::make_pair(v,s));
        }
      }
      for(count i=0;i<merge.size();i++){
        v=merge[i].first;
        s=merge[i].second;
        ERD.firstJoin(vList,s,v,merge_value[s]);
        LevelList[1].set_i_j_ofAdj(s,v,merge_value[s]);
        vList.push_back (v);
      }
      return merge.size();
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseLaplacian(std::vector<std::pair<count,count>> Matchings){
      count a;
      count l;
      /*s=supernode - c = to be coarsed node - w = adjacent node to c*/
      node s;
      node c;
      node w;

      std::vector<count> reverse;
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        reverse[vList[i]]=i;
      }
      for(count i=0;i<Matchings.size();i++){
        c=Matchings[i].first;
        s=Matchings[i].second;
        L(s,s)-=1;
        for(count j=1;j<TopMatch[c].size();j++){
          w=reverse[TopMatch[c][j]];
          if(L(s,w)==0){
            L(s,w)=-1.;
            L(w,s)=-1.;
            /*Diagonal*/
            L(s,s)+=1.;
          }
          Adj(vList[s],vList[w])+=Adj(vList[c],vList[w]);
          Adj(vList[w],vList[s])=Adj(vList[s],vList[w]);
        }
        Adj(vList[s],vList[c])=0;
        Adj(vList[c],vList[s])=0;
      }
      arma::uvec indices(vList.size()-Matchings.size());
      a=0;
      l=0;
      for(count i=0;i<vList.size();i++){
        if(Matchings[l].first==i){
          l++;
        }
        else{
          indices(a)=i;
          a++;
        }
      }
      for(count i=0;i<Matchings.size();i++){
        vList.erase(vList.begin()+Matchings[i].first-i);
      }
      L=L.submat(indices, indices);

    }
} /* namespace NetworKit*/


// Boost.Geometry

// Copyright (c) 2018-2019 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP
#define BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/config.hpp>
#include <boost/geometry/geometries/infinite_line.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace arithmetic
{

// Calculates intersection point of two infinite lines.
// Returns true if the lines intersect.
// Returns false if lines are parallel (or collinear, possibly opposite)
template <typename Point, typename Type>
inline bool intersection_point(model::infinite_line<Type> const& p,
    model::infinite_line<Type> const& q, Point& ip)
{
    Type const denominator = p.b * q.a - p.a * q.b;

    static Type const zero = 0;
    if (math::equals(denominator, zero))
    {
        // Lines are parallel
        return false;
    }

    // Calculate the intersection coordinates
    geometry::set<0>(ip, (p.c * q.b - p.b * q.c) / denominator);
    geometry::set<1>(ip, (p.a * q.c - p.c * q.a) / denominator);

    return true;
}

//! Return a distance-side-measure for a point to a line
//! Point is located left of the line if value is positive,
//! right of the line is value is negative, and on the line if the value
//! is exactly zero
template <typename Type, typename CoordinateType>
inline
typename select_most_precise<Type, CoordinateType>::type
side_value(model::infinite_line<Type> const& line,
    CoordinateType const& x, CoordinateType const& y)
{
    // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_an_equation
    // Distance from point to line in general form is given as:
    // (a * x + b * y + c) / sqrt(a * a + b * b);
    // In most use cases comparisons are enough, saving the sqrt
    // and often even the division.
    // Also, this gives positive values for points left to the line,
    // and negative values for points right to the line.
    return line.a * x + line.b * y + line.c;
}

template <typename Type, typename Point>
inline
typename select_most_precise
<
    Type,
    typename geometry::coordinate_type<Point>::type
>::type
side_value(model::infinite_line<Type> const& line, Point const& p)
{
    return side_value(line, geometry::get<0>(p), geometry::get<1>(p));
}

// Returns true for two lines which are supposed to be (close to) collinear
// (which is not checked) and have a similar direction
// (in practice up to 45 degrees, TO BE VERIFIED)
// true: -----------------> p -----------------> q
// false: -----------------> p <----------------- q
template <typename Type>
inline
bool similar_direction(const model::infinite_line<Type>& p,
                       const model::infinite_line<Type>& q)
{
    return p.a * q.a >= 0 && p.b * q.b >= 0;
}

template <typename Type>
inline bool is_degenerate(const model::infinite_line<Type>& line)
{
    static Type const zero = 0;
    return math::equals(line.a, zero) && math::equals(line.b, zero);
}

} // namespace arithmetic

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP


//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <eve/function/cyl_bessel_jn.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include "producers.hpp"
#include <cmath>

TTS_CASE_TPL("wide random check on cyl_bessel_jn", EVE_TYPE)
{
  EVE_VALUE n;
  eve::uniform_prng<EVE_VALUE> p(0, 12);

  for(EVE_VALUE n=0; n < EVE_VALUE(5) ; n+= 0.25)
  {
    auto bjn = [n](auto x) -> EVE_VALUE {return boost::math::cyl_bessel_j(n, x); };
    auto  jn = [n](auto x) -> EVE_VALUE {return eve::cyl_bessel_jn(n, x); };
    TTS_RANGE_CHECK(p, bjn, jn);
  }
}


#pragma once

#include <random>
#include <vector>

#if USE_XSIMD
#include <xsimd/xsimd.hpp>
using vec_type = std::vector<double, XSIMD_DEFAULT_ALLOCATOR(double)>;
#elif USE_EIGEN
#include <Eigen/Dense>
using vec_type = std::vector<double, Eigen::aligned_allocator<double>>;
#else
using vec_type = std::vector<double>;
#endif

std::vector<vec_type> generate_signal(size_t n_samples,
    size_t in_size)
{
    std::vector<vec_type> signal(n_samples);
    for(auto& x : signal)
        x.resize(in_size, 0.0);

    std::default_random_engine generator;
    std::uniform_real_distribution<double> distribution(-1.0, 1.0);

    for(size_t i = 0; i < n_samples; ++i)
        for(size_t k = 0; k < in_size; ++k)
            signal[i][k] = distribution(generator);

    return std::move(signal);
}


#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <stdexcept>
#include <vector>

class NameValuePair {
  public:
/**
 * @param fname File containing data.
 */
    NameValuePair(const std::string& fname) {
      std::string line;
// open file for reading
      std::ifstream file(fname.c_str());
      if (!file)
        throw std::runtime_error("NameValuePair::NameValuePair: Unable to open input file");
// loop over each line in file
      while (std::getline(file, line)) {
        unsigned commentLoc = line.find('#');
        std::string myLine = line.substr(0, commentLoc);
        if (myLine != "") {
// only work on non-blank line: horribly ugly, but works
          unsigned eqLoc = myLine.find('=');
          std::string lhs, lhsStr, valueStr;
          lhsStr = myLine.substr(0, eqLoc);
          valueStr = myLine.substr(eqLoc+1, std::string::npos);
          std::istringstream lhsToken(lhsStr);
          lhsToken >> lhs;

          double value;
          std::istringstream tokens(valueStr);
          tokens >> value;
// insert it into map
          nvmap[lhs] = value;
        }
      }
    }

/**
 * @param name Name to lookup
 * @return value associated with name
 */
    double getValue(const std::string& name) const {
      std::map<std::string, double>::const_iterator itr =
        nvmap.find(name);
      if (itr != nvmap.end())
        return itr->second;
      throw std::runtime_error("NameValuePair::getValue: not found");
    }

/**
 * @param name Name to lookup
 * @return true if it exists, false otherwise
 */
    bool hasValue(const std::string& name) const {
      std::map<std::string, double>::const_iterator itr =
        nvmap.find(name);
      if (itr != nvmap.end())
        return true;
      return false;
    }

  private:
    std::map<std::string, double> nvmap;
};

/** Store sim data */
struct QuadData {
    int NDIM; /* Number of dimensions */
    int Np, Nq; /* Number of basis, Number of quadrature points */
    void show() {
      std::cout << "Np: " << Np << " Nq: " << Nq << std::endl;
    }
};

int cost(const QuadData& qd)
{
  int np = qd.Np, nq = qd.Nq, NDIM = qd.NDIM;
  return np*nq + NDIM*(2*np*nq+nq);
}

void
vol(int nloop, const QuadData& qd)
{
  int np = qd.Np, nq = qd.Nq;
  int NDIM = qd.NDIM;

  Eigen::VectorXd f(np), alpha(np), result(np);
  Eigen::VectorXd fQuad(nq), alphaQuad(nq);
  Eigen::MatrixXd interpMatrix(nq, np);
  Eigen::MatrixXd bigMatrix(np, nq);

  f = Eigen::VectorXd::Random(np);
  alpha = Eigen::VectorXd::Random(np);
  interpMatrix = Eigen::MatrixXd::Random(nq, np);
  bigMatrix = Eigen::MatrixXd::Random(np, nq);

  for (unsigned n=0; n<nloop; ++n)
  {
// interpolate to quadrature nodes
    fQuad.noalias() = interpMatrix*f; // Np*Nq (do this only once)
    for (unsigned d=0; d<NDIM; ++d)
    {
      alphaQuad.noalias() = interpMatrix*alpha; // Np*Nq
      alphaQuad.cwiseProduct(fQuad); // Nq
// compute updated solution
      result.noalias() = bigMatrix*fQuad; // Np*Nq
    }
  }
}

double run(int nloop, const QuadData& qd)
{
  clock_t t1 = clock();
  vol(nloop, qd);
  clock_t t2 = clock();
  return (double) (t2-t1)/CLOCKS_PER_SEC;
}

void printInfo(const QuadData& qd, double tm)
{
  std::cout << qd.NDIM << " " << qd.Np << " " << qd.Nq << " " << tm << std::endl;
}

void
runSimulation(const NameValuePair& nvpair)
{
  QuadData forceVolQuad, streamVolQuad;
  forceVolQuad.Np = nvpair.getValue("NpVolForce");
  forceVolQuad.Nq = nvpair.getValue("NqVolForce");
  forceVolQuad.NDIM = nvpair.getValue("VDIM");

  streamVolQuad.Np = nvpair.getValue("NpVolStream");
  streamVolQuad.Nq = nvpair.getValue("NqVolStream");
  streamVolQuad.NDIM = nvpair.getValue("CDIM");

  int nloop = nvpair.getValue("nloop");

  std::cout << std::endl;
  std::cout << "# Nloop  " << nloop << std::endl;

  std::cout << "# NDIM | Basis | Quadrature | Time (Force terms) " << std::endl;
// force terms
  double tForce = run(nloop, forceVolQuad);
  printInfo(forceVolQuad, tForce);

  std::cout << "# NDIM | Basis | Quadrature | Time (Streaming terms) " << std::endl;
// stream terms
  double tStream = run(nloop, streamVolQuad);
  printInfo(streamVolQuad, tStream);

// stats
  std::cout << std::endl;
  std::cout << "# Total time | Time per DOF " << std::endl;
  double tm = (tForce+tStream)/nloop;
  double tmPerDof = tm/forceVolQuad.Np;
  std::cout << tm << " " << tmPerDof << std::endl;

// cost
  std::cout << std::endl;
  std::cout << "# Theoretical Cost (Arb. Units)" << std::endl;
  std::cout << "Force vol. terms " << cost(forceVolQuad) << std::endl;
  std::cout << "Streaming vol. terms " << cost(streamVolQuad) << std::endl;
  std::cout << "Net: " << cost(forceVolQuad) + cost(streamVolQuad)  << std::endl;

  std::cout << std::endl;
}

int
main (int argc, char **argv)
{
  if (argc != 2) {
    std::cout << "Usage::" << std::endl;
    std::cout << " dg-int <input-file>" << std::endl;
    std::cout << "  input-file: Name of input file" << std::endl;

    exit(1);
  }
  std::string inFile(argv[1]); // input file
  NameValuePair nvpair(inFile);
  runSimulation(nvpair);

  return 0;
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Iterative solvers in ViennaCL (iterative.cpp and iterative.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// Necessary to obtain a suitable performance in ublas
#ifndef NDEBUG
 #define NDEBUG
#endif

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/jacobi_precond.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

using namespace boost::numeric;

int main()
{
  typedef float       ScalarType;

  //
  // Set up some ublas objects
  //
  ublas::vector<ScalarType> rhs;
  ublas::vector<ScalarType> rhs2;
  ublas::vector<ScalarType> ref_result;
  ublas::vector<ScalarType> result;
  ublas::compressed_matrix<ScalarType> ublas_matrix;

  //
  // Read system from file
  //
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //std::cout << "done reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }
  //std::cout << "done reading rhs" << std::endl;

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }
  //std::cout << "done reading result" << std::endl;

  //
  // Set up some ViennaCL objects
  //
  std::size_t vcl_size = rhs.size();
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix;
  viennacl::coordinate_matrix<ScalarType> vcl_coordinate_matrix;
  viennacl::vector<ScalarType> vcl_rhs(vcl_size);
  viennacl::vector<ScalarType> vcl_result(vcl_size);
  viennacl::vector<ScalarType> vcl_ref_result(vcl_size);

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  viennacl::copy(ref_result.begin(), ref_result.end(), vcl_ref_result.begin());

  //
  // Transfer ublas-matrix to GPU:
  //
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);

  //
  // alternative way: via STL. Sparse matrix as std::vector< std::map< unsigned int, ScalarType> >
  //
  std::vector< std::map< unsigned int, ScalarType> > stl_matrix(rhs.size());
  for (ublas::compressed_matrix<ScalarType>::iterator1 iter1 = ublas_matrix.begin1();
       iter1 != ublas_matrix.end1();
       ++iter1)
  {
    for (ublas::compressed_matrix<ScalarType>::iterator2 iter2 = iter1.begin();
         iter2 != iter1.end();
         ++iter2)
         stl_matrix[iter2.index1()][static_cast<unsigned int>(iter2.index2())] = *iter2;
  }
  viennacl::copy(stl_matrix, vcl_coordinate_matrix);
  viennacl::copy(vcl_coordinate_matrix, stl_matrix);

  //
  // set up ILUT preconditioners for ublas and ViennaCL objects:
  //
  std::cout << "Setting up preconditioners for uBLAS-matrix..." << std::endl;
  viennacl::linalg::ilut_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          ublas_block_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());

  std::cout << "Setting up preconditioners for ViennaCL-matrix..." << std::endl;
  viennacl::linalg::ilut_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilut(vcl_compressed_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< viennacl::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          vcl_block_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());

  //
  // set up Jacobi preconditioners for ViennaCL and ublas objects:
  //
  viennacl::linalg::jacobi_precond< ublas::compressed_matrix<ScalarType> >    ublas_jacobi(ublas_matrix, viennacl::linalg::jacobi_tag());
  viennacl::linalg::jacobi_precond< viennacl::compressed_matrix<ScalarType> > vcl_jacobi(vcl_compressed_matrix, viennacl::linalg::jacobi_tag());

  //
  // Conjugate gradient solver:
  //
  std::cout << "----- CG Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag());
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilut);
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_jacobi);

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag());
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag(1e-6, 20), vcl_ilut);
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag(1e-6, 20), vcl_jacobi);

  //
  // Stabilized BiConjugate gradient solver:
  //
  std::cout << "----- BiCGStab Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag());          //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilut); //with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_jacobi); //with preconditioner

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag());   //without preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), vcl_ilut); //with preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), vcl_jacobi); //with preconditioner

  //
  // GMRES solver:
  //
  std::cout << "----- GMRES Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilut);//with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_jacobi);//with preconditioner

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag(1e-6, 20), vcl_ilut);//with preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag(1e-6, 20), vcl_jacobi);//with preconditioner

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return 0;
}


#include <string>

#include <ros/ros.h>
#include <geometry_msgs/Quaternion.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

namespace
{

constexpr auto pi {3.141592653589793};

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "body_angle_visualizer");
  ros::NodeHandle n {};

  int target_number {1};
  std::string root_name {"openni_coordinater"};
  std::string to_name {"head"};
  std::string from_name {"torso"};
  {
    ros::NodeHandle pn {"~"};
    pn.getParam("target_number", target_number);
    pn.getParam("root", root_name);
    pn.getParam("to", to_name);
    pn.getParam("from", from_name);
  }

  const auto to_frame_name {to_name + '_' + std::to_string(target_number)};
  const auto from_frame_name {from_name + '_' + std::to_string(target_number)};

  ros::Publisher pub {n.advertise<geometry_msgs::Quaternion>("body_direction", 1)};
  ros::Rate r {5};
  tf2_ros::Buffer tfBuffer {};
  tf2_ros::TransformListener tfListener {tfBuffer};
  rviz_visual_tools::RvizVisualTools rvt {root_name, "rviz_visual_markers"};

  while (ros::ok()) {
    try {
      const auto to_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, to_frame_name, ros::Time{0}))};
      const auto from_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, from_frame_name, ros::Time{0}))};
      auto copy {from_pos};
      copy.translation() = Eigen::Vector3d::Zero();
      const auto from_ypr {copy.rotation().eulerAngles(2, 0, 1)};
      ROS_INFO("yaw pitch roll : %f %f %f", from_ypr(2), from_ypr(1), from_ypr(0));
      constexpr auto trim_half_rotation {[](double angle) {
        if (angle < -pi / 2)
          return angle + pi;
        if (angle > pi / 2)
          return angle - pi;
        return angle;
      }};
      const auto roll_angle {trim_half_rotation(from_ypr(0))};
      constexpr auto invert_half_rotation {[](double angle) {
        if (angle < -pi / 2)
          return -angle - pi;
        if (angle > pi / 2)
          return -angle + pi;
        return angle;
      }};
      const auto pitch_angle {invert_half_rotation(from_ypr(1))};
      //const auto stand_vec {to_pos.translation() - from_pos.translation()};
      //const auto stand_vec {from_pos * Eigen::Vector3d::UnitX()};
      const auto stand_vec {copy * Eigen::Vector3d::UnitY()};
      //const auto stand_quaternion {Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), stand_vec)};
      const auto stand_quaternion {copy.rotation()};

      pub.publish(tf2::toMsg(Eigen::Quaterniond{from_pos.rotation()}));

      rvt.deleteAllMarkers();
      rvt.publishArrow(Eigen::Affine3d{stand_quaternion}, rviz_visual_tools::BLUE, rviz_visual_tools::LARGE);

      Eigen::Affine3d text_pos {};
      text_pos.translation() = stand_vec.normalized() * .5;
      rvt.publishText(text_pos, std::to_string(roll_angle * 180 / pi), rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE, false);
      text_pos.translation() *= 1.1;
      rvt.publishText(text_pos, std::to_string(pitch_angle * 180 / pi), rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE, false);

      rvt.trigger();
    } catch (tf2::TransformException &e) {
      ROS_WARN("%s", e.what());
    }

    r.sleep();
  }
}