// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/math/find_polynomial_roots_jenkins_traub.h"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>

#include <cmath>
#include <complex>
#include <limits>
#include <vector>

#include "theia/math/polynomial.h"
#include "theia/math/util.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::VectorXd;
using Eigen::Vector3cd;
using Eigen::VectorXcd;

namespace {
// Machine precision constants.
static const double mult_eps = std::numeric_limits<double>::epsilon();
static const double sum_eps = std::numeric_limits<double>::epsilon();
static const double kAbsoluteTolerance = 1e-14;
static const double kRelativeTolerance = 1e-10;

enum class ConvergenceType {
  NO_CONVERGENCE = 0,
  LINEAR_CONVERGENCE = 1,
  QUADRATIC_CONVERGENCE = 2
};

// Perform division by a linear term of the form (z - x) and evaluate P at x.
void SyntheticDivisionAndEvaluate(const VectorXd& polynomial,
                                  const double x,
                                  VectorXd* quotient,
                                  double* eval) {
  quotient->setZero(polynomial.size() - 1);
  (*quotient)(0) = polynomial(0);
  for (int i = 1; i < polynomial.size() - 1; i++) {
    (*quotient)(i) = polynomial(i) + (*quotient)(i - 1) * x;
  }

  const VectorXd::ReverseReturnType& creverse_quotient = quotient->reverse();
  *eval = polynomial.reverse()(0) + creverse_quotient(0) * x;
}

// Perform division of a polynomial by a quadratic factor. The quadratic divisor
// should have leading 1s.
void QuadraticSyntheticDivision(const VectorXd& polynomial,
                                const VectorXd& quadratic_divisor,
                                VectorXd* quotient,
                                VectorXd* remainder) {
  CHECK_EQ(quadratic_divisor.size(), 3);
  CHECK_GE(polynomial.size(), 3);

  quotient->setZero(polynomial.size() - 2);
  remainder->setZero(2);

  (*quotient)(0) = polynomial(0);
  // If the quotient is a constant then polynomial is degree 2 and the math is
  // simple.
  if (quotient->size() == 1) {
    *remainder =
        polynomial.tail<2>() - polynomial(0) * quadratic_divisor.tail<2>();
    return;
  }

  (*quotient)(1) = polynomial(1) - polynomial(0) * quadratic_divisor(1);
  for (int i = 2; i < polynomial.size() - 2; i++) {
    (*quotient)(i) = polynomial(i) - (*quotient)(i - 2) * quadratic_divisor(2) -
        (*quotient)(i - 1) * quadratic_divisor(1);
  }

  const VectorXd::ReverseReturnType &creverse_quotient = quotient->reverse();
  (*remainder)(0) = polynomial.reverse()(1) -
                    quadratic_divisor(1) * creverse_quotient(0) -
                    quadratic_divisor(2) * creverse_quotient(1);
  (*remainder)(1) =
      polynomial.reverse()(0) - quadratic_divisor(2) * creverse_quotient(0);
}

// Determines whether the iteration has converged by examining the three most
// recent values for convergence.
template<typename T>
bool HasConverged(const T& sequence) {
  const bool convergence_condition_1 =
      std::abs(sequence(1) - sequence(0)) < std::abs(sequence(0)) / 2.0;
  const bool convergence_condition_2 =
      std::abs(sequence(2) - sequence(1)) < std::abs(sequence(1)) / 2.0;

  // If the sequence has converged then return true.
  return convergence_condition_1 && convergence_condition_2;
}

// Determines if the root has converged by measuring the relative and absolute
// change in the root value. This stopping criterion is a simple measurement
// that proves to work well. It is referred to as "Ward's method" in the
// following reference:
//
// Nikolajsen, Jorgen L. "New stopping criteria for iterative root finding."
// Royal Society open science (2014)
template <typename T>
bool HasRootConverged(const std::vector<T>& roots) {
  static const double kRootMagnitudeTolerance = 1e-8;

  if (roots.size() != 3) {
    return false;
  }

  const double e_i = std::abs(roots[2] - roots[1]);
  const double e_i_minus_1 = std::abs(roots[1] - roots[0]);
  const double mag_root = std::abs(roots[1]);
  if (e_i <= e_i_minus_1) {
    if (mag_root < kRootMagnitudeTolerance) {
      return e_i < kAbsoluteTolerance;
    } else {
      return e_i / mag_root <= kRelativeTolerance;
    }
  }

  return false;
}

// Implementation closely follows the three-stage algorithm for finding roots of
// polynomials with real coefficients as outlined in: "A Three-Stage Algorithm
// for Real Polynomaials Using Quadratic Iteration" by Jenkins and Traub, SIAM
// 1970. Please note that this variant is different than the complex-coefficient
// version, and is estimated to be up to 4 times faster.
class JenkinsTraubSolver {
 public:
  JenkinsTraubSolver(const VectorXd& coeffs,
                     VectorXd* real_roots,
                     VectorXd* complex_roots)
      : polynomial_(coeffs),
        real_roots_(real_roots),
        complex_roots_(complex_roots),
        num_solved_roots_(0) {}

  // Extracts the roots using the Jenkins Traub method.
  bool ExtractRoots();

 private:
  // Removes any zero roots and divides polynomial by z.
  void RemoveZeroRoots();

  // Computes the magnitude of the roots to provide and initial search radius
  // for the iterative solver.
  double ComputeRootRadius();

  // Computes the zero-shift applied to the K-Polynomial.
  void ComputeZeroShiftKPolynomial();

  // Stage 1 of the Jenkins-Traub method. This stage is not technically
  // necessary, but helps separate roots that are close to zero.
  void ApplyZeroShiftToKPolynomial(const int num_iterations);

  // Computes and returns the update of sigma(z) based on the current
  // K-polynomial.
  //
  // NOTE: This function is used by the fixed shift iterations (which hold sigma
  // constant) so sigma is *not* modified internally by this function. If you
  // want to change sigma, simply call
  //    sigma = ComputeNextSigma();
  VectorXd ComputeNextSigma();

  // Updates the K-polynomial based on the current value of sigma for the fixed
  // or variable shift stage.
  void UpdateKPolynomialWithQuadraticShift(
      const VectorXd& polynomial_quotient,
      const VectorXd& k_polynomial_quotient);

  // Apply fixed-shift iterations to the K-polynomial to separate the
  // roots. Based on the convergence of the K-polynomial, we apply a
  // variable-shift linear or quadratic iteration to determine a real root or
  // complex conjugate pair of roots respectively.
  ConvergenceType ApplyFixedShiftToKPolynomial(const std::complex<double>& root,
                                               const int max_iterations);

  // Applies one of the variable shifts to the K-Polynomial. Returns true upon
  // successful convergence to a good root, and false otherwise.
  bool ApplyVariableShiftToKPolynomial(
      const ConvergenceType& fixed_shift_convergence,
      const std::complex<double>& root);

  // Applies a quadratic shift to the K-polynomial to determine a pair of roots
  // that are complex conjugates. Return true if a root was successfully found.
  bool ApplyQuadraticShiftToKPolynomial(const std::complex<double>& root,
                                        const int max_iterations);

  // Applies a linear shift to the K-polynomial to determine a single real root.
  // Return true if a root was successfully found.
  bool ApplyLinearShiftToKPolynomial(const std::complex<double>& root,
                                     const int max_iterations);

  // These methods determine whether the root finding has converged based on the
  // machine roundoff error expected in evaluating the polynomials at the root.
  bool HasQuadraticSequenceConverged(const VectorXd& quotient,
                                     const std::complex<double>& root);
  bool HasLinearSequenceConverged(const VectorXd& quotient,
                                  const double root,
                                  const double p_at_root);

  // Adds the root to the output variables.
  void AddRootToOutput(const double real, const double imag);

  // Solves polynomials of degree <= 2.
  bool SolveClosedFormPolynomial();

  // Helper variables to manage the polynomials as they are being manipulated
  // and deflated.
  VectorXd polynomial_;
  VectorXd k_polynomial_;
  // Sigma is the quadratic factor the divides the K-polynomial.
  Vector3d sigma_;

  // Let us define a, b, c, and d such that:
  //   P(z) = Q_P * sigma(z) + b * (z + u) + a
  //   K(z) = Q_K * sigma(z) + d * (z + u ) + c
  //
  // where Q_P and Q_K are the quotients from polynomial division of
  // sigma(z). Note that this means for a given a root s of sigma:
  //
  //   P(s)      = a - b * s_conj
  //   P(s_conj) = a - b * s
  //   K(s)      = c - d * s_conj
  //   K(s_conj) = c - d * s
  double a_, b_, c_, d_;

  // Output reference variables.
  VectorXd* real_roots_;
  VectorXd* complex_roots_;
  int num_solved_roots_;

  // Keeps track of whether the linear and quadratic shifts have been attempted
  // yet so that we do not attempt the same shift twice.
  bool attempted_linear_shift_;
  bool attempted_quadratic_shift_;

  // Number of zero-shift iterations to perform.
  static const int kNumZeroShiftIterations = 20;

  // The number of fixed shift iterations is computed as
  //   # roots found * this multiplier.
  static const int kFixedShiftIterationMultiplier = 20;

  // If the fixed shift iterations fail to converge, we restart this many times
  // before considering the solve attempt as a failure.
  static const int kMaxFixedShiftRestarts = 20;

  // The maximum number of linear shift iterations to perform before considering
  // the shift as a failure.
  static const int kMaxLinearShiftIterations = 20;

  // The maximum number of quadratic shift iterations to perform before
  // considering the shift as a failure.
  static const int kMaxQuadraticShiftIterations = 20;

  // When quadratic shift iterations are stalling, we attempt a few fixed shift
  // iterations to help convergence.
  static const int kInnerFixedShiftIterations = 5;

  // During quadratic iterations, the real values of the root pairs should be
  // nearly equal since the root pairs are complex conjugates. This tolerance
  // measures how much the real values may diverge before consider the quadratic
  // shift to be failed.
  const double kRootPairTolerance = 0.01;
};

bool JenkinsTraubSolver::ExtractRoots() {
  if (polynomial_.size() == 0) {
    LOG(ERROR) << "Invalid polynomial of size 0 passed to "
                  "FindPolynomialRootsJenkinsTraub";
    return false;
  }

  // Remove any leading zeros of the polynomial.
  polynomial_ = RemoveLeadingZeros(polynomial_);

  const int degree = static_cast<int>(polynomial_.size()) - 1;

  // Allocate the output roots.
  if (real_roots_ != NULL) {
    real_roots_->setZero(degree);
  }
  if (complex_roots_ != NULL) {
    complex_roots_->setZero(degree);
  }

  // Normalize the polynomial.
  polynomial_ /= polynomial_(0);

  // Remove any zero roots.
  RemoveZeroRoots();

  // Choose the initial starting value for the root-finding on the complex
  // plane.
  double phi = DegToRad(49.0);

  // Iterate until the polynomial has been completely deflated.
  for (int i = 0; i < degree; i++) {
    // Compute the root radius.
    const double root_radius = ComputeRootRadius();

    // Solve in closed form if the polynomial is small enough.
    if (polynomial_.size() <= 3) {
      break;
    }

    // Stage 1: Apply zero-shifts to the K-polynomial to separate the small
    // zeros of the polynomial.
    ApplyZeroShiftToKPolynomial(kNumZeroShiftIterations);

    // Stage 2: Apply fixed shift iterations to the K-polynomial to separate the
    // roots further.
    std::complex<double> root;
    ConvergenceType convergence = ConvergenceType::NO_CONVERGENCE;
    for (int j = 0; j < kMaxFixedShiftRestarts; j++) {
      root = root_radius * std::complex<double>(std::cos(phi), std::sin(phi));
      convergence = ApplyFixedShiftToKPolynomial(
          root, kFixedShiftIterationMultiplier * (i + 1));

      if (convergence != ConvergenceType::NO_CONVERGENCE) {
        break;
      }

      // Rotate the initial root value on the complex plane and try again.
      phi += DegToRad(94.0);
    }

    // Stage 3: Find the root(s) with variable shift iterations on the
    // K-polynomial. If this stage was not successful then we return a failure.
    if (!ApplyVariableShiftToKPolynomial(convergence, root)) {
      return false;
    }
  }
  return SolveClosedFormPolynomial();
}

// Stage 1: Generate K-polynomials with no shifts (i.e. zero-shifts).
void JenkinsTraubSolver::ApplyZeroShiftToKPolynomial(
    const int num_iterations) {
  // K0 is the first order derivative of polynomial.
  k_polynomial_ = DifferentiatePolynomial(polynomial_) / polynomial_.size();
  for (int i = 1; i < num_iterations; i++) {
    ComputeZeroShiftKPolynomial();
  }
}

ConvergenceType JenkinsTraubSolver::ApplyFixedShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  // Compute the fixed-shift quadratic:
  // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
  sigma_(0) = 1.0;
  sigma_(1) = -2.0 * root.real();
  sigma_(2) = root.real() * root.real() + root.imag() * root.imag();

  // Compute the quotient and remainder for divinding P by the quadratic
  // divisor. Since this iteration involves a fixed-shift sigma these may be
  // computed once prior to any iterations.
  VectorXd polynomial_quotient, polynomial_remainder;
  QuadraticSyntheticDivision(
      polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);

  // Compute a and b from the above equations.
  b_ = polynomial_remainder(0);
  a_ = polynomial_remainder(1) - b_ * sigma_(1);

  // Precompute P(s) for later using the equation above.
  const std::complex<double> p_at_root = a_ - b_ * std::conj(root);

  // These two containers hold values that we test for convergence such that the
  // zero index is the convergence value from 2 iterations ago, the first
  // index is from one iteration ago, and the second index is the current value.
  Vector3cd t_lambda = Vector3cd::Zero();
  Vector3d sigma_lambda = Vector3d::Zero();
  VectorXd k_polynomial_quotient, k_polynomial_remainder;
  for (int i = 0; i < max_iterations; i++) {
    k_polynomial_ /= k_polynomial_(0);

    // Divide the shifted polynomial by the quadratic polynomial.
    QuadraticSyntheticDivision(
        k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
    d_ = k_polynomial_remainder(0);
    c_ = k_polynomial_remainder(1) - d_ * sigma_(1);

    // Test for convergence.
    const VectorXd variable_shift_sigma = ComputeNextSigma();
    const std::complex<double> k_at_root = c_ - d_ * std::conj(root);

    t_lambda.head<2>() = t_lambda.tail<2>().eval();
    sigma_lambda.head<2>() = sigma_lambda.tail<2>().eval();
    t_lambda(2) = root - p_at_root / k_at_root;
    sigma_lambda(2) = variable_shift_sigma(2);

    // Return with the convergence code if the sequence has converged.
    if (HasConverged(sigma_lambda)) {
      return ConvergenceType::QUADRATIC_CONVERGENCE;
    } else if (HasConverged(t_lambda)) {
      return ConvergenceType::LINEAR_CONVERGENCE;
    }

    // Compute K_next using the formula above.
    UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
                                        k_polynomial_quotient);
  }
  return ConvergenceType::NO_CONVERGENCE;
}

bool JenkinsTraubSolver::ApplyVariableShiftToKPolynomial(
    const ConvergenceType& fixed_shift_convergence,
    const std::complex<double>& root) {
  attempted_linear_shift_ = false;
  attempted_quadratic_shift_ = false;
  if (fixed_shift_convergence == ConvergenceType::LINEAR_CONVERGENCE) {
    return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
  } else if (fixed_shift_convergence ==
             ConvergenceType::QUADRATIC_CONVERGENCE) {
    return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
  }
  return false;
}

// Generate K-polynomials with variable-shifts. During variable shifts, the
// quadratic shift is computed as:
//                | K0(s1)  K0(s2)  z^2 |
//                | K1(s1)  K1(s2)    z |
//                | K2(s1)  K2(s2)    1 |
//    sigma(z) = __________________________
//                  | K1(s1)  K2(s1) |
//                  | K2(s1)  K2(s2) |
// Where K0, K1, and K2 are successive zero-shifts of the K-polynomial.
//
// The K-polynomial shifts are otherwise exactly the same as Stage 2 after
// accounting for a variable-shift sigma.
bool JenkinsTraubSolver::ApplyQuadraticShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  // Only proceed if we have not already tried a quadratic shift.
  if (attempted_quadratic_shift_) {
    return false;
  }

  const double kTinyRelativeStep = 0.01;

  // Compute the fixed-shift quadratic:
  // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
  sigma_(0) = 1.0;
  sigma_(1) = -2.0 * root.real();
  sigma_(2) = root.real() * root.real() + root.imag() * root.imag();

  // These two containers hold values that we test for convergence such that the
  // zero index is the convergence value from 2 iterations ago, the first
  // index is from one iteration ago, and the second index is the current value.
  VectorXd polynomial_quotient, polynomial_remainder, k_polynomial_quotient,
      k_polynomial_remainder;
  double poly_at_root(0), prev_poly_at_root(0), prev_v(0);
  bool tried_fixed_shifts = false;

  // These containers maintain a history of the predicted roots. The convergence
  // of the algorithm is determined by the convergence of the root value.
  std::vector<std::complex<double> > roots1, roots2;
  roots1.push_back(root);
  roots2.push_back(std::conj(root));
  for (int i = 0; i < max_iterations; i++) {
    // Terminate if the root evaluation is within our tolerance. This will
    // return false if we do not have enough samples.
    if (HasRootConverged(roots1) && HasRootConverged(roots2)) {
      AddRootToOutput(roots1[1].real(), roots1[1].imag());
      AddRootToOutput(roots2[1].real(), roots2[1].imag());
      polynomial_ = polynomial_quotient;
      return true;
    }

    QuadraticSyntheticDivision(
        polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);

    // Compute a and b from the above equations.
    b_ = polynomial_remainder(0);
    a_ = polynomial_remainder(1) - b_ * sigma_(1);

    // Solve for the roots of the quadratic factor sigma.
    std::complex<double> roots[2];
    VectorXd real, imag;
    FindQuadraticPolynomialRoots(sigma_, &real, &imag);
    roots[0] = std::complex<double>(real(0), imag(0));
    roots[1] = std::complex<double>(real(1), imag(1));

    // Check that the roots are close. If not, then try a linear shift.
    if (std::abs(std::abs(roots[0].real()) - std::abs(roots[1].real())) >
        kRootPairTolerance * std::abs(roots[1].real())) {
      return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
    }

    // If the iteration is stalling at a root pair then apply a few fixed shift
    // iterations to help convergence.
    poly_at_root =
        std::abs(a_ - roots[0].real() * b_) + std::abs(roots[0].imag() * b_);
    const double rel_step = std::abs((sigma_(2) - prev_v) / sigma_(2));
    if (!tried_fixed_shifts && rel_step < kTinyRelativeStep &&
        prev_poly_at_root > poly_at_root) {
      tried_fixed_shifts = true;
      ApplyFixedShiftToKPolynomial(roots[0], kInnerFixedShiftIterations);
    }

    // Divide the shifted polynomial by the quadratic polynomial.
    QuadraticSyntheticDivision(
        k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
    d_ = k_polynomial_remainder(0);
    c_ = k_polynomial_remainder(1) - d_ * sigma_(1);

    prev_v = sigma_(2);
    sigma_ = ComputeNextSigma();

    // Compute K_next using the formula above.
    UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
                                        k_polynomial_quotient);
    k_polynomial_ /= k_polynomial_(0);
    prev_poly_at_root = poly_at_root;

    // Save the roots for convergence testing.
    roots1.push_back(roots[0]);
    roots2.push_back(roots[1]);
    if (roots1.size() > 3) {
      roots1.erase(roots1.begin());
      roots2.erase(roots2.begin());
    }
  }

  attempted_quadratic_shift_ = true;
  return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
}

// Generate K-Polynomials with variable-shifts that are linear. The shift is
// computed as:
//   K_next(z) = 1 / (z - s) * (K(z) - K(s) / P(s) * P(z))
//   s_next = s - P(s) / K_next(s)
bool JenkinsTraubSolver::ApplyLinearShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  if (attempted_linear_shift_) {
    return false;
  }

  // Compute an initial guess for the root.
  double real_root = (root -
                      EvaluatePolynomial(polynomial_, root) /
                      EvaluatePolynomial(k_polynomial_, root)).real();

  VectorXd deflated_polynomial, deflated_k_polynomial;
  double polynomial_at_root(0), k_polynomial_at_root(0);

  // This container maintains a history of the predicted roots. The convergence
  // of the algorithm is determined by the convergence of the root value.
  std::vector<double> roots;
  roots.push_back(real_root);
  for (int i = 0; i < max_iterations; i++) {
    // Terminate if the root evaluation is within our tolerance. This will
    // return false if we do not have enough samples.
    if (HasRootConverged(roots)) {
      AddRootToOutput(roots[1], 0);
      polynomial_ = deflated_polynomial;
      return true;
    }

    const double prev_polynomial_at_root = polynomial_at_root;
    SyntheticDivisionAndEvaluate(
        polynomial_, real_root, &deflated_polynomial, &polynomial_at_root);

    // If the root is exactly the root then end early. Otherwise, the k
    // polynomial will be filled with inf or nans.
    if (std::abs(polynomial_at_root) <= kAbsoluteTolerance) {
      AddRootToOutput(roots[0], 0);
      polynomial_ = deflated_polynomial;
      return true;
    }

    // Update the K-Polynomial.
    SyntheticDivisionAndEvaluate(k_polynomial_, real_root,
                                 &deflated_k_polynomial, &k_polynomial_at_root);
    k_polynomial_ = AddPolynomials(
        deflated_k_polynomial,
        -k_polynomial_at_root / polynomial_at_root * deflated_polynomial);
    k_polynomial_ /= k_polynomial_(0);

    // Compute the update for the root estimation.
    k_polynomial_at_root = EvaluatePolynomial(k_polynomial_, real_root);
    const double delta_root = polynomial_at_root / k_polynomial_at_root;
    real_root -= delta_root;

    // Save the root so that convergence can be measured. Only the 3 most
    // recently root values are needed.
    roots.push_back(real_root);
    if (roots.size() > 3) {
      roots.erase(roots.begin());
    }

    // If the linear iterations appear to be stalling then we may have found a
    // double real root of the form (z - x^2). Attempt a quadratic variable
    // shift from the current estimate of the root.
    if (i >= 2 &&
        std::abs(delta_root) < 0.001 * std::abs(real_root) &&
        std::abs(prev_polynomial_at_root) < std::abs(polynomial_at_root)) {
      const std::complex<double> new_root(real_root, 0);
      return ApplyQuadraticShiftToKPolynomial(new_root,
                                              kMaxQuadraticShiftIterations);
    }
  }

  attempted_linear_shift_ = true;
  return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
}

bool JenkinsTraubSolver::HasQuadraticSequenceConverged(
    const VectorXd& quotient, const std::complex<double>& root) {
  const double z = std::sqrt(std::abs(sigma_(2)));
  const double t = -root.real() * b_;

  double e = 2.0 * std::abs(quotient(0));
  for (int i = 1; i < quotient.size(); i++) {
    e = e * z + std::abs(quotient(i));
  }
  e = e * z + std::abs(a_ + t);
  e *= 5.0 * mult_eps + 4.0 * sum_eps;
  e = e -
      (5.0 * mult_eps + 2.0 * sum_eps) * (std::abs(a_ + t) + std::abs(b_) * z);
  e = e + 2.0 * sum_eps * std::abs(t);
  return std::abs(a_ - b_ * root) < e;
}

bool JenkinsTraubSolver::HasLinearSequenceConverged(const VectorXd& quotient,
                                                    const double root,
                                                    const double p_at_root) {
  double e = std::abs(quotient(0)) * mult_eps / (sum_eps + mult_eps);
  const double abs_root = std::abs(root);
  for (int i = 0; i < quotient.size(); i++) {
    e = e * abs_root + std::abs(quotient(i));
  }
  const double machine_precision =
      (sum_eps + mult_eps) * e - mult_eps * std::abs(p_at_root);
  return std::abs(p_at_root) < machine_precision;
}

void JenkinsTraubSolver::AddRootToOutput(const double real, const double imag) {
  if (real_roots_ != NULL) {
    (*real_roots_)(num_solved_roots_) = real;
  }
  if (complex_roots_ != NULL) {
    (*complex_roots_)(num_solved_roots_) = imag;
  }
  ++num_solved_roots_;
}

void JenkinsTraubSolver::RemoveZeroRoots() {
  int num_zero_roots = 0;

  const VectorXd::ReverseReturnType& creverse_polynomial =
      polynomial_.reverse();
  while (creverse_polynomial(num_zero_roots) == 0) {
    ++num_zero_roots;
  }

  // The output roots have 0 as the default value so there is no need to
  // explicitly add the zero roots.
  polynomial_ = polynomial_.head(polynomial_.size() - num_zero_roots).eval();
}

bool JenkinsTraubSolver::SolveClosedFormPolynomial() {
  const int degree = static_cast<int>(polynomial_.size()) - 1;

  // Is the polynomial constant?
  if (degree == 0) {
    LOG(WARNING) << "Trying to extract roots from a constant "
                 << "polynomial in FindPolynomialRoots";
    // We return true with no roots, not false, as if the polynomial is constant
    // it is correct that there are no roots. It is not the case that they were
    // there, but that we have failed to extract them.
    return true;
  }

  // Linear
  if (degree == 1) {
    AddRootToOutput(-polynomial_(1) / polynomial_(0), 0);
    return true;
  }

  // Quadratic
  if (degree == 2) {
    VectorXd real, imaginary;
    FindQuadraticPolynomialRoots(polynomial_, &real, &imaginary);
    AddRootToOutput(real(0), imaginary(0));
    AddRootToOutput(real(1), imaginary(1));
    return true;
  }

  return false;
}

// Computes a lower bound on the radius of the roots of polynomial by examining
// the Cauchy sequence:
//
//    z^n + |a_1| * z^{n - 1} + ... + |a_{n-1}| * z - |a_n|
//
// The unique positive zero of this polynomial is an approximate lower bound of
// the radius of zeros of the original polynomial.
double JenkinsTraubSolver::ComputeRootRadius() {
  static const double kEpsilon = 1e-2;
  static const int kMaxIterations = 100;

  VectorXd poly = polynomial_;
  // Take the absolute value of all coefficients.
  poly = poly.array().abs();
  // Negate the last coefficient.
  poly(poly.size() - 1) *= -1.0;

  // Find the unique positive zero using Newton-Raphson iterations.
  double x0 = 1.0;
  return FindRootIterativeNewton(poly, x0, kEpsilon, kMaxIterations);
}

// The k polynomial with a zero-shift is
//  (K(x) - K(0) / P(0) * P(x)) / x.
//
// This is equivalent to:
//    K(x) - K(0)      K(0)     P(x) - P(0)
//    ___________   -  ____  *  ___________
//         x           P(0)          x
//
// Note that removing the constant term and dividing by x is equivalent to
// shifting the polynomial to one degree lower in our representation.
void JenkinsTraubSolver::ComputeZeroShiftKPolynomial() {
  // Evaluating the polynomial at zero is equivalent to the constant term
  // (i.e. the last coefficient).
  const double polynomial_at_zero = polynomial_(polynomial_.size() - 1);
  const double k_at_zero = k_polynomial_(k_polynomial_.size() - 1);

  k_polynomial_ = AddPolynomials(k_polynomial_.head(k_polynomial_.size() - 1),
                                 -k_at_zero / polynomial_at_zero *
                                 polynomial_.head(polynomial_.size() - 1));
}

// The iterations are computed with the following equation:
//              a^2 + u * a * b + v * b^2
//   K_next =  ___________________________ * Q_K
//                    b * c - a * d
//
//                      a * c + u * a * d + v * b * d
//             +  (z - _______________________________) * Q_P + b.
//                              b * c - a * d
//
// This is done using *only* realy arithmetic so it can be done very fast!
void JenkinsTraubSolver::UpdateKPolynomialWithQuadraticShift(
    const VectorXd& polynomial_quotient,
    const VectorXd& k_polynomial_quotient) {
  const double coefficient_q_k =
      (a_ * a_ + sigma_(1) * a_ * b_ + sigma_(2) * b_ * b_) /
      (b_ * c_ - a_ * d_);
  VectorXd linear_polynomial(2);
  linear_polynomial(0) = 1.0;
  linear_polynomial(1) =
      -(a_ * c_ + sigma_(1) * a_ * d_ + sigma_(2) * b_ * d_) /
      (b_ * c_ - a_ * d_);
  k_polynomial_ = AddPolynomials(
      coefficient_q_k * k_polynomial_quotient,
      MultiplyPolynomials(linear_polynomial, polynomial_quotient));

  k_polynomial_(k_polynomial_.size() - 1) += b_;
}

// Using a bit of algebra, the update of sigma(z) can be computed from the
// previous value along with a, b, c, and d defined above. The details of this
// simplification can be found in "Three Stage Variable-Shift Iterations for the
// Solution of Polynomial Equations With a Posteriori Error Bounds for the
// Zeros" by M.A. Jenkins, Doctoral Thesis, Stanford Univeristy, 1969.
//
// NOTE: we assume the leading term of quadratic_sigma is 1.0.
VectorXd JenkinsTraubSolver::ComputeNextSigma() {
  const double u = sigma_(1);
  const double v = sigma_(2);
  const VectorXd::ReverseReturnType& creverse_k_polynomial =
      k_polynomial_.reverse();
  const VectorXd::ReverseReturnType& creverse_polynomial =
      polynomial_.reverse();

  const double b1 = -creverse_k_polynomial(0) / creverse_polynomial(0);
  const double b2 = -(creverse_k_polynomial(1) + b1 * creverse_polynomial(1)) /
                    creverse_polynomial(0);

  const double a1 = b_* c_ - a_ * d_;
  const double a2 = a_ * c_ + u * a_ * d_ + v * b_* d_;
  const double c2 = b1 * a2;
  const double c3 = b1 * b1 * (a_ * a_ + u * a_ * b_ + v * b_ * b_);
  const double c4 = v * b2 * a1 - c2 - c3;
  const double c1 = c_ * c_ + u * c_ * d_ + v * d_ * d_ +
                    b1 * (a_ * c_ + u * b_ * c_ + v * b_ * d_) - c4;
  const double delta_u = -(u * (c2 + c3) + v * (b1 * a1 + b2 * a2)) / c1;
  const double delta_v = v * c4 / c1;

  // Update u and v in the quadratic sigma.
  VectorXd new_quadratic_sigma(3);
  new_quadratic_sigma(0) = 1.0;
  new_quadratic_sigma(1) = u + delta_u;
  new_quadratic_sigma(2) = v + delta_v;
  return new_quadratic_sigma;
}

}  // namespace

bool FindPolynomialRootsJenkinsTraub(const VectorXd& polynomial,
                                     VectorXd* real_roots,
                                     VectorXd* complex_roots) {
  JenkinsTraubSolver solver(polynomial, real_roots, complex_roots);
  return solver.ExtractRoots();
}

}  // namespace theia


#pragma once
#include <numeric>
#include <iostream>
#include <Eigen/Dense>
#include <autoppl/math/ess.hpp>

namespace ppl {

template <class Derived>
inline auto mean(const Eigen::MatrixBase<Derived>& m)
{ return m.colwise().mean(); }

template <class Derived>
inline auto sd(const Eigen::MatrixBase<Derived>& m)
{
    assert(m.rows() > 1);
    auto var = (m.rowwise() - ppl::mean(m))
                    .colwise().squaredNorm() / (m.rows() - 1);
    return var.array().sqrt().matrix();
}

inline void summary(const std::string& header,
                    const Eigen::MatrixXd& m,
                    double warmup_time,
                    double sampling_time)
{
    std::cout << "Warmup: " << warmup_time << std::endl;
    std::cout << "Sampling: " << sampling_time << std::endl;

    std::cout << header << std::endl;

    Eigen::MatrixXd mean = ppl::mean(m);
    std::cout << "Mean:\n"
              << mean << std::endl;

    Eigen::MatrixXd sd = ppl::sd(m);
    std::cout << "SD:\n"
              << sd << std::endl;

    Eigen::MatrixXd ess = ppl::math::ess(m);
    std::cout << "ESS:\n"
              << ess << std::endl;

    Eigen::MatrixXd ess_per_s = ess / sampling_time;
    std::cout << "ESS/s:\n"
              << ess_per_s << std::endl;
}

} // namespace ppl


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/tanh.hpp
 *
 * \brief Compute the hyperbolic tangent for each element of a vector or
 *  matrix expression.
 *
 * \author comcon1, [at pm dot me]
 *
 * <hr/>
 *
 * Distributed under 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_NUMERIC_UBLASX_OPERATION_TANH_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_TANH_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_tanh_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_tanh_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename T>
BOOST_UBLAS_INLINE
T tanh(T x)
{
    return ::std::tanh(x);
}

} // Namespace detail

/**
 * \brief Applies the \c std::tanh function to each element of a given vector
 *  expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::tanh to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_tanh_functor_traits<VectorExprT>::result_type tanh(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_tanh_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_tanh_functor_traits<VectorExprT>::signature_result_type signature_result_type;

    return expression_type(ve(), detail::tanh<signature_result_type>);
}

/**
 * \brief Applies the \c std::tanh function to each element of a given matrix
 *  expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::tanh to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_tanh_functor_traits<MatrixExprT>::result_type tanh(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_tanh_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_tanh_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

    return expression_type(me(), detail::tanh<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_BoostSolver.cpp
//! \author Alex Bennett
//! \brief  Boost Solver
//!
//---------------------------------------------------------------------------//

// Trilinos Includes
#include <Teuchos_Array.hpp>

// Boost Includes
#include <boost/numeric/odeint.hpp>

// FRENSIE Includes
#include "Utility_BoostSolver.hpp"

namespace Utility {

BoostSolver::BoostSolver(const Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> >& matrix,
                         const Teuchos::Array<double>& y) :
  m_system(BatemanSystem(matrix)),
  m_jacobian(BatemanJacobian(matrix))
{
  m_y.resize( y.size() );
  for(int i = 0; i != m_y.size(); i++)
  {
    m_y(i) = y[i];
  }
}

void BoostSolver::getNumberDensities(Teuchos::Array<double>& y)
{
  y.resize(m_y.size(),0);
  for(int i = 0; i != m_y.size(); i++)
  {
    y[i] = m_y(i);
  }
}

void BoostSolver::Solve(const double& time)
{
  size_t nstep = boost::numeric::odeint::integrate_adaptive(
           boost::numeric::odeint::rosenbrock4_controller<boost::numeric::odeint::rosenbrock4<double> >(1e-6,1e-6),
           std::make_pair( m_system, m_jacobian ),
           m_y,
           0.0,
           time,
           0.1);
}

void BoostSolver::BatemanSystem::operator()(const boost::numeric::ublas::vector<double>& y,
                                                  boost::numeric::ublas::vector<double>& dxdt,
                                            double time)
{
  for(int i = 0; i != dxdt.size(); i++)
  {
    dxdt(i) = 0.0;

    for(int j = 0; j != dxdt.size(); j++)
    {
      dxdt(i) += (*m_matrix)(i,j) * y(j);
    }
  }
}

void BoostSolver::BatemanJacobian::operator()(const boost::numeric::ublas::vector<double>& m_y,
                                                    boost::numeric::ublas::matrix<double>& jacobian,
                                              const double& time,
                                                    boost::numeric::ublas::vector<double>& dfdt )
{
  for(int i = 0; i != jacobian.size1(); i++)
  {
    for(int j = 0; j != jacobian.size2(); j++)
    {
      jacobian(i,j) = (*m_matrix)(i,j);
    }

    dfdt(i) = 0.0;
  }
}

} // end namespace utility

//---------------------------------------------------------------------------//
// end Utility_BoostSolver.cpp
//---------------------------------------------------------------------------//


#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");
  }
}