// SPDX-FileCopyrightText: 2015 - 2021 Marcin Łoś <marcin.los.91@gmail.com>
// SPDX-License-Identifier: MIT

#ifndef ADS_SIMULATION_BASIC_SIMULATION_2D_HPP
#define ADS_SIMULATION_BASIC_SIMULATION_2D_HPP

#include <array>
#include <cstddef>

#include <boost/range/counting_range.hpp>

#include "ads/lin/tensor.hpp"
#include "ads/simulation/boundary.hpp"
#include "ads/simulation/dimension.hpp"
#include "ads/util/function_value.hpp"
#include "ads/util/iter/product.hpp"

namespace ads {

class basic_simulation_2d {
public:
    virtual ~basic_simulation_2d() = default;

    basic_simulation_2d() = default;
    basic_simulation_2d(const basic_simulation_2d&) = delete;
    basic_simulation_2d& operator=(const basic_simulation_2d&) = delete;
    basic_simulation_2d(basic_simulation_2d&&) = delete;
    basic_simulation_2d& operator=(basic_simulation_2d&&) = delete;

protected:
    using vector_type = lin::tensor<double, 2>;
    using vector_view = lin::tensor_view<double, 2>;
    using value_type = function_value_2d;

    using index_type = std::array<int, 2>;
    using index_1d_iter_type = boost::counting_iterator<int>;
    using index_iter_type = util::iter_product2<index_1d_iter_type, index_type>;
    using index_range = boost::iterator_range<index_iter_type>;

    using point_type = std::array<double, 2>;

    struct L2 {
        double operator()(value_type a) const { return a.val * a.val; }
    };

    struct H10 {
        double operator()(value_type a) const { return a.dx * a.dx + a.dy * a.dy; }
    };

    struct H1 {
        double operator()(value_type a) const { return a.val * a.val + a.dx * a.dx + a.dy * a.dy; }
    };

    value_type eval_basis(index_type e, index_type q, index_type a, const dimension& x,
                          const dimension& y) const {
        auto loc = dof_global_to_local(e, a, x, y);

        const auto& bx = x.basis;
        const auto& by = y.basis;

        double B1 = bx.b[e[0]][q[0]][0][loc[0]];
        double B2 = by.b[e[1]][q[1]][0][loc[1]];
        double dB1 = bx.b[e[0]][q[0]][1][loc[0]];
        double dB2 = by.b[e[1]][q[1]][1][loc[1]];

        double v = B1 * B2;
        double dxv = dB1 * B2;
        double dyv = B1 * dB2;

        return {v, dxv, dyv};
    }

    double laplacian(index_type e, index_type q, index_type a, const dimension& x,
                     const dimension& y) const {
        auto loc = dof_global_to_local(e, a, x, y);

        const auto& bx = x.basis;
        const auto& by = y.basis;

        double B1 = bx.b[e[0]][q[0]][0][loc[0]];
        double B2 = by.b[e[1]][q[1]][0][loc[1]];
        double ddB1 = bx.b[e[0]][q[0]][2][loc[0]];
        double ddB2 = by.b[e[1]][q[1]][2][loc[1]];

        return B1 * ddB2 + ddB1 * B2;
    }

    template <typename Sol>
    value_type eval(const Sol& v, index_type e, index_type q, const dimension& x,
                    const dimension& y) const {
        value_type u{};
        for (auto b : dofs_on_element(e, x, y)) {
            double c = v(b[0], b[1]);
            value_type B = eval_basis(e, q, b, x, y);
            u += c * B;
        }
        return u;
    }

    index_range elements(const dimension& x, const dimension& y) const {
        return util::product_range<index_type>(x.element_indices(), y.element_indices());
    }

    index_range quad_points(const dimension& x, const dimension& y) const {
        auto rx = boost::counting_range(0, x.basis.quad_order);
        auto ry = boost::counting_range(0, y.basis.quad_order);
        return util::product_range<index_type>(rx, ry);
    }

    index_range dofs_on_element(index_type e, const dimension& x, const dimension& y) const {
        auto rx = x.basis.dof_range(e[0]);
        auto ry = y.basis.dof_range(e[1]);
        return util::product_range<index_type>(rx, ry);
    }

    index_range elements_supporting_dof(index_type dof, const dimension& x,
                                        const dimension& y) const {
        auto rx = x.basis.element_range(dof[0]);
        auto ry = y.basis.element_range(dof[1]);
        return util::product_range<index_type>(rx, ry);
    }

    bool supported_in(index_type dof, index_type e, const dimension& x, const dimension& y) const {
        auto xrange = x.basis.element_ranges[dof[0]];
        auto yrange = y.basis.element_ranges[dof[1]];
        return e[0] >= xrange.first && e[0] <= xrange.second && e[1] >= yrange.first
            && e[1] <= yrange.second;
    }

    index_type dof_global_to_local(index_type e, index_type a, const dimension& x,
                                   const dimension& y) const {
        const auto& bx = x.basis;
        const auto& by = y.basis;
        return {{a[0] - bx.first_dof(e[0]), a[1] - by.first_dof(e[1])}};
    }

    template <typename RHS>
    void update_global_rhs(RHS& global, const vector_type& local, index_type e, const dimension& x,
                           const dimension& y) const {
        for (auto a : dofs_on_element(e, x, y)) {
            auto loc = dof_global_to_local(e, a, x, y);
            global(a[0], a[1]) += local(loc[0], loc[1]);
        }
    }

    index_range dofs(const dimension& x, const dimension& y) const {
        auto rx = boost::counting_range(0, x.dofs());
        auto ry = boost::counting_range(0, y.dofs());
        return util::product_range<index_type>(rx, ry);
    }

    index_range internal_dofs(const dimension& x, const dimension& y) const {
        auto rx = boost::counting_range(1, x.dofs() - 1);
        auto ry = boost::counting_range(1, y.dofs() - 1);
        return util::product_range<index_type>(rx, ry);
    }

    double jacobian(index_type e, const dimension& x, const dimension& y) const {
        return x.basis.J[e[0]] * y.basis.J[e[1]];
    }

    double weight(index_type q, const dimension& x, const dimension& y) const {
        return x.basis.w[q[0]] * y.basis.w[q[1]];
    }

    point_type point(index_type e, index_type q, const dimension& x, const dimension& y) const {
        double px = x.basis.x[e[0]][q[0]];
        double py = y.basis.x[e[1]][q[1]];
        return {px, py};
    }

    auto overlapping_dofs(int dof, int begin, int end, const dimension& x) const {
        using std::max;
        using std::min;

        auto minx = max(begin, dof - x.B.degree);
        auto maxx = min(end, dof + x.B.degree + 1);

        return boost::counting_range(minx, maxx);
    }

    index_range overlapping_dofs(index_type dof, const dimension& x, const dimension& y) const {
        auto rx = overlapping_dofs(dof[0], 0, x.dofs(), x);
        auto ry = overlapping_dofs(dof[1], 0, y.dofs(), y);
        return util::product_range<index_type>(rx, ry);
    }

    index_range overlapping_dofs(index_type dof, const dimension& Ux, const dimension& Uy,
                                 const dimension& Vx, const dimension& Vy) const {
        auto xrange = Ux.basis.element_ranges[dof[0]];
        auto yrange = Uy.basis.element_ranges[dof[1]];

        auto x0 = Vx.basis.first_dof(xrange.first);
        auto x1 = Vx.basis.last_dof(xrange.second) + 1;

        auto y0 = Vy.basis.first_dof(yrange.first);
        auto y1 = Vy.basis.last_dof(yrange.second) + 1;

        auto rx = boost::counting_range(x0, x1);
        auto ry = boost::counting_range(y0, y1);

        return util::product_range<index_type>(rx, ry);
    }

    index_range overlapping_internal_dofs(index_type dof, const dimension& x,
                                          const dimension& y) const {
        auto rx = overlapping_dofs(dof[0], 1, x.dofs() - 1, x);
        auto ry = overlapping_dofs(dof[1], 1, y.dofs() - 1, y);
        return util::product_range<index_type>(rx, ry);
    }

    int linear_index(index_type dof, const dimension& x, const dimension& y) const {
        auto order = reverse_ordering<2>({x.dofs(), y.dofs()});
        return order.linear_index(dof[0], dof[1]);
    }

    template <typename Fun>
    void for_boundary_dofs(const dimension& x, const dimension& y, Fun&& fun) const {
        for (auto jx = 0; jx < x.dofs(); ++jx) {
            fun({jx, 0});
            fun({jx, y.dofs() - 1});
        }
        for (auto jy = 1; jy < y.dofs() - 1; ++jy) {
            fun({0, jy});
            fun({x.dofs() - 1, jy});
        }
    }

    bool is_boundary(int dof, const dimension& x) const { return dof == 0 || dof == x.dofs() - 1; }

    bool is_boundary(index_type dof, const dimension& x, const dimension& y) const {
        return is_boundary(dof[0], x) || is_boundary(dof[1], y);
    }

    template <typename MT1, typename MT2>
    double kron(const MT1& A, const MT2& B, index_type i, index_type j) const {
        return A(i[0], j[0]) * B(i[1], j[1]);
    }

    template <typename RHS, typename Fun,
              typename = std::enable_if<std::is_arithmetic<std::result_of_t<Fun(double)>>{}>>
    void dirichlet_bc(RHS& u, boundary side, dimension& x, dimension& y, Fun&& fun) const {
        bool horizontal = side == boundary::top || side == boundary::bottom;
        auto& basis = horizontal ? x : y;
        const auto& other = horizontal ? y : x;

        lin::vector buf{{basis.dofs()}};
        compute_projection(buf, basis.basis, std::forward<Fun>(fun));
        lin::solve_with_factorized(basis.M, buf, basis.ctx);

        int idx = side == boundary::left || side == boundary::bottom ? 0 : other.dofs() - 1;
        for (int i = 0; i < basis.dofs(); ++i) {
            if (horizontal) {
                u(i, idx) = buf(i);
            } else {
                u(idx, i) = buf(i);
            }
        }
    }

    template <typename RHS>
    void dirichlet_bc(RHS& u, boundary side, dimension& x, dimension& y, double value) const {
        dirichlet_bc(u, side, x, y, [value](double) { return value; });
    }

    template <typename Norm, typename Fun>
    double norm(const dimension& Ux, const dimension& Uy, Norm&& norm, Fun&& fun) const {
        double val = 0;

        for (auto e : elements(Ux, Uy)) {
            double J = jacobian(e, Ux, Uy);
            for (auto q : quad_points(Ux, Uy)) {
                double w = weight(q, Ux, Uy);
                auto x = point(e, q, Ux, Uy);
                auto d = fun(x);
                val += norm(d) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Fun>
    double normL2(const dimension& Ux, const dimension& Uy, Fun&& fun) const {
        return norm(Ux, Uy, L2{}, fun);
    }

    template <typename Fun>
    double normH1(const dimension& Ux, const dimension& Uy, Fun&& fun) const {
        return norm(Ux, Uy, H1{}, fun);
    }

    template <typename Sol, typename Norm>
    double norm(const Sol& u, const dimension& Ux, const dimension& Uy, Norm&& norm) const {
        double val = 0;

        for (auto e : elements(Ux, Uy)) {
            double J = jacobian(e, Ux, Uy);
            for (auto q : quad_points(Ux, Uy)) {
                double w = weight(q, Ux, Uy);
                value_type uu = eval(u, e, q, Ux, Uy);
                val += norm(uu) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Sol>
    double normL2(const Sol& u, const dimension& Ux, const dimension& Uy) const {
        return norm(u, Ux, Uy, L2{});
    }

    template <typename Sol>
    double normH1(const Sol& u, const dimension& Ux, const dimension& Uy) const {
        return norm(u, Ux, Uy, H1{});
    }

    template <typename Sol, typename Fun, typename Norm>
    double error(const Sol& u, const dimension& Ux, const dimension& Uy, Norm&& norm,
                 Fun&& fun) const {
        double error = 0;

        for (auto e : elements(Ux, Uy)) {
            double J = jacobian(e, Ux, Uy);
            for (auto q : quad_points(Ux, Uy)) {
                double w = weight(q, Ux, Uy);
                auto x = point(e, q, Ux, Uy);
                value_type uu = eval(u, e, q, Ux, Uy);

                auto d = uu - fun(x);
                error += norm(d) * w * J;
            }
        }
        return std::sqrt(error);
    }

    template <typename Sol, typename Fun>
    double errorL2(const Sol& u, const dimension& Ux, const dimension& Uy, Fun&& fun) const {
        return error(u, Ux, Uy, L2{}, fun);
    }

    template <typename Sol, typename Fun>
    double error_relative_L2(const Sol& u, const dimension& Ux, const dimension& Uy,
                             Fun&& fun) const {
        return error_relative(u, Ux, Uy, L2{}, fun);
    }

    template <typename Sol, typename Fun>
    double errorH1(const Sol& u, const dimension& Ux, const dimension& Uy, Fun&& fun) const {
        return error(u, Ux, Uy, H1{}, fun);
    }

    template <typename Sol, typename Fun, typename Norm>
    double error_relative(const Sol& u, const dimension& Ux, const dimension& Uy, Norm&& norm,
                          Fun&& fun) const {
        return error(u, Ux, Uy, norm, fun) / this->norm(Ux, Uy, norm, fun);
    }

    template <typename Sol, typename Fun>
    double error_relative_H1(const Sol& u, const dimension& Ux, const dimension& Uy,
                             Fun&& fun) const {
        return error_relative(u, Ux, Uy, H1{}, fun);
    }
};

}  // namespace ads

#endif  // ADS_SIMULATION_BASIC_SIMULATION_2D_HPP


#include "nnig_hierarchy.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <stan/math/prim/prob.hpp>
#include <vector>

#include "algorithm_state.pb.h"
#include "hierarchy_prior.pb.h"
#include "ls_state.pb.h"
#include "src/utils/rng.h"

double NNIGHierarchy::like_lpdf(
    const Eigen::RowVectorXd &datum,
    const Eigen::RowVectorXd &covariate /*= Eigen::RowVectorXd(0)*/) const {
  return stan::math::normal_lpdf(datum(0), state.mean, sqrt(state.var));
}

double NNIGHierarchy::marg_lpdf(
    const NNIG::Hyperparams &params, const Eigen::RowVectorXd &datum,
    const Eigen::RowVectorXd &covariate /*= Eigen::RowVectorXd(0)*/) const {
  double sig_n = sqrt(params.scale * (params.var_scaling + 1) /
                      (params.shape * params.var_scaling));
  return stan::math::student_t_lpdf(datum(0), 2 * params.shape, params.mean,
                                    sig_n);
}

NNIG::State NNIGHierarchy::draw(const NNIG::Hyperparams &params) {
  // Update state values from their prior centering distribution
  auto &rng = bayesmix::Rng::Instance().get();
  NNIG::State out;
  out.var = stan::math::inv_gamma_rng(params.shape, params.scale, rng);
  out.mean = stan::math::normal_rng(params.mean,
                                    sqrt(state.var / params.var_scaling), rng);
  return out;
}

void NNIGHierarchy::update_summary_statistics(
    const Eigen::RowVectorXd &datum, const Eigen::RowVectorXd &covariate,
    bool add) {
  if (add) {
    data_sum += datum(0);
    data_sum_squares += datum(0) * datum(0);
  } else {
    data_sum -= datum(0);
    data_sum_squares -= datum(0) * datum(0);
  }
}

NNIG::Hyperparams NNIGHierarchy::get_posterior_parameters() {
  // Initialize relevant variables
  if (card == 0) {  // no update possible
    return *hypers;
  }
  // Compute posterior hyperparameters
  NNIG::Hyperparams post_params;
  double y_bar = data_sum / (1.0 * card);  // sample mean
  double ss = data_sum_squares - card * y_bar * y_bar;
  post_params.mean = (hypers->var_scaling * hypers->mean + data_sum) /
                     (hypers->var_scaling + card);
  post_params.var_scaling = hypers->var_scaling + card;
  post_params.shape = hypers->shape + 0.5 * card;
  post_params.scale = hypers->scale + 0.5 * ss +
                      0.5 * hypers->var_scaling * card *
                          (y_bar - hypers->mean) * (y_bar - hypers->mean) /
                          (card + hypers->var_scaling);
  return post_params;
}

void NNIGHierarchy::clear_data() {
  data_sum = 0;
  data_sum_squares = 0;
  card = 0;
  cluster_data_idx = std::set<int>();
}

void NNIGHierarchy::initialize_state() {
  state.mean = hypers->mean;
  state.var = hypers->scale / (hypers->shape + 1);
}

void NNIGHierarchy::initialize_hypers() {
  if (prior->has_fixed_values()) {
    // Set values
    hypers->mean = prior->fixed_values().mean();
    hypers->var_scaling = prior->fixed_values().var_scaling();
    hypers->shape = prior->fixed_values().shape();
    hypers->scale = prior->fixed_values().scale();
    // Check validity
    if (hypers->var_scaling <= 0) {
      throw std::invalid_argument("Variance-scaling parameter must be > 0");
    }
    if (hypers->shape <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (hypers->scale <= 0) {
      throw std::invalid_argument("Scale parameter must be > 0");
    }
  }

  else if (prior->has_normal_mean_prior()) {
    // Set initial values
    hypers->mean = prior->normal_mean_prior().mean_prior().mean();
    hypers->var_scaling = prior->normal_mean_prior().var_scaling();
    hypers->shape = prior->normal_mean_prior().shape();
    hypers->scale = prior->normal_mean_prior().scale();
    // Check validity
    if (hypers->var_scaling <= 0) {
      throw std::invalid_argument("Variance-scaling parameter must be > 0");
    }
    if (hypers->shape <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (hypers->scale <= 0) {
      throw std::invalid_argument("Scale parameter must be > 0");
    }
  }

  else if (prior->has_ngg_prior()) {
    // Get hyperparameters:
    // for mu0
    double mu00 = prior->ngg_prior().mean_prior().mean();
    double sigma00 = prior->ngg_prior().mean_prior().var();
    // for lambda0
    double alpha00 = prior->ngg_prior().var_scaling_prior().shape();
    double beta00 = prior->ngg_prior().var_scaling_prior().rate();
    // for beta0
    double a00 = prior->ngg_prior().scale_prior().shape();
    double b00 = prior->ngg_prior().scale_prior().rate();
    // for alpha0
    double alpha0 = prior->ngg_prior().shape();
    // Check validity
    if (sigma00 <= 0) {
      throw std::invalid_argument("Variance parameter must be > 0");
    }
    if (alpha00 <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (beta00 <= 0) {
      throw std::invalid_argument("Rate parameter must be > 0");
    }
    if (a00 <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (b00 <= 0) {
      throw std::invalid_argument("Rate parameter must be > 0");
    }
    if (alpha0 <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    // Set initial values
    hypers->mean = mu00;
    hypers->var_scaling = alpha00 / beta00;
    hypers->shape = alpha0;
    hypers->scale = a00 / b00;
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void NNIGHierarchy::update_hypers(
    const std::vector<bayesmix::AlgorithmState::ClusterState> &states) {
  auto &rng = bayesmix::Rng::Instance().get();

  if (prior->has_fixed_values()) {
    return;
  }

  else if (prior->has_normal_mean_prior()) {
    // Get hyperparameters
    double mu00 = prior->normal_mean_prior().mean_prior().mean();
    double sig200 = prior->normal_mean_prior().mean_prior().var();
    double lambda0 = prior->normal_mean_prior().var_scaling();
    // Compute posterior hyperparameters
    double prec = 0.0;
    double num = 0.0;
    for (auto &st : states) {
      double mean = st.uni_ls_state().mean();
      double var = st.uni_ls_state().var();
      prec += 1 / var;
      num += mean / var;
    }
    prec = 1 / sig200 + lambda0 * prec;
    num = mu00 / sig200 + lambda0 * num;
    double mu_n = num / prec;
    double sig2_n = 1 / prec;
    // Update hyperparameters with posterior random sampling
    hypers->mean = stan::math::normal_rng(mu_n, sqrt(sig2_n), rng);
  }

  else if (prior->has_ngg_prior()) {
    // Get hyperparameters:
    // for mu0
    double mu00 = prior->ngg_prior().mean_prior().mean();
    double sig200 = prior->ngg_prior().mean_prior().var();
    // for lambda0
    double alpha00 = prior->ngg_prior().var_scaling_prior().shape();
    double beta00 = prior->ngg_prior().var_scaling_prior().rate();
    // for tau0
    double a00 = prior->ngg_prior().scale_prior().shape();
    double b00 = prior->ngg_prior().scale_prior().rate();
    // Compute posterior hyperparameters
    double b_n = 0.0;
    double num = 0.0;
    double beta_n = 0.0;
    for (auto &st : states) {
      double mean = st.uni_ls_state().mean();
      double var = st.uni_ls_state().var();
      b_n += 1 / var;
      num += mean / var;
      beta_n += (hypers->mean - mean) * (hypers->mean - mean) / var;
    }
    double var = hypers->var_scaling * b_n + 1 / sig200;
    b_n += b00;
    num = hypers->var_scaling * num + mu00 / sig200;
    beta_n = beta00 + 0.5 * beta_n;
    double sig_n = 1 / var;
    double mu_n = num / var;
    double alpha_n = alpha00 + 0.5 * states.size();
    double a_n = a00 + states.size() * hypers->shape;
    // Update hyperparameters with posterior random Gibbs sampling
    hypers->mean = stan::math::normal_rng(mu_n, sig_n, rng);
    hypers->var_scaling = stan::math::gamma_rng(alpha_n, beta_n, rng);
    hypers->scale = stan::math::gamma_rng(a_n, b_n, rng);
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void NNIGHierarchy::set_state_from_proto(
    const google::protobuf::Message &state_) {
  auto &statecast = google::protobuf::internal::down_cast<
      const bayesmix::AlgorithmState::ClusterState &>(state_);
  state.mean = statecast.uni_ls_state().mean();
  state.var = statecast.uni_ls_state().var();
  set_card(statecast.cardinality());
}

void NNIGHierarchy::write_state_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::UniLSState state_;
  state_.set_mean(state.mean);
  state_.set_var(state.var);

  auto *out_cast = google::protobuf::internal::down_cast<
      bayesmix::AlgorithmState::ClusterState *>(out);
  out_cast->mutable_uni_ls_state()->CopyFrom(state_);
  out_cast->set_cardinality(card);
}

void NNIGHierarchy::write_hypers_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::NNIGPrior hypers_;
  hypers_.mutable_fixed_values()->set_mean(hypers->mean);
  hypers_.mutable_fixed_values()->set_var_scaling(hypers->var_scaling);
  hypers_.mutable_fixed_values()->set_shape(hypers->shape);
  hypers_.mutable_fixed_values()->set_scale(hypers->scale);

  google::protobuf::internal::down_cast<bayesmix::NNIGPrior *>(out)
      ->mutable_fixed_values()
      ->CopyFrom(hypers_.fixed_values());
}


#include <iostream>
#include <limits>
#include <string>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

void test(VectorXd &vec)
{
	cout << vec << '\n';
	for (int i = 0; i < vec.rows(); i++)
	{
		vec[i] = i / 10.;
	}
	cout << vec << '\n';
}

void test(MatrixXd &mat)
{
	cout << mat << '\n';
	for (int i = 0; i < mat.rows(); i++)
		for (int j = 0; j < mat.cols(); j++)
			mat(i, j) = (i + j) / 10.;
	cout << mat << '\n';
	mat << 0.7, 0.8, 0.9,1.,2.,3.;
	cout << mat << '\n';
}

void test(double t[])
{
	t[0] = 0.;
	t[1] = 0.;
}

int main()
{
	ArrayXXd t1(2, 3);
	t1 << 1.1, 2.2, 3.3, 4.4, 5.5, 6.6;
	Array<unsigned int, Dynamic, 3> t2 = (t1 / 0.1).cast<unsigned int>();
	cout << t2 << endl;
	ArrayXXd t3(2, 3);
	t3 << 1.2, 2.1, 3.4, 4.5, 5.6, 6.7;
	ArrayXXd t4 = (t3 > t1).cast<double>();
	cout << t4 << endl;
	ArrayXXd t5 = (t3 > 4).cast<double>();
	cout << t5 << endl;

	double max = std::numeric_limits<double>::max();
	double inf = std::numeric_limits<double>::infinity();

	if (inf > max)
		cout << inf << " is greater than " << max << '\n';
	cout << inf + inf << endl;
	cout << 0.*inf << endl;
	char* inf_char = "inf";
	cout << strtod(inf_char, nullptr) << endl;
	cout << isinf(inf) << endl;

	/*
	ArrayXXd t1(3, 2);
	t1 << 1, 2, 3, 4, 5, 6;
	cout << t1 << endl;
	ArrayXXd t2(3, 2);
	t2 << 7, 8, 9, 0, 1, 2;
	cout << t2 << endl;
	cout << t1.block(0, 0, 1, 2).transpose() + t2.block(0, 0, 2, 1) << endl;

	double *q = t1.data();
	cout << *q << endl;
	q[0] = 90., q[2] = 80; // matrix and array elements are in colum stored in memory
	cout << *q << endl;
	cout << t1 << endl;
	*/

	/*
	VectorXd vec(4);
	vec << 1 , 2 , 3 , 4;
	test(vec);

	MatrixXd mat(2, 3);
	mat << 1, 2, 3, 4, 5, 6;
	test(mat);
	//
	VectorXd v2 = VectorXd::LinSpaced(10, 0., 12.);
	test(v2);
	v2 *= 2.;
	cout << v2 << '\n';

	Matrix3f A;
	Vector3f b;
	A << 1, 2, 3, 4, 5, 6, 7, 8, 10;
	b << 3, 3, 4;
	cout << "Here is the matrix A:\n" << A << endl;
	cout << "Here is the vector b:\n" << b << endl;
	Vector3f x = A.colPivHouseholderQr().solve(b);
	cout << "The solution is:\n" << x << endl;
	cout << b.array().cos() << endl;
	*/

	//
	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// ars::search_reflection.hpp                                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_SEARCH_REFLECTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_SEARCH_REFLECTION_HPP_ER_2009
#include <string>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/ars/constant.hpp>
#include <boost/ars/point.hpp>
#include <boost/ars/error.hpp>
#include <boost/ars/function/signature.hpp>
#include <boost/ars/function/adaptor.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// This function searches by reflection for initial starting points
// (x_0,x_1) such that if x_min = -inf, dy_0>0 and if x_min = inf, dy_0<0

//TODO even if x_min or x_max finite, |dy|>eps might be desirable
template<typename T>
unsigned
search_reflection(
    const T& x_min,
    const T& x_max,
    boost::function<typename ars::function::signature<T>::type> delegate,
    point<T>& p_0,
    point<T>& p_1,
    unsigned n_max
){
    static const char* function
    = "search_reflection(%1%, %2%, ...)";

    typedef point<T> point_t;
    typedef constant<T> const_;

    struct local{
        static bool ok_0(const point_t& p){
            return ( p.dy() >=  const_::eps_ ); }
        static bool ok_1(const point_t& p){
            return ( p.dy() <= (-const_::eps_) ); }
    };

    unsigned n = 0;
    T new_x_0 = p_0.x();
    T new_x_1 = p_1.x();
    bool ok_0, ok_1 = true;
    if(math::isinf(x_min)){ ok_0 = local::ok_0(p_0); }
    if(math::isinf(x_max)){ ok_1 = local::ok_1(p_1); }

    while(
        (!(ok_0 && ok_1))
    ){
        if(n>n_max){
            boost::format f(function);
            f % x_min % x_max;
            throw ars::exception(f.str(),"n>n_max",p_0,p_1);
        }

        if(!ok_0){
            T delta = (new_x_1-new_x_0);
            if(delta < const_::eps_ ){
                boost::format f(function);
                f % x_min % x_max;
                throw ars::exception(
                    f.str(),
                    "new_x_0-new_x_1< (- const_::eps_)",
                    p_0,p_1
                );
            }
            //TODO max(new_x_0,-highest) ?
            new_x_0 -=  delta;
            p_0 = create_point<T>(new_x_0,delegate);
            ok_0 = local::ok_0(p_0);
        }
        if(!ok_1){
            T delta = (new_x_1-new_x_0);
            if( delta < const_::eps_){
                boost::format f(function);
                f % x_min % x_max;
                throw ars::exception(
                    f.str(),
                    "new_x_1-new_x_0 > const_::eps_",
                    p_0,p_1
                );
            }
            new_x_1 += delta;
            p_1 = create_point<T>(new_x_1,delegate);
            ok_1 = local::ok_1(p_1);
        }
        ++n;
    }
    return n;
}

template<typename T>
unsigned search_reflection(
    const T& x_min,
    const T& x_max,
    boost::function<typename ars::function::signature<T>::type> delegate,
    const T& x_0,
    const T& x_1,
    point<T>& p_0,
    point<T>& p_1,
    unsigned n_max
){
    {
        p_0 = create_point<T>(x_0,delegate);
        p_1 = create_point<T>(x_1,delegate);
    }
    return search_reflection(
        x_min,
        x_max,
        delegate,
        p_0,
        p_1,
        n_max
    );
}

// TODO theck that
// T = remove_const< remove_reference< D> ::type >::type ::value_type
template<typename D,typename T> // D = const E& or E
unsigned search_reflection_dist(
    const T& x_min,
    const T& x_max,
    const D& dist,
    const T& x_0,
    const T& x_1,
    ars::point<T>& p_0,
    ars::point<T>& p_1,
    unsigned n_max
){
    typedef ars::function::adaptor<const D&> fnal_t;
    typedef typename ars::function::signature<T>::type   signature;
    typedef boost::function<signature>                  delegate_t;
    fnal_t fnal(dist);

    return search_reflection<T>(
        x_min,
        x_max,
        fnal, //automatic conversion
        x_0,
        x_1,
        p_0,
        p_1,
        n_max
    );
}

}// ars
}// detail
}// statistics
}// boost

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_PSB_INCLUDE
#define ITL_PSB_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/itl/utility/exception.hpp>

namespace itl {

/// Update of Hessian matrix for e.g. Quasi-Newton by Powell's symmetric Broyden formula
struct psb
{
    /// \f$ H_{k+1}=B_{k+1}^{-1}=H_k+\frac{(y_k-H_k\cdot s_k)s_k^T+s_k(y_k-H_k\cdot s_k)^T}{s_k^T\cdot s_k}-\frac{(y_k-H_k\cdot s_k)^T\cdot s_k}{(s_k^ts_k)^2}s_k\cdot s_k^T \f$
    template <typename Matrix, typename Vector>
    void operator() (Matrix& H, const Vector& y, const Vector& s)
    {
	typedef typename mtl::Collection<Vector>::value_type value_type;
	assert(num_rows(H) == num_cols(H));
	Vector     a(s - H * y);
	value_type gamma= 1 / dot (y, y);
        MTL_THROW_IF(gamma == 0.0, unexpected_orthogonality());

        H+= gamma * a * trans(y) + gamma * y * trans(a) - dot(a, y) * gamma * gamma * y * trans(y);
   }
};

} // namespace itl

#endif // ITL_PSB_INCLUDE


#include <iostream>
#include <vector>
#include <mtl/utility/tag.hpp>
#include <mtl/matrix/dense2D.hpp>
#include <mtl/fractalu.hpp>
#include <mtl/utility/property_map.hpp>
#include <mtl/mat_vec_mult.hpp>
#include <boost/timer.hpp>

#include <boost/mpl/if.hpp>

int main (int argc, char** argv) {
  using namespace std;
  using namespace mtl;

  if (argc < 2) {
    cout << "syntax: mat_vec_mult_timing size\n"; exit(1); }

  typedef double                                val_t;
  typedef dense2D<val_t, row_major, c_index>    matrix_type;
  typedef fractalu<val_t, 64>                   umatrix_type;
  std::size_t     size(atoi(argv[1]));
  matrix_type     matrix(dim_type(size, size), 1);
  std::vector<val_t>  vin(size, 1), vout(size, 7);
  umatrix_type    umatrix(dim_type(size, size), 1);

  cout << "vin is mtl type: " << is_mtl_type<std::vector<int> >::value
       << " is fortran indexed: " <<  is_fortran_indexed<std::vector<int> >::value << endl;
  cout << "matrix is mtl type: " << is_mtl_type<matrix_type>::value
       << " is fortran indexed: " <<  is_fortran_indexed<matrix_type>::value << endl;

  // cout << "matrix is boost::is_same<typename indexing<T>::type, c_index>::value

  boost::timer ti;
//   mat_vec_mult(matrix, vin, vout);
//   cout << ti.elapsed() << " s\n";
//   for (size_t i= 0; i < size; i++)
//     if (vout[i] != (int) size) cout << "vout[" << i << "] is " << vout[i] << endl;

//   ti.restart();
  dense_mat_vec_mult(matrix, vin, vout);
  cout << ti.elapsed() << " s with dense matrix vector product\n";
  for (size_t i= 0; i < size; i++)
    if (vout[i] != (int) size) cout << "vout[" << i << "] is " << vout[i] << endl;

  ti.restart();
  mat_vec_mult(umatrix, vin, vout);
  cout << ti.elapsed() << " s\n";
  for (size_t i= 0; i < size; i++)
    if (vout[i] != (int) size) cout << "vout[" << i << "] is " << vout[i] << endl;

  return 0;
}


/* Copyright (c) 2018-2019 the `graphkernels` developers
 * All rights reserved.
 */

#include "rest.h"

#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>

#include <algorithm>
#include <utility>

using std::vector;
using std::pair;

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::SparseMatrix;
using Eigen::VectorXd;

auto order_by_labels(const vector<int>& labels) {
    vector<pair<int, int>> map;
    map.reserve(labels.size());

    auto idx = 0;
    for (const auto label : labels) {
        map.emplace_back(label, idx++);
    }

    sort(map.begin(), map.end());
    return map;
}

auto compute_valid_vertex_pairs(
        const vector<pair<int, int>>& map1,
        const vector<pair<int, int>>& map2) {
    vector<pair<int, int>> pairs;
    pairs.reserve(map1.size() * map2.size());

    const auto comp = [](const auto& p_a, const auto& p_b){
        return p_a.first < p_b.first;
    };

    auto p = map2.cbegin();  // Memoize low limit (see below).
    for (auto i1 = map1.cbegin(); i1 != map1.cend(); ) {
        // Find range of map2 that contains vertices labelled "label1".
        auto [eq_cbegin, eq_cend] = std::equal_range(p, map2.cend(), *i1, comp);

        // Iterate over all equal values in map1.
        const auto label1 = i1->first;
        do {
            const auto num1 = i1->second;

            // Create all pairs between vertex of map1 and range of map2.
            for (auto p = eq_cbegin; p != eq_cend; ++p) {
                pairs.emplace_back(num1, p->second);
            }

            ++i1;
        } while (i1 != map1.cend() && i1->first == label1);

        // All vertices with that label have been exhausted in both maps.
        p = eq_cend;
    }

    sort(pairs.begin(), pairs.end());
    return pairs;
}

auto productAdjacency(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label) {
    // Step 1: Order vertices by labels; compute all valid vertex pairs
    const auto pairs = compute_valid_vertex_pairs(
            order_by_labels(v1_label),
            order_by_labels(v2_label));

    // Step 2: Compute new labels for vertices of the product graph.
    Eigen::Matrix<int, -1, -1, Eigen::RowMajor> H(v1_label.size(), v2_label.size());

    auto next_label = 0;
    for (const auto& [v1, v2] : pairs) {
        H(v1, v2) = next_label++;
    }

    // Step 3: Compute the adjacency matrix of the direct product graph.
    vector<Eigen::Triplet<double>> v;
    for (auto i = 0; i < e1.rows(); ++i) {
        const auto e1_s = e1(i, 0);
        const auto e1_t = e1(i, 1);
        const auto e1_label = e1(i, 2);

        for (auto j = 0; j < e2.rows(); ++j) {
            if (e1_label == e2(j, 2)) {
                const auto e2_s = e2(j, 0);
                const auto e2_t = e2(j, 1);

                if (v1_label[e1_s] == v2_label[e2_s]
                &&  v1_label[e1_t] == v2_label[e2_t]
                   ) {
                    v.emplace_back(H(e1_s, e2_s), H(e1_t, e2_t), 1.0);
                    v.emplace_back(H(e1_t, e2_t), H(e1_s, e2_s), 1.0);
                }

                if (v1_label[e1_s] == v2_label[e2_t]
                &&  v1_label[e1_t] == v2_label[e2_s]
                   ) {
                    v.emplace_back(H(e1_s, e2_t), H(e1_t, e2_s), 1.0);
                    v.emplace_back(H(e1_t, e2_s), H(e1_s, e2_t), 1.0);
                }
            }
        }
    }

    SparseMatrix<double> Ax(next_label, next_label);
    Ax.setFromTriplets(v.cbegin(), v.cend());

    return Ax;
}

double geometricRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        double lambda,
        int max_iterations,
        double eps) {
    // compute the adjacency matrix Ax of the direct product graph
    const SparseMatrix<double> Lx = lambda * productAdjacency(
            e1, e2, v1_label, v2_label);

    // inverse of I - lambda * Ax by fixed-poInt iterations
    const auto n_rows = Lx.rows();
    const VectorXd ones = VectorXd::Ones(n_rows);
    auto x = ones;
    VectorXd x_pre = VectorXd::Zero(n_rows);

    auto count = 0;
    do {
        x_pre = x;
        x = ones + Lx * x_pre;
        ++count;
    } while (count <= max_iterations && (x - x_pre).squaredNorm() > eps);
    return x.sum();
}

MatrixXd CalculateGeometricRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        double lambda,
        int max_iterations,
        double eps) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = geometricRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], lambda,
                    max_iterations, eps);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}

double exponentialRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        double beta) {
    // compute the adjacency matrix Ax of the direct product graph
    const MatrixXd Ax = productAdjacency(e1, e2, v1_label, v2_label);

    return Ax.exp().sum();
}

MatrixXd CalculateExponentialRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        double beta) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = exponentialRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], beta);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}

double kstepRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        const vector<double>& lambda_list) {
    // compute the adjacency matrix Ax of the direct product graph
    const SparseMatrix<double> Ax = productAdjacency(e1, e2, v1_label, v2_label);

    // prepare identity matrix
    const auto n_rows = Ax.rows();
    SparseMatrix<double> I{n_rows, n_rows};
    I.setIdentity();

    auto Sum = SparseMatrix<double>{n_rows, n_rows};
    Sum.setZero();

    // Compute products until k using:
    // https://en.wikipedia.org/wiki/Horner%27s_method
    auto k = lambda_list.size();
    while (k-- > 0) {
        Sum = (Sum * Ax) + lambda_list[k] * I;
    }

    return Sum.sum();
}

MatrixXd CalculateKStepRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        const vector<double>& par) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = kstepRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], par);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include "GaussSeq.h"
#include "ARSeq.h"
#include "utils.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
    using boost::random::uniform_real_distribution;

    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u (0, 0.25);

    // set precision of output
//    std::streamsize precision = std::numeric_limits<cpp_bin_float_50>::digits10;
//    std::cout << std::setprecision(10);

    // testing
    std::map<int, my_float> coeff {
        {1, -0.9},
    };
    ARSeq seq(coeff, 100);
    std::vector<my_float> v;
    for (auto i = 0; i < 1; i++) {
        v.push_back(u(gen));
    }

    seq.seed_prev_vals(v);

    seq.print_past_vals();
    for (auto i = 0; i < 100; i++) {
        seq.next();
    }
    std::cout << "After 100 iterations\n";
    seq.print_past_vals();

    for (auto i = 0; i < 900; i++) {
        seq.next();
    }
    std::cout << "After 1000 iterations\n";
    seq.print_past_vals();

    for (auto i = 0; i < 9000; i++) {
        seq.next();
    }
    std::cout << "After 10000 iterations\n";
    seq.print_past_vals();

    return 0;
}


#include <string>
#include <iostream>
#include <fstream>
#include <math.h>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace std;

string scan_match_file = "./scan_match.txt";
string odom_file = "./odom.txt";

int main(int argc, char **argv)
{
    // 放置激光雷达的时间和匹配值 t_s s_x s_y s_th
    vector<vector<double>> s_data;
    // 放置轮速计的时间和左右轮角速度 t_r w_L w_R
    vector<vector<double>> r_data;

    ifstream fin_s(scan_match_file);
    ifstream fin_r(odom_file);
    if (!fin_s || !fin_r)
    {
        cerr << "请在有scan_match.txt和odom.txt的目录下运行此程序" << endl;
        return 1;
    }

    // 读取激光雷达的匹配值
    while (!fin_s.eof())
    {
        double s_t, s_x, s_y, s_th;
        fin_s >> s_t >> s_x >> s_y >> s_th;
        s_data.push_back(vector<double>({s_t, s_x, s_y, s_th}));
    }
    fin_s.close();

    // 读取两个轮子的角速度
    while (!fin_r.eof())
    {
        double t_r, w_L, w_R;
        fin_r >> t_r >> w_L >> w_R;
        r_data.push_back(vector<double>({t_r, w_L, w_R}));
    }
    fin_r.close();

    // 第一步：计算中间变量J_21和J_22
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    // 设置数据长度
    A.conservativeResize(5000, 2);
    b.conservativeResize(5000);
    A.setZero();
    b.setZero();

    size_t id_r = 0;
    size_t id_s = 0;
    double last_rt = r_data[0][0];
    double w_Lt = 0;
    double w_Rt = 0;
    while (id_s < 5000)
    {
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];
        const double &s_th = s_data[id_s][3];
        // 里程计信息
        const double &r_t = r_data[id_r][0];
        const double &w_L = r_data[id_r][1];
        const double &w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计角度积分
        if (r_t < s_t)
        {
            double dt = r_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = r_t;
        }
        else
        {
            double dt = s_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = s_t;
            // 填充A, b矩阵
            //TODO: (3~5 lines)
            A(id_s, 0) = w_Lt;
            A(id_s, 1) = w_Rt;
            b(id_s) = s_th;

            //end of TODO
            w_Lt = 0;
            w_Rt = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解
    Eigen::Vector2d J21J22;
    //TODO: (1~2 lines)
    J21J22 = A.householderQr().solve(b);

    //end of TODO
    const double &J21 = J21J22(0);
    const double &J22 = J21J22(1);
    cout << "J21: " << J21 << endl;
    cout << "J22: " << J22 << endl;

    // 第二步，求解轮间距b
    Eigen::VectorXd C;
    Eigen::VectorXd S;
    // 设置数据长度
    C.conservativeResize(10000);
    S.conservativeResize(10000);
    C.setZero();
    S.setZero();

    id_r = 0;
    id_s = 0;
    last_rt = r_data[0][0];
    double th = 0;
    double cx = 0;
    double cy = 0;
    while (id_s < 5000)
    {
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];
        const double &s_x = s_data[id_s][1];
        const double &s_y = s_data[id_s][2];
        // 里程计信息
        const double &r_t = r_data[id_r][0];
        const double &w_L = r_data[id_r][1];
        const double &w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计位置积分
        if (r_t < s_t)
        {
            double dt = r_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = r_t;
        }
        else
        {
            double dt = s_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = s_t;
            // 填充C, S矩阵
            //TODO: (4~5 lines)
            C(2 * id_s) = cx;
            C(2 * id_s + 1) = cy;
            S(2 * id_s) = s_x;
            S(2 * id_s + 1) = s_y;

            //end of TODO
            cx = 0;
            cy = 0;
            th = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解，计算b, r_L, r_R
    double b_wheel;
    double r_L;
    double r_R;
    //TODO: (3~5 lines)
    b_wheel = C.householderQr().solve(S)(0);
    r_L = -b_wheel * J21J22(0);
    r_R = b_wheel * J21J22(1);

    //end of TODO
    cout << "b: " << b_wheel << endl;
    cout << "r_L: " << r_L << endl;
    cout << "r_R: " << r_R << endl;

    cout << "参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右" << endl;

    return 0;
}


// Cpush+ version 0.10
// defines a class for particle tracks
// in a uniform magnetic field, with
// a simple leapfrog pushing method.

#include <iostream>
#include <fstream>
#include <time.h>

#include <string>
#include <cmath>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

typedef Vector3d eigvec;
typedef Matrix3d eigmat;

const double pi = 3.1415926;
const eigmat I  = eigmat::Identity();

// particle track class
class track{
public:
  // initialise track properties
  double mass, charge;

  // object constructor
  track(string);

  // field interpolation method
  eigmat interpolate(eigvec x, double step){
    // retrieve vector field
    eigvec B_ = eigvec(0,0,1);

    // compute magnetic rotation map
    eigmat hatmap;
    hatmap << +0,     -B_[2], +B_[1],
              +B_[2], +0,     -B_[0],
              -B_[1], +B_[0], +0;
    hatmap *= (charge * step) / (2 * mass);
    return hatmap;
  }

  // leapfrog pushing method
  int leapfrog(double step, int nsteps){
    // set transverse velocity
    double eV = 1E-03;
    double vx = sqrt((2 * eV * abs(charge))/mass);

    // initialise dynamic variables
    eigvec x_ = eigvec(0,  0, 0);
    eigvec v_ = eigvec(vx, 0, 0);

    // backwards half-step the velocity
    eigmat hatmap = -(1./2.) * interpolate(x_, step);
    v_  = (I + hatmap).inverse() * (I - hatmap) * v_;

    // open a stream to write to file
    fstream track_io ("my_track.bin", ios::out | ios::binary);
    if (track_io.is_open()){

      // particle pushing loop
      for (int n = 0; n <= nsteps; n++){

        // write the position & velocity to file
        track_io << n << ' ' << ' ' << x_[0] << ' ' << x_[1] << ' ' << x_[2];
        track_io      << ' ' << ' ' << v_[0] << ' ' << v_[1] << ' ' << v_[2] << endl;

        // update the velocity and position
        hatmap = interpolate(x_, step);
        v_  = (I + hatmap).inverse() * (I - hatmap) * v_;
        x_ += v_ * step;
      }
    }
    return 0;
  }
};

// track object constructor
track::track(string ptype){
  if ((ptype == "antiproton") || (ptype == "pbar")){
    mass   = +1.67E-27;
    charge = -1.60E-19;
  }

  if ((ptype == "positron") || (ptype == "e+")){
    mass   = +9.11E-31;
    charge = +1.60-19;
  }
}

int main(){
  // example of the class
  track my_track ("pbar");
  double step = 6.56E-09;
  my_track.leapfrog(step, 10000);
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

using namespace std;

int dijkstra_dist(const weighted_graph &G, int s, int t) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  return dist_map[t];
}

void solve()
{
  int n; cin >> n;
  int m; cin >> m;
  int s; cin >> s;
  int a; cin >> a;
  int b; cin >> b;

  vector<weighted_graph> graphs(s, weighted_graph(n));
  vector<weight_map> weights(s);
  for (int i = 0; i < s; ++i) {
    weights[i] = boost::get(boost::edge_weight, graphs[i]);
  }

  int u, v, w;
  for (int i = 0; i < m; ++i) {
    cin >> u;
    cin >> v;
    for (int j = 0; j < s; ++j) {
      cin >> w;
      boost::add_edge(u, v, w, graphs[j]);
    }
  }

  int h;
  for (int i = 0; i < s; ++i) {
    cin >> h;
  }

  weighted_graph G(n);

  for (int i = 0; i < s; ++i) {
    vector<edge_desc> mst;
    boost::kruskal_minimum_spanning_tree(graphs[i], back_inserter(mst));
    for (auto e : mst) {
      int u = boost::source(e, graphs[i]);
      int v = boost::target(e, graphs[i]);
      boost::add_edge(u, v, weights[i][e], G);
    }
  }

  cout << dijkstra_dist(G, a, b) << endl;

}

int main() {
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


//
//  quadratic_polynomial.cpp
//  BGV-Adder
//
//  Created by Andris on 27/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "quadratic_polynomial.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

long quadratic_polynomial(int16_t aVal, int16_t bVal, int16_t cVal, int16_t xVal) {
    long k = 128; // Security parameter
    long L = 128; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    // Change to 65537 for noise warning. (Always fun times)
    // Compensate for that by setting L = 128. (Increase modchain basically)
    // Set P to 65537 and L to 32 to showcase decryption failures.

    long p = 65537; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    Ctxt aVal_ciphertext = Ctxt(publicKey);
    Ctxt bVal_ciphertext = Ctxt(publicKey);
    Ctxt cVal_ciphertext = Ctxt(publicKey);
    Ctxt xVal_ciphertext = Ctxt(publicKey);

    // Plaintext must be encrypted as a polynomial using zzx api.
    publicKey.Encrypt(aVal_ciphertext, NTL::ZZX(aVal));
    publicKey.Encrypt(bVal_ciphertext, NTL::ZZX(bVal));
    publicKey.Encrypt(cVal_ciphertext, NTL::ZZX(cVal));
    publicKey.Encrypt(xVal_ciphertext, NTL::ZZX(xVal));

    // Apply operations on the ciphertexts.

    aVal_ciphertext *= xVal_ciphertext;
    aVal_ciphertext *= xVal_ciphertext;
    bVal_ciphertext *= xVal_ciphertext;

    aVal_ciphertext += bVal_ciphertext;
    aVal_ciphertext += cVal_ciphertext;

    Ctxt cipherResult = aVal_ciphertext;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    long return_value = 0;
    NTL::ZZX plaintext_result;
    NTL::ZZX zzx;
    secretKey.Decrypt(zzx, cipherResult);
    conv(return_value, zzx[0]);
    if (return_value > p / 2) {
        return_value += (-1 * p);
    }

    return return_value;
}


#include <iostream>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Core>

#include "../include/Filter.h"

#include "../include/Robot.h"
#include "../include/Odom.h"
#include "../include/Imu.h"
#include "../include/Gps.h"

WeightingFilter weightingFilter;
WeightingFilter* pweightingFilter;

KalmanFilter kalmanFilter;
KalmanFilter *pkalmanFilter;

// Constructor
KalmanFilter::KalmanFilter()
{
    is_initialized_ = false;
    lastTimeStamp_ = 0;
    nowTimeStamp_ = 0;
    deltaTime_ = 0;
}

//Destructor
KalmanFilter::~KalmanFilter()
{
    delete pkalmanFilter;
}

void KalmanFilter::Initialization()
{
    lastTimeStamp_ = nowTimeStamp_ = getSysTime();

    Eigen::VectorXf x_in(5,1);
    x_in << 0,
            0,
            0,
            0,
            0;
    SetX(x_in);

    // state covariance matrix the prediction error
    Eigen::MatrixXf P_in(5,5);
    P_in << 0.1, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.1, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.1, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.1, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.1;
    SetP(P_in);

    //process covariance matrix
    Eigen::MatrixXf Q_in(5,5);
    Q_in << 0.1, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.1, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.1, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.1, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.1;
    SetQ(Q_in);

    //Observation
    Eigen::VectorXf z_in(3,1);
    z_in << 0.0,
            0.0,
            0.0;

    //measurement matrix
    Eigen::MatrixXf H_in(3,5);
    H_in << 0.0, 0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 0.0, 1.0,
            0.0, 0.0, 0.0, 0.0, 1.0;
    SetH(H_in);

    //measurement covariance matrix
    // R is provided by Sensor supplier
    Eigen::MatrixXf R_in(3,3);
    R_in << 0.1, 0.0, 0.0,
            0.0, 0.1, 0.0,
            0.0, 0.0, 0.1;
    SetR(R_in);

    is_initialized_ = true;

}

void KalmanFilter::SetX(Eigen::VectorXf x_in)
{
    x_ = x_in;
}

bool KalmanFilter::GetIsInitialized()
{
    return is_initialized_;
}

void KalmanFilter::SetF(Eigen::MatrixXf F_in)
{
    F_ = F_in;
}

void KalmanFilter::SetP(Eigen::MatrixXf P_in)
{
    P_ = P_in;
}

void KalmanFilter::SetQ(Eigen::MatrixXf Q_in)
{
    Q_ = Q_in;
}

void KalmanFilter::SetH(Eigen::MatrixXf H_in)
{
    H_ = H_in;
}

void KalmanFilter::SetR(Eigen::MatrixXf R_in)
{
    R_ = R_in;
}

void KalmanFilter::Prediction(float d_t) //预测
{
    float x_k, y_k, theta_k, v_k, w_k;
    x_k = x_[0];
    y_k = x_[1];
    theta_k = x_[2];
    v_k = x_[3];
    w_k = x_[4];

    x_ << x_k + v_k*d_t*cos(theta_k),y_k + v_k*d_t*sin(theta_k), theta_k + w_k*d_t, v_k,  w_k;
    Eigen::MatrixXf F_in(5,5);
    F_in << 1, 0, 0, d_t*cos(x_[2]), 0,
            0, 1, 0, d_t*sin(x_[2]), 0,
            0, 0, 1, 0, d_t,
            0, 0, 0, 1, 0,
            0, 0, 0, 0, 1;

    kalmanFilter.SetF(F_in);
    Eigen::MatrixXf Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}
/*
void KalmanFilter::CalculateJacobianMatrix()
{
    Eigen::MatrixXf Hj(4,4);

    //get state paraeters
    float px = x_(0);
    float py = x_(1);
    float vx = x_(2);
    float vy = x_(3);

    //pre-compute a set of terms to avoid repeated calculation
    Hj << 1.0, 0.0, 0.0, 0.0,   //GPS 与 ODOM与IMU加权平均 后的结果进行再次融合
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 0.0, 0.0,
          0.0, 0.0, 0.0, 0.0;

    SetH(Hj);
}
*/
void KalmanFilter::KFUpdate(const Eigen::VectorXf &z)
{
    int size = x_.size();
    std::cout << size << std::endl;
    Eigen::VectorXf y = z - H_ * x_;
    Eigen::MatrixXf S = H_ * P_ * H_.transpose() + R_;
    Eigen::MatrixXf K = P_ * H_.transpose() * S.inverse();
    x_ = x_ + (K * y);
    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(size, size);
    P_ = (I - K * H_) * P_;
    //std::cout << "x_:" << std::endl << x_ << std::endl;

}
/*
void KalmanFilter::EKFUpdate(const Eigen::VectorXf &z)
{

    // Eigen::VectorXf h = Eigen::VectorXf(4);
    CalculateJacobianMatrix();

    Eigen::VectorXf y = z - H_ * x_;
    Eigen::MatrixXf Ht = H_.transpose();
    Eigen::MatrixXf S = H_ * P_ * Ht + R_;
    Eigen::MatrixXf Si = S.inverse();
    Eigen::MatrixXf K = P_ * Ht * Si;

    x_ = x_ + (K * y);

    int x_size = x_.size();
    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
	std::cout << "x_:" << std::endl << x_ <<std::endl;

}
*/
Eigen::VectorXf KalmanFilter::GetX()
{
    return x_;
}

void KalmanFilter::setLastTimeStamp(const long int lastTimeStamp)
{
    this->lastTimeStamp_ = lastTimeStamp;
}

void KalmanFilter::setNowTimeStamp(const long int nowTimeStamp )
{
    this->nowTimeStamp_ = nowTimeStamp;
}
void KalmanFilter::setDeltaTime(const long int deltaTime)
{
    this->deltaTime_ = deltaTime;
}

void KalmanFilter::getLastTimeStamp(long int& lastTimeStamp)
{
    lastTimeStamp = this->lastTimeStamp_;
}
void KalmanFilter::getNowTimeStamp(long int& nowTimeStamp)
{
    nowTimeStamp = this->nowTimeStamp_;
}
void KalmanFilter::getDeltaTime(long int& deltaTime)
{
    deltaTime = this->deltaTime_;
}

WeightingFilter::WeightingFilter()
{

}

WeightingFilter::~WeightingFilter()
{
    delete pweightingFilter;
}

void WeightingFilter::WeightingFilterUpdate()  //只是进行位置融合
{
    float odom_x, odom_y, odom_theta;
    float imu_x, imu_y, imu_z, imu_theta;
    float Roll; //翻滚角
    float Pitch; //俯仰角
    float Yaw; //偏航角

    odom.GetPos(odom_x, odom_y, odom_theta);
    imu.GetPosition( imu_x, imu_y, imu_theta);

    float robot_x, robot_y, robot_theta;

    robot_x = WEIGHT * odom_x + (1- WEIGHT) * imu_x;
    robot_y = WEIGHT * odom_y + (1- WEIGHT) * imu_y;
    imu.GetPostureYPR(Roll, Pitch, Yaw);
    robot_theta = Yaw/57.3;

    robot.SetRobotPosition(robot_x, robot_y, 0);
    robot.SetRobotRotation(Roll, Pitch, Yaw);
    odom.SetPos(robot_x,robot_y,robot_theta);
    imu.SetPosition(robot_x,robot_y,robot_theta);
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/
#ifndef KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_
#define KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_

#include <Eigen/SVD>

namespace kindr {
//! Linear algebra methods
namespace linear_algebra {

/*!
 * \brief Gets a skew-symmetric matrix from a (column) vector
 * \param   vec 3x1-matrix (column vector)
 * \return skew   3x3-matrix
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 3> getSkewMatrixFromVector(const Eigen::Matrix<PrimType_, 3, 1>& vec) {
  Eigen::Matrix<PrimType_, 3, 3> mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

/*!
 * \brief Gets a 3x1 vector from a skew-symmetric matrix
 * \param   matrix 3x3-matrix
 * \return  column vector (3x1-matrix)
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 1> getVectorFromSkewMatrix(const Eigen::Matrix<PrimType_, 3, 3>& matrix) {
  return Eigen::Matrix<PrimType_, 3, 1> (matrix(2,1), matrix(0,2), matrix(1,0));
}

/*!
 * \brief Computes the Moore–Penrose pseudoinverse
 * info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
 * \param a: Matrix to invert
 * \param result: Result is written here
 * \param epsilon: Numerical precision (for example 1e-6)
 * \return true if successful
 */
template<typename _Matrix_Type_>
bool static pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon())
{
  Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

  typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV() * _Matrix_Type_( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() * svd.matrixU().adjoint();

  return true;
}

} // end namespace linear_algebra
} // end namespace kindr

#endif /* KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_ */


/**
 * @file pfasst/quadrature.hpp
 * @since v0.1.0
 */
#ifndef _PFASST__QUADRATURE_HPP_
#define _PFASST__QUADRATURE_HPP_

#include <cmath>
#include <exception>
#include <vector>
using namespace std;

#include <Eigen/Dense>
template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

#include "pfasst/config.hpp"
#include "pfasst/interfaces.hpp"
#include "pfasst/quadrature/polynomial.hpp"
#include "pfasst/quadrature/interface.hpp"
#include "pfasst/quadrature/gauss_lobatto.hpp"
#include "pfasst/quadrature/gauss_legendre.hpp"
#include "pfasst/quadrature/gauss_radau.hpp"
#include "pfasst/quadrature/clenshaw_curtis.hpp"
#include "pfasst/quadrature/uniform.hpp"

template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

namespace pfasst
{
  /**
   * Functionality related to computing quadrature nodes and weights.
   *
   * @note Please note, that all quadrature nodes are in the range \\( [0, 1] \\).
   */
  namespace quadrature
  {
    /**
     * Instantiates quadrature handler for given number of nodes and type descriptor.
     *
     * @tparam precision numerical type of the nodes (e.g. `double`)
     * @param[in] nnodes number of quadrature nodes
     * @param[in] qtype type descriptor of the quadrature
     * @returns instance of pfasst::quadrature::IQuadrature of specified type with desired number
     *   of nodes
     * @throws pfasst::ValueError if @p qtype is not a valid quadrature type descriptor
     */
    template<typename precision = pfasst::time_precision>
    shared_ptr<IQuadrature<precision>> quadrature_factory(const size_t nnodes,
                                                          const QuadratureType qtype)
    {
      if (qtype == QuadratureType::GaussLegendre) {
        return make_shared<GaussLegendre<precision>>(nnodes);
      } else if (qtype == QuadratureType::GaussLobatto) {
        return make_shared<GaussLobatto<precision>>(nnodes);
      } else if (qtype == QuadratureType::GaussRadau) {
        return make_shared<GaussRadau<precision>>(nnodes);
      } else if (qtype == QuadratureType::ClenshawCurtis) {
        return make_shared<ClenshawCurtis<precision>>(nnodes);
      } else if (qtype == QuadratureType::Uniform) {
        return make_shared<Uniform<precision>>(nnodes);
      } else {
        throw ValueError("invalid node type passed to compute_nodes.");
        return nullptr;
      }
    }

    /**
     * Compute quadrature nodes for given quadrature type descriptor.
     *
     * @tparam precision numerical type of the nodes (e.g. `double`)
     * @param[in] nnodes number of quadrature nodes to compute
     * @param[in] qtype type descriptor of the quadrature nodes
     * @returns std::vector of quadrature nodes of given type
     *
     * @see pfasst::quadrature::QuadratureType for valid types
     * @see pfasst::quadrature::quadrature_factory for further details
     */
    template<typename precision = pfasst::time_precision>
    vector<precision> compute_nodes(size_t nnodes, QuadratureType qtype)
    {
      return quadrature_factory<precision>(nnodes, qtype)->get_nodes();
    }

    /**
     * Compute weights to interpolate from @p src nodes to @p dst nodes.
     *
     * @tparam precision numerical type of the interpolation (e.g. `double`)
     */
    template<typename precision = time_precision>
    Matrix<precision> compute_interp(vector<precision> dst, vector<precision> src)
    {
      const size_t ndst = dst.size();
      const size_t nsrc = src.size();

      Matrix<precision> mat(ndst, nsrc);

      for (size_t i = 0; i < ndst; i++) {
        for (size_t j = 0; j < nsrc; j++) {
          precision den = 1.0;
          precision num = 1.0;

          for (size_t k = 0; k < nsrc; k++) {
            if (k == j) { continue; }
            den *= src[j] - src[k];
            num *= dst[i] - src[k];
          }

          if (abs(num) > 1e-32) {
            mat(i, j) = num / den;
          } else {
            mat(i, j) = 0.0;
          }
        }
      }

      return mat;
    }
  }  // ::pfasst::quadrature
}  // ::pfasst

#endif  // _PFASST__QUADRATURE_HPP_


//    Copyright 2017 Rainer Gemulla
//
//    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.
/** \file
 *
 * Illustrates matrix factorization with distributed alternating least squares.
 * We first create factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 =10;// 480189;//10000;
	mf_size_type size2 =10;// 17770;//10000;
	mf_size_type nnz = 10;//1408395;//1000000;
	double sigma = 1; // standard deviation
	double lambda =0;// 1/sigma/sigma;
	mf_size_type r = 5;

	// parameters for ALS
	unsigned epochs = 2;
	AlsRegularizer regularizer = ALS_L2;
	typedef SumLoss<NzslLoss, L2Loss> Loss;
	typedef NzslLoss TestLoss;
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = 100;//nnz/100;
	BalanceType type = BALANCE_NONE;// BALANCE_L2;;
	BalanceMethod method = BALANCE_SIMPLE;

	// parameters for distribution
	int tasksPerRank = 2;
	mf_size_type blocks = world.size() * tasksPerRank;

	mfStart();

	if (world.rank() == 0) {
	#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
	#endif
		// generate original factors by sampling from a normal(0,sigma) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
		generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and add small Gaussian noise
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		addRandom(v, random, boost::normal_distribution<>(0, 0.1));
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		v.sort();
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
		SparseMatrixCM vc;
		copyCm(v, vc);

		// create a test matrix (without noise)
		SparseMatrix vTest;
		generateRandom(vTest, testNnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Test matrix: "
			<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
		generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

		// distribute the input matrices and test matrix
		DistributedSparseMatrix dv = distributeMatrix("V", blocks, 1, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
				<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedSparseMatrixCM dvc = distributeMatrix("VC", 1, blocks, false, vc);
		LOG4CXX_INFO(logger, "Distributed data matrix (CM): "
				<< dvc.blocks1() << " x " << dvc.blocks2() << " blocks");
		DistributedSparseMatrix dvTest = distributeMatrix("Vtest", blocks, blocks, true, vTest);
		LOG4CXX_INFO(logger, "Distributed test matrix: "
				<< dvTest.blocks1() << " x " << dvTest.blocks2() << " blocks");
		DistributedDenseMatrix dw = distributeMatrix("W", blocks, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize
		DapFactorizationData<> data(dv, dw, dh, tasksPerRank, &dvc);
		DsgdFactorizationData<> testJob(dvTest, dw, dh, tasksPerRank);
		Trace trace;
		// here add fields to Trace
//		trace.addField("balancing-type", type);
//		trace.addField("balancing-method", method);
		Timer t;

		// run ALS to try to reconstruct the original factors
		t.start();
		dalsNzsl(data, epochs, trace, lambda, regularizer, type, method, &testJob);
		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// write trace to an R file
		string typeString, methodString;
		switch (type) {
		case BALANCE_NONE:
			typeString = "None";
			break;
		case BALANCE_L2:
			typeString = "L2";
			break;
		case BALANCE_NZL2:
			typeString = "Nzl2";
			break;
		}
		switch (method) {
		case BALANCE_SIMPLE:
			methodString = "Simple";
			break;
		case BALANCE_OPTIMAL:
			methodString = "Optimal";
			break;
		}
		string filename = "/tmp/dals-trace.R";
		LOG4CXX_INFO(logger, "Writing trace to " << filename);
		trace.toRfile(filename, "dals");
	}

	mfStop();
	mfFinalize();

	return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "quad_planarity.h"
#include <Eigen/Geometry>

template <typename DerivedV, typename DerivedF, typename DerivedP>
IGL_INLINE void igl::quad_planarity(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedP> & P)
{
  int nf = F.rows();
  P.setZero(nf,1);
  for (int i =0; i<nf; ++i)
  {
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v1 = V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v2 = V.row(F(i,1));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v3 = V.row(F(i,2));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v4 = V.row(F(i,3));
    Eigen::Matrix<typename DerivedV::Scalar,1,3> diagCross=(v3-v1).cross(v4-v2);
    typename Eigen::PlainObjectBase<DerivedV>::Scalar denom = diagCross.norm()*(((v3-v1).norm()+(v4-v2).norm())/2);
    if (fabs(denom)<1e-8)
      //degenerate quad is still planar
      P[i] = 0;
    else
      P[i] = (diagCross.dot(v2-v1)/denom);
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::quad_planarity<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


#pragma once

#include <Eigen/Dense>
#include <limits>

#include <solvers/qp.hpp>

namespace sqp {

template <typename T>
class SQP;

template <typename Scalar>
struct sqp_settings_t {
    Scalar tau = 0.5;       /**< line search iteration decrease, 0 < tau < 1 */
    Scalar eta = 0.25;      /**< line search parameter, 0 < eta < 1 */
    Scalar rho = 0.5;       /**< line search parameter, 0 < rho < 1 */
    Scalar eps_prim = 1e-4; /**< primal step termination threshold, eps_prim > 0 */
    Scalar eps_dual = 1e-4; /**< dual step termination threshold, eps_dual > 0 */
    int max_iter = 100;
    int line_search_max_iter = 20;
    bool second_order_correction = false;
    std::function<void(SQP<Scalar>&)> iteration_callback;

    bool validate() {
        bool valid;
        valid = 0.0 < tau && tau < 1.0 && 0.0 < eta && eta < 1.0 && 0.0 < rho && rho < 1.0 &&
                eps_prim < 0.0 && eps_dual < 0.0 && max_iter > 0 && line_search_max_iter > 0;
        return valid;
    }
};

typedef enum { SOLVED, MAX_ITER_EXCEEDED, INVALID_SETTINGS } Status;

struct Info {
    int iter;
    int qp_solver_iter;
    Status status;

    void print() {
        printf("SQP info:\n");
        printf("  iter: %d\n", iter);
        printf("  qp_solver_iter: %d\n", qp_solver_iter);
        printf("  status: ");
        switch (status) {
            case SOLVED:
                printf("SOLVED\n");
                break;
            case MAX_ITER_EXCEEDED:
                printf("MAX_ITER_EXCEEDED\n");
                break;
            case INVALID_SETTINGS:
                printf("INVALID_SETTINGS\n");
                break;
            default:
                printf("UNKNOWN\n");
                break;
        }
    }
};

template <typename Scalar_ = double>
struct NonLinearProblem {
    using Scalar = Scalar_;
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

    int num_var;
    int num_constr;

    virtual void objective(const Vector& x, Scalar& obj) = 0;
    virtual void objective_linearized(const Vector& x, Vector& grad, Scalar& obj) = 0;
    virtual void constraint(const Vector& x, Vector& c, Vector& l, Vector& u) = 0;
    virtual void constraint_linearized(const Vector& x, Matrix& Jc, Vector& c, Vector& l,
                                       Vector& u) = 0;
};

/*
 * minimize     f(x)
 * subject to   l <= c(x) <= u
 */
template <typename Scalar_>
class SQP {
   public:
    using Scalar = Scalar_;
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using Problem = NonLinearProblem<Scalar>;
    using Settings = sqp_settings_t<Scalar>;

    // Constants
    static constexpr Scalar DIV_BY_ZERO_REGUL = std::numeric_limits<Scalar>::epsilon();

    // enforce 16 byte alignment
    // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SQP();
    ~SQP() = default;

    void solve(Problem& prob, const Vector& x0, const Vector& lambda0);
    void solve(Problem& prob);

    inline const Vector& primal_solution() const { return x_; }
    inline Vector& primal_solution() { return x_; }

    inline const Vector& dual_solution() const { return lambda_; }
    inline Vector& dual_solution() { return lambda_; }

    inline const Settings& settings() const { return settings_; }
    inline Settings& settings() { return settings_; }

    inline const Info& info() const { return info_; }
    inline Info& info() { return info_; }

    // private:
    void run_solve(Problem& prob);

    bool termination_criteria(const Vector& x, Problem& prob);
    void solve_qp(Problem& prob, Vector& p, Vector& lambda);
    bool run_solve_qp(const Matrix& P, const Vector& q, const Matrix& A, const Vector& l,
                      const Vector& u, Vector& prim, Vector& dual);

    /** Second order correction by solving the same QP with corrected constraints. */
    void second_order_correction(Problem& prob, Vector& p, Vector& lambda);

    /** Line search in direction p using l1 merit function. */
    Scalar line_search(Problem& prob, const Vector& p);

    /** L1 norm of constraint violation */
    Scalar constraint_norm(const Vector& x, Problem& prob);

    /** L1 norm of constraint violation, for given constraint evaluation */
    Scalar constraint_norm(const Vector &constr, const Vector &l, const Vector &u) const;

    /** L_inf norm of constraint violation */
    Scalar max_constraint_violation(const Vector& x, Problem& prob);

    // Solver state variables
    Vector x_;
    Vector lambda_;
    Vector step_prev_;
    Vector grad_L_;
    Vector delta_grad_L_;

    Matrix Hess_;
    Vector grad_obj_;
    Scalar obj_;
    Matrix Jac_constr_;
    Vector constr_;
    Vector l_, u_;

    // info
    Scalar dual_step_norm_;
    Scalar primal_step_norm_;

    Settings settings_;
    Info info_;

    qp_solver::QPSolver<Scalar> qp_solver_;
};

extern template class SQP<double>;

}  // namespace sqp


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library 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
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_GEOMETRY_HPP
#define UTILITIES_GEOMETRY_GEOMETRY_HPP

#include "../UtilitiesAPI.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  class Point3d;
  class PointLatLon;
  class Vector3d;

  /// convert degrees to radians
  UTILITIES_API double degToRad(double degrees);

  /// convert radians to degrees
  UTILITIES_API double radToDeg(double radians);

  /// compute area from surface as Point3dVector
  UTILITIES_API boost::optional<double> getArea(const std::vector<Point3d>& points);

  /// compute Newall vector from surface as Point3dVector, direction is same as outward normal
  /// magnitude is twice the area
  UTILITIES_API boost::optional<Vector3d> getNewallVector(const std::vector<Point3d>& points);

  /// compute outward normal from surface as Point3dVector
  UTILITIES_API boost::optional<Vector3d> getOutwardNormal(const std::vector<Point3d>& points);

  /// compute centroid from surface as Point3dVector
  UTILITIES_API boost::optional<Point3d> getCentroid(const std::vector<Point3d>& points);

  /// reorder points to upper-left-corner convention
  UTILITIES_API std::vector<Point3d> reorderULC(const std::vector<Point3d>& points);

  /// removes collinear points, tolerance is for length of cross product after normalizing each line segment
  UTILITIES_API std::vector<Point3d> removeCollinear(const std::vector<Point3d>& points, double tol = 0.001);

  /// return distance between two points
  UTILITIES_API double getDistance(const Point3d& point1, const Point3d& point2);

  /// return distance between a point and a line segment
  /// returns 0 if lineSegment does not have length 2
  UTILITIES_API double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment);

  /// return distance between a point and a triangle
  /// returns 0 if triangle does not have length 3
  UTILITIES_API double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle);

  /// return angle (in radians) between two vectors
  UTILITIES_API double getAngle(const Vector3d& vector1, const Vector3d& vector2);

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  UTILITIES_API double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2);

  /// check if two vectors of points are equal (within tolerance) irregardless of initial ordering.
  UTILITIES_API bool circularEqual(const std::vector<Point3d>& points1, const std::vector<Point3d>& points2, double tol = 0.001);

  /// if point3d is within tol of any existing points then returns existing point
  /// otherwise adds point3d to allPoints and returns point3d
  UTILITIES_API Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol = 0.001);

  /// compute triangulation of vertices, holes are removed in the triangulation
  /// requires that vertices and holes are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > computeTriangulation(const std::vector<Point3d>& vertices, const std::vector<std::vector<Point3d> >& holes, double tol = 0.001);

  /// move all vertices towards point by distance, pass negative distance to move away from point
  /// no guarantee that resulting polygon will be valid
  UTILITIES_API std::vector<Point3d> moveVerticesTowardsPoint(const std::vector<Point3d>& vertices, const Point3d& point, double distance);

  /// reverse order of vertices
  UTILITIES_API std::vector<Point3d> reverse(const std::vector<Point3d>& vertices);

} // openstudio

#endif //UTILITIES_GEOMETRY_GEOMETRY_HPP