#include "drake/solvers/unrevised_lemke_solver.h"

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

#include <Eigen/LU>

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

using drake::log;

namespace drake {
namespace solvers {

namespace {

// A linear system solver that accommodates the inability of the LU
// factorization to be AutoDiff'd (true in Eigen 3, at least). For double types,
// the faster LU factorization and solve is used. For other types, QR
// factorization and solve is used.
template <class T>
class LinearSolver {
 public:
  explicit LinearSolver(const MatrixX<T>& m);

  VectorX<T> Solve(const VectorX<T>& v) const;

 private:
  Eigen::ColPivHouseholderQR<MatrixX<T>> qr_;
  Eigen::PartialPivLU<MatrixX<double>> lu_;
};

template <class T>
LinearSolver<T>::LinearSolver(const MatrixX<T>& M) {
  if (M.rows() > 0)
    qr_ = Eigen::ColPivHouseholderQR<MatrixX<T>>(M);
}

template <>
LinearSolver<double>::LinearSolver(
    const MatrixX<double>& M) {
  if (M.rows() > 0)
    lu_ = Eigen::PartialPivLU<MatrixX<double>>(M);
}

template <class T>
VectorX<T> LinearSolver<T>::Solve(
    const VectorX<T>& v) const {
  if (v.rows() == 0) {
    DRAKE_DEMAND(qr_.rows() == 0);
    return VectorX<T>(0);
  }
  return qr_.solve(v);
}

template <>
VectorX<double> LinearSolver<double>::Solve(
    const VectorX<double>& v) const {
  if (v.rows() == 0) {
    DRAKE_DEMAND(lu_.rows() == 0);
    return VectorX<double>(0);
  }
  return lu_.solve(v);
}
}  // anonymous namespace

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

template <typename T>
void UnrevisedLemkeSolver<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(
      "UnrevisedLemkeSolver doesn't support the feature of variable scaling.");
  }

  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.

  // Create a dummy variable for the number of pivots used.
  int num_pivots = 0;
  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 = SolveLcpLemke(
        constraint->M(), constraint->q(), &constraint_solution, &num_pivots);
    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);
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in row_indices and col_index) from a matrix M to a target
// vector, `out`. Let M be the matrix `in` augmented with a single column of
// ones (i.e., the "covering vector"); put another way, M = | in 1 |, where "in"
// refers the n × m-dimensional matrix `in` and 1 is a column of ones (so M is
// n × (m+1)-dimensional). Let R be a r × n "row selection" matrix, constructed
// using `row_indices`. R is constructed using r = `row_indices.size()` as
// follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the 3 × 3 identity matrix,
// `row_indices = { 2, 0}` and `col_index = 1`. This would make:
// R =  | 0 0 1 |      and R⋅M = | 0 0 1 1 |
//      | 1 0 0 |                | 1 0 0 1 |
//
// The second column (`col_index = 1`) of R⋅M is v = | 0 |
//                                                   | 0 |
//
// `row_indices` need not be in sorted order, and `out` need not be properly
// sized on entry (it will be resized as necessary). However, this method aborts
// if any element of `row_indices` is out of range, `col_index` is out of range,
// *or* if there are any duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubColumnWithCovering(
    const MatrixX<T>& in,
    const std::vector<int>& row_indices,
    int col_index, VectorX<T>* out) {
  DRAKE_ASSERT(ValidateIndices(row_indices, in.rows()));

  const int num_rows = row_indices.size();
  out->resize(num_rows);

  // Look for the covering vector first.
  if (col_index == in.cols()) {
    out->setOnes();
    return;
  }

  DRAKE_DEMAND(0 <= col_index && col_index < in.cols());
  const auto in_column = in.col(col_index);
  for (int i = 0; i < num_rows; i++) {
    DRAKE_ASSERT(row_indices[i] < in_column.size());
    (*out)[i] = in_column[row_indices[i]];
  }
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in `row_indices`) from n-dimensional vector `in` to a target
// vector, `out`. Let R be a r × n "row selection" matrix, constructed using
// r = `row_indices.size()` as follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the vector [ 1 2 3 ]ᵀ,
// and `row_indices = { 2, 0}`. This would make:
// R =  | 0 0 1 |      and v = R⋅`in` = | 3 |
//      | 1 0 0 |                       | 1 |
//
// `row_indices` need not be in sorted order, and `out` need not be properly
// sized on entry (it will be resized as necessary). However, this method aborts
// if any element of `row_indices` is out of range  *or* if there are any
// duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubVector(const VectorX<T>& in,
    const std::vector<int>& row_indices, VectorX<T>* out) {
  DRAKE_ASSERT(ValidateIndices(row_indices, in.size()));

  const int num_rows = row_indices.size();
  out->resize(num_rows);
  for (int i = 0; i < num_rows; i++) {
    DRAKE_ASSERT(row_indices[i] < in.rows());
    (*out)(i) = in(row_indices[i]);
  }
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in `row_indices`) into n-dimensional vector `out`. Let R be the
// r × n "row selection" matrix defined in the documentation of
// SelectSubVector(). Then, the result of the operation is:
// `out` = `out` + Rᵀ⋅(`v` - R⋅`out`)
//
// Consider the following example with `v` set to the vector [ 3 1 ]ᵀ,
// `row_indices = { 2, 0}`, and `out` set to the vector [ 4 5 6 ]. This would
// make:
// R =  | 0 0 1 |      and `out` + Rᵀ⋅(`v` - R⋅`out`) = | 1 |
//      | 1 0 0 |                                       | 5 |
//                                                      | 3 |
//
// `row_indices` need not be in sorted order. This method aborts if any element
// of `row_indices` is out of the range of `out`, if `row_indices` is not the
// same size as `v`, or if there are any duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SetSubVector(const VectorX<T>& v,
    const std::vector<int>& row_indices, VectorX<T>* out) {
  DRAKE_DEMAND(row_indices.size() == static_cast<size_t>(v.size()));
  DRAKE_ASSERT(ValidateIndices(row_indices, out->size()));
  for (size_t i = 0; i < row_indices.size(); ++i)
    (*out)[row_indices[i]] = v[i];
}

// Function for checking whether a set of indices that specify a view into
// a vector is valid. Returns `true` if row_indices are unique and each element
// lies in [0, vector_size-1] and `false` otherwise.
template <class T>
bool UnrevisedLemkeSolver<T>::ValidateIndices(
    const std::vector<int>& row_indices, int vector_size) {
  // Don't check anything for empty vectors.
  if (row_indices.empty())
    return true;

  // Sort the vector first.
  std::vector<int> sorted_row_indices = row_indices;
  std::sort(sorted_row_indices.begin(), sorted_row_indices.end());

  // Validate the maximum and minimum elements.
  if (sorted_row_indices.back() >= vector_size)
    return false;
  if (sorted_row_indices.front() < 0)
    return false;

  // Make sure that the vector is unique.
  return std::unique(sorted_row_indices.begin(), sorted_row_indices.end()) ==
         sorted_row_indices.end();
}

// Function for checking whether a set of indices that specify a view into
// a matrix is valid. Returns `true` if each element of row_indices is unique
// lies in [0, num_rows-1] and if each element of col_indices is unique and
// lies in [0, num_cols-1]. Returns `false` otherwise.
template <class T>
bool UnrevisedLemkeSolver<T>::ValidateIndices(
    const std::vector<int>& row_indices,
    const std::vector<int>& col_indices, int num_rows, int num_cols) {
  return ValidateIndices(row_indices, num_rows) &&
         ValidateIndices(col_indices, num_cols);
}

// Utility function for copying an r × c dimensional submatrix S (designated by
// the indices in row_indices and col_indices) from a matrix M to a target
// matrix, `out`. Let M be the matrix `in` augmented with a single column of
// ones (i.e., the "covering vector"); put another way, M = | in 1 |, where "in"
// refers the n × m-dimensional matrix `in` and 1 is a column of ones (so M is
// n × (m+1)-dimensional). Let R be a r × n "row selection" matrix, constructed
// using `row_indices` and C be a (m+1) × c-dimensional "column selection"
// matrix constructed using `col_indices`. R and C are constructed using
// r = `row_indices.size()` and c = `col_indices.size()` as follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
// Cᵢⱼ = { 1  if col_indices[j] = i, ∀i ∈ 0..m, ∀j ∈ 0..c-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the 3 × 3 identity matrix,
// `row_indices = { 2, 1, 0}` and `col_indices = { 1, 2, 3 }`. This would make:
// R =  | 0 0 1 |      C = | 0 0 0 |   and  R⋅M⋅C = | 0 1 1 |
//      | 0 1 0 |          | 1 0 0 |                | 1 0 1 |
//      | 1 0 0 |          | 0 1 0 |                | 0 0 1 |
//                         | 0 0 1 |
// `row_indices` and `col_indices` need not be in sorted order, and `out` need
// not be properly sized on entry (it will be resized as necessary). However,
// this method aborts if any element of `row_indices` or `col_indices` is out
// of range *or* if there are any duplicated elements.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubMatrixWithCovering(
    const MatrixX<T>& in,
    const std::vector<int>& row_indices,
    const std::vector<int>& col_indices, MatrixX<T>* out) {
  const int num_rows = row_indices.size();
  const int num_cols = col_indices.size();
  DRAKE_ASSERT(
      ValidateIndices(row_indices, col_indices, in.rows(), in.cols() + 1));
  out->resize(num_rows, num_cols);

  for (int i = 0; i < num_rows; i++) {
    const auto row_in = in.row(row_indices[i]);

    // `row_out` is a "view" into `out`: any modifications to row_out are
    // reflected in `out`.
    auto row_out = out->row(i);
    for (int j = 0; j < num_cols; j++) {
      if (col_indices[j] < in.cols()) {
        DRAKE_ASSERT(col_indices[j] >= 0);
        row_out(j) = row_in(col_indices[j]);
      } else {
        DRAKE_ASSERT(col_indices[j] == in.cols());
        row_out(j) = 1.0;
      }
    }
  }
}

// Determines the various index sets defined in Section 1.1 of [Dai 2018].
template <class T>
void UnrevisedLemkeSolver<T>::DetermineIndexSets() const {
  // Helper for determining index sets.
  auto DetermineIndexSetsHelper = [this](
      const std::vector<LCPVariable>& variables, bool is_z,
      std::vector<int>* variable_set,
      std::vector<int>* variable_set_prime) {
    variable_and_array_indices_.clear();
    for (int i = 0; i < static_cast<int>(variables.size()); ++i) {
      if (variables[i].is_z() == is_z)
        variable_and_array_indices_.emplace_back(variables[i].index(), i);
    }
    std::sort(variable_and_array_indices_.begin(),
              variable_and_array_indices_.end());

    // Construct the set and the primed set.
    for (const auto& variable_and_array_index_pair :
        variable_and_array_indices_) {
      variable_set->push_back(variable_and_array_index_pair.first);
      variable_set_prime->push_back(variable_and_array_index_pair.second);
    }
  };

  // Clear all sets.
  index_sets_.alpha.clear();
  index_sets_.alpha_bar.clear();
  index_sets_.alpha_prime.clear();
  index_sets_.alpha_bar_prime.clear();
  index_sets_.beta.clear();
  index_sets_.beta_bar.clear();
  index_sets_.beta_prime.clear();
  index_sets_.beta_bar_prime.clear();

  DetermineIndexSetsHelper(indep_variables_, false,
      &index_sets_.alpha, &index_sets_.alpha_prime);
  DetermineIndexSetsHelper(dep_variables_, false,
      &index_sets_.alpha_bar, &index_sets_.alpha_bar_prime);
  DetermineIndexSetsHelper(dep_variables_, true,
                           &index_sets_.beta, &index_sets_.beta_prime);
  DetermineIndexSetsHelper(indep_variables_, true,
                           &index_sets_.beta_bar, &index_sets_.beta_bar_prime);
}

// Verifies that each element of the pivoting set is unique. This is an
// expensive operation and should only be executed in Debug mode.
template <class T>
bool UnrevisedLemkeSolver<T>::IsEachUnique(
    const std::vector<LCPVariable>& vars) {
  // Copy the set.
  std::vector<LCPVariable> vars_copy = vars;
  std::sort(vars_copy.begin(), vars_copy.end());
  return (std::unique(vars_copy.begin(), vars_copy.end()) == vars_copy.end());
}

// Performs the pivoting operation, which is described in [Dai 2018].
// `M_prime_col` can be null, if the updated column of M' (pivoted version of M)
// is not needed and the driving_index corresponds to the artificial variable.
template <typename T>
bool UnrevisedLemkeSolver<T>::LemkePivot(
    const MatrixX<T>& M,
    const VectorX<T>& q,
    int driving_index,
    T zero_tol,
    VectorX<T>* M_prime_col,
    VectorX<T>* q_prime) const {
  DRAKE_DEMAND(q_prime);

  const int kArtificial = M.rows();
  DRAKE_DEMAND(driving_index >= 0 && driving_index <= kArtificial);

  // Verify that each member in the independent and dependent sets is unique.
  DRAKE_ASSERT(IsEachUnique(indep_variables_));
  DRAKE_ASSERT(IsEachUnique(dep_variables_));

  // If the driving index does not correspond to the artificial variable,
  // M_prime_col must be non-null.
  if (!IsArtificial(indep_variables_[driving_index]))
    DRAKE_DEMAND(M_prime_col);

  // Determine the sets.
  DetermineIndexSets();

  // Note: It is feasible to do a low-rank update to the factorization below,
  // since alpha and beta should change by no more than a single index between
  // consecutive pivots. Eigen only supports low-rank updates to Cholesky
  // factorizations at the moment, however.

  // Compute matrix and vector views.
  SelectSubMatrixWithCovering(M, index_sets_.alpha, index_sets_.beta,
                              &M_alpha_beta_);
  SelectSubMatrixWithCovering(M, index_sets_.alpha_bar, index_sets_.beta,
                  &M_alpha_bar_beta_);
  SelectSubVector(q, index_sets_.alpha, &q_alpha_);
  SelectSubVector(q, index_sets_.alpha_bar, &q_alpha_bar_);

  // Equation (2) from [Dai 2018].
  // Note: this equation, and those below, must be kept up-to-date with
  // [Dai 2018].
  LinearSolver<T> fMab(M_alpha_beta_);  // Factorized M_alpha_beta_.
  q_prime_beta_prime_ = -fMab.Solve(q_alpha_);

  // Check whether the solution is sufficiently close. We need to do this
  // because partial pivoting LU does not estimate rank (and, from prior
  // experience in solving LCPs), loss of rank need not lead to errors in
  // solving the LCP. We assume that if the factorization is good enough to
  // solve this linear system, it's good enough to solve the subsequent
  // linear system (below). NOTE: current unit tests do not exercise the
  // affirmative evaluation of the conditional (meaning that the "return false"
  // never gets called).
  // @TODO(edrumwri) Institute a unit test that exercises the affirmative
  //   evaluation branch of the conditional when such a LCP has been
  //   identified.
  if ((M_alpha_beta_ * q_prime_beta_prime_ + q_alpha_).norm() > zero_tol)
    return false;

  // Equation (3) from [Dai 2018].
  q_prime_alpha_bar_prime_ = M_alpha_bar_beta_ * q_prime_beta_prime_ +
      q_alpha_bar_;

  // Set the components of q'.
  SetSubVector(q_prime_beta_prime_, index_sets_.beta_prime, q_prime);
  SetSubVector(q_prime_alpha_bar_prime_, index_sets_.alpha_bar_prime, q_prime);

  DRAKE_LOGGER_DEBUG("q': {}", q_prime->transpose());

  // If it is not necessary to compute the column of M, quit now.
  if (!M_prime_col)
    return true;

  // Examine the driving variable.
  if (!indep_variables_[driving_index].is_z()) {
    DRAKE_LOGGER_DEBUG("Driving case #1: driving variable from w");
    // Case from Section 2.2.1.
    // Determine gamma by determining the position of the driving variable
    // in INDEPENDENT W (as defined in [Dai 2018]).
    const int n = static_cast<int>(indep_variables_.size());
    int gamma = 0;
    for (int i = 0; i < n; ++i) {
      if (!indep_variables_[i].is_z()) {
        if (indep_variables_[i].index() <
            indep_variables_[driving_index].index()) {
          ++gamma;
        }
      }
    }

    // From Equation (4) in [Dai 2018].
    DRAKE_ASSERT(index_sets_.alpha[gamma] ==
        indep_variables_[driving_index].index());

    // Set the unit vector.
    e_.setZero(index_sets_.beta.size());
    e_[gamma] = 1.0;

    // Equation (5).
    M_prime_driving_beta_prime_ = fMab.Solve(e_);

    // Equation (6).
    M_prime_driving_alpha_bar_prime_ = M_alpha_bar_beta_ *
        M_prime_driving_beta_prime_;
  } else {
    DRAKE_LOGGER_DEBUG("Driving case #2: driving variable from z");

    // Case from Section 2.2.2 of [Dai 2018].
    // Determine zeta.
    const int zeta = indep_variables_[driving_index].index();

    // Compute g_alpha and g_alpha_bar.
    SelectSubColumnWithCovering(M, index_sets_.alpha, zeta, &g_alpha_);
    SelectSubColumnWithCovering(M, index_sets_.alpha_bar, zeta, &g_alpha_bar_);

    // Equation (7).
    M_prime_driving_beta_prime_ = -fMab.Solve(g_alpha_);

    // Equation (8).
    M_prime_driving_alpha_bar_prime_ = g_alpha_bar_ +
        M_alpha_bar_beta_ * M_prime_driving_beta_prime_;
  }

  SetSubVector(M_prime_driving_beta_prime_, index_sets_.beta_prime,
               M_prime_col);
  SetSubVector(M_prime_driving_alpha_bar_prime_, index_sets_.alpha_bar_prime,
               M_prime_col);

  DRAKE_LOGGER_DEBUG("M' (driving): {}", M_prime_col->transpose());
  return true;
}

// Checks to see whether a given variable is the artificial variable.
template <class T>
bool UnrevisedLemkeSolver<T>::IsArtificial(const LCPVariable& v) const {
  const int n = static_cast<int>(dep_variables_.size());
  return v.is_z() && v.index() == n;
}

// Method for finding the index of the complement of an LCP variable in
// a tuple (strictly speaking, an unsorted vector) of independent variables.
// Aborts if the index is not found in the set or the variable is the artificial
// variable (it never should be).
template <class T>
int UnrevisedLemkeSolver<T>::FindComplementIndex(
    const LCPVariable& query) const {
  // Verify that the query is not the artificial variable.
  DRAKE_DEMAND(!IsArtificial(query));

  const auto iter = indep_variables_indices_.find(query.Complement());
  DRAKE_DEMAND(iter != indep_variables_indices_.end());
  return iter->second;
}

// Computes the solution using the current index sets. `z` must simply be
// non-null; it will be resized as necessary. Returns `true` if able to
// construct the solution and `false` if unable to find the solution to a
// necessary system of linear equations. Aborts (in LemkePivot()) if
// `artificial_index` does not correspond to the index of the artificial
// variable in the vector of independent_variables.
// @pre The artificial variable was the blocking variable, indicating that the
//      solution to the LCP can be obtained after a final pivoting operation.
// @pre `artificial_index` corresponds to the index of the artificial variable
//      in the vector of independent variables.
template <class T>
bool UnrevisedLemkeSolver<T>::ConstructLemkeSolution(
    const MatrixX<T>& M,
    const VectorX<T>& q,
    int artificial_index,
    T zero_tol,
    VectorX<T>* z) const {
  DRAKE_DEMAND(z);
  const int n = q.rows();

  // Compute the solution by pivoting the artificial variable, which was just
  // identified as the blocking variable, from the set of dependent variables
  // to the set of independent variables.
  VectorX<T> q_prime(n);
  if (!LemkePivot(M, q, artificial_index, zero_tol, nullptr, &q_prime))
    return false;

  z->setZero(n);
  for (int i = 0; i < static_cast<int>(dep_variables_.size()); ++i) {
    if (dep_variables_[i].is_z())
      (*z)[dep_variables_[i].index()] = q_prime[i];
  }
  return true;
}

// Computes the blocking index using the minimum ratio test. Returns `true`
// if successful, `false` if not (due to, e.g., the driving variable being
// "unblocked" or a cycle being detected). If `false`, `blocking_index` will
// set to -1 on return.
template <typename T>
bool UnrevisedLemkeSolver<T>::FindBlockingIndex(
    const T& zero_tol, const VectorX<T>& matrix_col, const VectorX<T>& ratios,
    int* blocking_index) const {
  DRAKE_DEMAND(blocking_index);
  DRAKE_DEMAND(ratios.size() == matrix_col.size());
  DRAKE_DEMAND(zero_tol > 0);

  const int n = matrix_col.size();
  T min_ratio = std::numeric_limits<double>::infinity();
  *blocking_index = -1;
  for (int i = 0; i < n; ++i) {
    if (matrix_col[i] < -zero_tol) {
      DRAKE_LOGGER_DEBUG("Ratio for index {}: {}", i, ratios[i]);
      if (ratios[i] < min_ratio) {
        min_ratio = ratios[i];
        *blocking_index = i;
      }
    }
  }

  if (*blocking_index < 0) {
    DRAKE_LOGGER_DEBUG("driving variable is unblocked- algorithm failed");
    return false;
  }

  // Determine all variables within the zero tolerance of the minimum ratio,
  // while simultaneously looking for the presence of the artificial variable
  // among the (possible multiple) minima.
  std::vector<int> blocking_indices;
  for (int i = 0; i < n; ++i) {
    if (matrix_col[i] < -zero_tol) {
      DRAKE_LOGGER_DEBUG("Ratio for index {}: {}", i, ratios[i]);
      if (ratios[i] < min_ratio + zero_tol) {
        if (IsArtificial(dep_variables_[i])) {
          // *Always* select the artificial variable, if multiple choices are
          // possible ([Cottle 1992] p. 280).
          *blocking_index = i;
          return true;
        }
        blocking_indices.push_back(i);
      }
    }
  }

  // If there are multiple blocking variables, replace the blocking index with
  // the cycling selection.
  if (blocking_indices.size() > 1) {
    auto& index = selections_[indep_variables_];

    // Verify that we have not run out of indices to select, which means that
    // cycling would be occurring, in spite of cycling prevention.
    if (index >= static_cast<int>(blocking_indices.size())) {
      DRAKE_LOGGER_DEBUG("Cycling detected- indicating failure.");
      *blocking_index = -1;
      return false;
    }
    *blocking_index = blocking_indices[index];
    ++index;
  }

  return true;
}

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

  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  // Find the minima of z and w.
  const T min_z = z.minCoeff();
  const auto w = M * z + q;
  const T min_w = w.minCoeff();

  // Compute the dot product of z and w.
  const T dot = w.dot(z);
  const int n = q.size();
  return (min_z > -mod_zero_tol && min_w > -mod_zero_tol &&
          abs(dot) < 10 * n * mod_zero_tol);
}

// Note: maintainers should read Section 4.4 - 4.4.5 of [Cottle 1992] to
// understand Lemke's Algorithm (and this function).
template <typename T>
bool UnrevisedLemkeSolver<T>::SolveLcpLemke(const MatrixX<T>& M,
                                     const VectorX<T>& q, VectorX<T>* z,
                                     int* num_pivots,
                                     const T& zero_tol) const {
  using std::max;
  using std::abs;
  DRAKE_DEMAND(num_pivots);

  DRAKE_LOGGER_DEBUG(
      "UnrevisedLemkeSolver::SolveLcpLemke() entered, M: {}, "
      "q: {}, ", M, q.transpose());

  const int n = q.size();
  const int max_pivots = 50 * n;  // O(n) pivots expected for solvable problems.

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

  // Update the pivots.
  *num_pivots = 0;

  // Look for immediate exit.
  if (n == 0) {
    DRAKE_LOGGER_DEBUG("-- LCP is zero dimensional");
    z->resize(0);
    return true;
  }

  // Denote the index of the artificial variable (i.e., the variable denoted
  // z₀ in [Cottle 1992], p. 266). Because z₀ is prone to being confused with
  // the first dimension of z in 0-indexed languages like C++, we refer to the
  // artificial variable as zₙ in this implementation (it is denoted zₙ₊₁ in
  // [Dai 2018], as that document uses the 1-indexing prevalent in algorithmic
  // descriptions).
  const int kArtificial = n;

  // Compute 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);

  // Checks to see whether the trivial solution z = 0 to the LCP w = Mz + q
  // solves the LCP. This must be the case if q is non-negative, as w would then
  // be non-negative, z would be non-negative (zero), and w'z = 0.
  if (q.minCoeff() > -mod_zero_tol) {
    z->setZero(q.size());
    DRAKE_LOGGER_DEBUG(" -- trivial solution found");
    DRAKE_LOGGER_DEBUG("UnrevisedLemkeSolver::SolveLcpLemke() exited");
    return true;
  }

  // Clear the cycling selections.
  selections_.clear();

  // If 'n' is identical to the size of the last problem solved, try using the
  // indices from the last problem solved.
  if (static_cast<size_t>(n) == dep_variables_.size()) {
    // Verify that the last call found a solution (indicated by the presence
    // of the artificial variable (zn) in the independent set).
    int zn_index = -1;
    for (int i = 0;
         i < static_cast<int>(indep_variables_.size()) && zn_index < 0; ++i) {
      if (IsArtificial(indep_variables_[i]))
        zn_index = i;
    }

    if (zn_index >= 0) {
      // Compute the candidate solution.
      if (ConstructLemkeSolution(M, q, zn_index, mod_zero_tol, z)) {
        if (IsSolution(M, q, *z, mod_zero_tol)) {
          // If z truly is the solution, return now, indicating only one pivot
          // (in the solution construction) was performed.
          ++(*num_pivots);
          return true;
        }
      } else {
        DRAKE_LOGGER_DEBUG(
            "Failed to solve linear system implied by last solution");
      }
    }
  }

  // Set the LCP variables. Start with all z variables independent and all w
  // variables dependent.
  indep_variables_.resize(n+1);
  dep_variables_.resize(n);
  for (int i = 0; i < n; ++i) {
    dep_variables_[i] = LCPVariable(false, i);
    indep_variables_[i] = LCPVariable(true, i);
  }
  // z needs one more variable (the artificial variable), whose index we
  // denote as n to keep it from corresponding to any actual vector index.
  indep_variables_[n] = LCPVariable(true, n);

  // Compute zn*, the smallest value of the artificial variable zn for which
  // w = q + zn >= 0. Let blocking denote a component of w that equals
  // zero when zn = zn*.
  int blocking_index = -1;
  bool blocking_index_found = FindBlockingIndex(
      mod_zero_tol, q, q, &blocking_index);
  DRAKE_DEMAND(blocking_index_found);

  // Pivot blocking, artificial. Note that we rely upon the dependent variables
  // being ordered sequentially in both arrays.
  LCPVariable blocking = dep_variables_[blocking_index];
  int driving_index = blocking.index();
  std::swap(dep_variables_[blocking_index], indep_variables_[kArtificial]);
  DRAKE_LOGGER_DEBUG("First blocking variable {}{}",
                     ((blocking.is_z()) ? "z" : "w"), blocking.index());
  DRAKE_LOGGER_DEBUG("First driving variable (artificial)");

  // Initialize the independent variable indices. We do this after the initial
  // variable swap for simplicity.
  for (int i = 0; i < static_cast<int>(indep_variables_.size()); ++i)
    indep_variables_indices_[indep_variables_[i]] = i;

  // Output the independent and dependent variable tuples.
  auto to_string = [](const std::vector<LCPVariable>& vars) -> std::string {
    std::ostringstream oss;
    for (int i = 0; i < static_cast<int>(vars.size()); ++i)
      oss << ((vars[i].is_z()) ? "z" : "w") << vars[i].index() << " ";
    return oss.str();
  };
  unused(to_string);  // ... when in release mode.
  DRAKE_LOGGER_DEBUG("Independent set variables: {}",
      to_string(indep_variables_));
  DRAKE_LOGGER_DEBUG("Dependent set variables: {}",
      to_string(dep_variables_));

  // Pivot up to the maximum number of times.
  VectorX<T> q_prime(n), M_prime_col(n);
  while (++(*num_pivots) < max_pivots) {
    DRAKE_LOGGER_DEBUG("New driving variable {}{}",
                       ((indep_variables_[driving_index].is_z()) ? "z" : "w"),
                       indep_variables_[driving_index].index());

    // Compute the permuted q and driving column of the permuted M matrix.
    if (!LemkePivot(
        M, q, driving_index, mod_zero_tol, &M_prime_col, &q_prime)) {
      DRAKE_LOGGER_DEBUG("Linear system solve failed.");
      z->setZero(n);
      return false;
    }

    // Find the blocking variable.
    if (!FindBlockingIndex(
        mod_zero_tol, M_prime_col,
        -(q_prime.array() / M_prime_col.array()).matrix(), &blocking_index)) {
      z->setZero(n);
      return false;
    }
    blocking = dep_variables_[blocking_index];
    DRAKE_LOGGER_DEBUG("Blocking variable {}{}",
                       ((blocking.is_z()) ? "z" : "w"), blocking.index());

    // See whether the artificial variable blocks the driving variable.
    if (blocking.index() == kArtificial) {
      DRAKE_DEMAND(blocking.is_z());

      // Pivot zn with the driving variable.
      std::swap(dep_variables_[blocking_index],
                indep_variables_[driving_index]);

      // Compute the permuted q, and convert it into a solution.
      if (ConstructLemkeSolution(M, q, driving_index, mod_zero_tol, z)) {
        if (IsSolution(M, q, *z))
          return true;

        DRAKE_LOGGER_DEBUG("Solution not computed to requested tolerance");
        z->setZero(n);
        return false;
      }

      // Otherwise, indicate failure.
      DRAKE_LOGGER_DEBUG(
          "Linear system solver failed to construct Lemke solution");
      z->setZero(n);
      return false;
    }

    // Pivot the blocking variable and the driving variable.
    std::swap(dep_variables_[blocking_index],
              indep_variables_[driving_index]);

    // Update the index map.
    auto indep_variables_indices_iter = indep_variables_indices_.find(
        dep_variables_[blocking_index]);
    indep_variables_indices_.erase(indep_variables_indices_iter);
    indep_variables_indices_[indep_variables_[driving_index]] =
        driving_index;

    // Make the driving variable the complement of the blocking variable.
    driving_index = FindComplementIndex(blocking);

    DRAKE_LOGGER_DEBUG("Independent set variables: {}",
        to_string(indep_variables_));
    DRAKE_LOGGER_DEBUG("Dependent set variables: {}",
        to_string(dep_variables_));
  }

  // If here, the maximum number of pivots has been exceeded.
  z->setZero(n);
  DRAKE_LOGGER_DEBUG("Maximum number of pivots exceeded");
  return false;
}

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

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

SolverId UnrevisedLemkeSolverId::id() {
  static const never_destroyed<SolverId> singleton{"Unrevised Lemke"};
  return singleton.access();
}

template <typename T>
SolverId UnrevisedLemkeSolver<T>::id() {
  return UnrevisedLemkeSolverId::id();
}

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

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

template <typename T>
bool UnrevisedLemkeSolver<T>::ProgramAttributesSatisfied(
    const MathematicalProgram& prog) {
  // This solver 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;
}

}  // namespace solvers
}  // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
    class ::drake::solvers::UnrevisedLemkeSolver)


/** Similarity Search
“© Copyright 2017  Hewlett Packard Enterprise Development LP
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder 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,
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.”
*/
/*
 * QueryHashFunctions.cpp
 *
 *  Created on: Mar 24, 2014
 *      Author: gomariat
 */

#include "QueryHashFunctions.h"
#include <random>
#include <cmath>
#include <iostream>
#include "IndexSearch.h"
#include "LSHManager.h"
#include "headers.h"
#include <unordered_set>
#include <vector>
#include <unistd.h>
#include <sys/time.h>
#include <armadillo>
#include <blaze/math/DynamicMatrix.h>
#include <blaze/math/DenseColumn.h>
#include <blaze/math/DynamicVector.h>

using blaze::DynamicMatrix;
using blaze::DynamicVector;

using namespace arma;

QueryHashFunctions::QueryHashFunctions() {

}

QueryHashFunctions::~QueryHashFunctions() {
	// TODO Auto-generated destructor stub
	cleanUp();
}
/**
 * Init precomputed arrays for a given query length, L,K
 * if the array is different of null, only clean the mmemory
 * assuming that the querylengths are the same;
 * parameters: lenght of each array
 * return: false if the memory can not be alllocated
 */
bool QueryHashFunctions::init(int k, int L, int perTotal, int queryLenght){

	kLength				= k;
	lLength				= L;
	perturbationNumber	= perTotal;
	queryLength			= queryLenght;

	long deltaLength	 	= queryLength * perturbationNumber;
	long aTimesXLength 		= lLength * kLength;
	long aTimesDeltaLength 	= lLength * kLength * perturbationNumber;

	if (deltaLength <0  || aTimesXLength<=0|| aTimesDeltaLength<0) {
		std::cout << endl <<"Invalid range of parameters. Please verify L, K, or perturbation";
		std::cout << endl <<"k:"<<k<<" L:"<<L<<" perTotal" << perTotal<<" queryLenght"<< queryLenght;
		return false;
	}

	//delta.resize(deltaLength);
	//aTimesDelta.resize(aTimesDeltaLength);
	//aTimesX.resize(aTimesXLength);

 	return true;
}
/*
 * Cleanup vectors and properties
 *
 */
void QueryHashFunctions::cleanUp(){

	//delta.resize(0);
	//aTimesDelta.resize(0);
	//aTimesX.resize(0);

	kLength				= 0;
	lLength				= 0;
	perturbationNumber	= 0;
	queryLength			= 0;
}
/**
 * Compute precomputed vectors in order to compute g(v) = (a*x +b)/w, where W=c/R
 * delta: store perturbations of the query
 * atimesx:compute the a*x, where x is the points int the query
 * atimesDelta: compute the a*delta, where delta is rand/sqrt(sum(x))
 */
bool QueryHashFunctions::computeVectors(vector<RealT>& query, int k, int L, int totalPerturbations, LSHFunctionT **lshR0Functions, DynamicMatrix<RealT> &a ) {

		if (!init(k,L,totalPerturbations,query.size())) {
			std:cout<<"Invalid memory allocation for vectors";
			return false;
		}

		// the first column is for query point
		DynamicVector<RealT> query_point(queryLength, query.data());
		// Other columns for perturbation points
		DynamicMatrix<RealT> query_pert(queryLength,1+totalPerturbations);
		column(query_pert,0) = query_point;

		// normalize the perturbation points
		for (int n=1; n<=perturbationNumber; n++) {
			arma_rng::set_seed(45354*n+n*n);
			Col<RealT> nn = normalise(randn<Col<RealT>>(queryLength));
			DynamicVector<RealT> delta_vec(queryLength, nn.memptr());
			column(query_pert, n) = delta_vec;
		}

		// for debugging
		//cout << "a size=" << a.rows() << "," << a.columns() << endl;
		//cout << "query_pert size=" << query_pert.rows() << "," << query_pert.columns() << endl;
		aTimesXDelta = a*query_pert; // using blaze matrix multiplication

		return true;
}
/**
 *  Computing buckets, h1 and h2 indexs for a L lenght and a query
 *  using precomputed vectors
 *  retruns the values in h1yL and h2byL
 */
void QueryHashFunctions::getIndex(int W, RealT rValue, Uns32T *h1byL, Uns32T *h2byL,LSHFunctionT **lshR0Functions,PUHashStructureT & uHash) {

	Uns32T pointULSHVectors[kLength];
	Uns32T h1;
	Uns32T h2;
	int pos1 = 0;
	int pos2 = 0;

	RealT accValue	= 0;
	int val=0;

		for (int l=0; l<lLength; l++) {
			pos1=l*kLength;
			for(int i = 0; i < kLength; i++){
				//compute hashes h(v)= (ax+b)/w;
				pointULSHVectors[i] = (Uns32T)(FLOOR_INT32((aTimesXDelta(pos1+i,0)/rValue + lshR0Functions[l][i].b) /W ));
			}

			h1 = computeProductModDefaultPrime(uHash->mainHashA, pointULSHVectors, kLength);
			h2 = computeProductModDefaultPrime(uHash->controlHash1, pointULSHVectors, kLength);

			if (h1 > UH_PRIME_DEFAULT){
			     h1 = h1 - UH_PRIME_DEFAULT;
			}
			h1= h1%uHash->hashTableSize;
			if (h2 > UH_PRIME_DEFAULT){
			       h2 = h2 - UH_PRIME_DEFAULT;
			}
			h1byL[l]=h1;
			h2byL[l]=h2;
		}
}
/**
 *  Computing buckets, h1 and h2 indexs for a L lenght and  a iPerturbation of the query
 *  using precomputed vectors
 *  returns the values in h1yL and h2byL
 */
void QueryHashFunctions::getIndex(int W, RealT rValue, Uns32T *h1byL, Uns32T *h2byL,LSHFunctionT **lshR0Functions,PUHashStructureT & uHash, int iPerturbation) {

	Uns32T pointULSHVectors[kLength];
	Uns32T h1;
	Uns32T h2;
	int pos1 = 0;
	int pos2 = 0;

	RealT accValue		= 0;

	for ( int l = 0; l< lLength; l++) {

		pos1 = l*kLength;
		//pos2 = iPerturbation*lLength*kLength+ l*kLength;
		for(int i = 0; i < kLength; i++) {
			//compute hashes g(v)= (sum(ax)+b)/w;
			// h(v)= floor(( aTimesX/R + aTimesDelta  +b)/w);
			pointULSHVectors[i] = (Uns32T)(FLOOR_INT32(((aTimesXDelta(pos1+i,0)/rValue)+aTimesXDelta(pos1+i,iPerturbation+1)+lshR0Functions[l][i].b ) / W ));
				}
		//sum(r'*a)mod prime
		h1 =computeProductModDefaultPrime(uHash->mainHashA, pointULSHVectors, kLength);
		h2= computeProductModDefaultPrime(uHash->controlHash1, pointULSHVectors, kLength);

		if (h1 > UH_PRIME_DEFAULT){
		      h1 = h1 - UH_PRIME_DEFAULT;
		}
		h1= h1% uHash->hashTableSize;

		if (h2 > UH_PRIME_DEFAULT){
		      h2 = h2 - UH_PRIME_DEFAULT;
		}

		h1byL[l]=h1;
		h2byL[l]=h2;
	}
}
/**
 * compute sum(r*a) for getting h index
 * h(a1, a2,.. ak)= (sum(r'*a mod prime) mod prime)
 *  by  applying h-(2^32-5 instead of mod prime
 *  See manual E2LSH code page 13
 */
inline Uns32T QueryHashFunctions::computeProductModDefaultPrime(Uns32T *a, Uns32T *b, IntT size){

	LongUns64T h = 0;
	IntT i = 0;
	for(IntT i = 0; i < size; i++){
		h = h + (LongUns64T)a[i] * (LongUns64T)b[i];
		h = (h & TWO_TO_32_MINUS_1) + 5 * (h >> 32);
		if (h >= UH_PRIME_DEFAULT) {
			h = h - UH_PRIME_DEFAULT;
		}
    CR_ASSERT(h < UH_PRIME_DEFAULT);
  }
  return h;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

#include <scitbx/math/icosahedron.h>

namespace scitbx { namespace math {

namespace {

  struct icosahedron_wrappers
  {
    typedef icosahedron<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("icosahedron", no_init)
        .def(init<int>((arg("level"))))
        .def_readonly("level", &w_t::level)
        .add_property("sites", make_getter(&w_t::sites, rbv()))
        .def("next_neighbors_distance", &w_t::next_neighbors_distance)
      ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_icosahedron()
  {
    icosahedron_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


/**
 * @file similarityCluster/similarityCluster.cpp
 * @brief
 * @author Max Fomichev
 * @date 02.12.2019
*/

#include <mutex>

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wgnu-zero-variadic-macro-arguments"
#pragma clang diagnostic ignored "-Wunused-parameter"
#pragma clang diagnostic ignored "-Wextra-semi"
#endif
#include <dlib/threads.h>
#if defined(__clang__)
#pragma clang diagnostic pop
#endif

#include "dbscan/dbscan.h"
#include "similarityCluster.h"

similarityCluster_t::similarityCluster_t(uint8_t _threads,
                                         const std::unordered_map<std::string, float> &_similarityThreshold,
                                         const langVecSet_t &_langVecSet,
                                         const groupSet_t &_groupSet) {
    std::unordered_map<std::string, std::unordered_map<std::string, categories_t>> catByLang;
    // iterate languages
    for (const auto &lv:_langVecSet) {
        if (lv.second.empty() || lv.second.begin()->second.empty()) {
            continue;
        }
        // get threshold value for the language
        auto threshold = 0.0f;
        {
            auto st = _similarityThreshold.find(lv.first);
            if (st != _similarityThreshold.end()) {
                threshold = st->second;
            } else {
                continue;
            }
        }

        // iterate categories
        dlib::parallel_for(_threads,
                           static_cast<std::size_t>(categories_t::SOCIETY),
                           static_cast<std::size_t>(categories_t::OTHER),
                           [&](std::size_t i) {
            auto category = static_cast<categories_t>(i);
            const auto ci = _groupSet.find(category);
            if (ci == _groupSet.end()) {
                return;
            }

            std::vector<std::string> fileNames;
            std::vector<std::vector<float>> vectors;
            // iterate filenames (j = {file, lang})
            for (const auto &j:ci->second) {
                if (j.second != lv.first) {
                    continue;
                }
                // find vector by file name
                auto vi = lv.second.find(j.first);
                if (vi == lv.second.end()) {
                    continue;
                }
                fileNames.push_back(j.first);
                vectors.push_back(vi->second);
            }
            dbscan_t dbscan(vectors, threshold, 32);

            clusterSet_t tmpClusterSet(dbscan.size(), std::make_pair(cluster_t(), ci->first));
            for (const auto &j:dbscan()) {
                // cluster id starts from 1
                tmpClusterSet[std::get<0>(j) - 1].first.emplace_back(fileNames[std::get<1>(j)], lv.first);
            }

            std::unique_lock<std::mutex> lck(m_mtx);
            m_clusters.insert(m_clusters.end(),
                              std::make_move_iterator(tmpClusterSet.begin()),
                              std::make_move_iterator(tmpClusterSet.end()));
        });
    }
}


#include <libasd/read_binary_as.hpp>

#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <type_traits>
#include <utility>
#include <sstream>
#include <limits>
#include <random>

template<typename T, typename RNG>
inline typename std::enable_if<std::is_integral<T>::value, T>::type
generate_random(RNG& rng)
{
    std::uniform_int_distribution<T> distro(std::numeric_limits<T>::min(),
                                            std::numeric_limits<T>::max());
    return distro(rng);
}

template<typename T, typename RNG>
inline typename std::enable_if<std::is_floating_point<T>::value, T>::type
generate_random(RNG& rng)
{
    std::uniform_real_distribution<T> distro(std::numeric_limits<T>::lowest(),
                                             std::numeric_limits<T>::max());
    return distro(rng);
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(test_simple_value_stream, T)
{
    std::mt19937 mt(123456789);

    for(std::size_t i=0; i<1000; ++i)
    {
        constexpr std::size_t size = sizeof(T);
        const auto value = generate_random<T>(mt);
        const char* const first =
            reinterpret_cast<const char*>(std::addressof(value));
        const char* const last  =
            reinterpret_cast<const char*>(std::addressof(value)) + size;

        const std::string str(first, last);
        std::istringstream iss(str);
        const T result = asd::detail::read_binary_as<T>(iss);

        BOOST_CHECK_EQUAL(value, result);
    }
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(test_simple_value_ptr, T)
{
    std::mt19937 mt(123456789);

    for(std::size_t i=0; i<1000; ++i)
    {
        const auto value = generate_random<T>(mt);
        const char* ptr =
            reinterpret_cast<const char*>(std::addressof(value));

        const T result = asd::detail::read_binary_as<T>(ptr);
        BOOST_CHECK_EQUAL(value, result);
    }
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(test_array_of_value_stream, T)
{
    std::mt19937 mt(123456789);

    for(std::size_t i=0; i<1000; ++i)
    {
        const std::size_t N = 100;
        constexpr std::size_t size = sizeof(T);
        std::vector<T> values(N);
        for(std::size_t i=0; i<values.size(); ++i)
        {
            values[i] = generate_random<T>(mt);
        }

        const char* const first =
            reinterpret_cast<const char*>(values.data());
        const char* const last  =
            reinterpret_cast<const char*>(values.data()) + size * N;

        const std::string str(first, last);
        std::istringstream iss(str);
        const auto result =
            asd::detail::read_binary_as<T, asd::container::vec>(iss, N);

        BOOST_CHECK(values == result);
    }

    for(std::size_t i=0; i<1000; ++i)
    {
        const std::size_t N = 100;
        constexpr std::size_t size = sizeof(T);
        std::vector<T> values(N);
        for(std::size_t i=0; i<values.size(); ++i)
        {
            values[i] = generate_random<T>(mt);
        }

        const char* const first =
            reinterpret_cast<const char*>(values.data());
        const char* const last  =
            reinterpret_cast<const char*>(values.data()) + size * N;

        const std::string str(first, last);
        std::istringstream iss(str);

        // deque version!
        const auto result =
            asd::detail::read_binary_as<T, asd::container::deq>(iss, N);
        const std::deque<T> answer(values.begin(), values.end());

        BOOST_CHECK(answer == result);
    }
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(test_array_of_value_ptr, T)
{
    std::mt19937 mt(123456789);

    for(std::size_t i=0; i<1000; ++i)
    {
        const std::size_t N = 100;
        std::vector<T> values(N);
        for(std::size_t i=0; i<values.size(); ++i)
        {
            values[i] = generate_random<T>(mt);
        }

        const char* ptr =
            reinterpret_cast<const char*>(values.data());

        const auto result =
            asd::detail::read_binary_as<T, asd::container::vec>(ptr, N);

        BOOST_CHECK(values == result);
    }

    for(std::size_t i=0; i<1000; ++i)
    {
        const std::size_t N = 100;
        std::vector<T> values(N);
        for(std::size_t i=0; i<values.size(); ++i)
        {
            values[i] = generate_random<T>(mt);
        }

        const char* ptr =
            reinterpret_cast<const char*>(values.data());

        // deque version!
        const auto result =
            asd::detail::read_binary_as<T, asd::container::deq>(ptr, N);
        const std::deque<T> answer(values.begin(), values.end());

        BOOST_CHECK(answer == result);
    }
}

boost::unit_test::test_suite*
init_unit_test_suite(int, char**)
{
    typedef boost::mpl::list<
        char,
        std::int16_t,
        std::int32_t,
        float,
        double
        > list_of_values;

    boost::unit_test::framework::master_test_suite()
        .add(BOOST_TEST_CASE_TEMPLATE(test_simple_value_stream, list_of_values));

    boost::unit_test::framework::master_test_suite()
        .add(BOOST_TEST_CASE_TEMPLATE(test_simple_value_ptr, list_of_values));

    boost::unit_test::framework::master_test_suite()
        .add(BOOST_TEST_CASE_TEMPLATE(test_array_of_value_stream, list_of_values));

    boost::unit_test::framework::master_test_suite()
        .add(BOOST_TEST_CASE_TEMPLATE(test_array_of_value_ptr, list_of_values));

    return 0;
}


//
// test/decode.cpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#define BOOST_TEST_MODULE TestDecode
#include <boost/test/unit_test.hpp>

#include <boost/radix/basic_codec.hpp>
#include <boost/radix/decode.hpp>
#include <boost/radix/static_obitstream_lsb.hpp>
#include <boost/range/algorithm/equal.hpp>

#include <vector>

#include "common.hpp"

// -----------------------------------------------------------------------------
//
template <std::size_t Bits>
class msb_codec
    : public boost::radix::basic_codec<
          boost::radix::bits::to_alphabet_size<Bits>::value> {
 public:
  msb_codec()
      : boost::radix::basic_codec<
            boost::radix::bits::to_alphabet_size<Bits>::value>(
            generate_alphabet(Bits)) {
  }
};

template <std::size_t Bits>
boost::radix::static_obitstream_msb<Bits> get_segment_packer(
    msb_codec<Bits> const&) {
  return boost::radix::static_obitstream_msb<Bits>();
}

template <std::size_t Bits>
bool is_invalid_whitespace_character(
    msb_codec<Bits> const& codec, char_type c) {
  return false;
}

// -----------------------------------------------------------------------------
//
template <std::size_t Bits>
class lsb_codec
    : public boost::radix::basic_codec<
          boost::radix::bits::to_alphabet_size<Bits>::value> {
 public:
  lsb_codec()
      : boost::radix::basic_codec<
            boost::radix::bits::to_alphabet_size<Bits>::value>(
            generate_alphabet(Bits)) {
  }
};

template <std::size_t Bits>
boost::radix::static_obitstream_lsb<Bits> get_segment_packer(
    lsb_codec<Bits> const&) {
  return boost::radix::static_obitstream_lsb<Bits>();
}

template <std::size_t Bits, typename ErrorHandler>
boost::radix::decode_validation::op validate_character(
    lsb_codec<Bits> const& codec, char_type c, ErrorHandler& errh) {
  return boost::radix::decode_validation::op_consume;
}

// -----------------------------------------------------------------------------
//
template <std::size_t Bits, typename DataGenerator, typename Decoder>
void test_decode(DataGenerator data_generator, Decoder decoder) {
  std::vector<char_type> alphabet = generate_alphabet(Bits);
  std::vector<bits_type> data     = data_generator(Bits);
  std::vector<bits_type> result;
  boost::radix::decode(
      alphabet.begin(), alphabet.end(), std::back_inserter(result), decoder);
  BOOST_TEST(boost::equal(data, result, is_equal_unsigned()));
}

template <std::size_t Bits>
void test_decode_msb() {
  test_decode<Bits>(generate_all_permutations_msb, msb_codec<Bits>());
}

template <std::size_t Bits>
void test_decode_lsb() {
  test_decode<Bits>(generate_all_permutations_lsb, lsb_codec<Bits>());
}

template <std::size_t Bits, typename DataGenerator, typename Encoder>
void test_decoder(DataGenerator data_generator, Encoder codec) {
  std::vector<char_type> alphabet = generate_alphabet(Bits);
  std::vector<bits_type> data     = data_generator(Bits);
  std::vector<bits_type> result;
  result.resize(boost::radix::decoded_size(data.size(), codec));
  boost::radix::decoder<Encoder, bits_type*> decoder =
      boost::radix::make_decoder(codec, result.data());
  decoder.append(alphabet.begin(), alphabet.end());
  decoder.resolve();
  result.resize(decoder.bytes_written());
  BOOST_TEST(boost::equal(data, result, is_equal_unsigned()));
}

// -----------------------------------------------------------------------------
//
BOOST_AUTO_TEST_CASE(decode_one_bit_msb) {
  test_decode_msb<1>();
}

BOOST_AUTO_TEST_CASE(decode_two_bit_msb) {
  test_decode_msb<2>();
}

BOOST_AUTO_TEST_CASE(decode_three_bit_msb) {
  test_decode_msb<3>();
}

BOOST_AUTO_TEST_CASE(decode_four_bit_msb) {
  test_decode_msb<4>();
}

BOOST_AUTO_TEST_CASE(decode_five_bit_msb) {
  test_decode_msb<5>();
}

BOOST_AUTO_TEST_CASE(decode_six_bit_msb) {
  test_decode_msb<6>();
}

BOOST_AUTO_TEST_CASE(decode_seven_bit_msb) {
  test_decode_msb<7>();
}

BOOST_AUTO_TEST_CASE(decode_one_bit_lsb) {
  test_decode_lsb<1>();
}

BOOST_AUTO_TEST_CASE(decode_two_bit_lsb) {
  test_decode_lsb<2>();
}

BOOST_AUTO_TEST_CASE(decode_three_bit_lsb) {
  test_decode_lsb<3>();
}

BOOST_AUTO_TEST_CASE(decode_four_bit_lsb) {
  test_decode_lsb<4>();
}

BOOST_AUTO_TEST_CASE(decode_five_bit_lsb) {
  test_decode_lsb<5>();
}

BOOST_AUTO_TEST_CASE(decode_six_bit_lsb) {
  test_decode_lsb<6>();
}

BOOST_AUTO_TEST_CASE(decode_seven_bit_lsb) {
  test_decode_lsb<7>();
}

BOOST_AUTO_TEST_CASE(decoder_one_bit_msb) {
  test_decoder<1>(generate_all_permutations_msb, msb_codec<1>());
}

BOOST_AUTO_TEST_CASE(decoder_two_bit_msb) {
  test_decoder<2>(generate_all_permutations_msb, msb_codec<2>());
}

BOOST_AUTO_TEST_CASE(decoder_three_bit_msb) {
  test_decoder<3>(generate_all_permutations_msb, msb_codec<3>());
}

BOOST_AUTO_TEST_CASE(decoder_four_bit_msb) {
  test_decoder<4>(generate_all_permutations_msb, msb_codec<4>());
}

BOOST_AUTO_TEST_CASE(decoder_five_bit_msb) {
  test_decoder<5>(generate_all_permutations_msb, msb_codec<5>());
}

BOOST_AUTO_TEST_CASE(decoder_six_bit_msb) {
  test_decoder<6>(generate_all_permutations_msb, msb_codec<6>());
}

BOOST_AUTO_TEST_CASE(decoder_seven_bit_msb) {
  test_decoder<7>(generate_all_permutations_msb, msb_codec<7>());
}


// Copyright 2005-2009 Daniel James.
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include "./config.hpp"

#ifdef TEST_STD_INCLUDES
#  include <functional>
#else
#  include <boost/functional/hash.hpp>
#endif

#include <boost/detail/lightweight_test.hpp>

#include <cmath>
#include <boost/functional/hash/detail/limits.hpp>
#include <boost/functional/hash/detail/float_functions.hpp>
#include <boost/detail/workaround.hpp>

#include <iostream>

#if defined(BOOST_MSVC)
#pragma warning(push)
#pragma warning(disable:4127)   // conditional expression is constant
#pragma warning(disable:4723)   // conditional expression is constant
#if BOOST_MSVC < 1400
#pragma warning(disable:4267)   // conversion from 'size_t' to 'unsigned int',
                                // possible loss of data
#endif
#endif

char const* float_type(float*) { return "float"; }
char const* float_type(double*) { return "double"; }
char const* float_type(long double*) { return "long double"; }

template <class T>
void float_tests(char const* name, T* = 0)
{
    std::cerr
        <<  "\n"
        <<  "Testing " BOOST_STRINGIZE(HASH_NAMESPACE) "::hash<"
        <<  name
        <<  ">\n"
        <<  "\n"
        <<  "boost::hash_detail::limits<T>::digits = "
        <<  boost::hash_detail::limits<T>::digits<< "\n"
        <<  "boost::hash_detail::limits<int>::digits = "
        <<  boost::hash_detail::limits<int>::digits<< "\n"
        <<  "boost::hash_detail::limits<std::size_t>::digits = "
        <<  boost::hash_detail::limits<std::size_t>::digits
        <<  "\n"
        <<  "\n"
        <<  "boost::hash_detail::call_ldexp<T>::float_type = "
        <<  float_type(static_cast<BOOST_DEDUCED_TYPENAME
                boost::hash_detail::call_ldexp<T>::float_type*>(0))
        <<  "\n"
        <<  "boost::hash_detail::call_frexp<T>::float_type = "
        <<  float_type(static_cast<BOOST_DEDUCED_TYPENAME
                boost::hash_detail::call_frexp<T>::float_type*>(0))
        <<  "\n"
        <<  "boost::hash_detail::select_hash_type<T>::type = "
        <<  float_type(static_cast<BOOST_DEDUCED_TYPENAME
                boost::hash_detail::select_hash_type<T>::type*>(0))
        <<  "\n"
        <<  "\n"
        ;

    HASH_NAMESPACE::hash<T> x1;

    T zero = 0;
    T minus_zero = (T) -1 * zero;

    BOOST_TEST(zero == minus_zero);
    BOOST_TEST(x1(zero) == x1(minus_zero));

#if defined(TEST_EXTENSIONS)
    BOOST_TEST(x1(zero) == HASH_NAMESPACE::hash_value(zero));
    BOOST_TEST(x1(minus_zero) == HASH_NAMESPACE::hash_value(minus_zero));
#endif

    using namespace std;

// Doing anything with infinity causes borland to crash.
#if defined(__BORLANDC__)
    std::cerr
        <<  "Not running infinity checks on Borland, as it causes it to crash."
            "\n";
#else
    if(boost::hash_detail::limits<T>::has_infinity) {
        T infinity = -log(zero);
        T infinity2 = (T) 1. / zero;
        T infinity3 = (T) -1. / minus_zero;
        T infinity4 = boost::hash_detail::limits<T>::infinity();

        T minus_infinity = log(zero);
        T minus_infinity2 = (T) -1. / zero;
        T minus_infinity3 = (T) 1. / minus_zero;

#if defined(TEST_EXTENSIONS)
        BOOST_TEST(x1(infinity) == HASH_NAMESPACE::hash_value(infinity));
        BOOST_TEST(x1(minus_infinity)
                == HASH_NAMESPACE::hash_value(minus_infinity));
#endif

        if(infinity == infinity2)
            BOOST_TEST(x1(infinity) == x1(infinity2));
        if(infinity == infinity3)
            BOOST_TEST(x1(infinity) == x1(infinity3));
        if(infinity == infinity4)
            BOOST_TEST(x1(infinity) == x1(infinity4));

        if(minus_infinity == minus_infinity2)
            BOOST_TEST(x1(minus_infinity) == x1(minus_infinity2));
        if(minus_infinity == minus_infinity3)
            BOOST_TEST(x1(minus_infinity) == x1(minus_infinity3));

        BOOST_TEST(infinity != minus_infinity);

        if(x1(infinity) == x1(minus_infinity)) {
            std::cerr<<"x1(infinity) == x1(-infinity) == "<<x1(infinity)<<"\n";
        }

        // This should really be 'has_denorm == denorm_present' but some
        // compilers don't have 'denorm_present'. See also a later use.
        if(boost::hash_detail::limits<T>::has_denorm) {
            if(x1(boost::hash_detail::limits<T>::denorm_min()) == x1(infinity))
            {
                std::cerr
                    <<  "x1(denorm_min) == x1(infinity) == "
                    <<  x1(infinity)
                    <<  "\n";
            }

            if(x1(boost::hash_detail::limits<T>::denorm_min()) ==
                x1(minus_infinity))
            {
                std::cerr
                    <<  "x1(denorm_min) == x1(-infinity) == "
                    <<  x1(minus_infinity)
                    <<  "\n";
            }
        }

        if(boost::hash_detail::limits<T>::has_quiet_NaN) {
            if(x1(boost::hash_detail::limits<T>::quiet_NaN()) == x1(infinity))
            {
                std::cerr
                    <<  "x1(quiet_NaN) == x1(infinity) == "
                    <<  x1(infinity)
                    <<  "\n";
            }

            if(x1(boost::hash_detail::limits<T>::quiet_NaN()) ==
                x1(minus_infinity))
            {
                std::cerr
                    <<  "x1(quiet_NaN) == x1(-infinity) == "
                    <<  x1(minus_infinity)
                    <<  "\n";
            }
        }
    }
#endif

    T max = (boost::hash_detail::limits<T>::max)();
    T half_max = max / 2;
    T quarter_max = max / 4;
    T three_quarter_max = max - quarter_max;

    // Check the limits::max is in range.
    BOOST_TEST(max != half_max);
    BOOST_TEST(max != quarter_max);
    BOOST_TEST(max != three_quarter_max);
    BOOST_TEST(half_max != quarter_max);
    BOOST_TEST(half_max != three_quarter_max);
    BOOST_TEST(quarter_max != three_quarter_max);

#if defined(TEST_EXTENSIONS)
    BOOST_TEST(x1(max) == HASH_NAMESPACE::hash_value(max));
    BOOST_TEST(x1(half_max) == HASH_NAMESPACE::hash_value(half_max));
    BOOST_TEST(x1(quarter_max) == HASH_NAMESPACE::hash_value(quarter_max));
    BOOST_TEST(x1(three_quarter_max) ==
        HASH_NAMESPACE::hash_value(three_quarter_max));
#endif

    // The '!=' tests could legitimately fail, but with my hash it indicates a
    // bug.
    BOOST_TEST(x1(max) == x1(max));
    BOOST_TEST(x1(max) != x1(quarter_max));
    BOOST_TEST(x1(max) != x1(half_max));
    BOOST_TEST(x1(max) != x1(three_quarter_max));
    BOOST_TEST(x1(quarter_max) == x1(quarter_max));
    BOOST_TEST(x1(quarter_max) != x1(half_max));
    BOOST_TEST(x1(quarter_max) != x1(three_quarter_max));
    BOOST_TEST(x1(half_max) == x1(half_max));
    BOOST_TEST(x1(half_max) != x1(three_quarter_max));
    BOOST_TEST(x1(three_quarter_max) == x1(three_quarter_max));

// Intel with gcc stdlib sometimes segfaults on calls to asin and acos.
#if !((defined(__INTEL_COMPILER) || defined(__ICL) || \
        defined(__ICC) || defined(__ECC)) && \
    (defined(__GLIBCPP__) || defined(__GLIBCXX__)))
    T v1 = asin((T) 1);
    T v2 = acos((T) 0);
    if(v1 == v2)
        BOOST_TEST(x1(v1) == x1(v2));

#if defined(TEST_EXTENSIONS)
    BOOST_TEST(x1(v1) == HASH_NAMESPACE::hash_value(v1));
    BOOST_TEST(x1(v2) == HASH_NAMESPACE::hash_value(v2));
#endif

#endif

#if defined(TEST_EXTENSIONS)
    BOOST_TEST(x1(boost::hash_detail::limits<T>::epsilon()) ==
            HASH_NAMESPACE::hash_value(
                boost::hash_detail::limits<T>::epsilon()));
#endif

    BOOST_TEST(boost::hash_detail::limits<T>::epsilon() != (T) 0);
    if(x1(boost::hash_detail::limits<T>::epsilon()) == x1((T) 0))
        std::cerr<<"x1(epsilon) == x1(0) == "<<x1((T) 0)<<"\n";

    BOOST_TEST(-boost::hash_detail::limits<T>::epsilon() != (T) 0);
    if(x1(-boost::hash_detail::limits<T>::epsilon()) == x1((T) 0))
        std::cerr<<"x1(-epsilon) == x1(0) == "<<x1((T) 0)<<"\n";

    BOOST_TEST((T) 1 + boost::hash_detail::limits<T>::epsilon() != (T) 1);
    if(x1((T) 1 + boost::hash_detail::limits<T>::epsilon()) == x1((T) 1))
        std::cerr<<"x1(1 + epsilon) == x1(1) == "<<x1((T) 1)<<"\n";

    BOOST_TEST((T) 1 - boost::hash_detail::limits<T>::epsilon() != (T) 1);
    if(x1((T) 1 - boost::hash_detail::limits<T>::epsilon()) == x1((T) 1))
        std::cerr<<"x1(1 - epsilon) == x1(1) == "<<x1((T) 1)<<"\n";

    BOOST_TEST((T) -1 + boost::hash_detail::limits<T>::epsilon() != (T) -1);
    if(x1((T) -1 + boost::hash_detail::limits<T>::epsilon()) == x1((T) -1))
        std::cerr<<"x1(-1 + epsilon) == x1(-1) == "<<x1((T) -1)<<"\n";

    BOOST_TEST((T) -1 - boost::hash_detail::limits<T>::epsilon() != (T) -1);
    if(x1((T) -1 - boost::hash_detail::limits<T>::epsilon()) == x1((T) -1))
        std::cerr<<"x1(-1 - epsilon) == x1(-1) == "<<x1((T) -1)<<"\n";

    // As before.
    if(boost::hash_detail::limits<T>::has_denorm) {
        if(x1(boost::hash_detail::limits<T>::denorm_min()) == x1(zero)) {
            std::cerr<<"x1(denorm_min) == x1(zero) == "<<x1(zero)<<"\n";
        }
#if !BOOST_WORKAROUND(__DECCXX_VER,<70190006) && defined(TEST_EXTENSIONS)
        // The Tru64/CXX standard library prior to 7.1 contains a bug in the
        // specialization of boost::hash_detail::limits::denorm_min() for long
        // doubles which causes this test to fail.
        if(x1(boost::hash_detail::limits<T>::denorm_min()) !=
            HASH_NAMESPACE::hash_value(
                boost::hash_detail::limits<T>::denorm_min()))
        {
            std::cerr
                <<  "x1(boost::hash_detail::limits<T>::denorm_min()) = "
                <<  x1(boost::hash_detail::limits<T>::denorm_min())
                <<  "\nhash_value(boost::hash_detail::limits<T>::denorm_min())"
                    " = "
                <<  HASH_NAMESPACE::hash_value(
                        boost::hash_detail::limits<T>::denorm_min())
                <<  "\nx1(0) = "
                <<  x1(0)
                <<  "\n";
        }
#endif
    }

// NaN also causes borland to crash.
#if !defined(__BORLANDC__) && defined(TEST_EXTENSIONS)
    if(boost::hash_detail::limits<T>::has_quiet_NaN) {
        if(x1(boost::hash_detail::limits<T>::quiet_NaN()) == x1(1.0)) {
            std::cerr<<"x1(quiet_NaN) == x1(1.0) == "<<x1(1.0)<<"\n";
        }
        BOOST_TEST(x1(boost::hash_detail::limits<T>::quiet_NaN()) ==
            HASH_NAMESPACE::hash_value(
                boost::hash_detail::limits<T>::quiet_NaN()));
    }
#endif
}

#if defined(BOOST_MSVC)
#pragma warning(pop)
#endif


//
// Created by AIYOJ on 2019-03-02.
//

#include "log/algocomp_log.h"
#include "calibration/isotonic_regression.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace algocomp {
    IsotonicRegression::IsotonicRegression() {
        _logger = spdlog::get(ACLog::_logger_name);
        _logger->info("Initialize Isotonic Regression.");
    }

    IsotonicRegression::~IsotonicRegression() {
        _logger->info("Leave Isotonic Regression.");
    }

    std::vector<float> IsotonicRegression::calibrate(const std::vector<float> &scores) {
        std::vector<float> calibrated_scores;
        for (int i = 0; i < scores.size(); ++i) {
            auto calibrated_score = get(scores[i], _search_type);
            calibrated_scores.push_back(calibrated_score);
        }

        return calibrated_scores;
    }

    float IsotonicRegression::calibrate(float score) {
        float calibrated_score = get(score, _search_type);
        return calibrated_score;
    }

    std::unique_ptr<IsotonicRegression> IsotonicRegression::load(const std::string &path) {
        auto logger = spdlog::get(ACLog::_logger_name);

        try {
            std::ifstream file;
            file.open(path);
            if (file.is_open()) {
                std::unique_ptr<IsotonicRegression> output_calibration = std::unique_ptr<IsotonicRegression>(
                        new IsotonicRegression());
                std::string line;
                float pre = 0;
                bool order = true;
                std::vector<std::pair<float, float >> tmp_bins;
                while (!file.eof()) {
                    std::getline(file, line);
                    line = boost::algorithm::trim_copy(line);
                    if (line.length() == 0) {
                        continue;
                    }

                    std::vector<std::string> splits;
                    boost::algorithm::split(splits, line, boost::algorithm::is_any_of(" "));

                    if (splits.size() != 2) {
                        logger->error("Invalid isotonic regression coef {}", line);
                        continue;
                    }

                    float bin = boost::lexical_cast<float>(splits[0]);
                    float value = boost::lexical_cast<float>(splits[1]);

                    if (bin > pre) {
                        order = true;
                        pre = bin;
                    } else if (bin < pre) {
                        order = false;
                        pre = bin;
                    } else {
                        logger->error("the same bin: {}", bin);
                        break;
                    }

                    tmp_bins.push_back(std::make_pair(bin, value));
                }

                if (order) {
                    output_calibration->update(tmp_bins);
                } else {
                    // sort
                    std::sort(tmp_bins.begin(), tmp_bins.end(),
                              [](const std::pair<float, float> &a, const std::pair<float, float> &b) {
                                  return a.first < b.first;
                              });
                    output_calibration->update(tmp_bins);
                }
                if (file.is_open()) {
                    file.close();
                }
                if (output_calibration) {
                    logger->info("Natively loaded isotonic regression from {}.", path);
                    return output_calibration;
                } else {
                    logger->error("Fail to read calibration file when natively loading isotonic regression from {}.",
                                  path);
                }
            } else {
                logger->error(
                        "Fail to open isotonic regression file when natively loading isotonic regression from {}.",
                        path);
            }

        } catch (std::exception &ex) {
            logger->error("Fail to natively load isotonic regression from {}, detail: {}.", path, ex.what());
        }
        return nullptr;
    }

    void IsotonicRegression::update(const std::vector<std::pair<float, float>> &bins) {
        _bins.clear();
        _bins = bins;
    }

    float IsotonicRegression::get(float bin, const SearchType search_type) {
        // upper bound
        auto upper_bound = [this](float key) {
            int first = 0;
            int len = _bins.size() - 1;
            int half, middle;

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                // The median is greater than the key and is looked up in the left half of the sequence containing last.
                if (_bins[middle].first > key) {
                    len = half;
                } else {  // The median is less than or equal to the key and is looked up in the right half of the sequence.
                    first = middle + 1;
                    len = len - half - 1;
                }
            }
            return first;
        };

        // lower bound
        auto lower_bound = [this](float key) {
            int first = 0, middle;
            int half, len;
            len = _bins.size();

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                if (_bins[middle].first < key) {
                    first = middle + 1;
                    len = len - half - 1;
                } else {
                    len = half;
                }
            }
            return first;
        };

        // search type
        if (search_type == SearchType::UPPERBOUND) {
            auto index = upper_bound(bin);
            return _bins[index].second;
        } else if (search_type == SearchType::LOWERBOUND) {
            auto index = lower_bound(bin);
            return _bins[index].second;
        } else {
            _logger->error("Unknown Search Type.");
        }
    }
}


#pragma once

#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/stringize.hpp>
#include <boost/preprocessor/cat.hpp>

#include "freq_index.hpp"
#include "positive_sequence.hpp"
#include "partitioned_sequence.hpp"
#include "uniform_partitioned_sequence.hpp"
#include "binary_freq_collection.hpp"
#include "block_freq_index.hpp"
#include "block_codecs.hpp"

namespace quasi_succinct {

    typedef freq_index<compact_elias_fano,
                       positive_sequence<strict_elias_fano>> ef_index;

    typedef freq_index<indexed_sequence,
                       positive_sequence<>> single_index;

    typedef freq_index<
        uniform_partitioned_sequence<>,
        positive_sequence<uniform_partitioned_sequence<strict_sequence>>
        > uniform_index;

    typedef freq_index<
        partitioned_sequence<>,
        positive_sequence<partitioned_sequence<strict_sequence>>
        > opt_index;

    typedef block_freq_index<quasi_succinct::optpfor_block> block_optpfor_index;

    typedef block_freq_index<quasi_succinct::varint_G8IU_block> block_varint_index;

    typedef block_freq_index<quasi_succinct::interpolative_block> block_interpolative_index;
}

#define QS_INDEX_TYPES (ef)(single)(uniform)(opt)(block_optpfor)(block_varint)(block_interpolative)


/**
 * \copyright
 * Copyright (c) 2012-2016, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "LinearSolver.h"

namespace
{
//! Describes the default linear solver type to be used together with the given
//! \c Matrix and \c Vector types.
template<typename Matrix, typename Vector>
struct DefaultLinearSolver;
}

namespace MathLib
{
template <typename Matrix, typename Vector, typename Solver>
std::unique_ptr<LinearSolver<Matrix, Vector>> createLinearSolver(
    BaseLib::ConfigTree const* const config)
{
    using AbstractLS = LinearSolver<Matrix, Vector>;
    return std::unique_ptr<AbstractLS>{new Solver{"", config}};
}

template <typename Matrix, typename Vector>
std::unique_ptr<LinearSolver<Matrix, Vector>> createLinearSolver(
    BaseLib::ConfigTree const* const config)
{
    using Solver = typename DefaultLinearSolver<Matrix, Vector>::Solver;
    return createLinearSolver<Matrix, Vector, Solver>(config);
}

} // MathLib

// specializes the DefaultLinearSolver<> class
#define DEFAULT_LINEAR_SOLVER(MAT, VEC, SLV) \
    namespace { \
    template<> struct DefaultLinearSolver<MAT, VEC> { \
        using Solver = SLV; \
    };}

// does explicit template instantiation of createLinearSolver<>()
#define CREATE_LINEAR_SOLVER(MAT, VEC, SLV) \
    namespace MathLib { \
    template std::unique_ptr<LinearSolver<MAT, VEC> > \
    createLinearSolver<MAT, VEC, SLV>(BaseLib::ConfigTree const*const); \
    template std::unique_ptr<LinearSolver<MAT, VEC> > \
    createLinearSolver<MAT, VEC>(BaseLib::ConfigTree const*const); \
    }

// both of the above
#define SPECIALIZE_CREATE_LINEAR_SOLVER(MAT, VEC, SLV) \
    DEFAULT_LINEAR_SOLVER(MAT, VEC, SLV) \
    CREATE_LINEAR_SOLVER(MAT, VEC, SLV)

#ifdef OGS_USE_EIGEN

#include <Eigen/QR>

namespace MathLib
{

//! Matches Eigen solvers to our interface.
class EigenDenseSolverWrapper
        : public LinearSolver<Eigen::MatrixXd, Eigen::VectorXd>
{
public:
    EigenDenseSolverWrapper(std::string const&, BaseLib::ConfigTree const*const) {}

    bool solve(Eigen::MatrixXd &A, Eigen::VectorXd &b, Eigen::VectorXd &x) override
    {
        _solver.compute(A);
        x = _solver.solve(b);
        return true; // TODO add checks
    }

private:
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> _solver;
};

} // namespace MathLib

SPECIALIZE_CREATE_LINEAR_SOLVER(Eigen::MatrixXd, Eigen::VectorXd,
                                MathLib::EigenDenseSolverWrapper)

#endif

#ifdef OGS_USE_LIS

#include "MathLib/LinAlg/EigenLis/EigenLisLinearSolver.h"

SPECIALIZE_CREATE_LINEAR_SOLVER(MathLib::EigenMatrix, MathLib::EigenVector,
                                MathLib::EigenLisLinearSolver)

#elif defined(USE_PETSC)

#include "MathLib/LinAlg/PETSc/PETScLinearSolver.h"

SPECIALIZE_CREATE_LINEAR_SOLVER(MathLib::PETScMatrix, MathLib::PETScVector,
                                MathLib::PETScLinearSolver)

#elif defined(OGS_USE_EIGEN)

#include "MathLib/LinAlg/Eigen/EigenLinearSolver.h"

SPECIALIZE_CREATE_LINEAR_SOLVER(MathLib::EigenMatrix, MathLib::EigenVector,
                                MathLib::EigenLinearSolver)

#endif


#pragma once

#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>

#include <vector>
#include <map>
#include <cstdint>

namespace ISUE {
  namespace RelocForests {

    /// Labeled data point for training
    class LabeledPixel {
    public:
      LabeledPixel(uint32_t frame, cv::Point2i pos, cv::Point3f label)
        : frame_(frame), pos_(pos), label_(label)
      {}
      uint32_t frame_;
      cv::Point2i pos_;
      cv::Point3f label_;
    };

    /// struct which holds pose for calculating world coordinates
    /// for labeled pixels
    struct Pose {
      Eigen::Matrix3d rotation;
      Eigen::Vector3d position;

      Pose(Eigen::Matrix3d rotation, Eigen::Vector3d position)
        :rotation(rotation), position(position)
      {}
    };

    class Data {
    public:
      Data()
      {
        num_frames_ = 0;
      }

      Data(std::string path_to_data)
        : data_path_(path_to_data)
      {
        num_frames_ = 0;
      };

      cv::Mat GetRGBImage(uint32_t frame)
      {
        if (rgb_images_.count(frame)) {
          return rgb_images_.at(frame);
        }
        else {
          cv::Mat img = cv::imread(data_path_ + rgb_names_.at(frame));
          rgb_images_.insert(std::pair<uint32_t, cv::Mat>(frame, img));
          return img;
        }
      }

      cv::Mat GetDepthImage(uint32_t frame)
      {
        if (depth_images_.count(frame)) {
          return depth_images_.at(frame);
        }
        else {
          cv::Mat img = cv::imread(data_path_ + depth_names_.at(frame), CV_LOAD_IMAGE_ANYDEPTH);
          depth_images_.insert(std::pair<uint32_t, cv::Mat>(frame, img));
          return img;
        }
      }

      Pose GetPose(uint32_t frame)
      {
        return poses_.at(frame);
      }

      void AddFrame(cv::Mat rgb_frame, cv::Mat depth_frame, Pose pose)
      {
        rgb_images_.insert(std::pair<uint32_t, cv::Mat>(num_frames_, rgb_frame));
        depth_images_.insert(std::pair<uint32_t, cv::Mat>(num_frames_, depth_frame));
        poses_.push_back(pose);
        num_frames_++;
      }

      void AddFrame(std::string rgb_frame, std::string depth_frame, Pose pose)
      {
        poses_.push_back(pose);
        depth_names_.push_back(depth_frame);
        rgb_names_.push_back(rgb_frame);
        num_frames_++;
      }

      int GetNumFrames()
      {
        return num_frames_;
      }

      /// Write all frames to a folder. Write all poses to poses.txt.
      void Serialize(std::string data_path)
      {
        std::ofstream o(data_path + "poses.txt");
        o << GetNumFrames() << "\n";
        for (int i = 0; i < GetNumFrames(); i++) {
          cv::Mat rgb = GetRGBImage(i);
          cv::Mat depth = GetDepthImage(i);
          cv::Mat depth_w;
          depth.convertTo(depth_w, CV_16U, 5000); // scaled meter values

          cv::imwrite(data_path + std::to_string(i) + "_rgb.png", rgb);
          cv::imwrite(data_path + std::to_string(i) + "_d.png", depth_w);

          Pose p = poses_.at(i);
          Eigen::Quaterniond q(p.rotation);
          o << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " "
            << p.position.x() << " " << p.position.y() << " " << p.position.z() << "\n";
        }
      }

      void Deserialize(std::string data_path)
      {
        data_path_ = data_path;
        float qx, qy, qz, qw, tx, ty, tz;

        std::ifstream in(data_path + "poses.txt");

        std::string num("");
        getline(in, num);
        std::stringstream num_s(num);

        in >> this->num_frames_;

        for (int i = 1; i <= GetNumFrames(); i++) {
          cv::Mat rgb =  cv::imread(data_path + std::to_string(i) + "_rgb.png");
          cv::Mat depth =  cv::imread(data_path + std::to_string(i) + "_d.png");

          std::string temp("");
          getline(in, temp);

          std::stringstream stream(temp);

          stream >> qx; stream >> qy;
          stream >> qz; stream >> qw;
          stream >> tx; stream >> ty; stream >> tz;

          Pose pose(Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(), Eigen::Vector3d(tx, ty, tz));

          AddFrame(rgb, depth, pose);
        }
      }

      private:
        std::vector<std::string> depth_names_;
        std::vector<std::string> rgb_names_;

        // loaded images
        std::map<uint32_t, cv::Mat> rgb_images_;
        std::map<uint32_t, cv::Mat> depth_images_;
        std::vector<Pose> poses_;

        std::string data_path_;

        uint32_t num_frames_;
    };
  }
}


/*
Copyright 2012 The University of New South Wales (UNSW).

This file is part of the 2012 team rUNSWift RoboCup entry. You may
redistribute it and/or modify it under the terms of the GNU General
Public License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version as
modified below. As the original licensors, we add the following
conditions to that license:

In paragraph 2.b), the phrase "distribute or publish" should be
interpreted to include entry into a competition, and hence the source
of any derived work entered into a competition must be made available
to all parties involved in that competition under the terms of this
license.

In addition, if the authors of a derived work publish any conference
proceedings, journal articles or other academic papers describing that
derived work, then appropriate academic citations to the original work
must be included in that publication.

This rUNSWift source is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
General Public License for more details.

You should have received a copy of the GNU General Public License along
with this source code; if not, write to the Free Software Foundation,
Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/

#pragma once

#include "LocalisationDefs.hpp"
#include "types/AbsCoord.hpp"
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>
#include <ios>

#define ABS_DIST_THRESHOLD 500
#define MAHA_DIST_THRESHOLD 3

#define TEAM_BALL_ABS_THRESHOLD 750.0
#define TEAM_BALL_ABS_FAR_THRESHOLD 2000.0
#define TEAM_BALL_MAHA_THRESHOLD 1.0

#define MY_ASSERT(X, Y) if (!(X)) { std::cout << "ERROR DETECTED: " << Y << std::endl; } assert(X)
#define MY_ASSERT_2(X) if (!(X)) { std::cout << "ERROR DETECTED" << std::endl; } assert(X)

bool absDistClose(const AbsCoord &abs1, const AbsCoord &abs2, float distThreshold);

bool absClose(const AbsCoord &abs1, const AbsCoord &abs2, float headingThreshold);

float minMahalanobisDistance(const AbsCoord &abs1, const AbsCoord &abs2);

float mahalanobisDistance(const AbsCoord &abs, const AbsCoord &state);

float mahalanobisDistancePos(const AbsCoord &abs, const AbsCoord &state);

float mahalanobisDistancePosSQR(const AbsCoord &abs, const AbsCoord &state);

float mahalanobisDistanceSQR(const PointF &obs, const PointF &state, const Eigen::Matrix<float, 2, 2> &state_var);

/**
 * Gets the variance that corresponds to being 95% confident the variable falls within the given limit.
 */
inline float get95CF(float limit) {
   return limit * limit / 4.0f;
}

template<int ROWS, int COLS>
Eigen::MatrixXd copyStaticToDynamicMatrix(const Eigen::Matrix<float, ROWS, COLS> &matrix) {
   Eigen::MatrixXd result(ROWS, COLS);
   for (int i = 0; i < ROWS; i++) {
      for (int j = 0; j < COLS; j++) {
         result(i, j) = matrix(i, j);
      }
   }
   return result;
}

Eigen::MatrixXd sparseMultiplication(
      const Eigen::MatrixXd &lhs, unsigned rowStart, unsigned colStart, unsigned rowEnd, unsigned colEnd,
      const Eigen::MatrixXd &rhs);

Eigen::MatrixXd sparseMultiplication(
      const Eigen::MatrixXd &lhs,
      const Eigen::MatrixXd &rhs, unsigned rowStart, unsigned colStart, unsigned rowEnd, unsigned colEnd);

inline void prettyOutputMatrix(std::string name, const Eigen::MatrixXd &matrix) {
   if (matrix.cols() == 1) {
      std::cout << name << ":";
      for (int i = 0; i < matrix.rows(); i++) {
         std::cout << "\t" << std::fixed << std::setprecision(1) << matrix(i, 0);
      }
      std::cout << std::endl;
   } else {
      std::cout << name << std::endl;
      for (int i = 0; i < matrix.rows(); i++) {
         for (int j = 0; j < matrix.cols(); j++) {
            std::cout << std::fixed << std::setprecision(1) << matrix(i, j) << "\t";
         }
         std::cout << std::endl;
      }
   }
}

/* logging */
void logAbsCoord(const AbsCoord abs, std::string name);
void logAbsCoordVar(const AbsCoord abs, std::string name);
void logAbsCoordCov(const AbsCoord abs, std::string name);


#include "Shapes/shapes.h"

#include <boost/format.hpp>
#include <boost/functional/overloaded_function.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <opencv2/opencv.hpp>

#include <algorithm>

#include <cassert>
#include <cctype>
#include <cmath>
#include <cstddef>

namespace {

std::string filterWhitespaces(const std::string& aData)
{
    return boost::copy_range<std::string>(aData | boost::adaptors::filtered([](const char c) { return !std::isspace(c); }));
}

bool checkDataCharsAreValid(const std::string& aData, const char aDot, const char aEmpty)
{
    return std::all_of(aData.cbegin(), aData.cend(), [=](const char c) { return c == aDot || c == aEmpty; });
}

boost::optional<Shapes::Error> validateData(const std::string& aData, const size_t aDataWidth, const size_t aDataHeight, const char aDot, const char aEmpty)
{
    if (aDot == aEmpty || !checkDataCharsAreValid(aData, aDot, aEmpty)) {
        return Shapes::Error::InvalidData;
    }

    if (aDataWidth == 0 || aDataHeight == 0 || aData.size() != aDataWidth * aDataHeight) {
        return Shapes::Error::InvalidDimensions;
    }

    return {};
}

template <typename T, boost::optional<T> (*recognize)(const cv::Mat&)>
boost::optional<Shapes::Shape> adaptRecognizer(const cv::Mat& mat)
{
    assert(recognize);
    auto result = recognize(mat);
    if (result) {
        return {{*result}};
    }
    return {};
}

Shapes::Shape recognizeMat(const cv::Mat& aMat)
{
    using Recognizer = boost::optional<Shapes::Shape> (*)(const cv::Mat&);
    const Recognizer recognizers[] = { adaptRecognizer<Shapes::Square, Shapes::recognizeSquare>,
        adaptRecognizer<Shapes::Circle, Shapes::recognizeCircle> };

    for (const auto& recognize : recognizers) {
        const auto result = recognize(aMat);
        if (result) {
            return *result;
        }
    }

    return Shapes::Unknown{};
}
}

namespace Shapes {

RecognitionResult recognize(const std::string& aChars, const size_t aDataWidth, const size_t aDataHeight, const char aDot, const char aEmpty)
{
    const auto filteredChars = filterWhitespaces(aChars);
    const auto error = validateData(filteredChars, aDataWidth, aDataHeight, aDot, aEmpty);
    if (error) {
        return *error;
    }

    auto bytes = boost::copy_range<std::vector<unsigned char>>(filteredChars | boost::adaptors::transformed([=](const char c) -> unsigned char {
        assert(c == aDot || c == aEmpty);
        return c == aDot ? 255 : 0;
    }));

    assert(bytes.size() == aDataWidth * aDataHeight);
    cv::Mat cvMat(aDataWidth, aDataHeight, CV_8UC1, bytes.data());
    return recognizeMat(cvMat);
}

std::string shapeToString(Shapes::Shape aShape)
{
    auto visitor = boost::make_overloaded_function(
        [](const Shapes::Square& square) { return (boost::format("square with left: %1% top: %2% side %3%") % square.left % square.top % square.side).str(); },
        [](const Shapes::Circle& circle) { return (boost::format("circle with x: %1% y: %2% radius: %3%") % circle.x % circle.y % circle.radius).str(); },
        [](const Shapes::Unknown&) { return std::string("unknown shape"); });

    return boost::apply_visitor(visitor, aShape);
}
}


#pragma once

#include <Eigen/Core>
#include <array>
#include <string>
#include <vector>

// #include "CellQuad4.hpp"
#include "Variable.hpp"

class CellQuad4;
class Node2d
{
public:
    Node2d();
    Node2d(unsigned int id, double x, double y);
    ~Node2d();

    void AddCellPointer(CellQuad4* p_cellquad4);
    unsigned int GetID() const;
    Eigen::Vector2d GetCoords() const;
    std::vector<CellQuad4*> GetCellPtrs();
    void AddMarker(std::string marker);
    bool IsInterior() const;
    bool IsCorner() const;

    Variable nodevar;

private:
    unsigned int id_;
    Eigen::Vector2d coords_;
    std::vector<CellQuad4*> pCellQuad4s_;

    //! markers to detect the node status
    std::vector<std::string> markers_;
};


#include <boost/geometry/algorithms/correct_closure.hpp>


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. 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.
 *
 *    3. Neither the name of the copyright holder 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 HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include <camera/camera.h>
#include <camera/camera_extrinsics.h>
#include <camera/camera_intrinsics.h>
#include <geometry/eight_point_algorithm_solver.h>
#include <geometry/fundamental_matrix_solver.h>
#include <geometry/rotation.h>
#include <math/random_generator.h>
#include <matching/pairwise_image_match.h>

#include <Eigen/Core>
#include <gtest/gtest.h>

namespace bsfm {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {
const int kImageWidth = 1920;
const int kImageHeight = 1080;
const double kVerticalFov = D2R(90.0);
const int kFeatureMatches = 200;
} //\namespace

TEST(EightPointAlgorithmSolver, TestEightPointAlgorithmSolver) {
  // Create a random number generator.
  math::RandomGenerator rng(0);

  // Create two cameras. By default they will have identity extrinsics.
  Camera camera1;
  Camera camera2;

  // Give the two cameras the same intrinsics.
  CameraIntrinsics intrinsics;
  intrinsics.SetImageLeft(0);
  intrinsics.SetImageTop(0);
  intrinsics.SetImageWidth(kImageWidth);
  intrinsics.SetImageHeight(kImageHeight);
  intrinsics.SetVerticalFOV(kVerticalFov);
  intrinsics.SetFU(intrinsics.f_v());
  intrinsics.SetCU(0.5 * kImageWidth);
  intrinsics.SetCV(0.5 * kImageHeight);

  camera1.SetIntrinsics(intrinsics);
  camera2.SetIntrinsics(intrinsics);

  // Translate and rotate the second camera.
  CameraExtrinsics extrinsics;
  extrinsics.Translate(2.0, 0.0, 0.0);
  Vector3d euler_angles(Vector3d::Random()*D2R(20.0));
  extrinsics.Rotate(EulerAnglesToMatrix(euler_angles));
  camera2.SetExtrinsics(extrinsics);

  // Create a bunch of points in 3D.
  PairwiseImageMatch image_match;
  while (image_match.feature_matches_.size() < kFeatureMatches) {
    const double x_world = rng.DoubleUniform(-4.0, 4.0);
    const double y_world = rng.DoubleUniform(-4.0, 4.0);
    const double z_world = rng.DoubleUniform(3.0, 10.0);

    // Project each of the 3D points into the two cameras.
    double u1 = 0.0, v1 = 0.0;
    double u2 = 0.0, v2 = 0.0;
    const bool in_camera1 =
        camera1.WorldToImage(x_world, y_world, z_world, &u1, &v1);
    const bool in_camera2 =
        camera2.WorldToImage(x_world, y_world, z_world, &u2, &v2);

    // Make sure the point is visible to both cameras... we want feature
    // matches.
    if (!(in_camera1 && in_camera2)) {
      continue;
    }

    // Store this as a feature match.
    FeatureMatch match;
    match.feature1_.u_ = u1;
    match.feature1_.v_ = v1;
    match.feature2_.u_ = u2;
    match.feature2_.v_ = v2;
    image_match.feature_matches_.push_back(match);
  }

  // Use the solver to find the fundamental matrix for the two sets of points.
  EightPointAlgorithmSolver solver;
  solver.AddMatchedImagePair(image_match);

  // Test both with and without normalization.
  FundamentalMatrixSolverOptions options;
  solver.SetOptions(options);

  std::vector<Matrix3d> fundamental_matrices1;
  std::vector<Matrix3d> fundamental_matrices2;
  EXPECT_TRUE(solver.ComputeFundamentalMatrices(fundamental_matrices1));

  options.normalize_features = false;
  solver.SetOptions(options);
  EXPECT_TRUE(solver.ComputeFundamentalMatrices(fundamental_matrices2));

  // We should have 1 fundamental matrix (we only put in 1 image pair).
  if (fundamental_matrices1.size() > 0 && fundamental_matrices2.size() > 0) {
    Matrix3d F_normalized = fundamental_matrices1[0];
    Matrix3d F_unnormalized = fundamental_matrices2[0];

    // For every feature pair, we should have x1^{T}*F*x2 = 0.
    for (const auto& match : image_match.feature_matches_) {
      Vector3d x1, x2;
      x1 << match.feature1_.u_, match.feature1_.v_, 1;
      x2 << match.feature2_.u_, match.feature2_.v_, 1;

      EXPECT_NEAR(0.0, x2.transpose() * F_normalized * x1, 1e-8);
      EXPECT_NEAR(0.0, x2.transpose() * F_unnormalized * x1, 1e-8);
    }
  }
}

}  //\namespace bsfm


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_INC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_INC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing if_inc capabilities

    Increments a value by @ref One if a predicate is @ref True .

    @par Semantic:

    For every parameters @c c of type @c C and @c t of type @c T :

    @code
    T r = if_inc(c, t);
    @endcode

    is similar to:

    @code
    T r = c ? t + One<T>() : t;
    @endcode
  **/
  Value if_inc(Value const& c, Value const& v0);

  //@overload
  Value if_inc(LogicalValue const& c, Value const& v0);
} }
#endif

#include <boost/simd/function/scalar/if_inc.hpp>
#include <boost/simd/function/simd/if_inc.hpp>

#endif