// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <vinecopulib/misc/tools_stats.hpp>
#include <vinecopulib/misc/tools_stl.hpp>

#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <cmath>
#include <iostream>
#include <wdm/eigen.hpp>

namespace vinecopulib {

namespace tools_select {

using namespace tools_stl;

//! @brief Calculates criterion for tree selection.
//! @param data Observations.
//! @param tree_criterion The criterion.
//! @param weights Vector of weights for each observation (can be empty).
inline double
calculate_criterion(const Eigen::MatrixXd& data,
                    std::string tree_criterion,
                    Eigen::VectorXd weights)
{
  double w = 0.0;
  Eigen::MatrixXd data_no_nan = data;
  tools_eigen::remove_nans(data_no_nan, weights);
  double freq =
    static_cast<double>(data_no_nan.rows()) / static_cast<double>(data.rows());
  if (data_no_nan.rows() > 10) {
    if (tree_criterion == "mcor") {
      w = tools_stats::pairwise_mcor(data_no_nan, weights);
    } else if (tree_criterion == "joe") {
      // mutual information for Gaussian copula
      w = wdm::wdm(tools_stats::qnorm(data_no_nan), "pearson", weights)(0, 1);
      w = -0.5 * std::log(1 - w * w);
    } else {
      w = wdm::wdm(data_no_nan, tree_criterion, weights)(0, 1);
    }

    if (std::isnan(w)) {
      w = 0.0;
    }
  }
  return std::fabs(w) * std::sqrt(freq);
}

//! @brief Evaluates maximal criterion for tree selection.
//! @param data Observations.
//! @param tree_criterion The criterion.
//! @param weights Vector of weights for each observation (can be empty).
inline Eigen::MatrixXd
calculate_criterion_matrix(const Eigen::MatrixXd& data,
                           const std::string& tree_criterion,
                           const Eigen::VectorXd& weights)
{
  size_t n = data.rows();
  size_t d = data.cols();
  Eigen::MatrixXd mat(d, d);
  mat.diagonal() = Eigen::VectorXd::Constant(d, 1.0);
  Eigen::MatrixXd pair_data(n, 2);
  for (size_t i = 1; i < d; ++i) {
    for (size_t j = 0; j < i; ++j) {
      pair_data.col(0) = data.col(i);
      pair_data.col(1) = data.col(j);
      Eigen::VectorXd pair_w = weights;
      mat(i, j) = calculate_criterion(pair_data, tree_criterion, pair_w);
      mat(j, i) = mat(i, j);
    }
  }
  return mat;
}

//! computes
inline std::vector<size_t>
get_disc_cols(std::vector<std::string> var_types)
{
  size_t d = var_types.size();
  std::vector<size_t> disc_cols(d);
  size_t disc_count = 0;
  for (size_t i = 0; i < d; ++i) {
    if (var_types[i] == "d") {
      disc_cols[i] = disc_count++;
    } else {
      disc_cols[i] = 0;
    }
  }
  return disc_cols;
}

inline VinecopSelector::VinecopSelector(const Eigen::MatrixXd& data,
                                        const FitControlsVinecop& controls,
                                        std::vector<std::string> var_types)
  : n_(data.rows())
  , d_(var_types.size())
  , var_types_(var_types)
  , controls_(controls)
  , pool_(controls_.get_num_threads())
  , trees_(std::vector<VineTree>(1))
  , threshold_(controls.get_threshold())
  , psi0_(controls.get_psi0())
{
  vine_struct_ = RVineStructure(tools_stl::seq_int(1, d_), 1, false);
}

inline VinecopSelector::VinecopSelector(const Eigen::MatrixXd& data,
                                        const RVineStructure& vine_struct,
                                        const FitControlsVinecop& controls,
                                        std::vector<std::string> var_types)
  : VinecopSelector(data, controls, var_types)
{
  vine_struct_ = vine_struct;
  structure_known_ = false;
}

inline std::vector<std::vector<Bicop>>
VinecopSelector::get_pair_copulas() const
{
  return pair_copulas_;
}

inline RVineStructure
VinecopSelector::get_rvine_structure() const
{
  return vine_struct_;
}

//! @brief Instantiates the object for storing pair copulas.
//! @param d Dimension of the vine copula.
//! @param trunc_lvl A truncation level (optional).
//! @return A nested vector such that `pc_store[t][e]` contains a Bicop.
//!     object for the pair copula corresponding to tree `t` and edge `e`.
inline std::vector<std::vector<Bicop>>
VinecopSelector::make_pair_copula_store(size_t d, size_t trunc_lvl)
{
  if (d < 1) {
    throw std::runtime_error("dimension must be be > 0.");
  }

  size_t n_trees = std::min(d - 1, trunc_lvl);
  std::vector<std::vector<Bicop>> pc_store(n_trees);
  for (size_t t = 0; t < n_trees; ++t) {
    pc_store[t].resize(d - 1 - t);
  }

  return pc_store;
}

inline void
VinecopSelector::select_all_trees(const Eigen::MatrixXd& data)
{
  loglik_ = 0.0;
  initialize_new_fit(data);
  for (size_t t = 0; t < d_ - 1; ++t) {
    select_tree(t); // select pair copulas (+ structure) of tree t
    loglik_ += get_loglik_of_tree(t);

    if (controls_.get_show_trace()) {
      std::stringstream tree_heading;
      std::cout << "** Tree: " << t << std::endl;
      print_pair_copulas_of_tree(t);
    }

    if (controls_.get_trunc_lvl() == t + 1) {
      // don't need to fit the remaining trees
      break;
    }
  }
  finalize(controls_.get_trunc_lvl());
}

inline void
VinecopSelector::sparse_select_all_trees(const Eigen::MatrixXd& data)
{
  // family set must be reset after each iteration of the threshold search
  auto family_set = controls_.get_family_set();
  double d = static_cast<double>(d_);

  std::vector<double> thresholded_crits;
  double mbicv_opt = std::numeric_limits<double>::max();
  bool needs_break = false;
  while (!needs_break) {
    // restore family set in case previous threshold iteration also
    // truncated the model
    controls_.set_family_set(family_set);
    controls_.set_trunc_lvl(std::numeric_limits<size_t>::max());
    initialize_new_fit(data);

    // decrease the threshold
    // (in the first iteration thresholded_crits is empty and the threshold is
    // set to 1.0, which fits an independence model)
    if (controls_.get_select_threshold()) {
      controls_.set_threshold(get_next_threshold(thresholded_crits));
      if (controls_.get_show_trace()) {
        std::cout << "***** threshold: " << controls_.get_threshold()
                  << std::endl;
      }
    }

    // helper variables for checking whether an optimum was found
    double mbicv = 0.0;
    double mbicv_trunc = 0.0;
    double loglik = 0.0;
    bool select_trunc_lvl = controls_.get_select_trunc_lvl();
    bool select_threshold = controls_.get_select_threshold();
    double num_changed = 0.0;
    double num_total = d * (d - 1.0) / 2.0;

    for (size_t t = 0; t < d_ - 1; ++t) {
      if (controls_.get_trunc_lvl() < t) {
        break; // don't need to fit the remaining trees
      }

      // select pair copulas (and possibly tree structure)
      select_tree(t);
      num_changed += d - 1 - static_cast<double>(t);

      // update fit statistic
      double loglik_tree = get_loglik_of_tree(t);
      loglik += loglik_tree;
      double mbicv_tree = get_mbicv_of_tree(t, loglik_tree);
      mbicv_trunc += mbicv_tree;

      // print trace for this tree level
      if (controls_.get_show_trace()) {
        std::cout << "** Tree: " << t;
        if (select_trunc_lvl) {
          std::cout << ", mbicv: " << mbicv_tree << ", loglik: " << loglik_tree;
        }
        std::cout << std::endl;
        print_pair_copulas_of_tree(t);
      }

      // mbicv comparison for truncation level (check only after 10% of
      // copulas change to avoid getting stuck in local minimum)
      if (num_changed / num_total > 0.1) {
        num_changed = 0.0;
        if (select_trunc_lvl & (mbicv_trunc >= mbicv) & (t > 0)) {
          // mbicv did not improve
          // check if it can be improved by removing trees
          loglik -= loglik_tree;
          mbicv_trunc -= mbicv_tree;
          while (t > 1) {
            loglik_tree = get_loglik_of_tree(t - 1);
            mbicv_tree = get_mbicv_of_tree(t - 1, loglik_tree);
            if (mbicv_tree <= 0)
              break;
            loglik -= loglik_tree;
            mbicv_trunc -= mbicv_tree;
            t--;
          }
          set_current_fit_as_opt(loglik);
          controls_.set_trunc_lvl(t);
          if (!select_threshold) {
            // fixed threshold, no need to continue
            needs_break = true;
          }
        } else {
          mbicv = mbicv_trunc;
        }
      }
    }

    if (controls_.get_show_trace()) {
      std::cout << "--> mbicv = " << mbicv << ", loglik = " << loglik
                << std::endl
                << std::endl;
    }

    // check whether mbicv-optimal model has been found
    if (mbicv == 0.0) {
      //// CASE: 0-truncated model is best for this threshold
      set_current_fit_as_opt(loglik);
      if (!select_threshold) {
        // threshold is fixed and trunc_lvl has been found -> stop
        needs_break = true;
      }
    } else if (mbicv >= mbicv_opt) {
      //// CASE: old model is optimal
      needs_break = true;
    } else {
      //// CASE: optimum hasn't been found
      set_current_fit_as_opt(loglik);
      mbicv_opt = mbicv;
      // while loop is only for threshold selection
      needs_break = needs_break || !select_threshold;
      // threshold is too close to 0
      needs_break = needs_break || (controls_.get_threshold() < 0.01);
      // prepare for possible next iteration
      thresholded_crits = get_thresholded_crits();
    }
  }

  // set final model
  trees_ = trees_opt_;
  finalize(controls_.get_trunc_lvl());
}

inline void
VinecopSelector::set_tree_to_indep(size_t t)
{
  // trees_[0] is base tree, see make_base_tree()
  for (auto e : boost::edges(trees_[t + 1])) {
    trees_[t + 1][e].pair_copula = Bicop();
  }
}

// extracts the current loglik value
inline double
VinecopSelector::get_loglik() const
{
  return loglik_;
}

// extracts the current threshold value
inline double
VinecopSelector::get_threshold() const
{
  return threshold_;
}

// extracts the number of observations
inline size_t
VinecopSelector::get_nobs() const
{
  return n_;
}

//! chooses threshold for next iteration such that at a proportion of at
//! least 2.5% of the previously thresholded pairs become non-thresholded.
inline double
VinecopSelector::get_next_threshold(std::vector<double>& thresholded_crits)
{
  if (thresholded_crits.size() == 0) {
    return 1.0;
  }
  // sort in descending order
  std::sort(thresholded_crits.begin(), thresholded_crits.end());
  std::reverse(thresholded_crits.begin(), thresholded_crits.end());
  // pick threshold that changes at least alpha*100 % of the pair-copulas
  double alpha = 0.05;
  size_t m = thresholded_crits.size();
  double new_index = std::ceil(static_cast<double>(m) * alpha) - 1;
  return thresholded_crits[static_cast<size_t>(new_index)];
}

//! @brief Adds edges allowed by either the proximity condition or the vine
//  structure.
//!
//! If all the edges allowed by the proximity condition are included, then
//! the function also calculates the edge weight
//! (e.g., 1-|tau| for tree_criterion = "itau").
//!
//! @param vine_tree Tree of a vine.
inline void
VinecopSelector::add_allowed_edges(VineTree& vine_tree)
{
  std::string tree_criterion = controls_.get_tree_criterion();
  if (structure_known_) {
    double threshold = controls_.get_threshold();
    std::mutex m;
    auto add_edge = [&](size_t v0) {
      tools_interface::check_user_interrupt(v0 % 50 == 0);
      for (size_t v1 = 0; v1 < v0; ++v1) {
        // check proximity condition: common neighbor in previous tree
        // (-1 means 'no common neighbor')
        if (find_common_neighbor(v0, v1, vine_tree) > -1) {
          auto pc_data = get_pc_data(v0, v1, vine_tree);
          double crit = calculate_criterion(
            pc_data, tree_criterion, controls_.get_weights());
          double w = 1.0 - static_cast<double>(crit >= threshold) * crit;
          {
            std::lock_guard<std::mutex> lk(m);
            auto e = boost::add_edge(v0, v1, w, vine_tree).first;
            vine_tree[e].weight = w;
            vine_tree[e].crit = crit;
          }
        }
      }
    };

    pool_.map(add_edge, boost::vertices(vine_tree));
    pool_.wait();
  } else {
    size_t tree = d_ - boost::num_vertices(vine_tree);
    size_t edges = boost::num_vertices(vine_tree) - 1;
    size_t trunc_lvl = vine_struct_.get_trunc_lvl();
    if (tree < trunc_lvl) {
      for (size_t v0 = 0; v0 < edges; ++v0) {
        tools_interface::check_user_interrupt(v0 % 10000 == 0);
        size_t v1 = vine_struct_.min_array(tree, v0) - 1;
        Eigen::MatrixXd pc_data = get_pc_data(v0, v1, vine_tree);
        EdgeIterator e = boost::add_edge(v0, v1, 1.0, vine_tree).first;
        double crit = calculate_criterion(
          pc_data.leftCols(2), tree_criterion, controls_.get_weights());
        vine_tree[e].weight = 1.0;
        vine_tree[e].crit = crit;
      }
    }
  }
}

//! @brief Selects the edges using the minimum spanning tree.
//!
//! See, e.g., Czado (2010), "Pair-copula constructions of multivariate
//! copulas", url: https://mediatum.ub.tum.de/doc/1079253/file.pdf
//! @param vine_tree Tree of a vine.
inline void
VinecopSelector::select_edges(VineTree& vine_tree)
{
  // has no effect if the input is already a tree
  min_spanning_tree(vine_tree);
}

inline void
VinecopSelector::finalize(size_t trunc_lvl)
{
  pair_copulas_ = make_pair_copula_store(d_, trunc_lvl);
  trunc_lvl = pair_copulas_.size(); // trunc_lvl may be <size_t>::max()

  if (structure_known_) {
    using namespace tools_stl;
    trees_opt_ = trees_;
    TriangularArray<size_t> mat(d_, trunc_lvl);
    std::vector<size_t> order(d_);

    if (trunc_lvl > 0) {
      std::vector<size_t> ning_set;

      // fill matrix column by column
      for (size_t col = 0; col < d_ - 1; ++col) {
        tools_interface::check_user_interrupt();
        // matrix above trunc_lvl is left empty
        size_t t =
          std::max(std::min(trunc_lvl, d_ - 1 - col), static_cast<size_t>(1));
        // start with highest tree in this column
        for (auto e : boost::edges(trees_[t])) {

          // find an edge that contains a leaf
          size_t v0 = boost::source(e, trees_[t]);
          size_t v1 = boost::target(e, trees_[t]);
          size_t min_deg = std::min(boost::out_degree(v0, trees_[t]),
                                    boost::out_degree(v1, trees_[t]));
          if (min_deg > 1) {
            continue; // not a leaf
          }
          // find position of leaf in the edge
          ptrdiff_t pos = (boost::out_degree(v1, trees_[t]) == 1);
          if (pos == 1) {
            trees_[t][e].pair_copula.flip();
          }

          // fill diagonal entry with leaf index
          order[col] = trees_[t][e].conditioned[pos];

          // entry in row t-1 is other index of the edge
          mat(t - 1, col) = trees_[t][e].conditioned[std::abs(1 - pos)];

          // assign fitted pair copula to appropriate entry, see
          // `Vinecop::get_pair_copula()`.
          if (trunc_lvl > 0) {
            pair_copulas_[t - 1][col] = trees_[t][e].pair_copula;
          }

          // initialize running set with full conditioning set of this edge
          ning_set = trees_[t][e].conditioning;

          // remove edge (must not be reused in another column!)
          boost::remove_edge(v0, v1, trees_[t]);
          break;
        }

        // fill column bottom to top
        for (size_t k = 1; k < t; ++k) {
          auto check_set = cat(order[col], ning_set);
          for (auto e : boost::edges(trees_[t - k])) {
            // search for an edge in lower tree that shares all
            // indices in the conditioning set + diagonal entry
            if (!is_same_set(trees_[t - k][e].all_indices, check_set)) {
              continue;
            }
            // found suitable edge ->
            // next matrix entry is conditioned variable of new edge
            // that's not equal to the diagonal entry of this column
            auto e_new = trees_[t - k][e];
            ptrdiff_t pos = (order[col] == e_new.conditioned[1]);
            if (pos == 1) {
              e_new.pair_copula.flip();
            }
            mat(t - k - 1, col) = e_new.conditioned[std::abs(1 - pos)];

            // assign fitted pair copula to appropriate entry, see
            // Vinecop::get_pair_copula().
            pair_copulas_[t - 1 - k][col] = e_new.pair_copula;

            // start over with conditioned set of next edge
            ning_set = e_new.conditioning;

            // remove edge (must not be reused in another column!)
            size_t v0 = boost::source(e, trees_[t - k]);
            size_t v1 = boost::target(e, trees_[t - k]);
            boost::remove_edge(v0, v1, trees_[t - k]);
            break;
          }
        }
      }

      // The last column contains a single element which must be different
      // from all other diagonal elements. Based on the properties of an
      // R-vine matrix, this must be the element next to it.
      order[d_ - 1] = mat(0, d_ - 2);

      // change to user-facing format
      // (variable index starting at 1 instead of 0)
      for (size_t i = 0; i < std::min(d_ - 1, trunc_lvl); ++i) {
        for (size_t j = 0; j < d_ - i - 1; ++j) {
          mat(i, j) += 1;
        }
      }
      for (size_t i = 0; i < d_; i++)
        order[i] += 1;
    } else {
      // order doesn't matter for truncated
      order = tools_stl::seq_int(1, d_);
    }

    // return as RVineStructure
    vine_struct_ = RVineStructure(order, mat);
  } else {

    for (size_t tree = 0; tree < pair_copulas_.size(); tree++) {
      size_t edge = 0;
      for (auto e : boost::edges(trees_[tree + 1])) {
        // trees_[0] is base tree, vine copula starts at trees_[1]
        pair_copulas_[tree][edge] = trees_[tree + 1][e].pair_copula;
        edge++;
      }
    }

    vine_struct_.truncate(trunc_lvl);
  }
}

//! @brief Gets pair copula pseudo-observations from h-functions.
//! @param v0,v1 vertex indices.
//! @param tree A vine tree.
//! @return The pseudo-observations for the pair coula, extracted from
//!     the h-functions calculated in the previous tree.
inline void
VinecopSelector::add_pc_info(const EdgeIterator& e, VineTree& tree)
{
  auto v0 = boost::source(e, tree);
  auto v1 = boost::target(e, tree);
  size_t n = tree[v0].hfunc1.size();
  tree[e].pc_data = Eigen::MatrixXd(n, 2);

  // find positions of common vertex in pair indices
  size_t ei_common = find_common_neighbor(v0, v1, tree);
  ptrdiff_t pos0 = find_position(ei_common, tree[v0].prev_edge_indices);
  ptrdiff_t pos1 = find_position(ei_common, tree[v1].prev_edge_indices);

  tree[e].var_types[0] = tree[v0].var_types[std::abs(1 - pos0)];
  tree[e].var_types[1] = tree[v1].var_types[std::abs(1 - pos1)];

  // collect pseudo observations for next tree
  tree[e].pc_data.col(0) = get_hfunc(tree[v0], pos0 == 0);
  tree[e].pc_data.col(1) = get_hfunc(tree[v1], pos1 == 0);
  if ((tree[e].var_types[0] == "d") | (tree[e].var_types[1] == "d")) {
    tree[e].pc_data.conservativeResize(n, 4);
    tree[e].pc_data.col(2) = get_hfunc_sub(tree[v0], pos0 == 0);
    tree[e].pc_data.col(3) = get_hfunc_sub(tree[v1], pos1 == 0);
  }

  tree[e].conditioned =
    set_sym_diff(tree[v0].all_indices, tree[v1].all_indices);
  tree[e].conditioning = intersect(tree[v0].all_indices, tree[v1].all_indices);
  tree[e].all_indices = cat(tree[e].conditioned, tree[e].conditioning);
}

inline Eigen::VectorXd
VinecopSelector::get_hfunc(const VertexProperties& vertex_data, bool is_first)
{
  if (is_first) {
    return vertex_data.hfunc1;
  } else {
    return vertex_data.hfunc2;
  }
}

inline Eigen::VectorXd
VinecopSelector::get_hfunc_sub(const VertexProperties& vertex_data,
                               bool is_first)
{
  if (is_first) {
    if (vertex_data.hfunc1_sub.size()) {
      return vertex_data.hfunc1_sub;
    } else {
      return vertex_data.hfunc1;
    }
  } else {
    if (vertex_data.hfunc2_sub.size()) {
      return vertex_data.hfunc2_sub;
    } else {
      return vertex_data.hfunc2;
    }
  }
}

inline Eigen::MatrixXd
VinecopSelector::get_pc_data(size_t v0, size_t v1, const VineTree& tree)
{
  size_t ei_common = find_common_neighbor(v0, v1, tree);
  auto pos0 = find_position(ei_common, tree[v0].prev_edge_indices);
  auto pos1 = find_position(ei_common, tree[v1].prev_edge_indices);

  Eigen::MatrixXd pc_data(tree[v0].hfunc1.size(), 2);
  pc_data.col(0) = get_hfunc(tree[v0], pos0 == 0);
  pc_data.col(1) = get_hfunc(tree[v1], pos1 == 0);
  return pc_data;
}

//! @brief Selects and fits next tree of the vine.
//!
//! The next tree is found the following way:
//!     1. Edges of the previous tree become edges in the new tree.
//!     2. All edges allowed by the proximity condition are added to the
//!        new graph.
//!     3. Collapse the new graph to a maximum spanning tree for edge
//!        weight.
//!     4. Populate edges with conditioned/conditioning sets and pseudo-
//!        observations.
//!     5. Fit and select a copula model for each edge.
//!
//! @param prev_tree Tree T_{k}.
//! @param controls The controls for fitting a vine copula
//!     (see FitControlsVinecop).
//! @param tree_opt The current optimal tree (used only for sparse
//!     selection).
inline void
VinecopSelector::select_tree(size_t t)
{
  auto new_tree = edges_as_vertices(trees_[t]);
  remove_edge_data(trees_[t]); // no longer needed

  if (t >= vine_struct_.get_trunc_lvl()) {
    // only important if proximity_ was previously false (partial selection)
    structure_known_ = true;
  }
  add_allowed_edges(new_tree);
  if (boost::num_vertices(new_tree) > 2) {
    select_edges(new_tree);
  }
  if (boost::num_vertices(new_tree) > 0) {
    add_edge_info(new_tree);      // for pc estimation and next tree
    remove_vertex_data(new_tree); // no longer needed
    if (controls_.get_selection_criterion() == "mbicv") {
      // adjust prior probability to tree level
      controls_.set_psi0(std::pow(psi0_, t + 1));
    }
    if (trees_opt_.size() > t + 1) {
      select_pair_copulas(new_tree, trees_opt_[t + 1]);
    } else {
      select_pair_copulas(new_tree);
    }
  }
  // make sure there is space for new tree
  trees_.resize(t + 2);
  trees_[t + 1] = new_tree;
}

inline double
VinecopSelector::get_mbicv_of_tree(size_t t, double loglik)
{
  double npars = get_npars_of_tree(t);
  size_t non_indeps = get_num_non_indeps_of_tree(t);
  size_t indeps = d_ - t - 1 - non_indeps;
  double psi0 = std::pow(psi0_, t + 1);
  double log_prior = static_cast<double>(non_indeps) * std::log(psi0) +
                     static_cast<double>(indeps) * std::log(1.0 - psi0);
  double n_eff = static_cast<double>(n_);
  if (controls_.get_weights().size() > 0) {
    n_eff = std::pow(controls_.get_weights().sum(), 2);
    n_eff /= controls_.get_weights().array().pow(2).sum();
  }

  return -2 * loglik + std::log(n_eff) * npars - 2 * log_prior;
}

//! @brief Calculates the log-likelihood of a tree.
inline double
VinecopSelector::get_loglik_of_tree(size_t t)
{
  double ll = 0.0;
  // trees_[0] is base tree, see make_base_tree()
  for (const auto& e : boost::edges(trees_[t + 1])) {
    ll += trees_[t + 1][e].pair_copula.get_loglik();
  }
  return ll;
}

//! @brief Calculates the numbers of parameters of a tree.
inline double
VinecopSelector::get_npars_of_tree(size_t t)
{
  double npars = 0.0;
  // trees_[0] is base tree, see make_base_tree()
  for (const auto& e : boost::edges(trees_[t + 1])) {
    npars += trees_[t + 1][e].pair_copula.get_npars();
  }
  return npars;
}

//! @brief Calculates the numbers of independence copulas in a tree.
inline size_t
VinecopSelector::get_num_non_indeps_of_tree(size_t t)
{
  size_t num_non_indeps = 0;
  // trees_[0] is base tree, see make_base_tree()
  for (const auto& e : boost::edges(trees_[t + 1])) {
    num_non_indeps += static_cast<size_t>(
      trees_[t + 1][e].pair_copula.get_family() == BicopFamily::indep);
  }
  return num_non_indeps;
}

//! @brief Prints indices, family, and parameters for each pair-copula
//! @param tree A vine tree.
inline void
VinecopSelector::print_pair_copulas_of_tree(size_t t)
{
  // trees_[0] is the base tree, see make_base_tree()
  for (auto e : boost::edges(trees_[t + 1])) {
    std::cout << get_pc_index(e, trees_[t + 1]) << " <-> "
              << trees_[t + 1][e].pair_copula.str() << std::endl;
  }
}

//! @brief Gets all criterion values that got thresholded to zero.
inline std::vector<double>
VinecopSelector::get_thresholded_crits()
{
  std::vector<double> crits;
  for (size_t t = 1; t < trees_.size(); ++t) {
    for (auto e : boost::edges(trees_[t])) {
      if (trees_[t][e].crit < controls_.get_threshold()) {
        crits.push_back(trees_[t][e].crit);
      }
    }
  }

  return crits;
}

inline void
VinecopSelector::initialize_new_fit(const Eigen::MatrixXd& data)
{
  trees_[0] = make_base_tree(data);
}

inline void
VinecopSelector::set_current_fit_as_opt(const double& loglik)
{
  threshold_ = controls_.get_threshold();
  trees_opt_ = trees_;
  loglik_ = loglik;
}

//! @brief Instantiates base tree of the vine.
//!
//! The base tree is a star on d + 1 variables, where the conditioned
//! set of each edge consists of a single number. When building the next
//! tree, the edges become vertices. Because the base graph was a star
//! all edges are allowed by the proximity condition, and the edges will
//! have a conditioned set consisting of the two vertex indices. This
//! will be the first actual tree of the vine.
//!
//! @param data nxd matrix of copula data.
//! @return A VineTree object containing the base graph.
inline VineTree
VinecopSelector::make_base_tree(const Eigen::MatrixXd& data)
{
  VineTree base_tree(d_);
  auto order = vine_struct_.get_order();
  auto disc_cols = get_disc_cols(var_types_);

  // a star connects the root node (d) with all other nodes
  for (size_t target = 0; target < d_; ++target) {
    tools_interface::check_user_interrupt(target % 10000 == 0);
    // add edge and extract edge iterator
    auto e = add_edge(d_, target, base_tree).first;
    // inititialize hfunc1 with actual data for variable "target"
    // data need are reordered to correspond to natural order (neccessary
    // when structure is fixed)
    base_tree[e].hfunc1 = data.col(order[target] - 1);
    if (var_types_[order[target] - 1] == "d") {
      base_tree[e].hfunc1_sub = data.col(d_ + disc_cols[order[target] - 1]);
      base_tree[e].var_types = { "d", "d" };
    }

    // identify edge with variable "target" and initialize sets
    base_tree[e].conditioned.reserve(2);
    base_tree[e].conditioned.push_back(order[target] - 1);
    base_tree[e].conditioning.reserve(d_ - 2);
    base_tree[e].all_indices = base_tree[e].conditioned;
  }

  return base_tree;
}

//! @brief Converts the edge set into vertex set of a new graph.
//!
//! Further information about the structure is passed along:
//!     - conditioned/conditioning set,
//!     - indices of vertices connected by the edge in the previous tree.
//!
//! @param tree T_{k}.
//! @return A edge-less graph of vertices, each representing one edge of the
//!     previous tree.
inline VineTree
VinecopSelector::edges_as_vertices(const VineTree& prev_tree)
{
  // start with full graph
  size_t d = num_edges(prev_tree);
  VineTree new_tree(d);

  // copy & paste information from previous tree
  int i = 0;
  for (auto e : boost::edges(prev_tree)) {
    new_tree[i].hfunc1 = prev_tree[e].hfunc1;
    new_tree[i].hfunc2 = prev_tree[e].hfunc2;
    new_tree[i].hfunc1_sub = prev_tree[e].hfunc1_sub;
    new_tree[i].hfunc2_sub = prev_tree[e].hfunc2_sub;
    new_tree[i].conditioned = prev_tree[e].conditioned;
    new_tree[i].conditioning = prev_tree[e].conditioning;
    new_tree[i].all_indices = prev_tree[e].all_indices;
    new_tree[i].prev_edge_indices.reserve(2);
    new_tree[i].prev_edge_indices.push_back(boost::source(e, prev_tree));
    new_tree[i].prev_edge_indices.push_back(boost::target(e, prev_tree));
    new_tree[i].var_types = prev_tree[e].var_types;
    ++i;
  }

  return new_tree;
}

//! @brief Finds common neighbor in previous tree.
//! @param v0,v1 vertices in the tree.
//! @param tree the current tree.
//! @return Gives the index of the vertex in the previous tree that was
//!     shared by e0, e1, the edge representations of v0, v1.
inline ptrdiff_t
VinecopSelector::find_common_neighbor(size_t v0,
                                      size_t v1,
                                      const VineTree& tree)
{
  auto ei0 = tree[v0].prev_edge_indices;
  auto ei1 = tree[v1].prev_edge_indices;
  auto ei_common = intersect(ei0, ei1);

  if (ei_common.size() == 0) {
    return -1;
  } else {
    return ei_common[0];
  }
}

//! @brief Computes a fit id; can be used to re-use already fitted pair-copulas.
//! @param edge.
inline double
VinecopSelector::compute_fit_id(const EdgeProperties& e)
{
  double id = 0.0;
  if (controls_.needs_sparse_select()) {
    // the formula is quite arbitrary, but sufficient for
    // identifying situations where fits can be re-used
    id = (e.pc_data.col(0) - 2 * e.pc_data.col(1)).sum();
    id += 5.0 * static_cast<double>(e.crit < controls_.get_threshold());
  }

  return id;
}

//! @brief Collapses a graph to the minimum spanning tree.
//! @param graph The input graph.
//! @return the input graph with all non-MST edges removed.
inline void
VinecopSelector::min_spanning_tree(VineTree& graph)
{
  size_t d = num_vertices(graph);
  std::vector<size_t> targets(d);
  prim_minimum_spanning_tree(graph, targets.data());
  for (size_t v1 = 0; v1 < d; ++v1) {
    for (size_t v2 = 0; v2 < v1; ++v2) {
      if ((v2 != targets[v1]) & (v1 != targets[v2])) {
        boost::remove_edge(v1, v2, graph);
      }
    }
  }
}

//! @brief Adds conditioned info and data for each edge.
//!
//! See, e.g., Czado (2010), "Pair-copula constructions of multivariate
//! copulas", url: https://mediatum.ub.tum.de/doc/1079253/file.pdf
//! @param tree A vine tree.
inline void
VinecopSelector::add_edge_info(VineTree& tree)
{
  for (auto e : boost::edges(tree)) {
    add_pc_info(e, tree);
  }
}

//! @brief Removes data (hfunc1/hfunc2/pc_data) from all edges of a vine tree.
//! @param tree A vine tree.
inline void
VinecopSelector::remove_edge_data(VineTree& tree)
{
  for (auto e : boost::edges(tree)) {
    tree[e].hfunc1 = Eigen::VectorXd();
    tree[e].hfunc2 = Eigen::VectorXd();
    tree[e].hfunc1_sub = Eigen::VectorXd();
    tree[e].hfunc2_sub = Eigen::VectorXd();
    tree[e].pc_data = Eigen::MatrixXd(0, 2);
  }
}

//! @brief Removes data (hfunc1/hfunc2/pc_data) from all vertices of a vine
//! tree.
//! @param tree A vine tree.
inline void
VinecopSelector::remove_vertex_data(VineTree& tree)
{
  for (auto v : boost::vertices(tree)) {
    tree[v].hfunc1 = Eigen::VectorXd();
    tree[v].hfunc2 = Eigen::VectorXd();
    tree[v].hfunc1_sub = Eigen::VectorXd();
    tree[v].hfunc2_sub = Eigen::VectorXd();
  }
}

//! @brief Fits and selects a pair copula for each edges.
//! @param tree A vine tree preprocessed with `add_edge_info()`.
//! @param tree_opt The current optimal tree (used only for sparse
//!     selection).
inline void
VinecopSelector::select_pair_copulas(VineTree& tree, const VineTree& tree_opt)
{
  auto select_pc = [&](EdgeIterator e) -> void {
    tools_interface::check_user_interrupt();
    bool is_thresholded = (tree[e].crit < controls_.get_threshold());
    bool used_old_fit = false;

    tree[e].fit_id = compute_fit_id(tree[e]);
    if (boost::num_edges(tree_opt) > 0) {
      auto old_fit = find_old_fit(tree[e].fit_id, tree_opt);
      if (old_fit.second) { // indicates if match was found
        // data and thresholding status haven't changed,
        // we can use old fit
        used_old_fit = true;
        tree[e].pair_copula = tree_opt[old_fit.first].pair_copula;
      }
    }

    if (!used_old_fit) {
      tree[e].pair_copula = vinecopulib::Bicop();
      tree[e].pair_copula.set_var_types(tree[e].var_types);
      if (!is_thresholded) {
        tree[e].pair_copula.select(tree[e].pc_data, controls_);
      }
    }

    tree[e].hfunc1 = tree[e].pair_copula.hfunc1(tree[e].pc_data);
    tree[e].hfunc2 = tree[e].pair_copula.hfunc2(tree[e].pc_data);
    if (tree[e].var_types[1] == "d") {
      auto sub_data = tree[e].pc_data;
      sub_data.col(1) = sub_data.col(3);
      tree[e].hfunc1_sub = tree[e].pair_copula.hfunc1(sub_data);
    }
    if (tree[e].var_types[0] == "d") {
      auto sub_data = tree[e].pc_data;
      sub_data.col(0) = sub_data.col(2);
      tree[e].hfunc2_sub = tree[e].pair_copula.hfunc2(sub_data);
    }
  };

  // make sure that Bicop.select() doesn't spawn new threads
  size_t num_threads = controls_.get_num_threads();
  controls_.set_num_threads(0);
  pool_.map(select_pc, boost::edges(tree));
  pool_.wait();
  controls_.set_num_threads(num_threads);
}

//! @brief Finds the fitted pair-copula from the previous iteration.
inline FoundEdge
VinecopSelector::find_old_fit(double fit_id, const VineTree& old_graph)
{
  auto edge = boost::edge(0, 1, old_graph).first;
  bool fit_with_same_id = false;
  for (auto e : boost::edges(old_graph)) {
    if (fit_id == old_graph[e].fit_id) {
      fit_with_same_id = true;
      edge = e;
    }
  }
  return std::make_pair(edge, fit_with_same_id);
}

//! @brief Gets edge index for the vine (like 1, 2; 3).
//! @param e A descriptor for the edge.
//! @param tree A vine tree.
inline std::string
VinecopSelector::get_pc_index(const EdgeIterator& e, const VineTree& tree)
{
  std::stringstream index;
  // add 1 everywhere for user-facing representation (boost::graph
  // starts at 0)
  index << tree[e].conditioned[0] + 1 << "," << tree[e].conditioned[1] + 1;
  if (tree[e].conditioning.size() > 0) {
    index << " | ";
    for (unsigned int i = 0; i < tree[e].conditioning.size(); ++i) {
      index << tree[e].conditioning[i] + 1;
      if (i < tree[e].conditioning.size() - 1)
        index << ",";
    }
  }

  return index.str().c_str();
}
}
}


/***************************************************************************
 *  @file       longest_common_subsequence.hpp
 *  @author     alan.w
 *  @date       09  August 2014
 *  @remark     CLRS Algorithms implementation, using C++ templates.
 ***************************************************************************/

#ifndef LONGEST_COMMON_SUBSEQUENCE_HPP
#define LONGEST_COMMON_SUBSEQUENCE_HPP

#include <functional>
#include "matrix.hpp"
#include "color.hpp"

namespace ch15 {
/**
 * @brief  as a part of an entry
 */
enum class Arrow
{
    NA, LEFT, UP, DIAGONAL
};

/**
 * @brief The Entry struct
 *
 * as an entry of Table
 */
template<typename T, typename A>
struct Entry
{
    //! Ctors
    Entry() = default;
    Entry(const T& len, const A& arw = Arrow::NA):
        length(len), arrow(arw)
    {}

    //! update this entry
    Entry operator ()(const T& len, const A& arw = Arrow::NA)
    {
        length  =   len;
        arrow   =   arw;

        return *this;
    }

    T length = 0;
    A arrow = Arrow::NA;
};

/**
 * @brief operator <<
 *
 * called when printing a boost matrix
 */
template<typename T, typename A>
inline std::ostream&
operator <<(std::ostream& os , const Entry<T,A>& etr)
{
    os << color::green("[")
       << etr.length << ",";

    switch (etr.arrow)
    {
    case Arrow::NA      :   os << " ";      break;
    case Arrow::LEFT    :   os << "<";      break;
    case Arrow::DIAGONAL:   os << "\\";     break;
    case Arrow::UP      :   os << "^";      break;
    }

    os << color::green("]");

    return os;
}

/**
 * @brief operator >=
 *
 * overloaded for comparison in build_lcs_table
 */
template<typename T, typename A>
inline bool
operator >=(const Entry<T,A>& lhs, const Entry<T,A>& rhs)
{
    return lhs.length >= rhs.length;
}

//! @alias for Table
template<typename T, typename A>
using Table = ch15::Matrix<Entry<T,A>>;

/**
 * @brief build lcs table
 * @param lhs   X
 * @param rhs   Y
 *
 * @pseudocode  LCS-LENGTH
 * @page    394, CLRS
 * @complx  O(mn)
 *
 * @note    the tables b and c in the peudocode have been merged in to one.
 *          so only one table is returned
 */
template<typename Range, typename A = Arrow>
Table<typename Range::size_type, A>
build_lcs_table(const Range& lhs, const Range& rhs)
{
    //! types def
    using SizeType  =   typename Range::size_type;
    using TableType =   Table<SizeType, A>;

    //! init
    SizeType rows = lhs.size() + 1;
    SizeType cols = rhs.size() + 1;
    TableType lcs(rows, cols);
    //! ^^^^^^^^^^^^^^^^^^^^^^@attention  :
    //!     the Entry's default ctor grantee that its member length has
    //!     default value 0.so no need to implementlines 4 - 7 explicitly.

    //! build lcs table
    for(SizeType r = 1; r != rows; ++r)
        for(SizeType c = 1; c != cols; ++c)
        {
            if(lhs[r - 1]   ==  rhs[c - 1])
                lcs(r,c) (lcs(r - 1, c - 1).length + 1   ,  Arrow::DIAGONAL);
            else if(lcs(r - 1, c) >= lcs(r, c - 1))
                lcs(r,c) (lcs(r - 1, c).length           ,  Arrow::UP);
            else
                lcs(r,c) (lcs(r, c - 1).length           ,  Arrow::LEFT);
        }

    return lcs;
}

/**
 * @brief The LongestCommonSubsequence class
 */
template<typename Range, typename A = ch15::Arrow>
class LongestCommonSubsequence
{
public:
    //! types def
    using SizeType  =   typename Range::size_type;
    using TableType =   ch15::Table<SizeType, A>;
    using Pointer   =   const Range*;

    //! Ctor
    LongestCommonSubsequence(const Range& l, const Range& r):
        lhs(&l),
        rhs(&r),
        maze(ch15::build_lcs_table(l,r))
    {}

    /**
     * @brief print_maze
     *
     * print the maze built by build_lcs_table
     */
    void print_maze()const
    {
        ch15::print(maze);
    }

    /**
     * @brief return the generated longest common sequence
     *
     * @pseudocode PRINT-LCS
     * @page    395, CLRS
     * @complx  O(m + n)
     */
    Range generate() const
    {
        assert(lhs && rhs);
        using Lambda = std::function<void(SizeType, SizeType)>;
        Range lcs;

        //! a recursive lamda that performs the real work
        Lambda build_lcs = [&lcs, &build_lcs, this](SizeType row, SizeType col)
        {
            //! stop condition
            if(row == 0 || col == 0)    return;

            //! build the longest common sequence
            if(maze(row, col).arrow ==  Arrow::DIAGONAL)
            {
                build_lcs(row - 1, col - 1);
                lcs.push_back((*lhs)[row - 1]);
            }
            else if(maze(row, col).arrow ==  Arrow::UP)
                build_lcs(row - 1, col);
            else
                build_lcs(row, col - 1);
        };

        //! call the lambda
        build_lcs(lhs->size(), rhs->size());
        return lcs;
    }

private:
    Pointer lhs;
    Pointer rhs;
    TableType maze;
};

}//namespace
#endif // LONGEST_COMMON_SUBSEQUENCE_HPP

//! @test   print ch15::table
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "longest_common_subsequence.hpp"
//#include "color.hpp"

//int main()
//{
//    using TableType = ch15::Table<int, ch15::Arrow>;

//    TableType tbl(3,3, ch15::Entry<int, ch15::Arrow>());
//    std::cout << tbl << std::endl;

//    std::cout << color::red("\nend\n");
//    return 0;
//}

//! @test   build_lcs_table
//!         i.e. LCS-LENGTH
//!
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "longest_common_subsequence.hpp"
//#include "color.hpp"

//int main()
//{
//    //! the same strings as that on page 394, CLRS.
//    std::vector<std::string> lhs = {"A", "B", "C", "B", "D", "A", "B"};
//    std::vector<std::string> rhs = {"B", "D", "C", "A", "B", "A"};

//    auto ret = ch15::build_lcs_table(lhs, rhs);
//    ch15::print(ret);

//    std::cout << color::red("\nend\n");
//    return 0;
//}

//! @test   The LongestCommonSubsequence class
//!         i.e. LCS-LENGTH and PRINT-LCS
//!
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "longest_common_subsequence.hpp"
//#include "color.hpp"

//int main()
//{
//    //! strings used on page 394, CLRS.
//    std::string lhs = "ABCBDAB";
//    std::string rhs = "BDCABA";

//    using LCS   =   ch15::LongestCommonSubsequence<std::string>;
//    LCS lcs(lhs, rhs);
//    lcs.print_maze();

//    auto sequence = lcs.generate();
//    std::cout << "The longest common sequence = ";
//    std::cout << color::yellow(sequence) << std::endl;

//    std::cout << color::red("\nend\n");
//    return 0;
//}
//! @output:
//[0, ] [0, ] [0, ] [0, ] [0, ] [0, ] [0, ]

//[0, ] [0,^] [0,^] [0,^] [1,\] [1,<] [1,\]

//[0, ] [1,\] [1,<] [1,<] [1,^] [2,\] [2,<]

//[0, ] [1,^] [1,^] [2,\] [2,<] [2,^] [2,^]

//[0, ] [1,\] [1,^] [2,^] [2,^] [3,\] [3,<]

//[0, ] [1,^] [2,\] [2,^] [2,^] [3,^] [3,^]

//[0, ] [1,^] [2,^] [2,^] [3,\] [3,^] [4,\]

//[0, ] [1,\] [2,^] [2,^] [3,^] [4,\] [4,^]

//The longest common sequence = BCBA

//end


// CodigoCFD.cpp : Este archivo contiene la función "main". La ejecución del programa comienza y termina ahí.
//

#include <Eigen/Eigen>

#include <mesh/mesh.h>
#include <mesh/reader/luismi_format_mesh_reader.h>

#include <math/interpolation.h>
#include <material/material.h>
#include <material/properties/constant_properties.h>
#include <material/properties/ideal_gas.h>
#include <material/material_factory.h>

#include <field/state_vector.h>
#include <field/field.h>

#include <core/environment.h>
int main()
{
    core::Environment simulation;
    simulation.loadMesh();
    simulation.buildMaterials();
    simulation.createBoundary();
    simulation.initializeFields();
    simulation.solve();

    return 0;
}

// Ejecutar programa: Ctrl + F5 o menú Depurar > Iniciar sin depurar
// Depurar programa: F5 o menú Depurar > Iniciar depuración

// Sugerencias para primeros pasos: 1. Use la ventana del Explorador de soluciones para agregar y administrar archivos
//   2. Use la ventana de Team Explorer para conectar con el control de código fuente
//   3. Use la ventana de salida para ver la salida de compilación y otros mensajes
//   4. Use la ventana Lista de errores para ver los errores
//   5. Vaya a Proyecto > Agregar nuevo elemento para crear nuevos archivos de código, o a Proyecto > Agregar elemento existente para agregar archivos de código existentes al proyecto
//   6. En el futuro, para volver a abrir este proyecto, vaya a Archivo > Abrir > Proyecto y seleccione el archivo .sln


#define BOOST_TEST_MODULE "test_small_inverse_matrix"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../src/InverseMatrix.hpp"

#include "test_Defs.hpp"
using ax::test::tolerance;
using ax::test::seed;

#include <random>

BOOST_AUTO_TEST_CASE(matrix_2x2)
{
    constexpr std::size_t msize = 2;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    ax::Matrix<double, msize,msize> mat;
    double det = 0e0;

    while(det == 0e0)
    {
        std::array<std::array<double, msize>, msize> rand1;
        for(std::size_t i=0; i<msize; ++i)
            for(std::size_t j=0; j<msize; ++j)
                mat(i,j) = rand1[i][j] = randreal(mt);

        det = ax::determinant(mat);
    }

    const ax::Matrix<double, msize,msize> inv = inverse(mat);
    const ax::Matrix<double, msize,msize> E   = inv * mat;

    for(std::size_t i=0; i<msize; ++i)
        for(std::size_t j=0; j<msize; ++j)
            if(i==j)
                BOOST_CHECK_CLOSE(E(i,j), 1e0, tolerance);
            else
                BOOST_CHECK_SMALL(E(i,j), tolerance);
}

BOOST_AUTO_TEST_CASE(matrix_3x3)
{
    constexpr std::size_t msize = 3;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    ax::Matrix<double, msize,msize> mat;
    double det = 0e0;

    while(det == 0e0)
    {
        std::array<std::array<double, msize>, msize> rand1;
        for(std::size_t i=0; i<msize; ++i)
            for(std::size_t j=0; j<msize; ++j)
                mat(i,j) = rand1[i][j] = randreal(mt);

        det = ax::determinant(mat);
    }

    const ax::Matrix<double, msize,msize> inv = inverse(mat);
    const ax::Matrix<double, msize,msize> E   = inv * mat;

    for(std::size_t i=0; i<msize; ++i)
        for(std::size_t j=0; j<msize; ++j)
            if(i==j)
                BOOST_CHECK_CLOSE(E(i,j), 1e0, tolerance);
            else
                BOOST_CHECK_SMALL(E(i,j), tolerance);
}


//#include <dlib/rand.h>
#include <dlib/svm_threaded.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <random>
using namespace std;
//using namespace dlib;

//typedef matrix<double,2,1> sample_type;

int main(void){
    ofstream datafile;
    datafile.open ("svm_data_test.txt");

   // dlib::rand rnd;
    double x;
    double y;
    double r;
    double theta;

    for(int i=0; i < 50; i++){
       r = ((double)rand() / RAND_MAX)*10.0;
       while (r > 10){
         r = ((double)rand() / RAND_MAX)*10.0;
       }
       if (r < 0){r*=-1;}
       theta = (double)rand();
       x = r*cos(theta);
       if (x < 0){x*=-1;}
       y = r*sin(theta);
       if (y<0){y*=-1;}
       datafile << x << " " << y << " " << "+1\n";
    }

    for(int i=0; i < 50; i++){
       r = ((double)rand() / RAND_MAX)*10.0 + 10.0;
       while (r < 10){
         r = ((double)rand() / RAND_MAX)*10.0 + 10.0;
       }
       theta = (double)rand();
       x = r*cos(theta);
       y = r*sin(theta);
       datafile << x << " " << y << " " << "-1\n";
    }
    datafile.close();

}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU 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 ANU OR THE 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.                                                               *
 ******************************************************************************/

// Note: this code has been downloaded from the homepage of the "Computer
// Vision Laboratory" at EPFL Lausanne, and was originally developped by the
// authors of [4]. I only adapted it to Eigen.

#ifndef OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>

namespace opengv
{
namespace absolute_pose
{
namespace modules
{

class Epnp
{
public:
  Epnp(void);
  ~Epnp();

  void set_maximum_number_of_correspondences(const int n);
  void reset_correspondences(void);
  void add_correspondence(
      const double X,
      const double Y,
      const double Z,
      const double x,
      const double y,
      const double z);

  double compute_pose(double R[3][3], double T[3]);

  void relative_error(
      double & rot_err,
      double & transl_err,
      const double Rtrue[3][3],
      const double ttrue[3],
      const double Rest[3][3],
      const double test[3]);

  void print_pose(const double R[3][3], const double t[3]);
  double reprojection_error(const double R[3][3], const double t[3]);

private:
  void choose_control_points(void);
  void compute_barycentric_coordinates(void);
  void fill_M(
      Eigen::MatrixXd & M,
      const int row,
      const double * alphas,
      const double u,
      const double v);
  void compute_ccs(const double * betas, const Eigen::MatrixXd & ut);
  void compute_pcs(void);

  void solve_for_sign(void);

  void find_betas_approx_1(
      const Eigen::Matrix<double,6,10> & L_6x10,
      const Eigen::Matrix<double,6,1> & Rho,
      double * betas);
  void find_betas_approx_2(
      const Eigen::Matrix<double,6,10> & L_6x10,
      const Eigen::Matrix<double,6,1> & Rho,
      double * betas);
  void find_betas_approx_3(
      const Eigen::Matrix<double,6,10> & L_6x10,
      const Eigen::Matrix<double,6,1> & Rho,
      double * betas);
  void qr_solve(
      Eigen::Matrix<double,6,4> & A,
      Eigen::Matrix<double,6,1> & b,
      Eigen::Matrix<double,4,1> & X);

  double dot(const double * v1, const double * v2);
  double dist2(const double * p1, const double * p2);

  void compute_rho(Eigen::Matrix<double,6,1> & Rho);
  void compute_L_6x10(
      const Eigen::MatrixXd & Ut,
      Eigen::Matrix<double,6,10> & L_6x10 );

  void gauss_newton(
      const Eigen::Matrix<double,6,10> & L_6x10,
      const Eigen::Matrix<double,6,1> & Rho,
      double current_betas[4]);
  void compute_A_and_b_gauss_newton(
      const Eigen::Matrix<double,6,10> & L_6x10,
      const Eigen::Matrix<double,6,1> & Rho,
      double cb[4],
      Eigen::Matrix<double,6,4> & A,
      Eigen::Matrix<double,6,1> & b);

  double compute_R_and_t(
      const Eigen::MatrixXd & Ut,
      const double * betas,
      double R[3][3],
      double t[3]);

  void estimate_R_and_t(double R[3][3], double t[3]);

  void copy_R_and_t(
      const double R_dst[3][3],
      const double t_dst[3],
      double R_src[3][3],
      double t_src[3]);

  void mat_to_quat(const double R[3][3], double q[4]);

  double uc, vc, fu, fv;

  double * pws, * us, * alphas, * pcs;
  int * signs; //added!
  int maximum_number_of_correspondences;
  int number_of_correspondences;

  double cws[4][3], ccs[4][3];
  double cws_determinant;
};

}
}
}

#endif /* OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_ */


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/ratio.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/ring.hpp>

#include <ratio>
using namespace boost::hana;

int main() {
    auto ratios = make<Tuple>(
          std::ratio<0>{}
        , std::ratio<1, 3>{}
        , std::ratio<1, 2>{}
        , std::ratio<2, 6>{}
        , std::ratio<3, 1>{}
        , std::ratio<7, 8>{}
        , std::ratio<3, 5>{}
        , std::ratio<2, 1>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    {
        // mult
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                mult(std::ratio<3, 4>{}, std::ratio<5, 10>{}),
                std::ratio<3*5, 4*10>{}
            ));
        }

        // one
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                one<ext::std::Ratio>(),
                std::ratio<1, 1>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                one<ext::std::Ratio>(),
                std::ratio<2, 2>{}
            ));
        }

        // laws
        test::TestRing<ext::std::Ratio>{ratios};
    }
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "Grid.hpp"
#include "SketchUtils.hpp"
#include "CountMinSketch.hpp"

BOOST_AUTO_TEST_CASE( GridTest_PositiveDimRanges ) {
	float gridSize = 0.2;

	Grid* grid = new Grid(gridSize);

	std::vector<double> point = { 0.1, 0.1 };
	std::vector<int> coordsValues = grid->getGridCoords(point);

	std::vector<int> coordsExpected = { 0, 0 };

	BOOST_CHECK_EQUAL_COLLECTIONS(coordsValues.begin(), coordsValues.end(),
			coordsExpected.begin(), coordsExpected.end());

	std::vector<std::vector<int>> nearCoordsValues = grid->getNearCoords(
			coordsExpected);
	BOOST_CHECK_EQUAL(5, nearCoordsValues.size());
}

BOOST_AUTO_TEST_CASE( GridTest_MixedDimRanges ) {
	float gridSize = 0.2;

	Grid* grid = new Grid(gridSize);

	std::vector<double> point = { -0.5, -0.5 };
	std::vector<int> coordsValues = grid->getGridCoords(point);
	std::vector<int> coordsExpected = { -2, -2 };

	BOOST_CHECK_EQUAL_COLLECTIONS(coordsValues.begin(), coordsValues.end(),
			coordsExpected.begin(), coordsExpected.end());

	std::vector<std::vector<int>> nearCoordsValues = grid->getNearCoords(
			coordsExpected);
	BOOST_CHECK_EQUAL(5, nearCoordsValues.size());

	point =	{	0.9, 0.9};
	coordsValues = grid->getGridCoords(point);
	coordsExpected = {	4, 4};

	BOOST_CHECK_EQUAL_COLLECTIONS(coordsValues.begin(), coordsValues.end(),
			coordsExpected.begin(), coordsExpected.end());

}

BOOST_AUTO_TEST_CASE( SketchUtilsTest ) {
	float cmDelta = 0.01;
	float cmEpsilon = 0.05;
	SketchUtils* utils = new SketchUtils(cmDelta, cmEpsilon);

	BOOST_CHECK_EQUAL(utils->getBitNo(), 275);
	BOOST_CHECK_EQUAL(utils->getHashNo(), 5);
}

BOOST_AUTO_TEST_CASE( CountMinSketchTest_SingleWindow ) {
	int thld = 2;
	int bitNo = 20;
	int wSize = 10;
	int hashNo = 10;

	CountMinSketch* countMin = new CountMinSketch(wSize, bitNo, thld);

	BOOST_CHECK_EQUAL(false, countMin->update(0, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(true, countMin->update(1, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(2, std::vector<int> { 0, hashNo + 0 }));

	BOOST_CHECK_EQUAL(false, countMin->update(3, std::vector<int> { 1, hashNo + 0 }));
	BOOST_CHECK_EQUAL(true, countMin->update(4, std::vector<int> { 1, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(5, std::vector<int> { 1, hashNo + 0 }));
}

BOOST_AUTO_TEST_CASE( CountMinSketchTest_MultipleWindows ) {
	int thld = 2;
	int bitNo = 20;
	int wSize = 4;
	int hashNo = 10;

	CountMinSketch* countMin = new CountMinSketch(wSize, bitNo, thld);

	// win 1
	//std::cout << "win1" << std::endl;
	BOOST_CHECK_EQUAL(false, countMin->update(0, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(true, countMin->update(1, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(2, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(3, std::vector<int> { 0, hashNo + 0 }));

	// win 2
	//std::cout << "win2" << std::endl;
	BOOST_CHECK_EQUAL(true, countMin->update(4, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(5, std::vector<int> { 0, hashNo + 1 }));
	BOOST_CHECK_EQUAL(false, countMin->update(6, std::vector<int> { 1, hashNo + 3 }));
	BOOST_CHECK_EQUAL(false, countMin->update(7, std::vector<int> { 1, hashNo + 2 }));

	// win 3
	//std::cout << "win3" << std::endl;
	BOOST_CHECK_EQUAL(false, countMin->update(8, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(9, std::vector<int> { 1, hashNo + 3 }));
	BOOST_CHECK_EQUAL(false, countMin->update(10, std::vector<int> { 0, hashNo + 1 }));
	BOOST_CHECK_EQUAL(false, countMin->update(11, std::vector<int> { 0, hashNo + 0 }));

	// win 4
	//std::cout << "win4" << std::endl;
	BOOST_CHECK_EQUAL(true, countMin->update(12, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(13, std::vector<int> { 1, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(14, std::vector<int> { 0, hashNo + 0 }));
	BOOST_CHECK_EQUAL(false, countMin->update(15, std::vector<int> { 0, hashNo + 0 }));
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file DcsLossFunc.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_DCS_LOSS_FUNCTION_HPP
#define STEAM_DCS_LOSS_FUNCTION_HPP

#include <Eigen/Core>

#include <steam/problem/lossfunc/LossFunctionBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Huber loss function class
//////////////////////////////////////////////////////////////////////////////////////////////
class DcsLossFunc : public LossFunctionBase
{
 public:

  /// Convenience typedefs
  typedef std::shared_ptr<DcsLossFunc> Ptr;
  typedef std::shared_ptr<const DcsLossFunc> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical)
  //////////////////////////////////////////////////////////////////////////////////////////////
  DcsLossFunc(double k) : k2_(k*k) {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Cost function (basic evaluation of the loss function)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double cost(double whitened_error_norm) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Weight for iteratively reweighted least-squares (influence function div. by error)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double weight(double whitened_error_norm) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Huber constant
  //////////////////////////////////////////////////////////////////////////////////////////////
  double k2_;
};

} // steam

#endif // STEAM_DCS_LOSS_FUNCTION_HPP


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/partition.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(
    hana::partition(hana::tuple_c<int, 1, 2, 3, 4, 5, 6, 7>, [](auto x) {
        return x % hana::int_c<2> != hana::int_c<0>;
    })
    ==
    hana::make_pair(
        hana::tuple_c<int, 1, 3, 5, 7>,
        hana::tuple_c<int, 2, 4, 6>
    )
);

BOOST_HANA_CONSTANT_CHECK(
    hana::partition(hana::tuple_t<void, int, float, char, double>, hana::trait<std::is_floating_point>)
    ==
    hana::make_pair(
        hana::tuple_t<float, double>,
        hana::tuple_t<void, int, char>
    )
);

// partition.by is syntactic sugar
BOOST_HANA_CONSTANT_CHECK(
    hana::partition.by(hana::trait<std::is_floating_point>,
                       hana::tuple_t<void, int, float, char, double>)
    ==
    hana::make_pair(
        hana::tuple_t<float, double>,
        hana::tuple_t<void, int, char>
    )
);

int main() { }


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/matrix/convert.h++>
#include <mla/operations/cuthill_mckee.h++>

typedef boost::mpl::list<
//	mla::matrix::DenseRowMajor<float>,
//	mla::matrix::DenseRowMajor<double>,
//	mla::matrix::SparseCRS<float>,
//	mla::matrix::SparseCRS<double>,
	mla::matrix::SparseDOK<float>,
	mla::matrix::SparseDOK<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_operations)

BOOST_AUTO_TEST_CASE_TEMPLATE( matrix_cuthill_mckee_arrowhead, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	MatrixType A(matrix_size, matrix_size);

	// sets an arrowhead matrix
	for(unsigned int i = 0; i < matrix_size; i++)
	{
		A.setValue( i, i, (Scalar)1.0f );
		A.setValue( 0, i, (Scalar)1.0f );
		A.setValue( i, 0, (Scalar)1.0f );
	}

	std::vector<size_t> indices = mla::cuthill_mckee(A);

	// the indices should be the same size as the matrix
	BOOST_CHECK_EQUAL( indices.size(), matrix_size );

	// Cuthill-McKee reorders matrices as arrowhead matrices
	for(size_t i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_EQUAL( indices[i], i );
	}

}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2020 TierIV. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <gtest/gtest.h>

#include <autoware_utils/geometry/boost_geometry.h>

#include <boost/geometry/geometry.hpp>

namespace bg = boost::geometry;

using autoware_utils::Point2d;
using autoware_utils::Point3d;

TEST(boost_geometry, boost_geometry_distance)
{
  {
    const Point2d p1(1.0, 2.0);
    const Point2d p2(2.0, 4.0);
    EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(5));
  }

  {
    const Point3d p1(1.0, 2.0, 3.0);
    const Point3d p2(2.0, 4.0, 6.0);
    EXPECT_DOUBLE_EQ(bg::distance(p1, p2), std::sqrt(14));
  }
}

TEST(boost_geometry, to_3d)
{
  const Point2d p_2d(1.0, 2.0);
  const Point3d p_3d(1.0, 2.0, 3.0);
  EXPECT_TRUE(p_2d.to_3d(3.0) == p_3d);
}

TEST(boost_geometry, to_2d)
{
  const Point2d p_2d(1.0, 2.0);
  const Point3d p_3d(1.0, 2.0, 3.0);
  EXPECT_TRUE(p_3d.to_2d() == p_2d);
}

TEST(boost_geometry, toMsg)
{
  using autoware_utils::toMsg;

  {
    const Point3d p(1.0, 2.0, 3.0);
    const geometry_msgs::Point p_msg = toMsg(p);

    EXPECT_DOUBLE_EQ(p_msg.x, 1.0);
    EXPECT_DOUBLE_EQ(p_msg.y, 2.0);
    EXPECT_DOUBLE_EQ(p_msg.z, 3.0);
  }
}

TEST(boost_geometry, fromMsg)
{
  using autoware_utils::fromMsg;

  geometry_msgs::Point p_msg;
  p_msg.x = 1.0;
  p_msg.y = 2.0;
  p_msg.z = 3.0;

  const Point3d p = fromMsg(p_msg);

  EXPECT_DOUBLE_EQ(p.x(), 1.0);
  EXPECT_DOUBLE_EQ(p.y(), 2.0);
  EXPECT_DOUBLE_EQ(p.z(), 3.0);
}