#include "IterativeClosestPoint.h"

#include <Eigen/Geometry>

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/BooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/DataContainerSelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/IntFilterParameter.h"
#include "SIMPLib/FilterParameters/StringFilterParameter.h"
#include "SIMPLib/Geometry/VertexGeom.h"

#include "DREAM3DReview/DREAM3DReviewConstants.h"
#include "DREAM3DReview/DREAM3DReviewVersion.h"

#include "DREAM3DReview/DREAM3DReviewFilters/util/nanoflann.hpp"

namespace
{
template <typename Derived>
struct VertexGeomAdaptor
{
  const Derived& obj;

  VertexGeomAdaptor(const Derived& obj_)
  : obj(obj_)
  {
  }

  inline const Derived& derived() const
  {
    return obj;
  }

  inline size_t kdtree_get_point_count() const
  {
    return derived()->getNumberOfVertices();
  }

  inline float kdtree_get_pt(const size_t idx, const size_t dim) const
  {
    if(dim == 0)
    {
      return derived()->getVertexPointer(idx)[0];
    }
    if(dim == 1)
    {
      return derived()->getVertexPointer(idx)[1];
    }

    return derived()->getVertexPointer(idx)[2];
  }

  template <class BBOX>
  bool kdtree_get_bbox(BBOX& /*bb*/) const
  {
    return false;
  }
};
} // namespace

enum createdPathID : RenameDataPath::DataID_t
{
  AttributeMatrixID20 = 20,
  ArrayID21 = 21,
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::IterativeClosestPoint()
{
  initialize();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::~IterativeClosestPoint() = default;
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::initialize()
{
  clearErrorCode();
  clearWarningCode();
  setCancel(false);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setupFilterParameters()
{
  FilterParameterVectorType parameters;
  DataContainerSelectionFilterParameter::RequirementType dcsReq;
  dcsReq.dcGeometryTypes = {IGeometry::Type::Vertex};
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Moving Vertex Geometry", MovingVertexGeometry, FilterParameter::Category::RequiredArray, IterativeClosestPoint, dcsReq));
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Target Vertex Geometry", TargetVertexGeometry, FilterParameter::Category::RequiredArray, IterativeClosestPoint, dcsReq));
  parameters.push_back(SIMPL_NEW_INTEGER_FP("Number of Iterations", Iterations, FilterParameter::Category::Parameter, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_BOOL_FP("Apply Transform to Moving Geometry", ApplyTransform, FilterParameter::Category::Parameter, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_STRING_FP("Transform Attribute Matrix Name", TransformAttributeMatrixName, FilterParameter::Category::CreatedArray, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_STRING_FP("Transform Array Name", TransformArrayName, FilterParameter::Category::CreatedArray, IterativeClosestPoint));
  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::dataCheck()
{
  clearErrorCode();
  clearWarningCode();

  getDataContainerArray()->getPrereqGeometryFromDataContainer<VertexGeom>(this, m_MovingVertexGeometry);
  getDataContainerArray()->getPrereqGeometryFromDataContainer<VertexGeom>(this, m_TargetVertexGeometry);

  if(getIterations() < 1)
  {
    setErrorCondition(-1, "Number if iterations must be at least 1");
  }

  DataContainer::Pointer dc = getDataContainerArray()->getPrereqDataContainer(this, m_MovingVertexGeometry);

  if(getErrorCode() < 0)
  {
    return;
  }

  AttributeMatrix::Pointer am = dc->createNonPrereqAttributeMatrix(this, DataArrayPath(m_MovingVertexGeometry.getDataContainerName(), getTransformAttributeMatrixName(), ""), {1},
                                                                   AttributeMatrix::Type::Generic, AttributeMatrixID20);

  if(getErrorCode() < 0)
  {
    return;
  }

  am->createNonPrereqArray<DataArray<float>>(this, getTransformArrayName(), 0, {4, 4}, ArrayID21);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::execute()
{
  initialize();
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  VertexGeom::Pointer moving = getDataContainerArray()->getDataContainer(m_MovingVertexGeometry.getDataContainerName())->getGeometryAs<VertexGeom>();
  VertexGeom::Pointer movingCopy = std::dynamic_pointer_cast<VertexGeom>(moving->deepCopy());
  VertexGeom::Pointer target = getDataContainerArray()->getDataContainer(m_TargetVertexGeometry.getDataContainerName())->getGeometryAs<VertexGeom>();

  float* movingPtr = moving->getVertexPointer(0);
  float* movingCopyPtr = movingCopy->getVertexPointer(0);
  float* targetPtr = target->getVertexPointer(0);

  size_t numMovingVerts = moving->getNumberOfVertices();
  std::vector<size_t> cDims(1, 3);
  FloatArrayType::Pointer dynTarget = FloatArrayType::CreateArray(numMovingVerts, cDims, "tmp", true);
  dynTarget->initializeWithZeros();
  float* dynTargetPtr = dynTarget->getPointer(0);

  using Adaptor = VertexGeomAdaptor<VertexGeom::Pointer>;
  const Adaptor adaptor(target);

  notifyStatusMessage("Building kd-tree index...");

  using KDtree = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Adaptor<float, Adaptor>, Adaptor, 3>;
  KDtree index(3, adaptor, nanoflann::KDTreeSingleIndexAdaptorParams(30));
  index.buildIndex();

  size_t iters = m_Iterations;
  const size_t nn = 1;

  typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::ColMajor> PointCloud;
  typedef Eigen::Matrix<float, 4, 4, Eigen::ColMajor> UmeyamaTransform;

  UmeyamaTransform globalTransform;
  globalTransform << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;

  int64_t progIncrement = iters / 100;
  int64_t prog = 1;
  int64_t progressInt = 0;
  int64_t counter = 0;

  for(size_t i = 0; i < iters; i++)
  {
    if(getCancel())
    {
      return;
    }

    for(size_t j = 0; j < numMovingVerts; j++)
    {
      size_t id;
      float dist;
      nanoflann::KNNResultSet<float> results(nn);
      results.init(&id, &dist);
      index.findNeighbors(results, movingCopyPtr + (3 * j), nanoflann::SearchParams());
      dynTargetPtr[3 * j + 0] = targetPtr[3 * id + 0];
      dynTargetPtr[3 * j + 1] = targetPtr[3 * id + 1];
      dynTargetPtr[3 * j + 2] = targetPtr[3 * id + 2];
    }

    Eigen::Map<PointCloud> moving_(movingCopyPtr, 3, numMovingVerts);
    Eigen::Map<PointCloud> target_(dynTargetPtr, 3, numMovingVerts);

    UmeyamaTransform transform = Eigen::umeyama(moving_, target_, false);

    for(size_t j = 0; j < numMovingVerts; j++)
    {
      Eigen::Vector4f position(movingCopyPtr[3 * j + 0], movingCopyPtr[3 * j + 1], movingCopyPtr[3 * j + 2], 1);
      Eigen::Vector4f transformedPosition = transform * position;
      std::memcpy(movingCopyPtr + (3 * j), transformedPosition.data(), sizeof(float) * 3);
    }

    globalTransform = transform * globalTransform;

    if(counter > prog)
    {
      progressInt = static_cast<int64_t>((static_cast<float>(counter) / iters) * 100.0f);
      QString ss = QObject::tr("Performing Registration Iterations || %1% Completed").arg(progressInt);
      notifyStatusMessage(ss);
      prog = prog + progIncrement;
    }
    counter++;
  }

  float* transformPtr = getDataContainerArray()
                            ->getDataContainer(m_MovingVertexGeometry.getDataContainerName())
                            ->getAttributeMatrix(m_TransformAttributeMatrixName)
                            ->getAttributeArrayAs<DataArray<float>>(m_TransformArrayName)
                            ->getPointer(0);

  if(m_ApplyTransform)
  {
    for(size_t j = 0; j < numMovingVerts; j++)
    {
      Eigen::Vector4f position(movingPtr[3 * j + 0], movingPtr[3 * j + 1], movingPtr[3 * j + 2], 1);
      Eigen::Vector4f transformedPosition = globalTransform * position;
      std::memcpy(movingPtr + (3 * j), transformedPosition.data(), sizeof(float) * 3);
    }
  }

  globalTransform.transposeInPlace();
  std::memcpy(transformPtr, globalTransform.data(), sizeof(float) * 16);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer IterativeClosestPoint::newFilterInstance(bool copyFilterParameters) const
{
  IterativeClosestPoint::Pointer filter = IterativeClosestPoint::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getCompiledLibraryName() const
{
  return DREAM3DReviewConstants::DREAM3DReviewBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getBrandingString() const
{
  return "DREAM3DReview";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << DREAM3DReview::Version::Major() << "." << DREAM3DReview::Version::Minor() << "." << DREAM3DReview::Version::Patch();
  return version;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getGroupName() const
{
  return SIMPL::FilterGroups::ReconstructionFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getSubGroupName() const
{
  return SIMPL::FilterSubGroups::AlignmentFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getHumanLabel() const
{
  return "Iterative Closest Point";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid IterativeClosestPoint::getUuid() const
{
  return QUuid("{6c8fb24b-5b12-551c-ba6d-ae2fa7724764}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::Pointer IterativeClosestPoint::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
std::shared_ptr<IterativeClosestPoint> IterativeClosestPoint::New()
{
  struct make_shared_enabler : public IterativeClosestPoint
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getNameOfClass() const
{
  return QString("IterativeClosestPoint");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::ClassName()
{
  return QString("IterativeClosestPoint");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setMovingVertexGeometry(const DataArrayPath& value)
{
  m_MovingVertexGeometry = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DataArrayPath IterativeClosestPoint::getMovingVertexGeometry() const
{
  return m_MovingVertexGeometry;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTargetVertexGeometry(const DataArrayPath& value)
{
  m_TargetVertexGeometry = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DataArrayPath IterativeClosestPoint::getTargetVertexGeometry() const
{
  return m_TargetVertexGeometry;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setIterations(const int& value)
{
  m_Iterations = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
int IterativeClosestPoint::getIterations() const
{
  return m_Iterations;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setApplyTransform(const bool& value)
{
  m_ApplyTransform = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
bool IterativeClosestPoint::getApplyTransform() const
{
  return m_ApplyTransform;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTransformAttributeMatrixName(const QString& value)
{
  m_TransformAttributeMatrixName = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getTransformAttributeMatrixName() const
{
  return m_TransformAttributeMatrixName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTransformArrayName(const QString& value)
{
  m_TransformArrayName = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getTransformArrayName() const
{
  return m_TransformArrayName;
}


#include "drake/solvers/moby_lcp_solver.h"

#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <vector>

#include <Eigen/LU>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

#include "drake/common/autodiff.h"
#include "drake/common/drake_assert.h"
#include "drake/common/never_destroyed.h"
#include "drake/common/text_logging.h"

namespace drake {
namespace solvers {

namespace {

template <typename Scalar>
bool CheckLemkeTrivial(int n, const Scalar& zero_tol, const VectorX<Scalar>& q,
                       VectorX<Scalar>* z) {
  // see whether trivial solution exists
  if (q.minCoeff() > -zero_tol) {
    z->resize(n);
    z->fill(0);
    return true;
  }

  return false;
}

// AutoDiff-supported linear system solver for performing principle pivoting
// transformations. The matrix is supposed to be a linear basis, but it's
// possible that the basis becomes degenerate (meaning that the matrix becomes
// singular) due to accumulated roundoff error from pivoting. Recovering from
// a degenerate basis is currently an open problem;
// see http://www.optimization-online.org/DB_FILE/2011/03/2948.pdf, for
// example. The caller would ideally terminate at this point, but
// compilation of householderQr().rank() with AutoDiff currently generates
// template errors. Continuing on blindly means that the calling pivoting
// algorithm might continue on for some time.
template <class T>
VectorX<T> LinearSolve(const MatrixX<T>& M, const VectorX<T>& b) {
  // Special case necessary because Eigen doesn't always handle empty matrices
  // properly.
  if (M.rows() == 0) {
    DRAKE_ASSERT(b.size() == 0);
    return VectorX<T>(0);
  }
  return M.householderQr().solve(b);
}

// Linear system solver, specialized for double types. This method is faster
// than the QR factorization necessary for AutoDiff support. It is assumed that
// the matrix is full rank (see notes for generic LinearSolve() above).
template <>
VectorX<double> LinearSolve(
    const MatrixX<double>& M, const VectorX<double>& b) {
  // Special case necessary because Eigen doesn't always handle empty matrices
  // properly.
  if (M.rows() == 0) {
    DRAKE_ASSERT(b.size() == 0);
    return VectorX<double>(0);
  }
  return M.partialPivLu().solve(b);
}

// Utility function for copying part of a matrix (designated by the indices
// in rows and cols) from in to a target matrix, out. This template approach
// allows selecting parts of both sparse and dense matrices for input; only
// a dense matrix is returned.
template <typename Derived, typename T>
void selectSubMat(const Eigen::MatrixBase<Derived>& in,
                  const std::vector<unsigned>& rows,
                  const std::vector<unsigned>& cols, MatrixX<T>* out) {
  const int num_rows = rows.size();
  const int num_cols = cols.size();
  out->resize(num_rows, num_cols);

  for (int i = 0; i < num_rows; i++) {
    const auto row_in = in.row(rows[i]);
    auto row_out = out->row(i);
    for (int j = 0; j < num_cols; j++) {
      row_out(j) = row_in(cols[j]);
    }
  }
}

// TODO(sammy-tri) this could also use a more efficient implementation.
template <typename T>
void selectSubVec(const VectorX<T>& in,
                  const std::vector<unsigned>& rows, VectorX<T>* out) {
  const int num_rows = rows.size();
  out->resize(num_rows);
  for (int i = 0; i < num_rows; i++) {
    (*out)(i) = in(rows[i]);
  }
}

template <typename Derived>
Eigen::SparseVector<double> makeSparseVector(
    const Eigen::MatrixBase<Derived>& in) {
  DRAKE_ASSERT(in.cols() == 1);
  Eigen::SparseVector<double> out(in.rows());
  for (int i = 0; i < in.rows(); i++) {
    if (in(i) != 0.0) {
      out.coeffRef(i) = in(i);
    }
  }
  return out;
}

template <typename Derived>
Eigen::Index minCoeffIdx(const Eigen::MatrixBase<Derived>& in) {
  Eigen::Index idx;
  in.minCoeff(&idx);
  return idx;
}

const double kSqrtEps = std::sqrt(std::numeric_limits<double>::epsilon());
}  // anonymous namespace

template <typename T>
void MobyLCPSolver<T>::SetLoggingEnabled(bool enabled) {
  log_enabled_ = enabled; }

template <typename T>
std::ostream& MobyLCPSolver<T>::Log() const {
  if (log_enabled_) {
    return std::cerr;
  }
  return null_stream_;
}

template <typename T>
void MobyLCPSolver<T>::ClearIndexVectors() const {
  // clear all vectors
  all_.clear();
  tlist_.clear();
  bas_.clear();
  nonbas_.clear();
  j_.clear();
}

template <>
void MobyLCPSolver<Eigen::AutoDiffScalar<Vector1d>>::DoSolve(
    const MathematicalProgram&, const Eigen::VectorXd&,
    const SolverOptions&, MathematicalProgramResult*) const {
  throw std::logic_error(
      "MobyLCPSolver cannot yet be used in a MathematicalProgram "
      "while templatized as an AutoDiff");
}

// TODO(edrumwri): Break the following code out into a special
// MobyLcpMathematicalProgram class.
template <typename T>
void MobyLCPSolver<T>::DoSolve(
    const MathematicalProgram& prog,
    const Eigen::VectorXd& initial_guess,
    const SolverOptions& merged_options,
    MathematicalProgramResult* result) const {
  if (!prog.GetVariableScaling().empty()) {
    static const logging::Warn log_once(
      "MobyLCPSolver doesn't support the feature of variable scaling.");
  }

  // Moby doesn't use initial guess or the solver options.
  unused(initial_guess);
  unused(merged_options);

  // Solve each individual LCP, writing the result back to the decision
  // variables through the binding and returning true iff all LCPs are
  // feasible.
  //
  // If any is infeasible, returns false and does not alter the decision
  // variables.
  //
  // TODO(ggould-tri) This could also be solved by constructing a single large
  // square matrix and vector, and then copying the elements of the individual
  // Ms and qs into the appropriate places.  That would be equivalent to this
  // implementation but might perform better if the solver were to parallelize
  // internally.

  const auto& bindings = prog.linear_complementarity_constraints();
  Eigen::VectorXd x_sol(prog.num_vars());
  for (const auto& binding : bindings) {
    Eigen::VectorXd constraint_solution(binding.GetNumElements());
    const std::shared_ptr<LinearComplementarityConstraint> constraint =
        binding.evaluator();
    bool solved = SolveLcpLemkeRegularized(
        constraint->M(), constraint->q(), &constraint_solution);
    if (!solved) {
      result->set_solution_result(SolutionResult::kUnknownError);
      return;
    }
    for (int i = 0; i < binding.evaluator()->num_vars(); ++i) {
      const int variable_index =
          prog.FindDecisionVariableIndex(binding.variables()(i));
      x_sol(variable_index) = constraint_solution(i);
    }
  }
  result->set_optimal_cost(0.0);
  result->set_x_val(x_sol);
  result->set_solution_result(SolutionResult::kSolutionFound);
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpFast(const MatrixX<T>& M,
                                    const VectorX<T>& q, VectorX<T>* z,
                                    const T& zero_tol) const {
  using std::abs;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> zz, w, qbas;
  MatrixX<T> Mmix, Msub;

  const unsigned N = q.rows();
  const unsigned UINF = std::numeric_limits<unsigned>::max();

  if (M.rows() != N || M.cols() != N)
    throw std::logic_error("M's dimensions do not match that of q.");

  Log() << "MobyLCPSolver::SolveLcpFast() entered" << std::endl;

  // look for trivial solution
  if (N == 0) {
    Log() << "MobyLCPSolver::SolveLcpFast() - empty problem" << std::endl;
    z->resize(0);
    return true;
  }

  // set zero tolerance if necessary
  T mod_zero_tol = zero_tol;
  if (mod_zero_tol < 0)
    mod_zero_tol = ComputeZeroTolerance(M);

  // prepare to setup basic and nonbasic variable indices for z
  nonbas_.clear();
  bas_.clear();

  // see whether to warm-start
  if (z->size() == q.size()) {
    Log() << "MobyLCPSolver::SolveLcpFast() - warm starting activated"
          << std::endl;

    for (unsigned i = 0; i < z->size(); i++) {
      if (abs((*z)[i]) < mod_zero_tol) {
        bas_.push_back(i);
      } else {
        nonbas_.push_back(i);
      }
    }

    if (log_enabled_) {
      std::ostringstream str;
      str << " -- non-basic indices:";
      for (unsigned i = 0; i < nonbas_.size(); i++) str << " " << nonbas_[i];
      Log() << str.str() << std::endl;
    }
  } else {
    // get minimum element of q (really w)
    Eigen::Index minw;
    const T minw_val = q.minCoeff(&minw);
    if (minw_val > -mod_zero_tol) {
      Log() << "MobyLCPSolver::SolveLcpFast() - trivial solution found"
            << std::endl;
      z->resize(N);
      z->fill(0);
      return true;
    }

    // setup basic and nonbasic variable indices
    nonbas_.push_back(minw);
    bas_.resize(N - 1);
    for (unsigned i = 0, j = 0; i < N; i++) {
      if (i != minw) {
        bas_[j++] = i;
      }
    }
  }

  // Loop for the maximum number of pivots.
  const unsigned MAX_PIV = 2 * N;
  for (pivots_ = 1; pivots_ <= MAX_PIV; pivots_++) {
    // select nonbasic indices
    selectSubMat(M, nonbas_, nonbas_, &Msub);
    selectSubMat(M, bas_, nonbas_, &Mmix);
    selectSubVec(q, nonbas_, &zz);
    selectSubVec(q, bas_, &qbas);
    zz *= -1;

    // Solve for nonbasic z.
    zz = LinearSolve(Msub, zz.eval());

    // Eigen doesn't handle empty matrices properly, which causes the code
    // below to abort in the absence of the conditional.
    unsigned minw;
    if (Mmix.rows() == 0) {
      w = VectorX<T>();
      minw = UINF;
    } else {
      w = Mmix * zz;
      w += qbas;
      minw = minCoeffIdx(w);
    }

    // TODO(sammy-tri) this log can't print when minw is UINF.
    // LOG() << "MobyLCPSolver::SolveLcpFast() - minimum w after pivot: "
    // << _w[minw] << std::endl;

    // if w >= 0, check whether any component of z < 0
    if (minw == UINF || w[minw] > -mod_zero_tol) {
      // find the (a) minimum of z
      unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF;
      if (log_enabled_ && zz.rows() > 0) {
        Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: "
              << zz[minz] << std::endl;
      }
      if (minz < UINF && zz[minz] < -mod_zero_tol) {
        // get the original index and remove it from the nonbasic set
        unsigned idx = nonbas_[minz];
        nonbas_.erase(nonbas_.begin() + minz);

        // move index to basic set and continue looping
        bas_.push_back(idx);
        std::sort(bas_.begin(), bas_.end());
      } else {
        // found the solution
        z->resize(N);
        z->fill(0);

        // set values of z corresponding to _z
        for (unsigned i = 0, j = 0; j < nonbas_.size(); i++, j++) {
          (*z)[nonbas_[j]] = zz[i];
        }

        Log() << "MobyLCPSolver::SolveLcpFast() - solution found!" << std::endl;
        return true;
      }
    } else {
      Log() << "(minimum w too negative)" << std::endl;

      // one or more components of w violating w >= 0
      // move component of w from basic set to nonbasic set
      unsigned idx = bas_[minw];
      bas_.erase(bas_.begin() + minw);
      nonbas_.push_back(idx);
      std::sort(nonbas_.begin(), nonbas_.end());

      // look whether any component of z needs to move to basic set
      unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF;
      if (log_enabled_ && zz.rows() > 0) {
        Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: "
              << zz[minz] << std::endl;
      }
      if (minz < UINF && zz[minz] < -mod_zero_tol) {
        // move index to basic set and continue looping
        unsigned k = nonbas_[minz];
        Log() << "MobyLCPSolver::SolveLcpFast() - moving index " << k
              << " to basic set" << std::endl;

        nonbas_.erase(nonbas_.begin() + minz);
        bas_.push_back(k);
        std::sort(bas_.begin(), bas_.end());
      }
    }
  }

  Log() << "MobyLCPSolver::SolveLcpFast() - maximum allowable pivots exceeded"
        << std::endl;

  // if we're here, then the maximum number of pivots has been exceeded
  z->setZero(N);
  return false;
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpFastRegularized(const MatrixX<T>& M,
                                               const VectorX<T>& q,
                                               VectorX<T>* z, int min_exp,
                                               unsigned step_exp, int max_exp,
                                               const T& zero_tol) const {
  Log() << "MobyLCPSolver::SolveLcpFastRegularized() entered" << std::endl;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> wx;
  MatrixX<T> MM;

  // look for fast exit
  if (q.size() == 0) {
    z->resize(0);
    return true;
  }

  // copy MM
  MM = M;

  // A discourse on the zero tolerance in the context of regularization:
  // The zero tolerance is used to determine when an element of w or z is
  // effectively zero though its floating point value is negative. The question
  // is whether the regularization process will change the zero tolerance
  // necessary to solve the problem numerically. In such a case, the infinity
  // norm of M would be small while the infinity norm of MM (regularized M)
  // would be large. Consider the case of a symmetric, indefinite matrix with
  // maximum and minimum eigenvalues of a and -a, respectively. The matrix could
  // be made positive definite (and thereby guaranteed to possess a solution to
  // the linear complementarity problem) by adding an identity matrix times
  // (a+ε) to the LCP matrix, where ε > 0 (its magnitude will depend upon the
  // magnitude of a). The infinity norm (and hence the zero tolerance) could
  // then be expected to grow by a factor of approximately two during the
  // regularization process. In other words, recomputing the zero tolerance
  // for each regularization update to the LCP matrix appears wasteful. For
  // this reason, we compute it only once below, but a practical effect is
  // not discernible at this time.

  // Assign value for zero tolerance, if necessary.
  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  Log() << " zero tolerance: " << mod_zero_tol << std::endl;

  // store the total pivots
  unsigned total_piv = 0;

  // try non-regularized version first
  bool result = SolveLcpFast(MM, q, z, mod_zero_tol);
  if (result) {
    // verify that solution truly is a solution -- check z
    if (z->minCoeff() >= -mod_zero_tol) {
      // check w
      wx = (M * (*z)) + q;
      if (wx.minCoeff() >= -mod_zero_tol) {
        // Check element-wise operation of z*wx.
        wx = z->array() * wx.eval().array();
        const T wx_min = wx.minCoeff();
        const T wx_max = wx.maxCoeff();

        if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) {
          Log() << "  solved with no regularization necessary!" << std::endl;
          Log() << "  pivots / total pivots: " << pivots_ << " " << pivots_
                << std::endl;
          Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited"
                << std::endl;

          return true;
        } else {
          Log() << "MobyLCPSolver::SolveLcpFastRegularized() - "
                << "'<w, z> not within tolerance(min value: " << wx_min
                << " max value: " << wx_max << ")" << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
              << "'w' not solved to desired tolerance" << std::endl;
        Log() << "  minimum w: " << wx.minCoeff() << std::endl;
      }
    } else {
      Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
            << "'z' not solved to desired tolerance" << std::endl;
      Log() << "  minimum z: " << z->minCoeff() << std::endl;
    }
  } else {
    Log() << "  MobyLCPSolver::SolveLcpFastRegularized() "
          << "- solver failed with zero regularization" << std::endl;
  }

  // update the pivots
  total_piv += pivots_;

  // start the regularization process
  int rf = min_exp;
  while (rf < max_exp) {
    // setup regularization factor
    double lambda =
        std::pow(static_cast<double>(10.0), static_cast<double>(rf));

    Log() << "  trying to solve LCP with regularization factor: " << lambda
          << std::endl;

    // regularize M
    MM = M;
    for (unsigned i = 0; i < M.rows(); i++) {
      MM(i, i) += lambda;
    }

    // try to solve the LCP
    result = SolveLcpFast(MM, q, z, mod_zero_tol);

    // update total pivots
    total_piv += pivots_;

    if (result) {
      // verify that solution truly is a solution -- check z
      if (z->minCoeff() > -mod_zero_tol) {
        // check w
        wx = (MM * (*z)) + q;
        if (wx.minCoeff() > -mod_zero_tol) {
          // Check element-wise operation of z*wx.
          wx = z->array() * wx.eval().array();
          const T wx_min = wx.minCoeff();
          const T wx_max = wx.maxCoeff();

          if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) {
            Log() << "  solved with regularization factor: " << lambda
                  << std::endl;
            Log() << "  pivots / total pivots: " << pivots_ << " " << total_piv
                  << std::endl;
            Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited"
                  << std::endl;
            pivots_ = total_piv;
            return true;
          } else {
            Log() << "MobyLCPSolver::SolveLcpFastRegularized() - "
                  << "'<w, z> not within tolerance(min value: " << wx_min
                  << " max value: " << wx_max << ")" << std::endl;
          }
        } else {
          Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
                << "'w' not solved to desired tolerance" << std::endl;
          Log() << "  minimum w: " << wx.minCoeff() << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
              << "'z' not solved to desired tolerance" << std::endl;
        Log() << "  minimum z: " << z->minCoeff() << std::endl;
      }
    }

    // increase rf
    rf += step_exp;
  }

  Log() << "  unable to solve given any regularization!" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited" << std::endl;

  // store total pivots
  pivots_ = total_piv;

  // still here?  failure...
  return false;
}

// Retrieves the solution computed by Lemke's Algorithm.
// T is irrelevant for this method (necessary only for the member function).
// MatrixType allows both dense and sparse matrices to be used.
// Scalar allows this method to be used for when the T is AutoDiffXd but
// the caller wants to use sparse methods.
// TODO(edrumwri): Address this kludge when calling sparse LCP solves from
//                 MobyLCPSolver<AutoDiffXd> has been prevented.
template <typename T>
template <typename MatrixType, typename Scalar>
void MobyLCPSolver<T>::FinishLemkeSolution(const MatrixType& M,
                                           const VectorX<Scalar>& q,
                                           const VectorX<Scalar>& x,
                                           VectorX<Scalar>* z) const {
  using std::abs;
  using std::max;
  std::vector<unsigned>::iterator iiter;
  int idx;
  for (idx = 0, iiter = bas_.begin(); iiter != bas_.end(); iiter++, idx++) {
    (*z)(*iiter) = x(idx);
  }

  // TODO(sammy-tri) Is there a more efficient way to resize and
  // preserve the data?
  z->conservativeResize(q.size());

  // check to see whether tolerances are satisfied
  if (log_enabled_) {
    VectorX<T> wl = (M * (*z)) + q;
    const T minw = wl.minCoeff();
    const T w_dot_z = abs(wl.dot(*z));
    Log() << "  z: " << z << std::endl;
    Log() << "  w: " << wl << std::endl;
    Log() << "  minimum w: " << minw << std::endl;
    Log() << "  w'z: " << w_dot_z << std::endl;
  }
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpLemke(const MatrixX<T>& M,
                                     const VectorX<T>& q, VectorX<T>* z,
                                     const T& piv_tol,
                                     const T& zero_tol) const {
  using std::max;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> result, dj, dl, x, xj, Be, u, z0;
  MatrixX<T> Bl, t1, t2;

  if (log_enabled_) {
    Log() << "MobyLCPSolver::SolveLcpLemke() entered" << std::endl;
    Log() << "  M: " << std::endl << M;
    Log() << "  q: " << q << std::endl;
  }

  const unsigned n = q.size();
  const unsigned max_iter = std::min(unsigned{1000}, 50 * n);

  if (M.rows() != n || M.cols() != n)
    throw std::logic_error("M's dimensions do not match that of q.");

  // update the pivots
  pivots_ = 0;

  // look for immediate exit
  if (n == 0) {
    z->resize(0);
    return true;
  }

  // come up with a sensible value for zero tolerance if none is given
  T mod_zero_tol = zero_tol;
  if (mod_zero_tol <= 0)
    mod_zero_tol = ComputeZeroTolerance(M);

  if (CheckLemkeTrivial(n, mod_zero_tol, q, z)) {
    Log() << " -- trivial solution found" << std::endl;
    Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
    return true;
  }

  // Lemke's algorithm doesn't seem to like warmstarting
  //
  // TODO(sammy-tri) this is not present in the sparse solver, and it
  // causes subtle dead code below.
  z->fill(0);

  // copy z to z0
  z0 = *z;

  ClearIndexVectors();

  // initialize variables
  z->resize(n * 2);
  z->fill(0);
  unsigned t = 2 * n;
  unsigned entering = t;
  unsigned leaving = 0;
  for (unsigned i = 0; i < n; i++) {
    all_.push_back(i);
  }
  unsigned lvindex;
  unsigned idx;
  std::vector<unsigned>::iterator iiter;

  // determine initial basis
  if (z0.size() != n) {
    // setup the nonbasic indices
    for (unsigned i = 0; i < n; i++) nonbas_.push_back(i);
  } else {
    for (unsigned i = 0; i < n; i++) {
      if (z0[i] > 0) {
        bas_.push_back(i);
      } else {
        nonbas_.push_back(i);
      }
    }
  }

  // determine initial values
  if (!bas_.empty()) {
    Log() << "-- initial basis not empty (warmstarting)" << std::endl;

    // start from good initial basis
    Bl.resize(n, n);
    Bl.setIdentity();
    Bl *= -1;

    // select columns of M corresponding to z vars in the basis
    selectSubMat(M, all_, bas_, &t1);

    // select columns of I corresponding to z vars not in the basis
    selectSubMat(Bl, all_, nonbas_, &t2);

    // setup the basis matrix
    Bl.resize(n, t1.cols() + t2.cols());
    Bl.block(0, 0, t1.rows(), t1.cols()) = t1;
    Bl.block(0, t1.cols(), t2.rows(), t2.cols()) = t2;

    // Solve B*x = -q.
    x = LinearSolve(Bl, q);
  } else {
    Log() << "-- using basis of -1 (no warmstarting)" << std::endl;

    // use standard initial basis
    Bl.resize(n, n);
    Bl.setIdentity();
    Bl *= -1;
    x = q;
  }

  // check whether initial basis provides a solution
  if (x.minCoeff() >= 0.0) {
    Log() << " -- initial basis provides a solution!" << std::endl;
    FinishLemkeSolution(M, q, x, z);
    Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
    return true;
  }

  // use a new pivot tolerance if necessary
  const T naive_piv_tol = n * max(T(1), M.template lpNorm<Eigen::Infinity>()) *
      std::numeric_limits<double>::epsilon();
  const T mod_piv_tol = (piv_tol > 0) ? piv_tol : naive_piv_tol;

  // determine initial leaving variable
  Eigen::Index min_x;
  const T min_x_val = x.topRows(n).minCoeff(&min_x);
  const T tval = -min_x_val;
  for (size_t i = 0; i < nonbas_.size(); i++) {
    bas_.push_back(nonbas_[i] + n);
  }
  lvindex = min_x;
  iiter = bas_.begin();
  std::advance(iiter, lvindex);
  leaving = *iiter;
  Log() << " -- x: " << x << std::endl;
  Log() << " -- first pivot: leaving index=" << lvindex
        << "  entering index=" << entering << " minimum value: " << tval
        << std::endl;

  // pivot in the artificial variable
  *iiter = t;  // replace w var with _z0 in basic indices
  u.resize(n);
  for (unsigned i = 0; i < n; i++) {
    u[i] = (x[i] < 0) ? 1 : 0;
  }
  Be = (Bl * u) * -1;
  u *= tval;
  x += u;
  x[lvindex] = tval;
  Bl.col(lvindex) = Be;
  Log() << "  new q: " << x << std::endl;

  // main iterations begin here
  for (pivots_ = 0; pivots_ < max_iter; pivots_++) {
    if (log_enabled_) {
      std::ostringstream basic;
      for (unsigned i = 0; i < bas_.size(); i++) {
        basic << " " << bas_[i];
      }
      Log() << "basic variables:" << basic.str() << std::endl;
      Log() << "leaving: " << leaving << " t:" << t << std::endl;
    }

    // check whether done; if not, get new entering variable
    if (leaving == t) {
      Log() << "-- solved LCP successfully!" << std::endl;
      FinishLemkeSolution(M, q, x, z);
      Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
      return true;
    } else if (leaving < n) {
      entering = n + leaving;
      Be.resize(n);
      Be.fill(0);
      Be[leaving] = -1;
    } else {
      entering = leaving - n;
      Be = M.col(entering);
    }
    dl = Be;

    // See comments above on the possibility of this solve failing.
    dl = LinearSolve(Bl, dl.eval());

    // ** find new leaving variable
    j_.clear();
    for (unsigned i = 0; i < dl.size(); i++) {
      if (dl[i] > mod_piv_tol) {
        j_.push_back(i);
      }
    }

    // check for no new pivots; ray termination
    if (j_.empty()) {
      Log()
          << "MobyLCPSolver::SolveLcpLemke() - no new pivots (ray termination)"
          << std::endl;
      Log() << "MobyLCPSolver::SolveLcpLemke() exiting" << std::endl;
      z->setZero(n);
      return false;
    }

    if (log_enabled_) {
      std::ostringstream j;
      for (unsigned i = 0; i < j_.size(); i++) j << " " << j_[i];
      Log() << "d: " << dl << std::endl;
      Log() << "j (before min ratio):" << j.str() << std::endl;
    }

    // select elements j from x and d
    selectSubVec(x, j_, &xj);
    selectSubVec(dl, j_, &dj);

    // compute minimal ratios x(j) + EPS_DOUBLE ./ d(j), d > 0
    result.resize(xj.size());
    result.fill(mod_zero_tol);
    result = xj.eval().array() + result.array();
    result = result.eval().array() / dj.array();
    const T theta = result.minCoeff();

    // NOTE: lexicographic ordering is not used here to prevent
    // cycling (see [Cottle 1992], pp. 340-342). Cycling is indirectly prevented
    // by (a) limiting the maximum number of pivots and (b) using
    // regularized solvers, as necessary. In other words, cycling may cause
    // solver to fail when the LCP is theoretically solvable, but wrapping the
    // solver with regularization practically addresses the problem (albeit,
    // at the cost of additional computation).

    // find indices of minimal ratios, d> 0
    //   divide _x(j) ./ d(j) -- remove elements above the minimum ratio
    for (int i = 0; i < result.size(); i++) {
      result(i) = xj(i) / dj(i);
    }

    for (iiter = j_.begin(), idx = 0; iiter != j_.end();) {
      if (result[idx++] <= theta) {
        iiter++;
      } else {
        iiter = j_.erase(iiter);
      }
    }
    if (log_enabled_) {
      std::ostringstream j;
      for (unsigned i = 0; i < j_.size(); i++) {
        j << " " << j_[i];
      }
      Log() << "j (after min ratio):" << j.str() << std::endl;
    }

    // if j is empty, then likely the zero tolerance is too low
    if (j_.empty()) {
      Log() << "zero tolerance too low?" << std::endl;
      Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
      z->setZero(n);
      return false;
    }

    // check whether artificial index among these
    tlist_.clear();
    for (size_t i = 0; i < j_.size(); i++) {
      tlist_.push_back(bas_[j_[i]]);
    }
    if (std::find(tlist_.begin(), tlist_.end(), t) != tlist_.end()) {
      iiter = std::find(bas_.begin(), bas_.end(), t);
      lvindex = iiter - bas_.begin();
    } else {
      // several indices pass the minimum ratio test, pick one randomly
      //      lvindex = _j[rand() % _j.size()];
      // NOTE: solver seems *much* more capable of solving when we pick the
      // first
      // element rather than picking a random one
      lvindex = j_[0];
    }

    // set leaving = bas(lvindex)
    iiter = bas_.begin();
    std::advance(iiter, lvindex);
    leaving = *iiter;

    // ** perform pivot
    const T ratio = x[lvindex] / dl[lvindex];
    dl *= ratio;
    x -= dl;
    x[lvindex] = ratio;
    Bl.col(lvindex) = Be;
    *iiter = entering;
    Log() << " -- pivoting: leaving index=" << lvindex
          << "  entering index=" << entering << std::endl;
  }

  Log() << " -- maximum number of iterations exceeded (n=" << n
        << ", max=" << max_iter << ")" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
  z->setZero(n);
  return false;
}

template <class T>
bool MobyLCPSolver<T>::SolveLcpLemkeRegularized(const MatrixX<T>& M,
                                                const VectorX<T>& q,
                                                VectorX<T>* z, int min_exp,
                                                unsigned step_exp, int max_exp,
                                                const T& piv_tol,
                                                const T& zero_tol) const {
  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> wx;

  Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() entered" << std::endl;

  // look for fast exit
  if (q.size() == 0) {
    z->resize(0);
    return true;
  }

  // copy MM
  MatrixX<T> MM = M;

  // Assign value for zero tolerance, if necessary. See discussion in
  // SolveLcpFastRegularized() to see why this tolerance is computed here once,
  // rather than for each regularized version of M.
  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  Log() << " zero tolerance: " << mod_zero_tol << std::endl;

  // store the total pivots
  unsigned total_piv = 0;

  // try non-regularized version first
  bool result = SolveLcpLemke(MM, q, z, piv_tol, mod_zero_tol);
  if (result) {
    // verify that solution truly is a solution -- check z
    if (z->minCoeff() >= -mod_zero_tol) {
      // check w
      wx = (M * (*z)) + q;
      if (wx.minCoeff() >= -mod_zero_tol) {
        // Check element-wise operation of z*wx.
        wx = z->array() * wx.eval().array();

        const T wx_min = wx.minCoeff();
        const T wx_max = wx.maxCoeff();
        if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) {
          Log() << "  solved with no regularization necessary!" << std::endl;
          Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited"
                << std::endl;

          return true;
        } else {
          Log() << "MobyLCPSolver::SolveLcpLemke() - "
                << "'<w, z> not within tolerance(min value: " << wx_min
                << " max value: " << wx_max << ")" << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpLemke() - 'w' not solved to desired "
            "tolerance"
              << std::endl;
        Log() << "  minimum w: " << wx.minCoeff() << std::endl;
      }
    } else {
      Log() << "  MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired "
          "tolerance"
            << std::endl;
      Log() << "  minimum z: " << z->minCoeff() << std::endl;
    }
  }

  // update the pivots
  total_piv += pivots_;

  // start the regularization process
  int rf = min_exp;
  while (rf < max_exp) {
    // setup regularization factor
    double lambda =
        std::pow(static_cast<double>(10.0), static_cast<double>(rf));

    Log() << "  trying to solve LCP with regularization factor: " << lambda
          << std::endl;

    // regularize M
    MM = M;
    for (unsigned i = 0; i < M.rows(); i++) {
      MM(i, i) += lambda;
    }

    // try to solve the LCP
    result = SolveLcpLemke(MM, q, z, piv_tol, mod_zero_tol);

    // update total pivots
    total_piv += pivots_;

    if (result) {
      // verify that solution truly is a solution -- check z
      if (z->minCoeff() > -mod_zero_tol) {
        // check w
        wx = (MM * (*z)) + q;
        if (wx.minCoeff() > -mod_zero_tol) {
          // Check element-wise operation of z*wx.
          wx = z->array() * wx.eval().array();

          const T wx_min = wx.minCoeff();
          const T wx_max = wx.maxCoeff();
          if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) {
            Log() << "  solved with regularization factor: " << lambda
                  << std::endl;
            Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited"
                  << std::endl;
            pivots_ = total_piv;
            return true;
          } else {
            Log() << "MobyLCPSolver::SolveLcpLemke() - "
                  << "'<w, z> not within tolerance(min value: " << wx_min
                  << " max value: " << wx_max << ")" << std::endl;
          }
        } else {
          Log() << "  MobyLCPSolver::SolveLcpLemke() - 'w' not solved to "
              "desired tolerance"
                << std::endl;
          Log() << "  minimum w: " << wx.minCoeff() << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired "
            "tolerance"
              << std::endl;
        Log() << "  minimum z: " << z->minCoeff() << std::endl;
      }
    }

    // increase rf
    rf += step_exp;
  }

  Log() << "  unable to solve given any regularization!" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited" << std::endl;

  // store total pivots
  pivots_ = total_piv;

  // still here?  failure...
  return false;
}

template <typename T>
MobyLCPSolver<T>::MobyLCPSolver()
    : SolverBase(&id, &is_available, &is_enabled,
                 &ProgramAttributesSatisfied) {}

template <typename T>
MobyLCPSolver<T>::~MobyLCPSolver() = default;

SolverId MobyLcpSolverId::id() {
  static const never_destroyed<SolverId> singleton{"Moby LCP"};
  return singleton.access();
}

template <typename T>
SolverId MobyLCPSolver<T>::id() {
  return MobyLcpSolverId::id();
}

template <typename T>
bool MobyLCPSolver<T>::is_available() {
  return true;
}

template <typename T>
bool MobyLCPSolver<T>::is_enabled() {
  return true;
}

template <typename T>
bool MobyLCPSolver<T>::ProgramAttributesSatisfied(
    const MathematicalProgram& prog) {
  // This solver currently imposes restrictions that its problem:
  //
  // (1) Contains only linear complementarity constraints,
  // (2) Has no element of any decision variable appear in more than one
  //     constraint, and
  // (3) Has every element of every decision variable in a constraint.
  //
  // Restriction 1 could reasonably be relaxed by reformulating other
  // constraint types that can be expressed as LCPs (eg, convex QLPs),
  // although this would also entail adding an output stage to convert
  // the LCP results back to the desired form.  See eg. @RussTedrake on
  // how to convert a linear equality constraint of n elements to an
  // LCP of 2n elements.
  //
  // There is no obvious way to relax restriction 2.
  //
  // Restriction 3 could reasonably be relaxed to simply let unbound
  // variables sit at 0.
  if (prog.required_capabilities() != ProgramAttributes({
        ProgramAttribute::kLinearComplementarityConstraint})) {
    return false;
  }

  // Check that the available LCPs cover the program and no two LCPs cover the
  // same variable.
  const auto& bindings = prog.linear_complementarity_constraints();
  for (int i = 0; i < static_cast<int>(prog.num_vars()); ++i) {
    int coverings = 0;
    for (const auto& binding : bindings) {
      if (binding.ContainsVariable(prog.decision_variable(i))) {
        coverings++;
      }
    }
    if (coverings != 1) {
      return false;
    }
  }

  return true;
}

// Instantiate templates.
template class MobyLCPSolver<double>;
template class MobyLCPSolver<Eigen::AutoDiffScalar<Vector1d>>;

}  // namespace solvers
}  // namespace drake


/**
  STD
 **/

#include <iostream>
#include <string>
#include <vector>
#include <cmath>

/**
  ROS RELATED
 **/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
//#include <iiwa_msgs/JointPosition.h>
#include <iiwa_msgs/JointPosition.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/MarkerArray.h>

/**
  PCL RELATED
 **/
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/pca.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl_conversions/pcl_conversions.h>
#include "preprocessing.hpp"
#include <pcl/filters/extract_indices.h>

/**
  EIGEN RELATED
 **/

#include <Eigen/Core>

const std::string BASE_LINK = "iiwa_link_0";
const std::string EE_LINK = "iiwa_link_ee";

using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;

void optimize_normals(PointCloudT::Ptr &trajectory, pcl::PointCloud<pcl::Normal>::Ptr &trajectory_normals) {
  Eigen::Vector3f gt_normal(0, 0, -1);
  for(unsigned int i = 0; i < trajectory->size(); i++) {
    // go thru the elements in the trajectory and optimize the normals by looking at the previous and current normal
    auto normal = trajectory_normals->points[i].getNormalVector3fMap();
    if(normal[2] > 0) {
      normal[0] *= -1;
      normal[1] *= -1;
      normal[2] *= -1;
    }

    // look at the normals around it
    unsigned int first_point;
    unsigned int second_point;
    if(i == trajectory->size() - 1) {
      first_point = i - 2;
      second_point = i - 1;
    }
    else if (i == 0) {
      first_point = i + 1;
      second_point = i + 2;
    }
    else {
      first_point = i - 1;
      second_point = i + 1;
    }

    auto first_normal = trajectory_normals->points[first_point].getNormalVector3fMap();
    auto second_normal = trajectory_normals->points[second_point].getNormalVector3fMap();
    auto curr_normal = trajectory_normals->points[i].getNormalVector3fMap();
    auto cos_theta = first_normal.dot(curr_normal) /
        (first_normal.norm() * curr_normal.norm());
    auto theta_between_first_curr =  acos(cos_theta) * 180.0 / M_PI;

    cos_theta = first_normal.dot(second_normal) /
        (first_normal.norm() * second_normal.norm());
    auto theta_between_first_second =  acos(cos_theta) * 180.0 / M_PI;

    if(theta_between_first_curr > 10.0) {
      // then first and curr has a large difference
      if(theta_between_first_second < 10.0) {
        // then the curr point has an issue, lets change its normal
        trajectory_normals->points[i]._Normal::normal_x = (first_normal.x() + second_normal.x()) / 2;
        trajectory_normals->points[i]._Normal::normal_y = (first_normal.x() + second_normal.y()) / 2;
        trajectory_normals->points[i]._Normal::normal_z = (first_normal.x() + second_normal.z()) / 2;
      }

    }

  }
}

Eigen::Quaternionf get_rotation(Eigen::Vector3f x, Eigen::Vector3f z){
  if(z[2] > 0) {
    z[2] = - z[2];
    z[1] = - z[1];
    z[0] = - z[0];
  }

  x = x.normalized();
  z = z.normalized();

  Eigen::Vector3f y = (z.cross(x)).normalized();

  Eigen::Matrix3f pose_rotm;
  pose_rotm << x, y, z;
  Eigen::Quaternionf q(pose_rotm);
  return q.normalized();
}

PointCloudT project_trajectory_onto_surface_method2(const PointCloudT::Ptr &trajectory, const PointCloudT::Ptr & arm_cloud, const pcl::PointCloud<pcl::Normal>::Ptr &trajectory_normals, pcl::PointCloud<pcl::Normal>::Ptr &new_normals){
  // if two points in the trajectory are on almost the same direction, then we don't need to add all the points

  PointCloudT new_trajectory;

  Eigen::Vector3f prev_direction;
  Eigen::Vector3f curr_direction;
  bool defined_prev_dir = false;
  for(size_t i = 0; i < trajectory->points.size() - 1; i ++) {
    auto curr_point = trajectory->points[i];
    auto curr_normal = trajectory_normals->points[i];
    auto next_point = trajectory->points[i + 1];
    curr_direction = next_point.getArray3fMap() - curr_point.getArray3fMap();
    if(!defined_prev_dir) {
      // add the first point of the trajectory
      prev_direction = curr_direction;
      new_trajectory.points.push_back(curr_point);
      new_normals->points.push_back(curr_normal);
      defined_prev_dir = true;
    }
    else{
      auto cos_theta = curr_direction.dot(prev_direction) /
          (curr_direction.norm() * prev_direction.norm());
      auto theta =  acos(cos_theta) * 180.0 / M_PI;
      if(theta > 30 && theta <= 40) {
        // add the curr point to the trajectory
        new_trajectory.points.push_back(curr_point);
        new_normals->points.push_back(curr_normal);
      }
    }
  }

  // add the last point of the trajectory
  new_trajectory.points.push_back(trajectory->points[trajectory->points.size()-1]);
  new_normals->points.push_back(trajectory_normals->points[trajectory_normals->points.size() - 1]);
  new_normals->height = 1;
  new_normals->width = new_normals->points.size();
  return new_trajectory;
}

/**
 * @brief find_trajectory_from_p_cloud in this function, the trajectory is created from the point cloud of the artery.
 * @param artery_cloud artery to create the trajectory from
 * @param transformed_cloud generated trajectory will be written here
 * @return
 */
PointCloudT find_trajectory_from_p_cloud(const PointCloudT::Ptr &artery_cloud, PointCloudT::Ptr &transformed_cloud){

  /**
  find the PCA of the artery cloud and then project it to the
  eigen vector space.
  **/

  // find the centroid of the original cloud
  Eigen::Vector4f pcaCentroid;
  pcl::compute3DCentroid(*artery_cloud, pcaCentroid);

  pcl::PCA<PointT> cpca = new pcl::PCA<PointT>;
  cpca.setInputCloud(artery_cloud);
  cpca.project(*artery_cloud, *transformed_cloud); // original cloud is projected to the eigen vector space
  Eigen::Matrix3f eigenVectorsPCA = cpca.getEigenVectors();
  Eigen::Vector3f eigenValuesPCA = cpca.getEigenValues();

  /**
    Project the eigen vectors to the eigen space as well
  **/

  // rotation matrx to go to eigen vector space
//  Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
//  tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.
//  tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t

  /**
   X DIRECTION SHOWS THE LARGEST EIGEN VALUE DIRECTION
   START FROM SMALLEST X VALUE AND GO USING SOME INTERVAL
   next_point = curr_point + interval
   **/

   PointT min_p, max_p;
   pcl::getMinMax3D(*transformed_cloud, min_p, max_p);

   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
   kdtree.setInputCloud(transformed_cloud);
   auto result_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

   // every 2 cm we look for the knn to find the point
   for(float start = min_p.x; start < max_p.x; start += 0.02f) {

     pcl::PointXYZ searchPoint;

     searchPoint.x = start;
     searchPoint.y = 0;
     searchPoint.z = 0;

     // K nearest neighbor search

     int K = 5;

     std::vector<int> pointIdxKNNSearch(K);
     std::vector<float> pointKNNSquaredDistance(K);

     /*std::cout << "K nearest neighbor search at (" << searchPoint.x
               << " " << searchPoint.y
               << " " << searchPoint.z
               << ") with K=" << K << std::endl;*/

     if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
     {
       pcl::PointXYZ result_point;
       result_point.x = 0;
       result_point.y = 0;
       result_point.z = 0;
       for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i) {
         result_point.x += (*transformed_cloud)[ pointIdxKNNSearch[i] ].x;
         result_point.y += (*transformed_cloud)[ pointIdxKNNSearch[i] ].y;
         result_point.z += (*transformed_cloud)[ pointIdxKNNSearch[i] ].z;

       }
       result_point.x  = result_point.x / pointIdxKNNSearch.size ();
       result_point.y  = result_point.y / pointIdxKNNSearch.size ();
       result_point.z  = result_point.z / pointIdxKNNSearch.size ();
       result_cloud->points.push_back(result_point);
     }
   }
   // project the resulting trajectory to its old place
   cpca.reconstruct(*result_cloud, *result_cloud);

   // After the projection some points in the trajectory have exactly the same coordinates.
   // Below code is to get rid of them
   PointCloudT trajectory;
   size_t ind = 0;
   PointT prev_point;
   for(const auto &point : result_cloud->points) {
     if(ind == 0 ) {
       trajectory.points.push_back(point);
     }
     else {
       if(std::abs(prev_point.x - point.x) > 1e-4f || std::abs(prev_point.y - point.y) > 1e-4f || std::abs(prev_point.z - point.z) > 1e-4f) {
         // this check is necessary to make sure not to add the same point two times.
          trajectory.points.push_back(point);
       }
     }
     prev_point = point;
     ind  += 1;
   }
   return trajectory;
}

int main(int argc, char **argv)
{

  ros::init(argc, argv, "find_trajectory");
  ros::NodeHandle nh;
  std::string catkin_directory_path;
  nh.getParam("trajectory_extraction/catkin_directory_path", catkin_directory_path);
  /**
    READ THE ARM AND ARTERY POINT CLOUDS
   **/

  auto artery_cloud = std::make_shared<PointCloudT>();
  auto artery_cloud_cam_base = std::make_shared<PointCloudT>();
  auto arm_cloud = std::make_shared<PointCloudT>();
  auto transformed_cloud = std::make_shared<PointCloudT>();

  if (pcl::io::loadPCDFile<PointT> (catkin_directory_path + "src/artery_downsampled_robot_base.pcd", *artery_cloud) == -1) //* load the artery
  {
    PCL_ERROR ("Couldn't read file\n");
  }

  if (pcl::io::loadPCDFile<PointT> (catkin_directory_path + "src/arm_downsampled_robot_base.pcd", *arm_cloud) == -1) //* load the arm
  {
    PCL_ERROR ("Couldn't read file\n");
  }

  /**
    FIND THE TRAJECTORY FROM THE ARTERY POINT CLOUD
   **/

   auto trajectory = std::make_shared<PointCloudT>(find_trajectory_from_p_cloud(artery_cloud, transformed_cloud));

   /**
     PROJECT THE TRAJECTORY TO THE ARM SURFACE AND RECOVER THE NORMAL DIRECTIONS TO DECIDE THE ORIENTATION OF THE PROBE
    **/

   // For now lets project it directly up

   pcl::PointXYZ minPt, maxPt;
   pcl::getMinMax3D (*arm_cloud, minPt, maxPt);
   float max_z  = maxPt.z;

   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_n;
   kdtree_n.setInputCloud(arm_cloud);
   auto trajectory_projected = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

   auto indices_to_extract = pcl::make_shared<std::vector<int>>();

   std::vector<geometry_msgs::PoseStamped> poses;
   PointT prev_point;
   PointT first_point;
   first_point.x = trajectory->points[0].x;
   first_point.y = trajectory->points[0].y;
   first_point.z = max_z;

   int K_1 = 2;
   std::vector<int> pointIdxKNNSearch_1(K_1);
   std::vector<float> pointKNNSquaredDistance_1(K_1);

   size_t num_neighbors_1 = kdtree_n.nearestKSearch (first_point, K_1, pointIdxKNNSearch_1, pointKNNSquaredDistance_1);
   max_z = (*arm_cloud)[ std::size_t(pointIdxKNNSearch_1[0])].z;

   for(const auto &point : *trajectory) {
     PointT searchPoint;
     int K = 5;
     searchPoint.x = point.x;
     searchPoint.y = point.y;
     searchPoint.z = max_z;
     std::vector<int> pointIdxKNNSearch(K);
     std::vector<float> pointKNNSquaredDistance(K);

     size_t num_neighbors = kdtree_n.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance);
     //std::cout << num_neighbors << std::endl;

     if ( num_neighbors > 0 )
     {
       auto avg_depth = 0.0f;
       for(int ind : pointIdxKNNSearch) {
         avg_depth += (*arm_cloud)[ std::size_t(ind)].z;
       }
       avg_depth /= num_neighbors;
       auto inner_circle_avg_depth = 0.0f;
       auto variance = 0.002f;
       unsigned int inner_circle_count = 0;
       for(int ind : pointIdxKNNSearch) {
         if(std::abs((*arm_cloud)[ std::size_t(ind)].z - avg_depth) < variance) {
           inner_circle_avg_depth += (*arm_cloud)[ std::size_t(ind)].z;
           inner_circle_count += 1;
         }
       }
       if(inner_circle_count == 0) {
         inner_circle_avg_depth = avg_depth;
       }
       else{
         inner_circle_avg_depth /= inner_circle_count;
       }
       PointT new_point;
       (*arm_cloud)[ std::size_t(0) ].x = searchPoint.x;
       (*arm_cloud)[ std::size_t(0) ].y = searchPoint.y;
       (*arm_cloud)[ std::size_t(0) ].z = inner_circle_avg_depth;
       trajectory_projected->points.push_back((*arm_cloud)[ std::size_t(0)]);
     }
   }

   /**
     FIND THE NORMAL DIRECTION OF THE TRAJECTORY POINTS AND WRITE THEM INTO A TEXT FILE TO USE TO MOVE THE ROBOT
   **/

   // Create the normal estimation class, and pass the input dataset to it
   pcl::NormalEstimation<PointT, pcl::Normal> ne;
   ne.setInputCloud (trajectory_projected);

   // Pass the original data (before downsampling) as the search surface
   ne.setSearchSurface (arm_cloud);

   // Create an empty kdtree representation, and pass it to the normal estimation object.
   // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
   pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
   ne.setSearchMethod (tree);

   // Output datasets
   pcl::PointCloud<pcl::Normal>::Ptr trajectory_normals (new pcl::PointCloud<pcl::Normal>);

   // Use all neighbors in a sphere of radius 2cm
   // TODO can be also 3 cm
   ne.setRadiusSearch (0.03);

   // Compute the features
   ne.compute (*trajectory_normals);

   // std::cout << trajectory_projected->points.size() << std::endl;
   // std::cout << trajectory_normals->points.size() << std::endl;

   optimize_normals(trajectory_projected, trajectory_normals);

//   pcl::PointCloud<pcl::Normal>::Ptr new_normals (new pcl::PointCloud<pcl::Normal>);

//   trajectory_projected = std::make_shared<PointCloudT>(project_trajectory_onto_surface_method2(trajectory_projected, arm_cloud, trajectory_normals, new_normals));
//   trajectory_normals = new_normals;

   // std::cout << trajectory_projected->points.size() << std::endl;
   // std::cout << trajectory_normals->points.size() << std::endl;

//   for(auto &point : trajectory_normals->points) {
//     point._Normal::normal_x *= -1;
//     point._Normal::normal_y *= -1;
//     point._Normal::normal_z *= -1;
//   }

   // visualize normals
   pcl::visualization::PCLVisualizer viewer("PCL Viewer");
   viewer.setBackgroundColor (1, 1, 1);
   pcl::visualization::PointCloudColorHandlerCustom<PointT> arm_handler(arm_cloud, 0, 0, 255);
   viewer.addPointCloud(arm_cloud, arm_handler, "trajectory_cloud");
   viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(trajectory_projected, trajectory_normals, 1, 0.03f, "normals");
   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0, 0, "normals");
   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, "normals");
   viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "trajectory_cloud");

   while (!viewer.wasStopped ())
   {
     viewer.spinOnce ();
   }

   /**
     FIND THE POSES
   **/

   ofstream myfile (catkin_directory_path + "src/artery_in_robot_base.txt");
   if (!myfile.is_open()) {
    std::cout << "ERROR" << std::endl;
   }

   for(size_t i = 1; i < trajectory_projected->size(); i++) {
     const auto &prev_point = trajectory_projected->points[i-1];
     const auto &curr_point = trajectory_projected->points[i];

     Eigen::Vector3f direction = curr_point.getArray3fMap() - prev_point.getArray3fMap();
     Eigen::Quaternionf q = get_rotation(direction, trajectory_normals->points[i].getNormalVector3fMap());
     myfile << prev_point.x << " " << prev_point.y << " " << prev_point.z << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n";
   }

   myfile.close();

   // Extract inliers
   pcl::ExtractIndices<PointT> extract;
   extract.setInputCloud(arm_cloud);
   extract.setIndices(indices_to_extract);
   extract.setNegative(true);     // Extract the inliers
   extract.filter(*arm_cloud); // cloud_inliers contains the plane

  return 0;
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "BinarySystem.h"

#include "DensityOperator.h"
#include "Interaction.h"
#include "StateVector.h"

#include "Algorithm.h"
#include "SliceIterator.tcc"

#include "BlitzTiny.h"

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/algorithm_ext/for_each.hpp>

using composite::SubSystemFree;

using namespace structure;

using cppqedutils::sliceiterator::fullRange;

namespace {

#include "details_BinaryHelper.h"

}

//////////
//      //
// Base //
//      //
//////////

binary::Base::Base(InteractionPtr ia)
  : QuantumSystem<2>(Dimensions(ia->getFrees()[0]->getDimension(),ia->getFrees()[1]->getDimension())),
    free0_(ia->getFrees()[0]), free1_(ia->getFrees()[1]), ia_(ia)
{
}

double binary::Base::highestFrequency_v() const
{
  using std::max;
  return max(ia_.get()->highestFrequency(),max(free0_.get()->highestFrequency(),free1_.get()->highestFrequency()));
}

std::ostream& binary::Base::streamParameters_v(std::ostream& os) const
{
  using namespace std;
  os<<"Binary System\nDimensions: "<<getDimensions()<<". Total: "<<getTotalDimension()
    <<"\n\nSubsystem Nr. 0\n";     free0_.get()->streamParameters(os);
  os<<    "Subsystem Nr. 1\n";     free1_.get()->streamParameters(os);
  os<<"0 - 1 - Interaction\n";
  return ia_.get()->streamParameters(os);
}

#define SUCCESSIVE_Ranges(f0,f1,ia) ptrdiff_t l=-1, u;  \
  if ((u=l+free0_.nAvr<LA_Av>())>l) {                          \
    PROCESS_Range( averages(blitz::Range(l+1,u)) , f0)  \
  }                                                     \
  if ((l=u+free1_.nAvr<LA_Av>())>u) {                          \
    PROCESS_Range( averages(blitz::Range(u+1,l)) , f1)  \
  }                                                     \
  if ((u=l+ia_.nAvr<LA_Av>())>l) {                             \
    PROCESS_Range( averages(blitz::Range(l+1,u)) , ia)  \
  }                                                     \

void binary::Base::process_v(Averages& averages) const
{
#define PROCESS_Range(av,ss) Averages temp(av); ss.process(temp);

  SUCCESSIVE_Ranges(free0_,free1_,ia_) ;

#undef  PROCESS_Range

}

std::ostream& binary::Base::stream_v(const Averages& averages, std::ostream& os, int precision) const
{
  const auto
    av0 =free0_.getAv(),
    av1 =free1_.getAv();
  const auto
    av01=   ia_.getAv();

#define PROCESS_Range(av,ss) ss->stream(av,os,precision);

  SUCCESSIVE_Ranges(av0,av1,av01) ;

#undef  PROCESS_Range

  return os;

}

#undef  SUCCESSIVE_Ranges

////////////////////////////
//                        //
// Averaged - Liouvillean //
//                        //
////////////////////////////

namespace binary {

// These possibilities get instantiated through compilation, so that explicit instantiation is not necessary here.

template<LiouvilleanAveragedTag LA>
std::ostream& streamKey(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia)
{
  os<<"Binary system\n";
  free0.streamKey<LA>(os,i);
  free1.streamKey<LA>(os,i);
  return ia.streamKey<LA>(os,i);
}
template std::ostream& streamKey<LA_Li>(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia);
template std::ostream& streamKey<LA_Av>(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia);

template<LiouvilleanAveragedTag LA>
size_t nAvr(const SSF& free0, const SSF& free1, const SSI& ia)
{
  return free0.nAvr<LA>() + free1.nAvr<LA>() + ia.nAvr<LA>();
}
template size_t nAvr< LA_Li >(const SSF& free0, const SSF& free1, const SSI& ia); // explicit instantiation
template size_t nAvr< LA_Av >(const SSF& free0, const SSF& free1, const SSI& ia); // explicit instantiation

template<LiouvilleanAveragedTag LA>
const Averages average(double t, const LazyDensityOperator& ldo, const SSF& free0, const SSF& free1, const SSI& ia, size_t numberAvr)
{
  using boost::copy;

  const Averages
    a0 {quantumdata::partialTrace<V0>(ldo,[&](const auto& m){return free0.average<LA>(t,m);})},
    a1 {quantumdata::partialTrace<V1>(ldo,[&](const auto& m){return free1.average<LA>(t,m);})},
    a01{ia.average<LA>(t,ldo)};

  Averages a(numberAvr);

  copy(a01,copy(a1,copy(a0,a.begin())));

  return a;
}

} // binary

///////////
//       //
// Exact //
//       //
///////////

bool binary::Exact::applicableInMaster_v() const
{
  return free0_.applicableInMaster() && free1_.applicableInMaster() && ia_.applicableInMaster();
}

void binary::Exact::actWithU_v(double t, StateVectorLow& psi, double t0) const
{
  if (const auto ex=free0_.getEx()) for(auto& psiS : fullRange<V0>(psi)) ex->actWithU(t,psiS,t0);
  if (const auto ex=free1_.getEx()) for(auto& psiS : fullRange<V1>(psi)) ex->actWithU(t,psiS,t0);

  ia_.actWithU(t,psi,t0);

}

/////////////////
//             //
// Hamiltonian //
//             //
/////////////////

void binary::Hamiltonian::addContribution_v(double t, const StateVectorLow& psi, StateVectorLow& dpsidt, double t0) const
{
  const auto lambda=[=](auto ha) {
    return [=](const auto& psiS, auto& dpsidtS) {
      ha->addContribution(t,psiS,dpsidtS,t0);
    };
  };

  if (const auto ha=free0_.getHa()) boost::range::for_each(fullRange<V0>(psi),fullRange<V0>(dpsidt),lambda(ha));
  if (const auto ha=free1_.getHa()) boost::range::for_each(fullRange<V1>(psi),fullRange<V1>(dpsidt),lambda(ha));

  ia_.addContribution(t,psi,dpsidt,t0);

}

/////////////////
//             //
// Liouvillean //
//             //
/////////////////

void binary::Liouvillean::actWithJ_v(double t, StateVectorLow& psi, size_t i) const
{
  const auto
    li0 =free0_.getLi(),
    li1 =free1_.getLi();

  size_t n=free0_.nAvr<LA_Li>();
  if (li0 && i<n) {
    for(auto& psiS : fullRange<V0>(psi)) li0->actWithJ(t,psiS,i);
    return;
  }

  i-=n;
  if (li1 && i<(n=free1_.nAvr<LA_Li>())) {
    for(auto& psiS : fullRange<V1>(psi)) li1->actWithJ(t,psiS,i);
    return;
  }

  i-=n;
  if (i<ia_.nAvr<LA_Li>())
    ia_.actWithJ(t,psi,i);

}

void binary::Liouvillean::actWithSuperoperator_v(double t, const DensityOperatorLow& rho, DensityOperatorLow& drhodt, size_t i) const
{
  const auto lambda=[=,&i](auto li) {
    return [=,&i](const auto& rhoS, auto& drhodtS) {
      li->actWithSuperoperator(t,rhoS,drhodtS,i);
    };
  };

  typedef tmptools::Vector<0,2> V0;
  typedef tmptools::Vector<1,3> V1;

  const auto
    li0 =free0_.getLi(),
    li1 =free1_.getLi();

  size_t n=free0_.nAvr<LA_Li>();
  if (li0 && i<n) {
    boost::range::for_each(fullRange<V0>(rho),fullRange<V0>(drhodt),lambda(li0));
    return;
  }

  i-=n;
  if (li1 && i<(n=free1_.nAvr<LA_Li>())) {
    boost::range::for_each(fullRange<V1>(rho),fullRange<V1>(drhodt),lambda(li1));
    return;
  }

  i-=n;
  if (i<ia_.nAvr<LA_Li>())
    ia_.actWithSuperoperator(t,rho,drhodt,i);

}

//////////////////
//              //
// Constructors //
//              //
//////////////////

#define BASE_ctor(Class) Class##Base(getFree0(),getFree1(),getIA())

template<bool IS_EX, bool IS_HA, bool IS_LI>
BinarySystem<IS_EX,IS_HA,IS_LI>::BinarySystem(binary::InteractionPtr ia)
: binary::Base(ia),
  BASE_ctor(Exact),
  BASE_ctor(Hamiltonian),
  BASE_ctor(Liouvillean)
{
}

#undef BASE_ctor

namespace {

using structure::SystemCharacteristics;

const SystemCharacteristics querySystemCharacteristics(binary::InteractionPtr ia)
{
  using namespace structure;

  const QuantumSystemPtr<1>
    free0=ia->getFrees()[0],
    free1=ia->getFrees()[1];

  return SystemCharacteristics{
    std::dynamic_pointer_cast<const Exact<1>>(free0) || std::dynamic_pointer_cast<const Exact<1>>(free1) || std::dynamic_pointer_cast<const Exact<2>>(ia),
    std::dynamic_pointer_cast<const Hamiltonian<1>>(free0) || std::dynamic_pointer_cast<const Hamiltonian<1>>(free1) || std::dynamic_pointer_cast<const Hamiltonian<2>>(ia),
    std::dynamic_pointer_cast<const Liouvillean<1>>(free0) || std::dynamic_pointer_cast<const Liouvillean<1>>(free1) || std::dynamic_pointer_cast<const Liouvillean<2>>(ia)};
}

}

#define DISPATCHER(EX,HA,LI) (all(querySystemCharacteristics(ia)==SystemCharacteristics{EX,HA,LI})) return std::make_shared<BinarySystem<EX,HA,LI> >(ia)

const binary::Ptr binary::make(InteractionPtr ia)
{
  if      DISPATCHER(true ,true ,true ) ;
  else if DISPATCHER(true ,true ,false) ;
  else if DISPATCHER(true ,false,true ) ;
  else if DISPATCHER(true ,false,false) ;
  else if DISPATCHER(false,true ,true ) ;
  else if DISPATCHER(false,true ,false) ;
  else if DISPATCHER(false,false,true ) ;
  else return std::make_shared<BinarySystem<false,false,false> >(ia);
}

#undef DISPATCHER

template class BinarySystem<true ,true ,true >;
template class BinarySystem<true ,true ,false>;
template class BinarySystem<true ,false,true >;
template class BinarySystem<true ,false,false>;
template class BinarySystem<false,true ,true >;
template class BinarySystem<false,true ,false>;
template class BinarySystem<false,false,true >;
template class BinarySystem<false,false,false>;


//=============================================================================================================
/**
* @file     mnemath.cpp
* @author   Christoph Dinh <chdinh@nmr.mgh.harvard.edu>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     July, 2012
*
* @section  LICENSE
*
* Copyright (C) 2012, Christoph Dinh and Matti Hamalainen. 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 MNE-CPP authors 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 OWNER 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.
*
*
* @brief    Definition of the MNEMath Class.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "mnemath.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Eigen>

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

#include <QDebug>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

VectorXd* MNEMath::combine_xyz(const VectorXd& vec)
{
    if (vec.size() % 3 != 0)
    {
        printf("Input must be a row or a column vector with 3N components\n");
        return NULL;
    }

    MatrixXd tmp = MatrixXd(vec.transpose());
    SparseMatrix<double>* s = make_block_diag(tmp,3);

    SparseMatrix<double> sC = *s*s->transpose();
    VectorXd* comb = new VectorXd(sC.rows());

    for(qint32 i = 0; i < sC.rows(); ++i)
        (*comb)[i] = sC.coeff(i,i);

    delete s;
    return comb;
}

//*************************************************************************************************************

double MNEMath::getConditionNumber(const MatrixXd& A, VectorXd &s)
{
    JacobiSVD<MatrixXd> svd(A);
    s = svd.singularValues();

    double c = s.maxCoeff()/s.minCoeff();

    return c;
}

//*************************************************************************************************************

double MNEMath::getConditionSlope(const MatrixXd& A, VectorXd &s)
{
    JacobiSVD<MatrixXd> svd(A);
    s = svd.singularValues();

    double c = s.maxCoeff()/s.mean();

    return c;
}

//*************************************************************************************************************

void MNEMath::get_whitener(MatrixXd &A, bool pca, QString ch_type, VectorXd &eig, MatrixXd &eigvec)
{
    // whitening operator
    SelfAdjointEigenSolver<MatrixXd> t_eigenSolver(A);//Can be used because, covariance matrices are self-adjoint matrices.

    eig = t_eigenSolver.eigenvalues();
    eigvec = t_eigenSolver.eigenvectors().transpose();

    MNEMath::sort<double>(eig, eigvec, false);
    qint32 rnk = MNEMath::rank(A);

    for(qint32 i = 0; i < eig.size()-rnk; ++i)
        eig(i) = 0;

    printf("Setting small %s eigenvalues to zero.\n", ch_type.toUtf8().constData());
    if (!pca)  // No PCA case.
        printf("Not doing PCA for %s\n", ch_type.toUtf8().constData());
    else
    {
        printf("Doing PCA for %s.",ch_type.toUtf8().constData());
        // This line will reduce the actual number of variables in data
        // and leadfield to the true rank.
        eigvec = eigvec.block(eigvec.rows()-rnk, 0, rnk, eigvec.cols());
    }
}

//*************************************************************************************************************

VectorXi MNEMath::intersect(const VectorXi &v1, const VectorXi &v2, VectorXi &idx_sel)
{
    std::vector<int> tmp;

    std::vector< std::pair<int,int> > t_vecIntIdxValue;

    //ToDo:Slow; map VectorXi to stl container
    for(qint32 i = 0; i < v1.size(); ++i)
        tmp.push_back(v1[i]);

    std::vector<int>::iterator it;
    for(qint32 i = 0; i < v2.size(); ++i)
    {
        it = std::search(tmp.begin(), tmp.end(), &v2[i], &v2[i]+1);
        if(it != tmp.end())
            t_vecIntIdxValue.push_back(std::pair<int,int>(v2[i], it-tmp.begin()));//Index and int value are swapped // to sort using the idx
    }

    std::sort(t_vecIntIdxValue.begin(), t_vecIntIdxValue.end(), MNEMath::compareIdxValuePairSmallerThan<int>);

    VectorXi p_res(t_vecIntIdxValue.size());
    idx_sel = VectorXi(t_vecIntIdxValue.size());

    for(quint32 i = 0; i < t_vecIntIdxValue.size(); ++i)
    {
        p_res[i] = t_vecIntIdxValue[i].first;
        idx_sel[i] = t_vecIntIdxValue[i].second;
    }

    return p_res;
}

//*************************************************************************************************************

//    static inline MatrixXd extract_block_diag(MatrixXd& A, qint32 n)
//    {

//        //
//        // Principal Investigators and Developers:
//        // ** Richard M. Leahy, PhD, Signal & Image Processing Institute,
//        //    University of Southern California, Los Angeles, CA
//        // ** John C. Mosher, PhD, Biophysics Group,
//        //    Los Alamos National Laboratory, Los Alamos, NM
//        // ** Sylvain Baillet, PhD, Cognitive Neuroscience & Brain Imaging Laboratory,
//        //    CNRS, Hopital de la Salpetriere, Paris, France
//        //
//        // Copyright (c) 2005 BrainStorm by the University of Southern California
//        // This software distributed  under the terms of the GNU General Public License
//        // as published by the Free Software Foundation. Further details on the GPL
//        // license can be found at http://www.gnu.org/copyleft/gpl.html .
//        //
//        //FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
//        // UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
//        // WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
//        // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
//        // LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
//        //
//        // Author: John C. Mosher 1993 - 2004
//        //
//        //
//        // Modifications for mne Matlab toolbox
//        //
//        //   Matti Hamalainen
//        //   2006

//          [mA,na] = size(A);		% matrix always has na columns
//          % how many entries in the first column?
//          bdn = na/n;			% number of blocks
//          ma = mA/bdn;			% rows in first block

//          % blocks may themselves contain zero entries.  Build indexing as above
//          tmp = reshape([1:(ma*bdn)]',ma,bdn);
//          i = zeros(ma*n,bdn);
//          for iblock = 1:n,
//            i((iblock-1)*ma+[1:ma],:) = tmp;
//          end

//          i = i(:); 			% row indices foreach sparse bd

//          j = [0:mA:(mA*(na-1))];
//          j = j(ones(ma,1),:);
//          j = j(:);

//          i = i + j;

//          bd = full(A(i)); 	% column vector
//          bd = reshape(bd,ma,na);	% full matrix

//    }

//*************************************************************************************************************

bool MNEMath::issparse(VectorXd &v)
{
    //ToDo: Figure out how to accelerate MNEMath::issparse(VectorXd &v)

    qint32 c = 0;
    qint32 n = v.rows();
    qint32 t = n/2;

    for(qint32 i = 0; i < n; ++i)
    {
        if(v(i) == 0)
            ++c;
        if(c > t)
            return true;
    }

    return false;
}

//*************************************************************************************************************

MatrixXd MNEMath::legendre(qint32 n, const VectorXd &X, QString normalize)
{
    MatrixXd y;

    Q_UNUSED(y);

    Q_UNUSED(n);
    Q_UNUSED(X);
    Q_UNUSED(normalize);

    //ToDo

    return y;
}

//*************************************************************************************************************

SparseMatrix<double>* MNEMath::make_block_diag(const MatrixXd &A, qint32 n)
{

    qint32 ma = A.rows();
    qint32 na = A.cols();
    float bdn = ((float)na)/n;      // number of submatrices

//    std::cout << std::endl << "ma " << ma << " na " << na << " bdn " << bdn << std::endl;

    if(bdn - floor(bdn))
    {
        printf("Width of matrix must be even multiple of n\n");
        return NULL;
    }

    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletList;
    tripletList.reserve(bdn*ma*n);

    qint32 current_col, current_row, i, r, c;
    for(i = 0; i < bdn; ++i)
    {
        current_col = i * n;
        current_row = i * ma;

        for(r = 0; r < ma; ++r)
            for(c = 0; c < n; ++c)
                tripletList.push_back(T(r+current_row, c+current_col, A(r, c+current_col)));
    }

    SparseMatrix<double>* bd = new SparseMatrix<double>((int)floor((float)ma*bdn+0.5),na);
//    SparseMatrix<double> p_Matrix(nrow, ncol);
    bd->setFromTriplets(tripletList.begin(), tripletList.end());

    return bd;
}

//*************************************************************************************************************

int MNEMath::nchoose2(int n)
{

    //nchoosek(n, k) with k = 2, equals n*(n-1)*0.5

    int t_iNumOfCombination = (int)(n*(n-1)*0.5);

    return t_iNumOfCombination;
}

//*************************************************************************************************************

qint32 MNEMath::rank(const MatrixXd& A, double tol)
{
    JacobiSVD<MatrixXd> t_svdA(A);//U and V are not computed
    VectorXd s = t_svdA.singularValues();
    double t_dMax = s.maxCoeff();
    t_dMax *= tol;
    qint32 sum = 0;
    for(qint32 i = 0; i < s.size(); ++i)
        sum += s[i] > t_dMax ? 1 : 0;
    return sum;
}

//*************************************************************************************************************

MatrixXd MNEMath::rescale(const MatrixXd &data, const RowVectorXf &times, QPair<QVariant,QVariant> baseline, QString mode)
{
    MatrixXd data_out = data;
    QStringList valid_modes;
    valid_modes << "logratio" << "ratio" << "zscore" << "mean" << "percent";
    if(!valid_modes.contains(mode))
    {
        qWarning() << "\tWarning: mode should be any of : " << valid_modes;
        return data_out;
    }
    printf("\tApplying baseline correction ... (mode: %s)\n", mode.toUtf8().constData());

    qint32 imin = 0;
    qint32 imax = times.size();

    if(!baseline.first.isValid())
        imin = 0;
    else
    {
        float bmin = baseline.first.toFloat();
        for(qint32 i = 0; i < times.size(); ++i)
        {
            if(times[i] >= bmin)
            {
                imin = i;
                break;
            }
        }
    }
    if (!baseline.second.isValid())
        imax = times.size();
    else
    {
        float bmax = baseline.second.toFloat();
        for(qint32 i = times.size()-1; i >= 0; --i)
        {
            if(times[i] <= bmax)
            {
                imax = i+1;
                break;
            }
        }
    }

    VectorXd mean = data_out.block(0, imin,data_out.rows(),imax-imin).rowwise().mean();
    if(mode.compare("mean") == 0)
    {
        data_out -= mean.rowwise().replicate(data.cols());
    }
    else if(mode.compare("logratio") == 0)
    {
        for(qint32 i = 0; i < data_out.rows(); ++i)
            for(qint32 j = 0; j < data_out.cols(); ++j)
                data_out(i,j) = log10(data_out(i,j)/mean[i]); // a value of 1 means 10 times bigger
    }
    else if(mode.compare("ratio") == 0)
    {
        data_out = data_out.cwiseQuotient(mean.rowwise().replicate(data_out.cols()));
    }
    else if(mode.compare("zscore") == 0)
    {
        MatrixXd std_mat = data.block(0, imin, data.rows(), imax-imin) - mean.rowwise().replicate(imax-imin);
        std_mat = std_mat.cwiseProduct(std_mat);
        VectorXd std_v = std_mat.rowwise().mean();
        for(qint32 i = 0; i < std_v.size(); ++i)
            std_v[i] = sqrt(std_v[i] / (float)(imax-imin));

        data_out -= mean.rowwise().replicate(data_out.cols());
        data_out = data_out.cwiseQuotient(std_v.rowwise().replicate(data_out.cols()));
    }
    else if(mode.compare("percent") == 0)
    {
        data_out -= mean.rowwise().replicate(data_out.cols());
        data_out = data_out.cwiseQuotient(mean.rowwise().replicate(data_out.cols()));
    }

    return data_out;
}

//*************************************************************************************************************