#include "polynomial.h"
#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <stack>
#include <array>
#include <complex>

using namespace polynomial;

Polynomial::Polynomial(const std::vector<double>& nums) {
    /* Instantiate a polynomial from a list of coefficients in increasing order. You must include all terms, including
     * a constant term at the beginning and terms with a 0 coefficient EX: x^2 = {0, 0, 1}
     */

    degree = nums.size() - 1; // subtract one because the polynomial vector also includes a constant term
    coefs = nums;
    // remove extra terms at the front if the coeficient is 0 to ensure stability with rest of the code
    while (coefs.back() == 0 && degree > 0){
        coefs.pop_back();
        degree -= 1;
    }
}

Polynomial::Polynomial(const std::vector<double>& x, const std::vector<double>& y, const int& order){
    /* Fit an n-th degree polynomial of y as a function of x using least squares regression. Order is the degree
     * of the resulting polynomial
     */

    //necessary assumptions in order to be able to do calculations
    assert(x.size() == y.size());
    assert(x.size() >= order + 1);

    Eigen::MatrixXd X(x.size(), order+1);
    Eigen::VectorXd Y = Eigen::VectorXd::Map(&y.front(), y.size());
    Eigen::VectorXd result;

    // populate matrix
    for (size_t i = 0; i < x.size(); i++){
        for (size_t j = 0; j < order + 1; j++){
            X(i, j) = pow(x.at(i), j);
        }
    }
    // solve matrix
    result = X.householderQr().solve(Y);
    // add coefficients to the coefs vector
    for (int i = 0; i < order+1; i++){
        coefs.push_back(result[i]);
    }
    degree = order;
}

Polynomial Polynomial::operator+(const double& num){
    std::vector<double> new_coefs(coefs);
    new_coefs[0] += num;
    return Polynomial(new_coefs);
}

Polynomial Polynomial::operator-(const double& num) {
    std::vector<double> new_coefs(coefs);
    new_coefs[0] -= num;
    return Polynomial(new_coefs);
}

Polynomial Polynomial::operator*(const double& num){
    std::vector<double> new_coefs(degree+1);
    std::transform(coefs.begin(), coefs.end(), new_coefs.begin(), [&num](double elm){return elm *= num;});
    return Polynomial(new_coefs);
}

Polynomial Polynomial::operator/(const double& num) {
    assert(num != 0);
    std::vector<double> new_coefs(degree+1);
    std::transform(coefs.begin(), coefs.end(), new_coefs.begin(), [&num](double elm){return elm /= num;});
    return Polynomial(new_coefs);
}

Polynomial Polynomial::operator+(const Polynomial& other){
    int new_degree = std::max(degree, other.degree);
    std::vector<double> new_coefs(new_degree+1);
    for (int i = 0; i <= degree; i++){
        new_coefs[i] += coefs[i];
    }
    for (int i = 0; i <= other.degree; i++){
        new_coefs[i] += other.coefs[i];
    }
    return Polynomial(new_coefs);
}

Polynomial Polynomial::operator-(const Polynomial& other){
    int new_degree = std::max(degree, other.degree);
    std::vector<double> new_coefs(new_degree+1);
    for (int i = 0; i <= degree; i++){
        new_coefs[i] += coefs[i];
    }
    for (int i = 0; i <= other.degree; i++){
        new_coefs[i] -= other.coefs[i];
    }
    return Polynomial(new_coefs);
}

double Polynomial::operator()(const double& x){
    return val(x);
}

double Polynomial::val(const double& x){
    /* Return the value of the polynomial at x
     */
    double res = coefs[0];
    for (int i = 1; i <= degree; i++){
        res += coefs[i] * pow(x, i);
    }
    return res;
}

std::vector<double> Polynomial::roots(const double& real_threshold){
    /* Return all real roots of the polynomial. The roots are calculated using the eigenvalues of the companion matrix
     * and will be automatically sorted.
     * @param real_threshold: The maximum imagninary threshold at which a root can still be considered real
     */
    std::vector<double> roots;

    Eigen::MatrixXd companion(degree, degree);
    companion.setZero();

    companion(0, degree-1) = -(coefs[0] / coefs.back()); // intialize first row in companion matrix
    for (int row = 1; row < degree; row++){ // initialize rest of companion matrix
        companion(row, row-1) = 1;
        companion(row, degree-1) = -(coefs[row] / coefs.back());
    }

    Eigen::EigenSolver<Eigen::MatrixXd> es(companion, false); // false because we do not need the eigenvectors, only the eigenvalues
    for (std::complex<double> l : es.eigenvalues()){
        if (abs(l.imag()) < real_threshold){
            roots.push_back(l.real());
        }
    }
    sort(roots.begin(), roots.end());
    return roots;
}

std::vector<double> Polynomial::roots(const double& lb, const double& ub, const double& real_threshold){
    /* Return all real roots within the specified bounds, exclusive: (lb, ub)
     */
    std::vector<double> roots;

    Eigen::MatrixXd companion(degree, degree);
    companion.setZero();

    companion(0, degree-1) = -(coefs[0] / coefs.back()); // intialize first row in companion matrix
    for (int row = 1; row < degree; row++){ // initialize rest of companion matrix
        companion(row, row-1) = 1;
        companion(row, degree-1) = -(coefs[row] / coefs.back());
    }

    Eigen::EigenSolver<Eigen::MatrixXd> es(companion, false); // false because we do not need the eigenvectors, only the values
    for (std::complex<double> l : es.eigenvalues()){
        if (abs(l.imag()) < real_threshold && l.real() > lb && l.real() < ub){
            roots.push_back(l.real());
        }
    }
    sort(roots.begin(), roots.end());
    return roots;
}

std::vector<double> Polynomial::budans_roots(const double& lb, const double& ub, const int& newton_iter, const double& precision){
    /* This polynomial root function uses budan's theorem to aproximate a range that each root lies within, then
     * newtons method is used to find the exact value of the root. This method is faster than the other roots method when
     * the polynomial has a degree higher than 10
     * @param newton_iter is the number of iterations of newtons method, more iterations will increase precision of the roots
     * @param precision is the maximum range in the budan's approximation of the roots. Higher precision will be better but more time consuming
     */
    // IT IS VERY HIGHLY RECOMMENDED YOU PASS BOUNDS TO THIS METHOD TO ENSURE IT DOES NOT OVERFLOW ON HIGH DEGREE POLYNOMIALS
    std::vector<double> roots;
    std::vector<Polynomial> eqs;
    eqs.push_back(deriv()); // add the first derivative to the budan table
    for (int i = 1; i < degree; i++){ // populate vector with the rest of the derivatives of the polynomial
        eqs.push_back(eqs.back().deriv()); // get last polynomial, take its derivative, then add it to the end of the vector
    }

    // if degree is > 8, and bounds not passed, the doubles will overflow
    double x1 = lb;
    double x2 = ub;

    std::vector<std::array<double, 2>> one_root_regions; // final regions, each with one root, note regions are half-open: (x1, x2]
    std::stack<std::array<double, 2>> regions_stack;

    regions_stack.push({x1, x2});
    while (!regions_stack.empty()){
        std::array<double, 2> reg = regions_stack.top();
        double l = reg[0];
        double r = reg[1];
        regions_stack.pop();
        int left_sign_switches = 0;
        double last = val(l);
        for (Polynomial p : eqs){
            // increase count whenever the last and p.val(l) have opposite signs
            double val = p.val(l);
            if ((val > 0) && (last < 0)){
                last = val;
                left_sign_switches++;
            }else if((val < 0) && (last > 0)){
                last = val;
                left_sign_switches++;
            }else if(last == 0){
                last = val;
            }
        }

        int right_sign_switches = 0;
        last = val(r);
        for (Polynomial p : eqs){
            // increase count whenever the last and p.val(r) have opposite signs
            double val = p.val(r);
            if ((val > 0) && (last < 0)){
                last = val;
                right_sign_switches++;
            }else if((val < 0) && (last > 0)){
                last = val;
                right_sign_switches++;
            }else if(last == 0){
                last = val;
            }
        }

        int s = left_sign_switches - right_sign_switches;
        if (s == 0){ // no roots in region
            continue;
        }else if (s == 1){
            if ((r - l) < precision){
                one_root_regions.push_back(reg);
            }else{
                double m = (l + r)/2;
                regions_stack.push({l, m});
                regions_stack.push({m, r});
            }
        }else{ // if more then 1 roots in the region, split into left and rights sides then add both to the stack
            if ((r - l) < (precision)){
                continue;
            }else{
                double m = (l + r)/2;
                regions_stack.push({l, m});
                regions_stack.push({m, r});
            }
        }
    }

    for (std::array<double, 2> arr : one_root_regions){
        double x = (arr[0] + arr[1]) / 2; // initial geuss is the midpoint of the region
        for (int i = 0; i < newton_iter; i++){
            x = x - (val(x) / eqs[0].val(x));
        }
        if (x > arr[0] && x <= arr[1]){
            roots.push_back(x);
        }
    }
    sort(roots.begin(), roots.end());
    return roots;
}

Polynomial Polynomial::deriv(){
    /* Take the derivative of the polynomial and return it as a new polynomial
     */
    std::vector<double> res_coefs(degree);
    for (int i = 1; i <= degree; i++){
        res_coefs[i-1] = coefs[i] * i;
    }
    return Polynomial(res_coefs);
}

Polynomial Polynomial::integ(){
    /* Compute the indefinite integral of the polynomial, this does not include any integration constant
     */
    std::vector<double> new_coefs(degree+2);
    for (int i = 0; i <= degree; i++){
        new_coefs[i+1] = coefs[i] / (i+1);
    }
    return Polynomial(new_coefs);
}

Polynomial Polynomial::integ(const double& lbnd) {
    /* Compute the indefinite integral of the polynomial, but scale it to be 0 at lbnd
     */
    std::vector<double> new_coefs(degree+2);
    for (int i = 0; i <= degree; i++){
        new_coefs[i+1] = coefs[i] / (i+1);
    }
    Polynomial temp(new_coefs);
    return (temp - temp.val(lbnd));
}

double Polynomial::integ(const double& lb, const double& ub) {
    /* Compute the definite integral of the polynomial from lb to ub
     */
    std::vector<double> new_coefs(degree+2);
    for (int i = 0; i <= degree; i++){
        new_coefs[i+1] = coefs[i] / (i+1);
    }
    Polynomial poly(new_coefs);
    return poly.val(ub) - poly.val(lb);
}

std::vector<double> Polynomial::solve(const double& num){
    /* Solve for all occurrences of when the polynomial equals num, by default this uses the companion matrix method, so
     * for very high degree polynomials you should consider using budan's method
     */
    std::vector<double> new_coefs = coefs;
    new_coefs[0] -= num;
    Polynomial temp(new_coefs);
    return temp.roots();
}

double Polynomial::solve(const double& num, const double& lb, const double& ub, const int& newton_iter){
    /* This version of solve is meant for when a function is strictly monotonic over the interval (lb, ub) and it is known
     * that exactly one solution to exists within the bounds, for example solving a cdf function.
     */
    std::vector<double> new_coefs = coefs;
    new_coefs[0] -= num;
    Polynomial f(new_coefs);
    Polynomial f_prime = f.deriv();
    double x = (lb+ub)/2; // initial geuss is midpoint
    for (int i = 0; i < newton_iter; i++){
        x = x - (f.val(x) / f_prime.val(x));
    }
    return x;
}

void Polynomial::print() {
    if (degree == 0){
        std::cout << coefs[0] << std::endl;
        return;
    }
    std::cout << coefs[degree] << "x^" << degree;
    for (int i = degree-1; i > 0; i--){
        if (coefs[i] > 0){
            std::cout << " + ";
        }else{
            std::cout << " - ";
        }
        std::cout << abs(coefs[i]) << "x^" << i;
    }
    if (coefs[0] > 0){
        std::cout << " + " << coefs[0] << std::endl;
    }else{
        std::cout << " - " << abs(coefs[0]) << std::endl;
    }
    return;
}


//
// -----------------------------------------------
// Provides Extreme Learning Machine algorithm.
// -----------------------------------------------
//
// Refer to:
// G.-B. Huang et al., “Extreme Learning Machine for Regression and Multiclass Classification,” IEEE TSMC, vol. 42, no. 2, pp. 513-529, 2012.
//
// Vladislavs D.
//

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Cholesky>

using namespace std;
using namespace Eigen;

#ifndef ELM_H
#define ELM_H

int compare( const void *a, const void *b );

//template <typename Derived>
MatrixXd buildTargetMatrix( double *Y, int nLabels );

// entry function to train the ELM model
// INPUT: X, Y, nhn, C
// OUTPUT: inW, bias, outW
template <typename Derived>
int elmTrain( double *X, int dims, int nsmp,
       	      double *Y,
	      const int nhn, const double C,
	      MatrixBase<Derived> &inW, MatrixBase<Derived> &bias, MatrixBase<Derived> &outW ) {

	// map the samples into the matrix object
	MatrixXd mX = Map<MatrixXd>( X, dims, nsmp );

	// build target matrix
	MatrixXd mTargets = buildTargetMatrix( Y, nsmp );

	// generate random input weight matrix - inW
	inW = MatrixXd::Random( nhn, dims );

	// generate random bias vectors
	bias = MatrixXd::Random( nhn, 1 );

	// compute the pre-H matrix
	MatrixXd preH = inW * mX;

	// compute hidden neuron output
	MatrixXd H = (1 + (-preH.array()).exp()).cwiseInverse();

	// build matrices to solve Ax = b
	MatrixXd A = (MatrixXd::Identity(nhn,nhn)).array()*(1/C) + (H * H.transpose()).array();
	MatrixXd b = H * mTargets.transpose();

	// solve the output weights as a solution to a system of linear equations
	outW = A.llt().solve( b );

	return 0;

}

// entry function to predict class labels using the trained ELM model on test data
// INPUT : X, inW, bias, outW
// OUTPUT : scores
template <typename Derived>
int elmPredict( double *X, int dims, int nsmp,
		MatrixBase<Derived> &mScores,
		MatrixBase<Derived> &inW, MatrixBase<Derived> &bias, MatrixBase<Derived> &outW ) {

	// map the sample into the Eigen's matrix object
	MatrixXd mX = Map<MatrixXd>( X, dims, nsmp );

	// build the pre-H matrix
	MatrixXd preH = inW * mX + bias.replicate( 1, nsmp );

	// apply the activation function
	MatrixXd H = ( 1 + (-preH.array()).exp() ).cwiseInverse();

	// compute output scores
	mScores = (H.transpose() * outW).transpose();

	return 0;
}

// --------------------------
// Helper functions
// --------------------------

// compares two integer values
//int compare( const void* a, const void *b ) {
//	return ( *(int *) a - *(int *) b );
//}

int compare( const void *a, const void *b )
{
	const double *da = (const double *) a;
	const double *db = (const double *) b;
	return (*da > *db) - (*da < *db);
}

// builds 1-of-K target matrix from labels array
//template <typename Derived>
MatrixXd buildTargetMatrix( double *Y, int nLabels ) {

	// make a temporary copy of the labels array
	double *tmpY = new double[ nLabels ];
	for ( int i = 0 ; i < nLabels ; i++ ) {
		tmpY[i] = Y[i];
	}

	// sort the array of labels
	qsort( tmpY, nLabels, sizeof(double), compare );

	// count unique labels
	int nunique = 1;
	for ( int i = 0 ; i < nLabels - 1 ; i++ ) {
		if ( tmpY[i] != tmpY[i+1] )
			nunique++;
	}

    delete [] tmpY;

	MatrixXd targets( nunique, nLabels );
	targets.fill( 0 );

	// fill in the ones
	for ( int i = 0 ; i < nLabels ; i++ ) {
		int idx = Y[i] - 1;
		targets( idx, i ) = 1;
	}

	// normalize the targets matrix values (-1/1)
	targets *= 2;
	targets.array() -= 1;

	return targets;

}

#endif


#pragma once
#include <cstddef>
#include <cmath>
#include <Eigen/Dense>
#include <fastad_bits/util/shape_traits.hpp>
#include <fastad_bits/reverse/core/var_view.hpp>
#include <autoppl/expression/constraint/transformer.hpp>
#include <autoppl/util/value.hpp>
#include <autoppl/util/ad_boost/cov_inv_transform.hpp>

namespace ppl {
namespace expr {
namespace constraint {

struct PosDef {

    /**
     * Returns the number of unconstrained parameters based on
     * the rows (which is also cols) of a positive-definite matrix.
     */
    static constexpr size_t size(size_t rows)
    { return (rows * (rows + 1)) / 2; }

    /**
     * Transforms from constrained (c) to unconstrained (uc).
     */
    template <class CType, class UCType>
    static constexpr void transform(const CType& c,
                                    UCType& uc)
    {
        using value_t = typename CType::Scalar;
        using mat_t = Eigen::Matrix<value_t, Eigen::Dynamic, Eigen::Dynamic>;
        Eigen::LLT<mat_t> llt(c);
        mat_t lower = llt.matrixL();
        lower.diagonal().array() = lower.diagonal().array().log();

        size_t k = 0;
        for (int j = 0; j < lower.cols(); ++j) {
            for (int i = j; i < lower.rows(); ++i, ++k) {
                uc(k) = lower(i,j);
            }
        }
    }

    /**
     * Inverse transforms from unconstrained parameters (uc),
     * which is vector-like in the sense that operator()(index) is defined,
     * to constrained parameter (c), which is matrix-like.
     * Lower should also be matrix-like supporting operator()(index, index)
     * which is used a temporary storage for the transformation.
     */
    template <class LowerType, class UCType, class CType>
    static constexpr void inv_transform(LowerType& lower,
                                        const UCType& uc,
                                        CType& c)
    { ad::boost::cov_inv_transform(lower, uc, c); }
};

// Specialization: Positive-Definite (matrix)
template <class ValueType>
struct Transformer<ValueType, mat, PosDef>
{
    using value_t = ValueType;
    using shape_t = mat;
    using var_t = util::var_t<value_t, shape_t>;
    using constraint_t = PosDef;
    using uc_view_t = ad::util::shape_to_raw_view_t<value_t, vec>;
    using view_t = ad::util::shape_to_raw_view_t<value_t, shape_t>;

    // only continuous value types can be constrained
    static_assert(util::is_cont_v<value_t>);

    /**
     * Constructs a Transformer object.
     * It represents a positive-definite matrix, and hence
     * ignores cols (second parameter) and treats rows as both rows and cols.
     *
     * @param   rows    number of constrained rows (and cols)
     */
    Transformer(size_t rows,
                size_t,
                constraint_t=constraint_t())
        : uc_val_(nullptr, constraint_t::size(rows))
        , c_val_(nullptr, rows, rows)
        , lower_(nullptr, rows, rows)
        , v_val_(nullptr)
    {}

    void transform() {
        constraint_t::transform(c_val_, uc_val_);
    }

    /**
     * Inverse transforms from unconstrained parameters to constrained parameters.
     * Only the first visitor of the visit count will invoke the actual transformation.
     * The reference count is used to reset the visit count if
     * the visit count has reached refcnt.
     *
     * @return  viewer of constrained parameters
     */
    void inv_transform(size_t refcnt) {
        ++*v_val_;
        if (*v_val_ == 1) {
            constraint_t::inv_transform(lower_, uc_val_, c_val_);
        }
        *v_val_ = *v_val_ % refcnt;
    }

    /**
     * Inverse transform from unconstrained parameters to constrained parameters.
     * This should not have any memory dependency through calling bind.
     * It is expected that uc and c are vector-like
     * in the sense that either row or column is 1.
     * In debug mode, we check that uc and c have correct sizes.
     *
     * @param   uc  vector of unconstrained parameters to transform
     * @param   c   vector of constrained parameters to populate
     */
    //template <class UCVecType, class CVecType>
    //void inv_transform(const UCVecType& uc,
    //                   CVecType& c) const
    //{
    //    assert(static_cast<size_t>(uc.size()) == size_uc());
    //    assert(static_cast<size_t>(c.size()) == size_c());
    //    assert(uc.rows() == 1 || uc.cols() == 1);
    //    assert(c.rows() == 1 || c.cols() == 1);

    //    using vec_t = Eigen::Matrix<value_t, Eigen::Dynamic, 1>;
    //    using mat_t = Eigen::Matrix<value_t, Eigen::Dynamic, Eigen::Dynamic>;
    //    mat_t uc_vec = uc;
    //    mat_t c_vec = c;
    //    mat_t lower(rows_c(), rows_c());
    //    lower.setZero();
    //    Eigen::Map<vec_t> uc_mp(uc_vec.data(), uc.size());
    //    Eigen::Map<mat_t> c_mp(c_vec.data(), rows_c(), rows_c());
    //    constraint_t::inv_transform(lower, uc_vec, c_mp);
    //    c = c_vec;
    //}

    /**
     * Creates an AD expression representing the inverse transform.
     * User must ensure that this gets called exactly refcnt number of times.
     *
     * @param   uc_val      beginning of unconstrained parameter values
     * @param   uc_adj      beginning of unconstrained parameter adjoints
     * @param   c_val       beginning of constrained value region.
     *                      The first size_c() elements will be used as temporary region
     *                      to compute a lower-triangular matrix.
     * @param   v_val       beginning of visit count
     * @param   refcnt      total reference count to determine when to loop visit count back to 0
     */
    template <class CurrPtrPack, class PtrPack>
    auto inv_transform_ad(const CurrPtrPack& curr_pack,
                          const PtrPack&,
                          size_t refcnt) const {
        ad::VarView<value_t, ad::vec> uc_view(curr_pack.uc_val,
                                              curr_pack.uc_adj,
                                              size_uc());
        return ad::boost::CovInvTransformNode(uc_view,
                                              curr_pack.c_val,
                                              curr_pack.c_val + size_c(),
                                              rows_c(),
                                              curr_pack.v_val,
                                              refcnt);
    }

    /**
     * Creates an AD expression representing the log-jacobian of inverse transform.
     * In general, this may need to reuse computed values from inverse transform.
     * User must guarantee that inverse transform AD expression that is bound to the same
     * resources as the return value of this function is evaluated before.
     */
    template <class CurrPtrPack, class PtrPack>
    auto logj_inv_transform_ad(const CurrPtrPack& curr_pack,
                               const PtrPack&) const {
        ad::VarView<value_t, ad::vec> uc_view(curr_pack.uc_val,
                                              curr_pack.uc_adj,
                                              size_uc());
        return ad::boost::LogJCovInvTransformNode(uc_view, rows_c());
    }

    /**
     * Initializes unconstrained values such that constrained (pos-def) matrix is identity.
     * This is equivalent to simply setting the unconstrained values to 0,
     * since diagonal is first exponentiated and the Cholesky decomposition of identity is identity.
     */
    template <class GenType, class ContDist>
    void init(GenType&, ContDist&) {
        uc_val_.setZero();
    }

    void activate_refcnt(size_t) const {}

    var_t& get_c() { return util::get(c_val_); }
    const var_t& get_c() const { return util::get(c_val_); }

    /**
     * Returns the dimension information for the viewers of unconstrained
     * and constrained parameters.
     */
    constexpr size_t size_uc() const { return uc_val_.size(); }
    constexpr size_t rows_uc() const { return uc_val_.rows(); }
    constexpr size_t cols_uc() const { return 1; }
    constexpr size_t size_c() const { return c_val_.size(); }
    constexpr size_t rows_c() const { return c_val_.rows(); }
    constexpr size_t cols_c() const { return c_val_.cols(); }

    /**
     * Returns the number of elements required to bind and compute
     * unconstrained, constrained parameters and visit count.
     */
    constexpr size_t bind_size_uc() const { return size_uc(); }
    constexpr size_t bind_size_c() const { return 2*size_c(); }
    constexpr size_t bind_size_v() const { return 1; }

    /**
     * Binds unconstrained viewer to unconstrained region (viewed as a vector),
     * constrained viewer to constrained region (viewed as a matrix),
     * and internal visit count to visit count region.
     */
    template <class CurrPtrPack, class PtrPack>
    void bind(const CurrPtrPack& curr_pack,
              const PtrPack&)
    {
        util::bind(uc_val_, curr_pack.uc_val, rows_uc(), cols_uc());
        util::bind(lower_, curr_pack.c_val, rows_c(), cols_c());
        util::bind(c_val_, curr_pack.c_val + size_c(), rows_c(), cols_c());
        util::bind(v_val_, curr_pack.v_val, 1, 1);
    }

private:
    uc_view_t uc_val_;
    view_t c_val_;
    view_t lower_;  // temporary lower-triangular matrix
    size_t* v_val_;
};

} // namespace constraint
} // namespace expr

constexpr inline auto pos_def()
{
    return expr::constraint::PosDef();
}

} // namespace ppl


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;
using Sophus::SE3d;
using Sophus::SO3d;

/************************************************
  * 이 프로그램은 포즈 그래프 최적화를 위해 g2o 솔버를 사용하는 방법을 보여줍니다.
  * sphere.g2o는 인위적으로 생성된 포즈 그래프이므로 최적화해 보겠습니다.
  * 전체 그래프는 load 함수를 통해 직접 읽을 수 있지만 더 깊은 이해를 위해 읽기 코드를 직접 구현합니다.
  * 이 섹션에서는 lie 대수학을 사용하여 포즈 그래프를 표현하며 노드와 가장자리의 방식은 사용자 정의입니다.
 * **********************************************/

typedef Matrix<double, 6, 6> Matrix6d;

// 주어진 오류에 대한 J_R^{-1}의 근사값 찾기
Matrix6d JRInv(const SE3d &e) {
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
    J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
    // J = J * 0.5 + Matrix6d::Identity();
    J = Matrix6d::Identity();    // try Identity if you want
    return J;
}

// lie 대수 edge
typedef Matrix<double, 6, 1> Vector6d;

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3d(
            Quaterniond(data[6], data[3], data[4], data[5]),
            Vector3d(data[0], data[1], data[2])
        ));
    }

    virtual bool write(ostream &os) const override {
        os << id() << " ";
        Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
        return true;
    }

    virtual void setToOriginImpl() override {
        _estimate = SE3d();
    }

    // 왼쪽 곱하기 업데이트
    virtual void oplusImpl(const double *update) override {
        Vector6d upd;
        upd << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(upd) * _estimate;
    }
};

// 두 lie 대수 노드 사이의 edge
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
        for (int i = 0; i < information().rows() && is.good(); i++)
            for (int j = i; j < information().cols() && is.good(); j++) {
                is >> information()(i, j);
                if (i != j)
                    information()(j, i) = information()(i, j);
            }
        return true;
    }

    virtual bool write(ostream &os) const override {
        VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]);
        VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3d m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";

        // information matrix
        for (int i = 0; i < information().rows(); i++)
            for (int j = i; j < information().cols(); j++) {
                os << information()(i, j) << " ";
            }
        os << endl;
        return true;
    }

    // 오류 계산은 책의 유도와 일치합니다.
    virtual void computeError() override {
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        _error = (_measurement.inverse() * v1.inverse() * v2).log();
    }

    // 야코비안 계산
    virtual void linearizeOplus() override {
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3d::exp(_error));
        // J를 I로 근사하려고 합니까?
        _jacobianOplusXi = -J * v2.inverse().Adj();
        _jacobianOplusXj = J * v2.inverse().Adj();
    }
};

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // g2o 설정
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 그래프 모델
    optimizer.setAlgorithm(solver);   // 솔버 설정
    optimizer.setVerbose(true);       // 디버그 출력 켜기

    int vertexCnt = 0, edgeCnt = 0; // 정점과 모서리의 수

    vector<VertexSE3LieAlgebra *> vectices;
    vector<EdgeSE3LieAlgebra *> edges;
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // vertex
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 edge
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 연결된 두 정점
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;

    // 사용자 정의 정점이 g2o에 등록되지 않고 사용되기 때문에 구현하려면 여기에 저장하십시오.
    // SE3 정점과 가장자리로 위장하여 g2o_viewer가 인식할 수 있도록 합니다.
    ofstream fout("result_lie.g2o");
    for (VertexSE3LieAlgebra *v:vectices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for (EdgeSE3LieAlgebra *e:edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    return 0;
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/MPRealSupport>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <iomanip>
#include <iostream>

#include "gauss_hermite_quadrature.hpp"
#include "spectral/mpfr/import_std_math.hpp"

//#include <quadmath.h>

namespace mp = boost::multiprecision;  // Reduce the typing a bit later...

using namespace mpfr;

typedef mp::mpfr_float_backend<100000> mfloat_t;
typedef mp::number<mfloat_t> mpfr_float_t;

using namespace std;

void GaussHermiteQuadrature::init(int ndigits)
{
  mpreal::set_default_prec(ndigits);
  typedef mpreal mfloat_t;
  //  typedef double mfloat_t;
  typedef Eigen::Matrix<mfloat_t, Eigen::Dynamic, Eigen::Dynamic> MatrixXmp;
  MatrixXmp A(N, N);
  A.fill(0);
  for (int i = 0; i < N - 1; ++i) {
    const double beta = ::math::sqrt(1.0 * (i + 1)) / ::math::sqrt(2.0);
    A(i, i + 1) = beta;
    A(i + 1, i) = beta;
  }

  Eigen::SelfAdjointEigenSolver<MatrixXmp> eigensolver;
  // Eigen::EigenSolver<MatrixXmp> eigensolver;
  eigensolver.compute(A, Eigen::ComputeEigenvectors);
  const double pi = boost::math::constants::pi<double>();
  const double m = ::math::sqrt(pi);
  // compute weights
  const auto V = eigensolver.eigenvectors();

  if (V.rows() == 0) {
    cerr << "abort";
    exit(-1);
  }
  std::vector<double> weights(N);
  for (int i = 0; i < N; ++i) {
    // eigenvectors are normalized by eigen
    weights[i] = (V(0, i) * V(0, i) * m).toDouble();
  }

  const auto w = eigensolver.eigenvalues();
  std::vector<double> points(N);
  for (int i = 0; i < N; ++i) {
    points[i] = w(i).toDouble();
  }

  // sort
  std::vector<std::pair<double, double> > pairs(N);
  for (int i = 0; i < N; ++i) {
    pairs[i] = std::make_pair(points[i], weights[i]);
  }

  std::sort(pairs.begin(), pairs.end());

  for (int i = 0; i < N; ++i) {
    pts_[i] = pairs[i].first;
    wts_[i] = pairs[i].second;
  }
}


#pragma once

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

namespace bold
{
  enum class HalfHull
  {
    Top,
    Bottom
  };

  // TODO is it better to specify the side as a template parameter to compile away conditions?
  // TODO below doesn't seem to work with ushort... is the problem being unsigned, or integral?

  template<typename T>
  class HalfHullBuilder
  {
    typedef Eigen::Matrix<T,2,1> Vector2;

  public:
    /** Calculates half of a convex hull along a line of points.
     *
     * Inputs are assumed to be ordered in increasing x direction, with no duplicate x values.
     */
    std::vector<Vector2> findHalfHull(std::vector<Vector2> const& input, HalfHull side)
    {
      int n = input.size(),
          k = 0;

      std::vector<Vector2> hull(n);

      if (side == HalfHull::Bottom)
      {
        for (int i = 0; i < n; i++)
        {
          while (k >= 2 && cross(hull[k-2], hull[k-1], input[i]) <= 0)
            k--;

          hull[k++] = input[i];
        }
      }
      else
      {
        for (int i = 0; i < n; i++)
        {
          while (k >= 2 && cross(hull[k-2], hull[k-1], input[i]) >= 0)
            k--;

          hull[k++] = input[i];
        }
      }

      hull.resize(k);

      return hull;
    }

  private:
    static T cross(Vector2 const& O, Vector2 const& A, Vector2 const& B)
    {
      return (A.x() - O.x()) * (B.y() - O.y()) - (A.y() - O.y()) * (B.x() - O.x());
    }
  };
}


#include <NTL/lzz_pX.h>
#include <helib/NumbTh.h>
#include <helib/ArgMap.h>

NTL_CLIENT

void compute_a_vals(Vec<long>& a, long p, long e)
// computes a[m] = a(m)/m! for m = p..(e-1)(p-1)+1,
// as defined by Chen and Han.
// a.length() is set to (e-1)(p-1)+2

{
   long p_to_e = power_long(p, e);
   long p_to_2e = power_long(p, 2*e);

   long len = (e-1)*(p-1)+2;

   zz_pPush push(p_to_2e);

   zz_pX x_plus_1_to_p = power(zz_pX(INIT_MONO, 1) + 1, p);
   zz_pX denom = InvTrunc(x_plus_1_to_p - zz_pX(INIT_MONO, p), len);
   zz_pX poly = MulTrunc(x_plus_1_to_p, denom, len);
   poly *= p;

   a.SetLength(len);

   long m_fac = 1;
   for (long m = 2; m < p; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
   }

   for (long m = p; m < len; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
      long c = rep(coeff(poly, m));
      long d = GCD(m_fac, p_to_2e);
      if (d == 0 || d > p_to_e || c % d != 0) Error("cannot divide");
      long m_fac_deflated = (m_fac / d) % p_to_e;
      long c_deflated = (c / d) % p_to_e;
      a[m] = MulMod(c_deflated, InvMod(m_fac_deflated, p_to_e), p_to_e);
   }

}

int main(int argc, char **argv)
{
   ArgMap amap;

   long p = 2;
   amap.arg("p", p);

   long e = 2;
   amap.arg("e", e);

   amap.parse(argc, argv);

   Vec<long> a;

   compute_a_vals(a, p, e);

   long p_to_e = power_long(p, e);
   long len = (e-1)*(p-1)+2;

   zz_pPush push(p_to_e);

   zz_pX poly(0);
   zz_pX term(1);
   zz_pX X(INIT_MONO, 1);

   poly = 0;
   term = 1;

   for (long m = 0; m < p; m++) {
      term *= (X-m);
   }

   for (long m = p; m < len; m++) {
      poly += term * a[m];
      term *= (X-m);
   }

   cout << poly << "\n";

   for (long i = 0; i < 1000; i++) {
      zz_p x;
      random(x);
      zz_p fx = eval(poly, x);
      if (fx + (rep(x) % p) != x) Error("bad eval");
   }
}


// Copyright (c) 2020 Marcus Valtonen Örnhag
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "roots.hpp"
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace HomLib {
    Eigen::VectorXcd roots(const Eigen::VectorXd& coeffs) {
        // Create companion matrix
        int N = coeffs.size() - 1;
        Eigen::MatrixXd companion_mat(N, N);
        companion_mat.setZero();

        for (int i = 0; i < N; i++) {
             for (int j = 0; j < N; j++) {
                if (i == j + 1) {
                    companion_mat(i, j) = 1.0;
                }
                if (j == N - 1) {
                    companion_mat(i, j) = -coeffs[N - i] / coeffs[0];
                }
            }
        }

        // Compute roots via eigenvalues of companion matrix
        Eigen::VectorXcd eigvals = companion_mat.eigenvalues();

        return eigvals;
    }
}  // namespace HomLib


/*
//@HEADER
// ************************************************************************
//
// tutorial1.cc
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#include "pressio/solvers_linear.hpp"
#include "pressio/solvers_nonlinear.hpp"
#include <Eigen/Core>

struct MyRosenbrockSystem
{
  using scalar_type = double;
  using state_type  = Eigen::VectorXd;
  using residual_type = state_type;
  using jacobian_type = Eigen::MatrixXd;

  residual_type createResidual() const{ return residual_type(6);   }
  jacobian_type createJacobian() const{ return jacobian_type(6, 4);}

  void residual(const state_type& x, residual_type & res) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    const auto & x4 = x(3);
    res(0) = 10.*(x4 - x3*x3);
    res(1) = 10.*(x3 - x2*x2);
    res(2) = 10.*(x2 - x1*x1);
    res(3) = (1.-x1);
    res(4) = (1.-x2);
    res(5) = (1.-x3);
  }

  void jacobian(const state_type & x, jacobian_type & JJ) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    JJ.setZero();

    JJ(0,2) = -20.*x3;
    JJ(0,3) = 10.;
    JJ(1,1) = -20.*x2;
    JJ(1,2) = 10.;
    JJ(2,0) = -20.*x1;
    JJ(2,1) = 10.;
    JJ(3,0) = -1.;
    JJ(4,1) = -1.;
    JJ(5,2) = -1.;
  }
};

int main()
{
  namespace plog   = pressio::log;
  namespace pls    = pressio::linearsolvers;
  namespace pnonls = pressio::nonlinearsolvers;
  plog::initialize(pressio::logto::terminal);
  plog::setVerbosity({plog::level::info});

  using problem_t = MyRosenbrockSystem;
  problem_t problem;

  using state_t   = typename problem_t::state_type;
  state_t x(4);
  x(0) = -0.05; x(1) = 1.1; x(2) = 1.2; x(3) = 1.5;

  using hessian_t = Eigen::MatrixXd;
  using lin_tag      = pls::direct::HouseholderQR;
  using lin_solver_t = pls::Solver<lin_tag, hessian_t>;
  lin_solver_t linSolver;

  auto gnSolver = pnonls::create_gauss_newton(problem, x, linSolver);
  gnSolver.setTolerance(1e-5);
  gnSolver.solve(problem, x);

  // check solution
  std::cout << "Computed solution: \n "
            << "[" << x(0) << " " << x(1) << " " << x(2) << " " << x(3) << " " << "] \n"
            << "Expected solution: \n "
            << "[1.0000000156741, 0.99999999912477, 0.99999999651993, 0.99999998889888]"
            << std::endl;

  plog::finalize();
  return 0;
}


#include <fstream>

#include <boost/graph/graph_traits.hpp>
#include <CGAL/property_map.h>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/boost/graph/graph_traits_Linear_cell_complex_for_combinatorial_map.h>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3                Point;
typedef Kernel::Vector_3               Vector;

typedef CGAL::Linear_cell_complex_traits<3, Kernel> LCC_traits;
typedef CGAL::Linear_cell_complex_for_bgl_combinatorial_map_helper
          <2, 3, LCC_traits>::type LCC;

template<typename HalfedgeGraph,
         typename PointMap,
         typename NormalMap>
void calculate_face_normals(const HalfedgeGraph& g,
                            PointMap pm,
                            NormalMap nm)
{
  typedef boost::graph_traits<HalfedgeGraph> GraphTraits;
  typedef typename GraphTraits::face_iterator face_iterator;
  typedef typename GraphTraits::halfedge_descriptor halfedge_descriptor;
  typedef typename boost::property_traits<PointMap>::value_type point;
  typedef typename boost::property_traits<NormalMap>::value_type normal;

  face_iterator fb, fe;
  for(boost::tie(fb, fe) = faces(g); fb != fe; ++fb)
  {
    halfedge_descriptor edg = halfedge(*fb, g);
    halfedge_descriptor edgb = edg;

    point p0 = pm[target(edg, g)];
    edg = next(edg, g);
    point p1 = pm[target(edg, g)];
    edg = next(edg, g);
    point p2 = pm[target(edg, g)];
    edg = next(edg, g);

    if(edg == edgb) {
      // triangle
      nm[*fb] = CGAL::unit_normal(p1, p2, p0);
    } else {
      // not a triangle
      normal n(CGAL::NULL_VECTOR);

      do {
        n = n + CGAL::normal(p1, p2, p0);
        p0 = p1;
        p1 = p2;

        edg = next(edg, g);
        p2 = pm[target(edg, g)];
      } while(edg != edgb);

      nm[*fb] = n / CGAL::sqrt(n.squared_length());
    }
  }
}

int main(int argc, char** argv)
{
  typedef boost::property_map<LCC, CGAL::face_index_t>::const_type
                 Face_index_map;

  LCC lcc;
  CGAL::read_off((argc>1)?argv[1]:"cube.off", lcc);

  // Ad hoc property_map to store normals. Face_index_map is used to
  // map face_descriptors to a contiguous range of indices. See
  // http://www.boost.org/libs/property_map/doc/vector_property_map.html
  // for details.
  boost::vector_property_map<Vector, Face_index_map>
    normals(get(CGAL::face_index, lcc));

  calculate_face_normals(
    lcc // Graph
    , get(CGAL::vertex_point, lcc) // map from vertex_descriptor to point
    , normals // map from face_descriptor to Vector_3
    );

  std::cout << "Normals" << std::endl;
  for(LCC::Attribute_range<2>::type::iterator it=lcc.attributes<2>().begin();
      it!=lcc.attributes<2>().end(); ++it)
  {
    // Facet_iterator is a face_descriptor, so we can use it as the
    // key here
    std::cout << normals[it] << std::endl;
  }

  return 0;
}


/*
//@HEADER
// ************************************************************************
//
// tutorial1.cc
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#include <array>
#include <Eigen/Core>
#include "custom_data_types.hpp"
#include "pressio/ops.hpp"

struct MyRosenbrockSystem
{
  using scalar_type   = double;
  using state_type    = Eigen::VectorXd;
  using residual_type = CustomVector<scalar_type>;
  using jacobian_type = CustomMatrix<scalar_type>;

  residual_type createResidual() const{ return residual_type(6);   }
  jacobian_type createJacobian() const{ return jacobian_type(6, 4);}

  void residual(const state_type& x, residual_type & res) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    const auto & x4 = x(3);
    res(0) = 10.*(x4 - x3*x3);
    res(1) = 10.*(x3 - x2*x2);
    res(2) = 10.*(x2 - x1*x1);
    res(3) = (1.-x1);
    res(4) = (1.-x2);
    res(5) = (1.-x3);
  }

  void jacobian(const state_type & x, jacobian_type & JJ) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    JJ.fill(0.);

    JJ(0,2) = -20.*x3;
    JJ(0,3) = 10.;
    JJ(1,1) = -20.*x2;
    JJ(1,2) = 10.;
    JJ(2,0) = -20.*x1;
    JJ(2,1) = 10.;
    JJ(3,0) = -1.;
    JJ(4,1) = -1.;
    JJ(5,2) = -1.;
  }
};

namespace pressio{
template<class T> struct Traits<CustomVector<T>>{
  using scalar_type = T;
};

template<class T> struct Traits<CustomMatrix<T>>{
  using scalar_type = T;
};

namespace ops{
template<class T>
CustomVector<T> clone(const CustomVector<T> & src){ return src; }

template<class T>
CustomMatrix<T> clone(const CustomMatrix<T> & src){ return src; }

template<class T>
void set_zero(CustomVector<T> & o){ o.fill(0); }

template<class T>
void set_zero(CustomMatrix<T> & o){ o.fill(0); }

template<class T>
T norm2(const CustomVector<T> & v)
{
  T norm{0};
  for (std::size_t i=0; i<v.extent(0); ++i){
    norm += v(i)*v(i);
  }
  return std::sqrt(norm);
}

template<class HessianType, class T>
void product(pressio::transpose, pressio::nontranspose,
       const T alpha,
       const CustomMatrix<T> & A,
       const T beta,
       HessianType & H)
{
  for (std::size_t i=0; i<A.extent(1); ++i){
    for (std::size_t j=0; j<A.extent(1); ++j)
    {
      H(i,j) *= beta;
      for (std::size_t k=0; k<A.extent(0); ++k){
        H(i,j) += alpha * A(k,i) * A(k,j);
      }
    }
  }
}

template<class GradientType, class T>
void product(pressio::transpose,
       const T alpha,
       const CustomMatrix<T> & A,
       const CustomVector<T> & b,
       const T beta,
       GradientType & g)
{
  for (std::size_t i=0; i<g.rows(); ++i){
    g(i) *= beta;
    for (std::size_t k=0; k<A.extent(0); ++k){
      g(i) += alpha * A(k,i) * b(k);
    }
  }
}

template<class HessianType, class T>
HessianType product(pressio::transpose, pressio::nontranspose,
                    T alpha,
                    const CustomMatrix<T> & A)
{
  HessianType H(A.extent(1), A.extent(1));
  product(pressio::transpose(), pressio::nontranspose(), alpha, A, (T)0, H);
  return H;
}

template<class T>
void update(CustomVector<T> & v, T a, const CustomVector<T> & v1, T b)
{
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*a + b*v1(i);
  }
}

template<class T>
void scale(CustomVector<T> & v, T factor){
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*factor;
  }
}
}}//end namespace pressio::ops

#include "pressio/solvers_linear.hpp"
#include "pressio/solvers_nonlinear.hpp"

int main()
{
  namespace plog   = pressio::log;
  namespace pls    = pressio::linearsolvers;
  namespace pnonls = pressio::nonlinearsolvers;
  plog::initialize(pressio::logto::terminal);
  plog::setVerbosity({plog::level::info});

  using problem_t = MyRosenbrockSystem;
  problem_t problem;

  using state_t   = Eigen::VectorXd;
  state_t x(4);
  x[0] = -0.05; x[1] = 1.1; x[2] = 1.2; x[3] = 1.5;

  using hessian_t    = Eigen::MatrixXd;
  using lin_tag      = pls::direct::HouseholderQR;
  using lin_solver_t = pls::Solver<lin_tag, hessian_t>;
  lin_solver_t linSolver;

  auto gnSolver = pnonls::create_gauss_newton(problem, x, linSolver);
  gnSolver.setTolerance(1e-5);
  gnSolver.solve(problem, x);

  // check solution
  std::cout << "Computed solution: \n "
            << "[" << x(0) << " " << x(1) << " " << x(2) << " " << x(3) << " " << "] \n"
            << "Expected solution: \n "
            << "[1.0000000156741, 0.99999999912477, 0.99999999651993, 0.99999998889888]"
            << std::endl;

  plog::finalize();
  return 0;
}


#pragma once

#include <Eigen/Core>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <cmath>
#include <map>
#include <utility>
#include "ear/helpers/assert.hpp"

namespace ear {
  namespace hoa {

    /// Associated Legendre function P_n^m(x), omitting the (-1)^m
    /// Condon-Shortley phase term.
    inline double Alegendre(int n, int m, double x) {
      return std::pow(-1.0, m) * boost::math::legendre_p(n, m, x);
    }

    /// Ambisonics Channel Number for order n and degree m.
    inline int to_acn(int n, int m) { return n * n + n + m; }

    /// Get the order n and degree m from a given Ambisonics Channel Number.
    inline std::pair<int, int> from_acn(int acn) {
      int n = (int)std::sqrt(acn);
      int m = acn - n * n - n;
      return {n, m};
    }

    // HOA normalisation functions corresponding to BS.2076-1 section 10.2

    enum class NormType {
      N3D,
      SN3D,
      FuMa,
    };

    inline double norm_N3D(int n, int abs_m) {
      return std::sqrt((2.0 * n + 1.0) *
                       boost::math::factorial<double>(n - abs_m) /
                       boost::math::factorial<double>(n + abs_m));
    }

    inline double norm_SN3D(int n, int abs_m) {
      return std::sqrt(boost::math::factorial<double>(n - abs_m) /
                       boost::math::factorial<double>(n + abs_m));
    }

    inline double norm_FuMa(int n, int abs_m) {
      static const std::map<std::pair<int, int>, double> conversion_factors = {
          {{0, 0}, 1.0 / std::sqrt(2.0)},
          {{1, 0}, 1.0},
          {{1, 1}, 1.0},
          {{2, 0}, 1.0},
          {{2, 1}, 2.0 / std::sqrt(3.0)},
          {{2, 2}, 2.0 / std::sqrt(3.0)},
          {{3, 0}, 1.0},
          {{3, 1}, std::sqrt(45.0 / 32.0)},
          {{3, 2}, 3.0 / std::sqrt(5.0)},
          {{3, 3}, std::sqrt(8.0 / 5.0)}};

      return conversion_factors.at({n, abs_m}) * norm_SN3D(n, abs_m);
    }

    using norm_f_t = double(int, int);

    inline norm_f_t &get_norm(NormType norm_type) {
      switch (norm_type) {
        case NormType::N3D:
          return norm_N3D;
        case NormType::SN3D:
          return norm_SN3D;
        case NormType::FuMa:
          return norm_FuMa;
        default:
          throw ear::internal_error("invalid value for norm_type");
      }
    }

    /// Spherical harmonic values for a given normalisation according to
    /// BS.2076-1 section 10.1.
    ///
    /// Here, az and el are the ADM azimuths and
    /// elevations converted to radians, rather than the HOA coordinate system.
    /// The only difference is that the elevation goes up from the equator, so
    /// we use sin rather than cos in the elevation term.
    template <typename NormT>
    double sph_harm(int n, int m, double az, double el, NormT norm) {
      double scale;
      if (m > 0)
        scale = std::sqrt(2.0) * std::cos(m * az);
      else if (m < 0)
        scale = -std::sqrt(2.0) * std::sin(m * az);
      else
        scale = 1;

      return norm(n, std::abs(m)) * Alegendre(n, std::abs(m), std::sin(el)) *
             scale;
    }

    /// get a matrix of HOA coefficients at the given positions
    template <typename NormT>
    Eigen::MatrixXd calc_Y_virt(Eigen::Matrix<double, Eigen::Dynamic, 3> points,
                                Eigen::VectorXi n, Eigen::VectorXi m,
                                NormT norm) {
      ear_assert(n.size() == m.size(), "n and m must be the same size");

      Eigen::MatrixXd Y_virt(n.size(), points.rows());
      for (Eigen::Index point_i = 0; point_i < points.rows(); point_i++) {
        for (Eigen::Index coeff_i = 0; coeff_i < n.size(); coeff_i++) {
          double az = -std::atan2(points(point_i, 0), points(point_i, 1));
          double el =
              std::atan2(points(point_i, 2),
                         std::hypot(points(point_i, 0), points(point_i, 1)));
          Y_virt(coeff_i, point_i) =
              sph_harm(n(coeff_i), m(coeff_i), az, el, norm);
        }
      }

      return Y_virt;
    }

    /// get a matrix of point-source panning values at the given points, given
    /// a function from a 3-vector to an optional vector of coefficients
    template <typename PanningFuncT>
    Eigen::MatrixXd calc_G_virt(Eigen::Matrix<double, Eigen::Dynamic, 3> points,
                                PanningFuncT panning_function) {
      Eigen::Index n_speakers =
          panning_function(Eigen::Vector3d{0, 1, 0}).get().size();

      Eigen::MatrixXd G_virt(n_speakers, points.rows());
      for (Eigen::Index point_i = 0; point_i < points.rows(); point_i++)
        G_virt.col(point_i) = panning_function(points.row(point_i)).get();

      return G_virt;
    }

    /// normalize a decode matrix such that the mean output power for the given
    /// input vectors is 1
    inline void normalize_decode_matrix(Eigen::MatrixXd &D,
                                        Eigen::MatrixXd Y_virt) {
      D *= std::sqrt(Y_virt.cols()) / (D * Y_virt).norm();
    }

    /// Vector which when multiplied with a HOA signal converts it from
    /// norm_from to norm_to
    template <typename NormToT, typename NormFromT>
    inline Eigen::VectorXd normalisation_conversion(Eigen::VectorXi n,
                                                    Eigen::VectorXi m,
                                                    NormToT norm_to,
                                                    NormFromT norm_from) {
      ear_assert(n.size() == m.size(), "n and m must be the same size");

      Eigen::VectorXd conversion(n.size());
      for (Eigen::Index coeff_i = 0; coeff_i < n.size(); coeff_i++)
        conversion(coeff_i) = norm_to(n(coeff_i), std::abs(m(coeff_i))) /
                              norm_from(n(coeff_i), std::abs(m(coeff_i)));

      return conversion;
    }

    /// Decoder matrix design using the AllRAD[0] technique.
    /// [0] F. Zotter and M. Frank, "All-round ambisonic panning and decoding,"
    /// JAES, vol. 60, no. 10, pp. 807-820, 2012.
    /// http://www.aes.org/e-lib/browse.cfm?elib=16554
    template <typename PanningFuncT, typename NormT>
    Eigen::MatrixXd allrad_design(
        Eigen::Matrix<double, Eigen::Dynamic, 3> points,
        PanningFuncT panning_function, Eigen::VectorXi n, Eigen::VectorXi m,
        NormT norm) {
      Eigen::MatrixXd Y_virt = calc_Y_virt(points, n, m, norm_N3D);
      Eigen::MatrixXd D_virt = Y_virt.transpose() / points.rows();
      Eigen::MatrixXd G_virt = calc_G_virt(points, panning_function);

      Eigen::MatrixXd D = G_virt * D_virt;

      normalize_decode_matrix(D, Y_virt);

      D *= normalisation_conversion(n, m, norm_N3D, norm).asDiagonal();

      return D;
    }

    /// load spherical-t-design points used to be used for AllRAD design
    Eigen::Matrix<double, Eigen::Dynamic, 3> load_points();

  }  // namespace hoa
}  // namespace ear


//
// $Id$
//
//
// Darren Kessner <darren@proteowizard.org>
//
// Copyright 2009 Spielberg Family Center for Applied Proteomics
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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 "LeastSquaresCalibrator.hpp"
#include <iostream>
#include <iomanip>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
namespace ublas = boost::numeric::ublas;

using namespace std;
using pwiz::data::CalibrationParameters;

namespace pwiz {
namespace calibration {

class LeastSquaresCalibratorImpl : public LeastSquaresCalibrator
{
    public:
    LeastSquaresCalibratorImpl(const vector<double>& trueMasses,
                               const vector<double>& observedFrequencies);

    virtual void calibrate();
    virtual const CalibrationParameters& parameters() const {return parameters_;}
    virtual double error() const {return error_;}

    private:
    const vector<double>& trueMasses_;
    const vector<double>& observedFrequencies_;
    CalibrationParameters parameters_;
    double error_;

    void calculateSums(double& sum_f2m2,
                       double& sum_f3m2,
                       double& sum_f4m2,
                       double& sum_fm,
                       double& sum_f2m);

    void calculateParametersFromSums(double sum_f2m2,
                                     double sum_f3m2,
                                     double sum_f4m2,
                                     double sum_fm,
                                     double sum_f2m);

    void calculateError();
};

auto_ptr<LeastSquaresCalibrator> LeastSquaresCalibrator::create(const vector<double>& trueMasses,
                                                        const vector<double>& observedFrequencies)
{
    return auto_ptr<LeastSquaresCalibrator>(new LeastSquaresCalibratorImpl(trueMasses, observedFrequencies));
}

LeastSquaresCalibratorImpl::LeastSquaresCalibratorImpl(const vector<double>& trueMasses,
                                                       const vector<double>& observedFrequencies)
:   trueMasses_(trueMasses),
    observedFrequencies_(observedFrequencies),
    error_(-1)
{
    if (trueMasses.size() != observedFrequencies.size())
        throw logic_error("[LeastSquaresCalibratorImpl::LeastSquaresCalibratorImpl()] Data size mismatch.");
}

void LeastSquaresCalibratorImpl::calibrate()
{
    // observed frequencies {fi}
    // true masses {mi}
    // calibration function m_AB(f) = A/f + B/f^2
    // error function E(A,B) = sum [(m_AB(fi) - mi)/mi]^2  (sum of squared relative deviations)
    // dE/dA == 0 == dE/dB gives:
    //   ( sum[1/(fi^2*mi^2)]  sum[1/(fi^3*mi^2)] ) (A) == ( sum[1/(fi*mi)]   )
    //   ( sum[1/(fi^3*mi^2)]  sum[1/(fi^4*mi^2)] ) (B) == ( sum[1/(fi^2*mi)] )

    double sum_f2m2 = 0;
    double sum_f3m2 = 0;
    double sum_f4m2 = 0;
    double sum_fm = 0;
    double sum_f2m = 0;

    calculateSums(sum_f2m2, sum_f3m2, sum_f4m2, sum_fm, sum_f2m);
    calculateParametersFromSums(sum_f2m2, sum_f3m2, sum_f4m2, sum_fm, sum_f2m);
    calculateError();
}

void LeastSquaresCalibratorImpl::calculateSums(double& sum_f2m2,
                                               double& sum_f3m2,
                                               double& sum_f4m2,
                                               double& sum_fm,
                                               double& sum_f2m)
{
    sum_f2m2 = 0;
    sum_f3m2 = 0;
    sum_f4m2 = 0;
    sum_fm = 0;
    sum_f2m = 0;

    vector<double>::const_iterator mit = trueMasses_.begin();
    vector<double>::const_iterator fit = observedFrequencies_.begin();

    for (; mit!=trueMasses_.end(); ++mit, ++fit)
    {
        double m = *mit;
        double f = *fit;
        double m2 = m*m;
        double f2 = f*f;
        double f3 = f2*f;
        double f4 = f3*f;

        sum_f2m2 += 1/(f2*m2);
        sum_f3m2 += 1/(f3*m2);
        sum_f4m2 += 1/(f4*m2);
        sum_fm += 1/(f*m);
        sum_f2m += 1/(f2*m);
    }
}

void LeastSquaresCalibratorImpl::calculateParametersFromSums(double sum_f2m2,
                                                             double sum_f3m2,
                                                             double sum_f4m2,
                                                             double sum_fm,
                                                             double sum_f2m)
{
    ublas::matrix<double> M(2,2);
    ublas::vector<double> p(2);

    M(0,0) = sum_f2m2;
    M(0,1) = M(1,0) = sum_f3m2;
    M(1,1) = sum_f4m2;
    p(0) = sum_fm;
    p(1) = sum_f2m;

    ublas::permutation_matrix<size_t> pm(2);
    int singular = lu_factorize(M, pm);
    if (singular)
        throw runtime_error("[LeastSquaresCalibratorImpl::calculateParametersFromSums()] Matrix is singular.");

    lu_substitute(M, pm, p);

    parameters_.A = p[0];
    parameters_.B = p[1];
}

void LeastSquaresCalibratorImpl::calculateError()
{
    // calculate root mean squared relative deviation

    double sum = 0;
    vector<double>::const_iterator mit = trueMasses_.begin();
    vector<double>::const_iterator fit = observedFrequencies_.begin();

    for (; mit!=trueMasses_.end(); ++mit, ++fit)
    {
        double m_true = *mit;
        double m_calc = parameters_.mz(*fit);
        double term = (m_calc - m_true)/m_true;
        sum += term*term;
    }

    error_ = sqrt(sum/trueMasses_.size());
}

} // namespace calibration
} // namespace pwiz


#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <tms_msg_ss/Beacon.h>
#include "kalman-Ndof.hpp"

using namespace std;
using namespace Eigen;

#define N 4

ros::Subscriber beacon_sub;
ros::Publisher marker_pub;

typedef struct {
	Vector3d loc;
	double r;
} beacon;

vector<beacon> rasp_pis;
beacon rasp_pi[N];
Kalman *kalman;

#define SD_DIST 0.1 // Error of beacon [m]
#define sqr(var) ((var)*(var))
#define dist2(var1, var2) (sqr(var1[0]-var2[0])+sqr(var1[1]-var2[1])+sqr(var1[2]-var2[2])) // square of distance

#define REPEAT_LIMIT 1000

//#define LOGGING
// #define DEBUG 1

Vector3d calc_position(vector<beacon> beacons, Vector3d initpos)
{
	Vector3d pos(0, 0, 0);
	int n = beacons.size();

	MatrixXd A(n, 3);
	VectorXd L(n);
	MatrixXd SigmaL(n, n);
	MatrixXd K(n, n);
	MatrixXd Sigma(n, n);
	VectorXd Error(3);
	double gain = 0.01;//0.1;

	pos = initpos;
	int repeat = 0;

	while (1) {
		repeat++;
		if(repeat>REPEAT_LIMIT)
			break;
		A.setZero();
		L.setZero();
		SigmaL.setZero();
		Sigma.setZero();

		K.setZero();
		Error.setZero();

		for (int i = 0; i<n; i++) {
			double distance = dist2(beacons[i].loc, pos);

			A(i, 0) = pos[0] - beacons[i].loc[0];
			A(i, 1) = pos[1] - beacons[i].loc[1];
			A(i, 2) = pos[2] - beacons[i].loc[2];

			L(i) = (sqr(beacons[i].r) - dist2(beacons[i].loc, pos)) / 2.0;

			K(i, i) = beacons[i].r;
			Sigma(i, i) = sqr(SD_DIST);
		}

		SigmaL = K * Sigma * K.transpose();

		Matrix3d Covariance;
		Covariance = (A.transpose() * SigmaL.inverse() * A).inverse();
		Error = Covariance * A.transpose() * SigmaL.inverse() * L;

		#ifdef LOGGING
		std::ofstream outf("test.txt");
		static int testCount = 1;
		outf << "[" << testCount++ << "]" << std::endl << std::endl;
		outf << "Covariance = " << std::endl << Covariance << std::endl << std::endl;
		outf << "det(S-1) = " << std::endl << (SigmaL.inverse()).determinant() << std::endl << std::endl;
		outf << "det(S) = " << std::endl << SigmaL.determinant() << std::endl << std::endl;
		outf << "det(Covariance) = " << std::endl << Covariance.determinant() << std::endl << std::endl;
		outf << "A.transpose() = " << std::endl << A.transpose() << std::endl << std::endl;
		outf << "SigmaL = " << std::endl << SigmaL << std::endl << std::endl;
		outf << "Error = " << std::endl << Error << std::endl << std::endl;
		outf << "SigmaL -1 = " << std::endl << SigmaL.inverse() << std::endl << std::endl;
		outf << "L = " << std::endl << L << std::endl << std::endl;

		outf << "C * at = " << std::endl << Covariance * A.transpose() << std::endl << std::endl;
		outf << "C * at * SigmaL-1 = " << std::endl << Covariance * A.transpose() * SigmaL.inverse() << std::endl << std::endl;
		outf << "C * at * SigmaL-1 * L = " << std::endl << Covariance * A.transpose() * SigmaL.inverse() * L << std::endl << std::endl;
		outf.flush();

		outf.close();
		#endif

		if (sqrt(sqr(Error(0)) + sqr(Error(1)) + sqr(Error(2))) > 1000.0) {
			return Vector3d(0,0,0); // �v�Z�����U
		}

		// reNew parent position
		pos[0] += gain * Error(0);
		pos[1] += gain * Error(1);
		pos[2] += gain * Error(2);

#if DEBUG
		cout << "Error " << sqrt(sqr(Error(0)) + sqr(Error(1)) + sqr(Error(2))) << endl;
		cout << "Pos " << std::endl << pos << std::endl << std::endl;
		cout << "Covariance = " << std::endl << Covariance << std::endl << std::endl;
#endif

		// do repeat until (dx,dy,dz) is under than 1 mm
		if (sqrt(sqr(Error(0)) + sqr(Error(1)) + sqr(Error(2))) < 0.001) {
			break;
		}
	}

	return pos;

}

void drawRaspi(beacon pi,int id,float r,float g,float b,float a)
{
	uint32_t shape = visualization_msgs::Marker::SPHERE;
	visualization_msgs::Marker marker;
	marker.header.frame_id = "/world_link";
	marker.header.stamp = ros::Time::now();
	marker.ns = "beacon";
	marker.id = id;
	marker.type = shape;
	marker.action = visualization_msgs::Marker::ADD;

	marker.pose.position.x = pi.loc[0];
	marker.pose.position.y = pi.loc[1];
	marker.pose.position.z = pi.loc[2];

	marker.scale.x = marker.scale.y = marker.scale.z = pi.r*2;

	marker.color.r = r;
	marker.color.g = g;
	marker.color.b = b;
	marker.color.a = a;
	marker.lifetime = ros::Duration();
	marker_pub.publish(marker);
}

void drawPos(Vector3d pos,int id,float r,float g,float b,float a)
{
	uint32_t shape = visualization_msgs::Marker::SPHERE;
	visualization_msgs::Marker marker;
	marker.header.frame_id = "/world_link";
	marker.header.stamp = ros::Time::now();
	marker.ns = "beacon";
	marker.id = id;
	marker.type = shape;
	marker.action = visualization_msgs::Marker::ADD;

	marker.pose.position.x = pos[0];
	marker.pose.position.y = pos[1];
	marker.pose.position.z = pos[2];//0.8

	marker.scale.x = marker.scale.y = 0.2;
	marker.scale.z = 0.2;//1.6

	marker.color.r = r;
	marker.color.g = g;
	marker.color.b = b;
	marker.color.a = a;
	marker.lifetime = ros::Duration();
	marker_pub.publish(marker);
}

ros::Time last_time;
ros::Time now;
Vector3d pos(8,3,0.8);
Vector3d last_pos(8,3,0.8);
Vector3d kf_pos(8,3,0.8);
void Callback(const tms_msg_ss::Beacon &msg)
{
	if(msg.pi_id<N)
		rasp_pi[msg.pi_id].r = msg.distance;

	ROS_INFO("%f %f %f %f",rasp_pi[0].r,rasp_pi[1].r,rasp_pi[2].r,rasp_pi[3].r);

  drawRaspi(rasp_pi[0],0,1.0,0.0,0.0,0.2);
	drawRaspi(rasp_pi[1],1,0.0,1.0,0.0,0.2);
	drawRaspi(rasp_pi[2],2,0.0,1.0,1.0,0.2);
	drawRaspi(rasp_pi[3],3,1.0,1.0,0.0,0.2);

	rasp_pis.clear();
	rasp_pis.push_back(rasp_pi[0]);
	rasp_pis.push_back(rasp_pi[1]);
	rasp_pis.push_back(rasp_pi[2]);
	rasp_pis.push_back(rasp_pi[3]);

	last_pos = pos;
	ROS_INFO("calc");
	pos = calc_position(rasp_pis, pos);
	ROS_INFO("calcend");
	ROS_INFO("pos:(%f,%f,%f)",pos[0],pos[1],pos[2]);

	if(pos[0]==0&&pos[1]==0&&pos[2]==0){
		return;
	}

	last_time = now;
	now = ros::Time::now();
	double delta_t = (now.sec+now.nsec*0.000000001)-(last_time.sec+last_time.nsec*0.000000001);
	ROS_INFO("callback:%f",delta_t);

	drawPos(pos,10,1.0,1.0,1.0,1.0);

	double velX = (pos[0]-last_pos[0])/delta_t;
	double velY = (pos[1]-last_pos[1])/delta_t;
	double velZ = (pos[2]-last_pos[2])/delta_t;

	static int count = 0;

	if(kalman == NULL)
		return;

	if(count==0)
	{
		count++;
		return;
	}
	else if(count==1)
	{
		kalman->init(0.1,1.0,0.1);//(0.1,0.1,1.0);
		double C[3][6];
		memset(C,0,sizeof(C));
		C[0][0]=C[1][1]=C[2][2]=1;
		kalman->setC((double *)&C);
		kalman->setX(0,pos[0]);
		kalman->setX(1,pos[1]);
		kalman->setX(2,pos[2]);
		kalman->setX(3,velX);
		kalman->setX(4,velY);
		kalman->setX(5,velZ);

		count++;
	}

	double A[3][6];
	memset(A,0,sizeof(A));
	A[0][0]=A[1][1]=A[2][2]=1;
	A[0][3]=A[1][4]=A[2][5]=delta_t;

	double obs[6]={pos[0],pos[1],pos[2],velX,velY,velZ};
	double input[3]={0,0,0};

	kalman->update(obs,input);

	kf_pos[0]=kalman->getX(0);
	kf_pos[1]=kalman->getX(1);
	kf_pos[2]=kalman->getX(2);

	ROS_INFO("kf_pos:(%f,%f,%f)",kf_pos[0],kf_pos[1],kf_pos[2]);

	drawPos(kf_pos,11,1.0,0.0,0.0,1.0);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "tms_ss_beacon");
	ros::NodeHandle nh;

	kalman = new Kalman(6,6,3);

	now = ros::Time::now();

	beacon_sub = nh.subscribe("/tms_ss_beacon",1,&Callback);
	marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);

	ROS_INFO("READY");

	rasp_pi[0].loc << 7.66, 5.62, 0.82;
	rasp_pi[1].loc << 10.12, 3.90, 0.58;
	rasp_pi[2].loc << 8.74, 0.31, 0.92;
	rasp_pi[3].loc << 5.65, 1.66, 0.73;

	rasp_pi[0].r = 3;
	rasp_pi[1].r = 1.5;
	rasp_pi[2].r = 2;
	rasp_pi[3].r = 4;

	ros::spin();

	return 0;
}


// Copyright Nick Thompson 2017.
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <string>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/legendre_stieltjes.hpp>

using boost::math::legendre_p;
using boost::math::legendre_p_zeros;
using boost::math::legendre_p_prime;
using boost::math::legendre_stieltjes;
using boost::multiprecision::cpp_bin_float_quad;
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_dec_float_100;

template<class Real>
void gauss_kronrod_rule(size_t order)
{
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    std::cout << std::fixed;
    auto gauss_nodes = boost::math::legendre_p_zeros<Real>(order);
    auto E = legendre_stieltjes<Real>(order + 1);
    std::vector<Real> gauss_weights(gauss_nodes.size(), std::numeric_limits<Real>::quiet_NaN());
    std::vector<Real> gauss_kronrod_weights(gauss_nodes.size(), std::numeric_limits<Real>::quiet_NaN());
    for (size_t i = 0; i < gauss_nodes.size(); ++i)
    {
        Real node = gauss_nodes[i];
        Real lp = legendre_p_prime<Real>(order, node);
        gauss_weights[i] = 2/( (1-node*node)*lp*lp);
        // P_n(x) = (2n)!/(2^n (n!)^2) pi_n(x), where pi_n is the monic Legendre polynomial.
        gauss_kronrod_weights[i] = gauss_weights[i] + static_cast<Real>(2)/(static_cast<Real>(order+1)*legendre_p_prime(order, node)*E(node));
    }

    std::cout << "static const std::vector<Real> gauss_nodes {\n";
    for (auto const & node : gauss_nodes)
    {
        std::cout << "    boost::lexical_cast<Real>(\"" << node << "\"),\n";
    }
    std::cout << "};\n\n";

    std::cout << "static const std::vector<Real> gauss_weights {\n";
    for (auto const & weight : gauss_weights)
    {
        std::cout << "    boost::lexical_cast<Real>(\"" << weight << "\"),\n";
    }
    std::cout << "};\n\n";

    std::cout << "static const std::vector<Real> gauss_kronrod_weights {\n";
    for (auto const & weight : gauss_kronrod_weights)
    {
        std::cout << "    boost::lexical_cast<Real>(\"" << weight << "\"),\n";
    }
    std::cout << "};\n\n";

    auto kronrod_nodes = E.zeros();
    std::vector<Real> kronrod_weights(kronrod_nodes.size());
    for (size_t i = 0; i < kronrod_weights.size(); ++i)
    {
        Real node = kronrod_nodes[i];
        kronrod_weights[i] = static_cast<Real>(2)/(static_cast<Real>(order+1)*legendre_p(order, node)*E.prime(node));
    }

    std::cout << "static const std::vector<Real> kronrod_nodes {\n";
    for (auto node : kronrod_nodes)
    {
        std::cout << "    boost::lexical_cast<Real>(\"" << node << "\"),\n";
    }
    std::cout << "};\n\n";

    std::cout << "static const std::vector<Real> kronrod_weights {\n";
    for (auto const & weight : kronrod_weights)
    {
        std::cout << "    boost::lexical_cast<Real>(\"" << weight << "\"),\n";
    }
    std::cout << "};\n\n";

}

int main()
{
    //std::cout << "Gauss-Kronrod 7-15 Rule:\n";
    //gauss_kronrod_rule<cpp_dec_float_100>(7);

    //std::cout << "\n\nGauss-Kronrod 10-21 Rule:\n";
    //gauss_kronrod_rule<cpp_dec_float_100>(10);

    std::cout << "\n\nGauss-Kronrod 15-31 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(15);
    /*
    std::cout << "\n\nGauss-Kronrod 20-41 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(20);

    std::cout << "\n\nGauss-Kronrod 25-51 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(25);

    std::cout << "\n\nGauss-Kronrod 30-61 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(30);

    std::cout << "\n\nGauss-Kronrod 35-71 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(35);

    std::cout << "\n\nGauss-Kronrod 40-81 Rule:\n";
    gauss_kronrod_rule<cpp_dec_float_100>(40);*/
}


/*=================================================================
* Transform vertices (n*3) with a 3D affine transform
*
* according to David Legland's matlab toolbox: geom3d
*
* Junjie Cao, 2013, jjcao1231@gmail.com
*
=================================================================*/

#include <mex.h>
#include <Eigen/Dense>
#include <vector>

Eigen::MatrixXd transform_point3d(Eigen::MatrixXd& verts, Eigen::Matrix4d& trans)
{
	int NP  = verts.rows();
	Eigen::MatrixXd res(NP,4);
	res.col(0) = verts.col(0);
	res.col(1) = verts.col(1);
	res.col(2) = verts.col(2);
	res.col(3) = Eigen::VectorXd::Ones(NP,1);
	//std::cout << res << std::endl;

	//std::cout << trans * res.row(0).transpose() << std::endl;
	res = res * trans.transpose();
	//std::cout << res << std::endl;
	res.conservativeResize(NP, 3);

	return res;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	////////////////// parse input
	if ( nrhs != 2)
		mexErrMsgTxt("2 arguments needed!");

	double *verts_, *trans_;
	verts_ = mxGetPr(prhs[0]);
	trans_ = mxGetPr(prhs[1]);
	int nverts = mxGetM(prhs[0]);
	if ( mxGetN(prhs[0]) != 3)
		mexErrMsgTxt("center must be n*3 matrix!");
	if ( mxGetN(prhs[1]) != 4 && mxGetM(prhs[1]) != 4)
		mexErrMsgTxt("vector must be 4*4 matrix!");

	Eigen::MatrixXd verts = Eigen::Map<Eigen::MatrixXd>(verts_, nverts, 3);
	Eigen::Matrix4d trans = Eigen::Map<Eigen::Matrix4d>(trans_);

	/////////////////////////////
	Eigen::MatrixXd newverts = transform_point3d(verts,trans);

	//////////////////////// output
	plhs[0] = mxCreateDoubleMatrix(nverts,3,mxREAL);
	double *newverts_ = mxGetPr(plhs[0]);
	Eigen::Map<Eigen::MatrixXd>(newverts_, nverts, 3) = newverts;
}


#include <Eigen/Dense>

#include <iostream>
#include <algorithm>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include "Functions.hpp"
#include "Types.h"
#include "NeuralNetwork.h"
#include "MatrixUtils.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

NeuralNetwork::NeuralNetwork(const std::vector<int>& layout,
    const std::string& funcName, const bool randomSeed) : m_layout(layout) {
    createLayers(randomSeed);
    createActivationFunction(funcName);
}

void NeuralNetwork::createLayers(const bool randomSeed) {
    // create layers. Ignore the first as this is the size of the input.
    m_weights.reserve(m_layout.size()-1);

    for (int i=1; i < m_layout.size(); ++i) {
        // size of matrix is defined by the number of nodes (rows) and the
        // size of the output vector from the previous layer (cols)
        Eigen::MatrixXd weights = Eigen::MatrixXd::Zero(m_layout[i-1], m_layout[i]);
        // initialise matrix with random numbers drawn from Gaussian with
        // zero mean and unit variance
        MatrixUtils::initializeRandomWeights(weights, randomSeed);

        m_weights.push_back(weights);
    }
}

void NeuralNetwork::createActivationFunction(const std::string& funcName) {
    auto functions = FunctionFactory::create(funcName);
    m_activation_func = functions.first;
    m_activation_deriv = functions.second;
}

void NeuralNetwork::train(MatrixXd input, MatrixXd expected,
    int maxIter, double alpha) {

    //weight learning rate by number of examples in batch
    alpha *= (1.0/input.rows());

    for (int i=0; i <= maxIter; ++i) {

        MatrixXd output = feedForward(input);
        auto errors = backPropagate(output, expected);
        updateWeights(errors, alpha);

        if (i % 100 == 0) {
            std::cout << "Epoch " << i << std::endl;
        }
    }

}

MatrixXd NeuralNetwork::feedForward(MatrixXd input) {
    // Remove cached vectors from previous run
    m_zVectors.clear();
    m_activations.clear();

    MatrixXd activation = input;
    m_activations.push_back(activation);

    for (int index=0; index < m_layout.size()-1; ++index) {
        // compute weighted input for this layer
        MatrixXd weights = m_weights[index];
        MatrixXd z = activation * weights;
        // cache result of weight-input combination for backprop
        m_zVectors.push_back(z);
        // compute the activation of function from the weighted input
        activation = z.unaryExpr(m_activation_func);
        // cache activation result of the weight-input combination for backprop
        m_activations.push_back(activation);
    }

    m_activations.pop_back();  // remove last activation

    return activation.transpose();
}

std::vector<MatrixXd> NeuralNetwork::backPropagate(MatrixXd output,
    MatrixXd actual) {

    if (checkIfMatriciesSizeMatch(output, actual)) {
        std::string msg = "Output and actual vectors must match in size.";
        throw std::runtime_error(msg);
    }

    if (checkIfFeedForwardPerformed()) {
        std::string msg = "Backpropagation may only be run after feed forward.";
        throw std::runtime_error(msg);
    }

    std::vector<MatrixXd> errors;

    // compute error for last layer of network
    MatrixXd cost = quadratic_cost_derivative(output, actual);

    MatrixXd sigmaPrime = getSigmaPrime();
    MatrixXd a = getActivation();

    MatrixXd delta = cost.transpose().cwiseProduct(sigmaPrime);
    MatrixXd wError = delta.transpose() * a;

    errors.push_back(wError);

    // compute error in previous layers of network
    for (int index = m_weights.size()-2; index >= 0; --index) {
        MatrixXd weights = m_weights[index+1];
        sigmaPrime = getSigmaPrime();
        a = getActivation();

        delta = (weights * delta.transpose()).cwiseProduct(sigmaPrime.transpose());
        wError = delta * a;

        errors.push_back(wError);
    }

    std::reverse(errors.begin(), errors.end());

    return errors;
}

void NeuralNetwork::updateWeights(const std::vector<Eigen::MatrixXd>& errors,
    double alpha) {

    for (int i=0; i < m_weights.size(); ++i) {
        MatrixXd wError = errors[i].transpose();
        m_weights[i] = m_weights[i] - alpha * wError;
    }
}

Eigen::MatrixXd NeuralNetwork::getSigmaPrime() {
    if (m_zVectors.empty()) {
        std::runtime_error("Z value cache is empty.");
    }

    MatrixXd z = m_zVectors.back();
    m_zVectors.pop_back();  // remove z vector from cache
    return z.unaryExpr(m_activation_deriv);
}

Eigen::MatrixXd NeuralNetwork::getActivation() {
    if (m_activations.empty()) {
        std::runtime_error("Activation cache is empty.");
    }

    MatrixXd a = m_activations.back();
    m_activations.pop_back();
    return a;
}

void NeuralNetwork::printWeights() {
    for (int i =0; i < m_weights.size(); ++i) {
        std::cout << "Layer " << i << "------------------------" << std::endl;
        std::cout << m_weights[i] << std::endl;
    }
    std::cout << "-------------------------------" << std::endl;
}


#include <iostream>
#include <armadillo>

arma::sp_mat generateLinkMatrix(std::size_t nNodes, std::size_t maxEdgesPerNode) {

	arma::sp_mat linkMatrix{nNodes, nNodes};
	for(int i=0; i<nNodes ; ++i){
		std::size_t nLinks = rand() % (maxEdgesPerNode+1);

		std::vector<std::size_t> pos;
		for(int j=0; j<nNodes; ++j){
			pos.push_back(j);
		}
		pos.erase(std::begin(pos) + i);

		for(int j=0; j<nLinks; ++j){
			std::size_t index = rand() % pos.size();
			linkMatrix(pos[index],i) = 1/static_cast<double>(nLinks);
			pos.erase(std::begin(pos)+index);
		}
	}

	return linkMatrix;
};

arma::vec rankSites(const arma::sp_mat& linkMatrix, const float& p) {

	arma::vec v = arma::ones<arma::vec>(linkMatrix.n_cols);
	double epsilon = 10e-10;
	while (true) {
		auto previous = v;
		v = (p/linkMatrix.n_cols) * v + (1-p) * (linkMatrix * v);
		if (all((v-previous) < epsilon)){
			break;
		}
	}

	return v;
}

double lerp(const double& value,
			const double& minIn, double maxIn,
			const double& minOut, const double& maxOut) {
	return minOut + (value + minIn) * (maxOut - minOut)/(maxIn - minIn);

}
void makeGraph(const arma::sp_mat& linkMatrix, const arma::vec& rank) {
	using namespace arma;

	std::cout << "digraph {" << std::endl;
	std::cout << "\tnode [style=\"filled\"]" << std::endl;
	for(int i=0; i< linkMatrix.n_cols; ++i) {
		sp_mat::const_iterator it     = linkMatrix.begin_col(i);
		sp_mat::const_iterator it_end = linkMatrix.end_col(i);
		std::cout << "\t" << i << " [";
		std::cout << "fontsize=" << "\""<< lerp(rank[i], rank.min(), rank.max(), 10, 300)<< "pt\"";
		if(rank[i] == rank.max())
			std::cout << " fillcolor=" << "\"green\"";
		std::cout << "]" << std::endl;
		for(; it != it_end; ++it) {
			std::cout << "\t" << i << "->" << it.row() << std::endl;
		}
	}
	std::cout << "}" << std::endl;
}

int main(int argc, char* argv[]) {
	srand (time(NULL));
	arma::sp_mat A = generateLinkMatrix(atoi(argv[1]), atoi(argv[2]));

	arma::vec v = rankSites(A, atof(argv[3]));

	makeGraph(A,v);
}


/* ! Example file that integrates the Lorenz system, showing how the structs defined in make_dace_compatible_with_odeint.hpp work.
*/

#include <dace/dace_s>
#include <boost/numeric/odeint.hpp>
#include <iostream>

using DACE;
using boost::numeric::odeint;

template <typename vectorType, typename timeType>
void lorenz(const AlgebraicVector< vectorType >& x, AlgebraicVector< vectorType >& dxdt, const timeType& t)
{
    const double sigma = 10.0;
    const double R = 28.0;
    const double b = 8.0 / 3.0;

    dxdt.resize( x.size() );
    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R  * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

int main()
{
    /* Initialise DACE */
    DACE::DA::init(8, 3); // 8th order, three variables

    /* Initial condition */
    AlgebraicVector<DA> x({10.0, 5.0, 5.0});
    x += 0.1 * DA::identity(3);

    /* Define stepper type */
    typedef runge_kutta_dopri5< AlgebraicVector<DA>, double ,AlgebraicVector<DA>,
                              double, vector_space_algebra > stepper; // Notice vector_space_algebra!
    int steps = integrate_adaptive( make_controlled<stepper>( 1E-10 , 1E-10 ), lorenz, x ,
                                    0.0, 10.0, 0.1 );
    std::cout << x << std::endl;
    std::cout << "steps: " << steps << std::endl;

    return 0;
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <algorithm>

typedef boost::adjacency_list<
    boost::vecS,
    boost::vecS,
    boost::undirectedS,
    boost::no_property,
    boost::property<boost::edge_weight_t, int>>
    Graph;
typedef boost::graph_traits<Graph>::edge_descriptor Edge;

void testcase()
{
  int n, m;
  std::cin >> n >> m;
  assert(n >= 0 && n <= 100);
  assert(m >= 0 && m <= n * (n - 1) / 2);

  Graph G(n);
  for (int i = 0; i < m; i++)
  {
    int a, b, w;
    std::cin >> a >> b >> w;
    boost::add_edge(a, b, w, G);
  }

  std::vector<Edge> spanning_tree;
  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

  auto weights = boost::get(boost::edge_weight, G);
  int sum_of_weights = 0;
  for (auto edge : spanning_tree)
  {
    sum_of_weights += weights[edge];
  }

  std::vector<int> distances(n);
  boost::dijkstra_shortest_paths(G, boost::vertex(0, G), boost::distance_map(boost::make_iterator_property_map(distances.begin(), boost::get(boost::vertex_index, G))));

  std::cout
      << sum_of_weights
      << " "
      << *std::max_element(distances.begin(), distances.end())
      << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#define _USE_MATH_DEFINES
#include "gmmtree.h"
#include <Eigen/Eigenvalues>
#include <cmath>

using namespace probreg;

namespace {
static const Float eps = 1.0e-15;

Float gaussianPdf(const Vector3& x, const Vector3& mu, const Matrix3& cov) {
    const Vector3 d = x - mu;
    const Float det = cov.determinant();
    if (det < eps) return 0.0;
    const Float c = 1.0 / (std::pow(det, 0.5) * std::pow(2.0 * M_PI, x.size() * 0.5));
    const Float ep = -0.5 * d.transpose() * cov.inverse() * d;
    return c * std::exp(ep);
}

Float logLikelihood(const NodeParamArray& nodes, const Matrix3X& points, Integer j0, Integer jn) {
    Float q = 0.0;
    for (Integer i = 0; i < points.cols(); ++i) {
        Float tmp = 0.0;
        for (Integer j = j0; j < jn; ++j) {
            if (std::get<0>(nodes[j]) < eps) continue;
            tmp += std::get<0>(nodes[j]) *
                   gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
        }
        q += std::log(std::max(tmp, eps));
    }
    return q;
}

Float complexity(const Matrix3& cov) {
    Eigen::SelfAdjointEigenSolver<Matrix3> es(cov);
    auto lmds = es.eigenvalues();
    std::sort(lmds.data(), lmds.data() + lmds.size(), std::greater<Float>());
    return lmds[2] / lmds.sum();
}

Integer child(Integer j) { return (j + 1) * N_NODE; }

Integer level(Integer l) { return N_NODE * (std::pow(N_NODE, l) - 1) / (N_NODE - 1); }

void accumulate(NodeParam& moments, Float gamma, const Vector& z) {
    if (gamma < eps) return;
    std::get<0>(moments) += gamma;
    std::get<1>(moments) += gamma * z;
    std::get<2>(moments) += gamma * z * z.transpose();
}

NodeParam mlEstimator(const NodeParam& moments, Integer n_points, Float lambda_d) {
    NodeParam node;
    std::get<0>(node) = std::get<0>(moments) / n_points;
    if (std::get<0>(moments) < lambda_d) {
        std::get<0>(node) = 0;
        std::get<1>(node).fill(0.0);
        std::get<2>(node) = Matrix3::Identity();
    } else {
        std::get<1>(node) = std::get<1>(moments) / std::get<0>(moments);
        std::get<2>(node) =
            std::get<2>(moments) / std::get<0>(moments) - std::get<1>(node) * std::get<1>(node).transpose();
    }
    return node;
}

}  // namespace

NodeParamArray probreg::buildGmmTree(const Matrix3X& points,
                                     Integer max_tree_level,
                                     Float lambda_s,
                                     Float lambda_d) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray nodes(n_total);
    auto idxs = (n_total * Vector::Random(n_total)).array().abs().cast<Integer>();
    Float sig2 = 0.0;
    for (Integer i = 0; i < points.cols(); ++i) {
        sig2 += (points.colwise() - points.col(i)).colwise().squaredNorm().sum();
    }
    sig2 /= points.cols() * points.cols() * points.rows() * N_NODE;
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(nodes[j]) = 1.0 / N_NODE;
        std::get<1>(nodes[j]) = points.col(idxs[j]);
        std::get<2>(nodes[j]) = Matrix3::Identity() * sig2;
    }
    VectorXi parent_idx = -VectorXi::Ones(points.cols());
    VectorXi current_idx = VectorXi::Zero(points.cols());

    for (Integer l = 0; l < max_tree_level; ++l) {
        Float prev_q = 0.0;
        while (true) {
            const NodeParamArray params =
                gmmTreeEstep(points, nodes, parent_idx, current_idx, max_tree_level);
            gmmTreeMstep(params, l, nodes, points.cols(), lambda_d);
            const Float q = logLikelihood(nodes, points, level(l), level(l + 1));
            if (std::abs(q - prev_q) < lambda_s) {
                break;
            }
            prev_q = q;
        }
        parent_idx = current_idx;
    }
    return nodes;
}

NodeParamArray probreg::gmmTreeEstep(const Matrix3X& points,
                                     const NodeParamArray& nodes,
                                     const VectorXi& parent_idx,
                                     VectorXi& current_idx,
                                     Integer max_tree_level) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray moments(n_total);
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(moments[j]) = 0.0;
        std::get<1>(moments[j]).fill(0.0);
        std::get<2>(moments[j]).fill(0.0);
    }

    for (Integer i = 0; i < points.cols(); ++i) {
        const Integer j0 = child(parent_idx[i]);
        Vector gamma = Vector::Zero(N_NODE);
        for (Integer j = j0; j < j0 + N_NODE; ++j) {
            gamma[j - j0] = std::get<0>(nodes[j]) *
                            gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
        }
        const Float den = gamma.sum();
        if (den > eps) {
            gamma /= den;
        }
        else {
            gamma.fill(0.0);
        }
        for (Integer j = j0; j < j0 + N_NODE; ++j) {
            accumulate(moments[j], gamma[j - j0], points.col(i));
        }
        Integer max_j;
        gamma.maxCoeff(&max_j);
        current_idx[i] = j0 + max_j;
    }
    return moments;
}

void probreg::gmmTreeMstep(
    const NodeParamArray& params, Integer l, NodeParamArray& nodes, Integer n_points, Float lambda_d) {
    const Integer lb = level(l);
    const Integer le = level(l + 1);

    for (Integer j = lb; j < le; ++j) {
        nodes[j] = mlEstimator(params[j], n_points, lambda_d);
    }
}

NodeParamArray probreg::gmmTreeRegEstep(const Matrix3X& points,
                                        const NodeParamArray& nodes,
                                        Integer max_tree_level,
                                        Float lambda_c) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray moments(n_total);
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(moments[j]) = 0.0;
        std::get<1>(moments[j]).fill(0.0);
        std::get<2>(moments[j]).fill(0.0);
    }

    for (Integer i = 0; i < points.cols(); ++i) {
        Integer search_id = -1;
        Vector gamma = Vector::Zero(N_NODE);
        for (Integer l = 0; l < max_tree_level; ++l) {
            const Integer j0 = child(search_id);
            for (Integer j = j0; j < j0 + N_NODE; ++j) {
                gamma[j - j0] = std::get<0>(nodes[j]) *
                                gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
            }
            const Float den = gamma.sum();
            if (den > eps) {
                gamma /= den;
            } else {
                gamma.fill(0.0);
            }
            gamma.maxCoeff(&search_id);
            search_id += j0;
            if (complexity(std::get<2>(nodes[search_id])) <= lambda_c) break;
            accumulate(moments[search_id], gamma[search_id - j0], points.col(i));
        }
    }
    return moments;
}