#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <EigenRand/EigenRand>

template<class Scalar>
Eigen::Matrix<Scalar, -1, 1> calcMean(const Eigen::Matrix<Scalar, -1, -1>& samples)
{
    return samples.rowwise().mean();
}

template<class Scalar>
Eigen::Matrix<Scalar, -1, -1> calcCov(const Eigen::Matrix<Scalar, -1, -1>& samples)
{
    Eigen::Matrix<Scalar, -1, -1> t = samples.colwise() - samples.rowwise().mean();
    return t * t.transpose() / (samples.cols() - 1);
}

template<class Scalar>
Eigen::Matrix<Scalar, -1, -1> calcMatMean(const Eigen::Matrix<Scalar, -1, -1>& samples)
{
    Eigen::Map<const Eigen::Matrix<Scalar, -1, -1>> reshaped{ samples.data(), samples.rows() * samples.rows(), samples.cols() / samples.rows() };
    Eigen::Matrix<Scalar, -1, -1> ret{ samples.rows(), samples.rows() };
    Eigen::Map<Eigen::Matrix<Scalar, -1, 1>> reshapedRet{ ret.data(), samples.rows() * samples.rows() };
    reshapedRet = reshaped.rowwise().mean();
    return ret;
}

template<class Scalar>
Eigen::Matrix<Scalar, -1, -1> calcMatVar(const Eigen::Matrix<Scalar, -1, -1>& samples)
{
    Eigen::Map<const Eigen::Array<Scalar, -1, -1>> reshaped{ samples.data(), samples.rows() * samples.rows(), samples.cols() / samples.rows() };
    Eigen::Matrix<Scalar, -1, -1> ret{ samples.rows(), samples.rows() };
    Eigen::Map<Eigen::Matrix<Scalar, -1, 1>> reshapedRet{ ret.data(), samples.rows() * samples.rows() };
    reshapedRet = reshaped.pow(2).rowwise().mean() - reshaped.rowwise().mean().pow(2);
    return ret;
}

template<class Ty>
auto relErrors(Ty real, Ty target, double minValue) -> decltype((real - target).abs() / real.abs().max(minValue))
{
    return (real - target).abs() / real.abs().max(minValue);
}

template<class Ty>
bool isSimilarWithErrors(Ty real, Ty target, double relError, double minValue)
{
    return (relErrors(real.array(), target.array(), minValue) <= relError).all();
}

#define EXPECT_SIMILAR_MATRIX(a, b, m) do\
{\
    bool t = isSimilarWithErrors(a, b, 0.1, m);\
    GTEST_TEST_BOOLEAN_(t, #a " is not similar with " #b, false, true, GTEST_NONFATAL_FAILURE_);\
    if (!t)\
    {\
        std::cout << #a "\n" << a << std::endl;\
        std::cout << #b "\n" << b << std::endl;\
        std::cout << "re: \n" << relErrors(a.array(), b.array(), m) << std::endl;\
    }\
} while(0);

static constexpr size_t numSamples = 5000;

template <class T>
class MvDistTest : public testing::Test
{
};

using ETypes = testing::Types<float, double>;

TYPED_TEST_CASE(MvDistTest, ETypes);

TYPED_TEST(MvDistTest, normal)
{
    std::cout << "SIMD arch: " << Eigen::SimdInstructionSetsInUse() << std::endl;
    Eigen::Rand::P8_mt19937_64 rng{ 42 };

    Eigen::Matrix<TypeParam, -1, 1> mean(9);
    mean << -1, 0, 1, 2, 3, 2, 1, 0, -1;

    Eigen::Matrix<TypeParam, -1, -1> cov(9, 9);
    cov.setZero();
    cov(0, 1) = 0.5;
    cov(2, 7) = 0.5;
    cov(3, 1) = 0.7;
    cov.diagonal() << 1, 1.2, 1.4, 1.6, 1.8, 1.6, 1.4, 1.2, 1;
    cov = cov * cov.transpose();

    auto gen = Eigen::Rand::makeMvNormalGen(mean, cov);
    auto samples = gen.generate(rng, numSamples).eval();

    auto mean2 = calcMean(samples);
    auto cov2 = calcCov(samples);

    EXPECT_SIMILAR_MATRIX(mean, mean2, 2.5);
    EXPECT_SIMILAR_MATRIX(cov, cov2, 2.5);
}

TYPED_TEST(MvDistTest, wishart)
{
    Eigen::Rand::P8_mt19937_64 rng{ 42 };

    int df = 12;
    Eigen::Matrix<TypeParam, -1, -1> scale(9, 9);
    scale.setZero();
    scale(0, 1) = 0.5;
    scale(2, 7) = 0.5;
    scale(3, 1) = 0.7;
    scale.diagonal() << 1, 1.2, 1.4, 1.6, 1.8, 1.6, 1.4, 1.2, 1;
    scale = scale * scale.transpose();

    auto gen = Eigen::Rand::makeWishartGen(df, scale);
    auto samples = gen.generate(rng, numSamples).eval();

    auto mean = (df * scale).eval();
    auto var = (scale.diagonal() * scale.diagonal().transpose()).eval();
    var.array() += scale.array() * scale.array();
    var *= df;

    auto mean2 = calcMatMean(samples);
    auto var2 = calcMatVar(samples);

    EXPECT_SIMILAR_MATRIX(mean, mean2, 5);
    EXPECT_SIMILAR_MATRIX(var, var2, 5);
}

TYPED_TEST(MvDistTest, invWishart)
{
    Eigen::Rand::P8_mt19937_64 rng{ 42 };

    float df = 15;
    int p = 9;
    Eigen::Matrix<TypeParam, -1, -1> scale(p, p);
    scale.setZero();
    scale(0, 1) = 0.5;
    scale(2, 7) = 0.5;
    scale(3, 1) = 0.7;
    scale.diagonal() << 1, 1.2, 1.4, 1.6, 1.8, 1.6, 1.4, 1.2, 1;
    scale = scale * scale.transpose();

    auto gen = Eigen::Rand::makeInvWishartGen(df, scale);
    auto samples = gen.generate(rng, numSamples).eval();

    auto mean = (scale / (df - p - 1)).eval();
    auto var = (scale.diagonal() * scale.diagonal().transpose() * (df - p - 1)).eval();
    var.array() += (scale.array() * scale.array()) * (df - p + 1);
    var /= (df - p) * (df - p - 1) * (df - p - 1) * (df - p - 3);

    auto mean2 = calcMatMean(samples);
    auto var2 = calcMatVar(samples);

    EXPECT_SIMILAR_MATRIX(mean, mean2, 5);
    EXPECT_SIMILAR_MATRIX(var, var2, 5);
}

TEST(MvDistTest, multinomial)
{
    Eigen::Rand::P8_mt19937_64 rng{ 42 };

    int n = 10;
    Eigen::Matrix<float, -1, 1> weight(9);
    weight << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    weight /= weight.sum();
    auto gen = Eigen::Rand::makeMultinomialGen(n, weight);
    auto samples = gen.generate(rng, numSamples).array().template cast<float>().matrix().eval();

    auto mean = (weight * n).eval();
    auto cov = (weight * weight.transpose() * -n).eval();
    cov.diagonal() = weight.array() * (1 - weight.array()) * n;

    auto mean2 = calcMean(samples);
    auto cov2 = calcCov(samples);

    EXPECT_SIMILAR_MATRIX(mean, mean2, 2.5);
    EXPECT_SIMILAR_MATRIX(cov, cov2, 2.5);
}

TYPED_TEST(MvDistTest, dirichlet)
{
    Eigen::Rand::P8_mt19937_64 rng{ 42 };

    Eigen::Matrix<TypeParam, -1, 1> weight(9);
    weight << .1, .2, .3, .4, .5, .6, .7, .8, .9;
    auto gen = Eigen::Rand::makeDirichletGen(weight);
    auto samples = gen.generate(rng, numSamples).eval();

    auto mean = (weight / weight.sum()).eval();
    auto cov = (-mean * mean.transpose()).eval();
    cov.diagonal() += mean;
    cov /= weight.sum() + 1;

    auto mean2 = calcMean(samples);
    auto cov2 = calcCov(samples);

    EXPECT_SIMILAR_MATRIX(mean, mean2, 2.5);
    EXPECT_SIMILAR_MATRIX(cov, cov2, 2.5);
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * 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 <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

void linalg_qrsolve(ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b, ub::vector<double> *residual)
{
    // check matrix for zero column
    int nonzero_found = 0;
    for(size_t j=0; j<A.size2(); j++) {
        nonzero_found = 0;
        for(size_t i=0; i<A.size1(); i++) {
            if(fabs(A(i,j))>0) {
                nonzero_found = 1;
            }
        }
        if(nonzero_found==0) {
            throw "qrsolve_zero_column_in_matrix";
        }
    }

    gsl_matrix_view m
        = gsl_matrix_view_array (&A(0,0), A.size1(), A.size2());

    gsl_vector_view gb
        = gsl_vector_view_array (&b(0), b.size());

    gsl_vector *gsl_x = gsl_vector_alloc (x.size());
    gsl_vector *tau = gsl_vector_alloc (x.size());
    gsl_vector *gsl_residual = gsl_vector_alloc (b.size());

    gsl_linalg_QR_decomp (&m.matrix, tau);

    gsl_linalg_QR_lssolve (&m.matrix, tau, &gb.vector, gsl_x, gsl_residual);

    for (size_t i =0 ; i < x.size(); i++)
        x(i) = gsl_vector_get(gsl_x, i);

    if(residual)
        for (size_t i =0 ; i < residual->size(); i++)
            (*residual)(i) = gsl_vector_get(gsl_residual, i);

    gsl_vector_free (gsl_x);
    gsl_vector_free (tau);
    gsl_vector_free (gsl_residual);
}

void linalg_constrained_qrsolve(ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b, ub::matrix<double> &constr)
{
    // check matrix for zero column
    int nonzero_found = 0;
    for(size_t j=0; j<A.size2(); j++) {
        nonzero_found = 0;
        for(size_t i=0; i<A.size1(); i++) {
            if(fabs(A(i,j))>0) {
                nonzero_found = 1;
            }
        }
        if(nonzero_found==0) {
            throw std::runtime_error("constrained_qrsolve_zero_column_in_matrix");
        }
    }

    // Transpose constr:
    constr = trans(constr);

    const int N = b.size();
    const int ngrid = x.size()/2;

    // temporary variables
    ub::matrix<double> Q(2*ngrid, 2*ngrid);       // Q matrix: QR decomposition of trans(B)
    ub::matrix<double> Q_k(2*ngrid, 2*ngrid);
    ub::identity_matrix<double> I (2*ngrid);
    ub::vector<double> v(2*ngrid);

    Q = ub::zero_matrix<double>(2*ngrid, 2*ngrid);
    Q_k = ub::zero_matrix<double>(2*ngrid, 2*ngrid);
    v = ub::zero_vector<double>(2*ngrid);

    double *tmp = & constr(0,0);
    gsl_matrix_view gsl_constr
      = gsl_matrix_view_array (tmp, constr.size1(), constr.size2());

    tmp = &b(0);
    gsl_vector_view gsl_b
         = gsl_vector_view_array (tmp, b.size());

    gsl_vector *tau_qr = gsl_vector_alloc (ngrid);

    gsl_linalg_QR_decomp (&gsl_constr.matrix, tau_qr);

    Q = I;

    for (int k = ngrid; k > 0 ; k--) {

        for (int icout = 0; icout < k - 1; icout++) {
             v(icout) = 0;
        }
        v(k - 1) = 1.0;

        for (int icout = k; icout < 2*ngrid; icout++) {
             v(icout) = gsl_matrix_get(&gsl_constr.matrix, icout, k - 1 );
        }

        Q_k = I - gsl_vector_get(tau_qr, k - 1 ) * outer_prod ( v, v );
        Q = prec_prod(Q, Q_k);

    }

    Q = trans(Q);
    gsl_vector_free (tau_qr);

    // Calculate A * Q and store the result in A
    A = prec_prod(A, Q);

    // A = [A1 A2], so A2 is just a block of A
    ub::matrix<double> A2 = ub::matrix_range<ub::matrix<double> >(A,
            ub::range (0, N), ub::range (ngrid, 2*ngrid)
         );

    tmp = &A2(0,0);
    gsl_matrix_view gsl_A2
         = gsl_matrix_view_array (tmp, A2.size1(), A2.size2());

    gsl_vector *z = gsl_vector_alloc (ngrid);
    gsl_vector *tau_solve = gsl_vector_alloc (ngrid);  // already done!
    gsl_vector *residual = gsl_vector_alloc (N);

    gsl_linalg_QR_decomp (&gsl_A2.matrix, tau_solve);
    gsl_linalg_QR_lssolve (&gsl_A2.matrix, tau_solve, &gsl_b.vector, z, residual);

    // Next two cycles assemble vector from y (which is zero-vector) and z
    // (which we just got by gsl_linalg_QR_lssolve)

    for (int i = 0; i < ngrid; i++ ) {
           x[i] = 0.0;
    }

    for (int i = ngrid; i < 2 * ngrid; i++ ) {
           x[i] = gsl_vector_get(z, i - ngrid);
    }

    // To get the final answer this vector should be multiplied by matrix Q
    // TODO: here i changed the sign, check again! (victor)
    x = -prec_prod( Q, x );

    gsl_vector_free (z);
    gsl_vector_free (tau_solve);
    gsl_vector_free (residual);
}

}}


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file tylorintegrator.hpp Solution for Problem 3c, implementing TaylorIntegrator class

//! \brief Implements an autonomous ODE integrator based on Taylor expansion
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class TaylorIntegrator {
public:
    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a Taylor expansion method
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function (and its derivatives).
    //! \param[in] odefun function handle for rhs f and its derivatives
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &odefun, double T, const State & y0, unsigned int N) const {
        // TODO: solve the autonomous ODE using a Taylor expansion method and suitable call to step
    }

private:

    //! \brief Perform a single step of the Taylor expansion for the solution of the autonomous ODE
    //! Compute a single explicit step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs and its derivatives.
    //! \param[in] odefun function handle for rhs f and the derivatives
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &odefun, double h,
              const State & y0, State & y1 /* TODO: optional: modify step signature */ ) const {
        // TODO: implement a single step of the Taylor expansion method using provided odefunction
    }

};

Nummerical Methods for CSE/PS12/solutions_ps12/taylorintegrator_template.hpp

Nummerical Methods for CSE/PS12/templates_ps12/taylorintegrator_template.hpp


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

using Eigen::Dynamic;
using Eigen::Matrix;

TEST(ProbDistributionsCategorical,Categorical) {
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.3, 0.5, 0.2;
  EXPECT_FLOAT_EQ(-1.203973, stan::math::categorical_log(1,theta));
  EXPECT_FLOAT_EQ(-0.6931472, stan::math::categorical_log(2,theta));
}
TEST(ProbDistributionsCategorical,Propto) {
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.3, 0.5, 0.2;
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log<true>(1,theta));
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log<true>(2,theta));
}

TEST(ProbDistributionsCategorical,VectorInt) {
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.3, 0.5, 0.2;
  std::vector<int> xs0;
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log(xs0,theta));

  std::vector<int> xs(3);
  xs[0] = 1;
  xs[1] = 3;
  xs[2] = 1;

  EXPECT_FLOAT_EQ(log(0.3) + log(0.2) + log(0.3),
                  stan::math::categorical_log(xs,theta));
}

using stan::math::categorical_log;

TEST(ProbDistributionsCategorical, error) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();

  unsigned int n = 1;
  unsigned int N = 3;
  Matrix<double,Dynamic,1> theta(N,1);
  theta << 0.3, 0.5, 0.2;

  EXPECT_NO_THROW(categorical_log(N, theta));
  EXPECT_NO_THROW(categorical_log(n, theta));
  EXPECT_NO_THROW(categorical_log(2, theta));
  EXPECT_THROW(categorical_log(N+1, theta), std::domain_error);
  EXPECT_THROW(categorical_log(0, theta), std::domain_error);

  theta(0) = nan;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = inf;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = -inf;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = -1;
  theta(1) = 1;
  theta(2) = 0;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);

  std::vector<int> ns(3);
  ns[0] = 3;
  ns[1] = 2;
  ns[2] = 3;
  EXPECT_THROW(categorical_log(ns,theta), std::domain_error);

  theta << 0.3, 0.5, 0.2;
  EXPECT_NO_THROW(categorical_log(ns,theta));
  ns[1] = -1;
  EXPECT_THROW(categorical_log(ns,theta), std::domain_error);

  ns[1] = 1;
  ns[2] = 12;
  EXPECT_THROW(categorical_log(ns,theta), std::domain_error);

}

TEST(ProbDistributionsCategorical, error_check) {
  boost::random::mt19937 rng;

  Matrix<double,Dynamic,Dynamic> theta(3,1);
  theta << 0.15,
    0.45,
    0.50;

  EXPECT_THROW(stan::math::categorical_rng(theta,rng),std::domain_error);
}

TEST(ProbDistributionsCategorical, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;

  int N = 10000;
  Matrix<double,Dynamic,Dynamic> theta(3,1);
  theta << 0.15,
    0.45,
    0.40;
  int K = theta.rows();
  boost::math::chi_squared mydist(K-1);

  Eigen::Matrix<double,Eigen::Dynamic,1> loc(theta.rows(),1);
  for(int i = 0; i < theta.rows(); i++)
    loc(i) = 0;

  for(int i = 0; i < theta.rows(); i++) {
    for(int j = i; j < theta.rows(); j++)
      loc(j) += theta(i);
  }

  int count = 0;
  int bin [K];
  double expect [K];
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * theta(i);
  }

  while (count < N) {
    int a = stan::math::categorical_rng(theta,rng);
    bin[a - 1]++;
    count++;
   }

  double chi = 0;

  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


#include <boost/random/mersenne_twister.hpp>
#include <boost/random/extreme_value_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include "fmath.hpp"

#include "random.h"

namespace SPN {
  namespace random {
    rand_gen _def_generator;
    boost::random::extreme_value_distribution<double> _gumbel(0.0, 1.0);
    boost::random::normal_distribution<double> _gaussian(0.0, 1.0);

    rand_gen& get_generator(void) { return _def_generator; }

    void set_seed(uint seed) { _def_generator.seed(seed); }

    double gumbel(void) { return _gumbel(_def_generator); }

    double gaussian(double loc, double scale) {
      _gaussian.param(boost::random::normal_distribution<double>::param_type(loc, scale));
      return _gaussian(_def_generator);
    }
  }
}


#ifndef HMonomialBasisForMinimumFrobeniusNormModel
#define HMonomialBasisForMinimumFrobeniusNormModel

#include "BasisForSurrogateModelBaseClass.hpp"
#include "QuadraticMonomial.hpp"
#include "VectorOperations.hpp"
#include <Eigen/Dense>
#include <vector>
#include "math.h"

class MonomialBasisForMinimumFrobeniusNormModel : public BasisForSurrogateModelBaseClass,
                                                  public QuadraticMonomial,
                                                  protected VectorOperations {
  private:
    int nb_nodes;
    Eigen::MatrixXd A_sysmat;
    Eigen::MatrixXd S_coeffsolve;
    Eigen::MatrixXd F_rhsmat;
    int counter;
    //! Evaluations of surrogate basis functions at nodes used to construct the basis
    std::vector<double> basis_values;
    std::vector<double> basis_constants;
    std::vector< std::vector<double> > basis_gradients;
    std::vector< std::vector< std::vector<double> > > basis_Hessians;
  public:
    MonomialBasisForMinimumFrobeniusNormModel ( int );
    void set_nb_nodes ( int );
    std::vector<double> &evaluate ( std::vector<double> const& );
    double evaluate ( std::vector<double> const&, int);
    double &value( int );
    std::vector<double> &gradient ( int );
    std::vector< std::vector<double> > &hessian ( int );
    void compute_basis_coefficients ( std::vector< std::vector<double> > const& );
    void compute_mat_vec_representation ( int );

};

#endif


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

#ifndef ITL_BROYDEN_INCLUDE
#define ITL_BROYDEN_INCLUDE

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

namespace itl {

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

	Vector     h(H * y), d(s - h);
	value_type gamma= 1 / dot(y, h);
	MTL_THROW_IF(gamma == 0.0, unexpected_orthogonality());
	Matrix     A(gamma * d * trans(y)),
	           H2(H + A * H);
	swap(H2, H); // faster than H= H2
   }
};

} // namespace itl

#endif // ITL_BROYDEN_INCLUDE


#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/loss_function.h>
#include <ceres/autodiff_cost_function.h>

#include <Eigen/Jacobi>
#include <Eigen/SVD>
#include <Eigen/LU>

#include <iostream>

#include <opencv2/core/utility.hpp>

#include <sphericalsfm/sfm.h>
#include <sphericalsfm/so3.h>

namespace sphericalsfm {

    class ParallelTriangulator : public cv::ParallelLoopBody
    {
    public:
        ParallelTriangulator( SfM &_sfm ) :sfm(_sfm) { }
        virtual void operator()(const cv::Range &range) const CV_OVERRIDE
        {
            for ( int j = range.start; j < range.end; j++ )
            {
                if ( !sfm.points.exists(j) ) continue;

                int firstcam = -1;
                int lastcam = -1;

                int nobs = 0;
                for ( int i = 0; i < sfm.numCameras; i++ )
                {
                    if ( !sfm.cameras.exists(i) ) continue;
                    if ( !( sfm.observations.exists(i,j) ) ) continue;
                    if ( firstcam == -1 ) firstcam = i;
                    lastcam = i;
                    nobs++;
                }

                sfm.SetPoint( j, Eigen::Vector3d::Zero() );
                if ( nobs < 3 ) continue;

                Eigen::MatrixXd A( nobs*2, 4 );

                int n = 0;
                for ( int i = 0; i < sfm.numCameras; i++ )
                {
                    if ( !sfm.cameras.exists(i) ) continue;
                    if ( !( sfm.observations.exists(i,j) ) ) continue;

                    Observation vec = sfm.observations(i,j);

                    Eigen::Vector2d point(vec(0)/sfm.intrinsics.focal,vec(1)/sfm.intrinsics.focal);
                    Eigen::Matrix4d P = sfm.GetPose(i).P;

                    A.row(2*n+0) = P.row(2) * point[0] - P.row(0);
                    A.row(2*n+1) = P.row(2) * point[1] - P.row(1);
                    n++;
                }

                Eigen::JacobiSVD<Eigen::MatrixXd> svdA(A,Eigen::ComputeFullV);
                Eigen::Vector4d Xh = svdA.matrixV().col(3);
                Eigen::Vector3d X = Xh.head(3)/Xh(3);

                sfm.SetPoint( j, X );
            }
        }
        ParallelTriangulator& operator=(const ParallelTriangulator &) {
            return *this;
        }
    private:
        SfM &sfm;
    };

    struct ReprojectionError
    {
        ReprojectionError( double _focal, double _x, double _y )
        : focal(_focal), x(_x), y(_y)
        {

        }

        template <typename T>
        bool operator()(const T* const camera_t,
                        const T* const camera_r,
                        const T* const point,
                        T* residuals) const
        {
            // transform from world to camera
            T p[3];
            ceres::AngleAxisRotatePoint(camera_r, point, p);
            p[0] += camera_t[0]; p[1] += camera_t[1]; p[2] += camera_t[2];

            // projection
            T xp = p[0] / p[2];
            T yp = p[1] / p[2];

            // intrinsics
            T fxp = T(focal) * xp;
            T fyp = T(focal) * yp;

            // residuals
            residuals[0] = fxp - T(x);
            residuals[1] = fyp - T(y);

            return true;
        }

        double focal, x, y;
    };

    SfM::SfM( const Intrinsics &_intrinsics )
    : intrinsics( _intrinsics ),
    numCameras( 0 ),
    numPoints( 0 ),
    nextCamera( -1 ),
    nextPoint( 0 )
    {
    }

    double * SfM::GetCameraPtr( int camera )
    {
        return (double*)&cameras(camera);
    }

    double * SfM::GetPointPtr( int point )
    {
        return (double*)&points(point);
    }

    int SfM::AddCamera( const Pose &initial_pose, const std::string &path )
    {
        nextCamera++;
        numCameras++;

        cameras( nextCamera ).head(3) = initial_pose.t;
        cameras( nextCamera ).tail(3) = initial_pose.r;
        paths( nextCamera ) = path;
        rotationFixed( nextCamera ) = false;
        translationFixed( nextCamera ) = false;

        return nextCamera;
    }

    int SfM::AddPoint( const Point &initial_position, const cv::Mat &descriptor )
    {
        numPoints++;

        points( nextPoint ) = initial_position;
        pointFixed( nextPoint ) = false;

        cv::Mat descriptor_copy;
        descriptor.copyTo( descriptor_copy );
        descriptors( nextPoint ) = descriptor_copy;

        return nextPoint++;
    }

    void SfM::MergePoint( int point1, int point2 )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            if ( !cameras.exists(i) ) continue;
            if ( !( observations.exists(i,point2) ) ) continue;

            observations(i,point1) = observations(i,point2);
        }

        RemovePoint( point2 );
    }

    void SfM::AddObservation( int camera, int point, const Observation &observation )
    {
        observations(camera,point) = observation;
    }

    void SfM::AddMeasurement( int i, int j, const Pose &measurement )
    {
        measurements(i,j) = measurement;
    }

    bool SfM::GetMeasurement( int i, int j, Pose &measurement )
    {
        if ( !measurements.exists(i,j) ) return false;

        measurement = measurements(i,j);

        return true;
    }

    bool SfM::GetObservation( int camera, int point, Observation &observation )
    {
        if ( !observations.exists(camera,point) ) return false;

        observation = observations(camera,point);

        return true;
    }

    void SfM::Retriangulate()
    {
        cv::parallel_for_(cv::Range(0,numPoints), [&](const cv::Range &range){
        //for ( int j = 0; j < numPoints; j++ )
        for ( int j = range.start; j < range.end; j++ )
        {
            if ( !points.exists(j) ) continue;

            int firstcam = -1;
            int lastcam = -1;

            int nobs = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;
                if ( firstcam == -1 ) firstcam = i;
                lastcam = i;
                nobs++;
            }

            SetPoint( j, Eigen::Vector3d::Zero() );
            if ( nobs < 3 ) continue;

            Eigen::MatrixXd A( nobs*2, 4 );

            int n = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                Observation vec = observations(i,j);

                Eigen::Vector2d point(vec(0)/intrinsics.focal,vec(1)/intrinsics.focal);
                Eigen::Matrix4d P = GetPose(i).P;

                A.row(2*n+0) = P.row(2) * point[0] - P.row(0);
                A.row(2*n+1) = P.row(2) * point[1] - P.row(1);
                n++;
            }

            Eigen::JacobiSVD<Eigen::MatrixXd> svdA(A,Eigen::ComputeFullV);
            Eigen::Vector4d Xh = svdA.matrixV().col(3);
            Eigen::Vector3d X = Xh.head(3)/Xh(3);

            SetPoint( j, X );
        }
        });
    }

    void SfM::PreOptimize()
    {
        loss_function = new ceres::CauchyLoss( 2.0 );
    }

    void SfM::ConfigureSolverOptions( ceres::Solver::Options &options )
    {
        options.minimizer_type = ceres::TRUST_REGION;
        options.linear_solver_type = ceres::SPARSE_SCHUR;
        options.max_num_iterations = 1000;
        options.max_num_consecutive_invalid_steps = 100;
        options.minimizer_progress_to_stdout = true;
        options.num_threads = 16;
    }

    void SfM::AddResidual( ceres::Problem &problem, int camera, int point )
    {
        Observation vec = observations(camera,point);

        ReprojectionError *reproj_error = new ReprojectionError(intrinsics.focal,vec(0),vec(1));
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3, 3, 3>(reproj_error);
        problem.AddResidualBlock(cost_function, loss_function, GetCameraPtr(camera), GetCameraPtr(camera)+3, GetPointPtr(point) );

        if ( translationFixed( camera ) ) problem.SetParameterBlockConstant( GetCameraPtr(camera) );
        if ( rotationFixed( camera ) ) problem.SetParameterBlockConstant( GetCameraPtr(camera)+3 );
        if ( pointFixed( point ) ) problem.SetParameterBlockConstant( GetPointPtr(point) );
    }

    bool SfM::Optimize()
    {
        if ( numCameras == 0 || numPoints == 0 ) return false;

        ceres::Problem problem;

        PreOptimize();

        bool added_one_camera = false;

        std::cout << "\tBuilding BA problem...\n";

        for ( int j = 0; j < numPoints; j++ )
        {
            if ( !points.exists(j) ) continue;
            if ( points(j).norm() == 0 ) continue;

            int nobs = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                nobs++;
            }

            if ( nobs < 3 ) continue;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                AddResidual( problem, i, j );
                added_one_camera = true;
            }
        }

        if ( !added_one_camera ) {
            std::cout << "didn't add any cameras\n";
            return false;
        }

        std::cout << "Running optimizer...\n";
        std::cout << "\t" << problem.NumResiduals() << " residuals\n";

        ceres::Solver::Options options;
        ConfigureSolverOptions( options );
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        std::cout << summary.FullReport() << "\n";
        if ( summary.termination_type == ceres::FAILURE )
        {
            std::cout << "error: ceres failed.\n";
            exit(1);
        }

        PostOptimize();

        return ( summary.termination_type == ceres::CONVERGENCE );
    }

    void SfM::PostOptimize()
    {

    }

    void SfM::Apply( const Pose &pose )
    {
        // x = PX
        // X -> pose*X
        // x = P' * (pose*X)
        // x = (P*poseinv) * (pose*X)
        Pose poseinv = pose.inverse();
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.postMultiply( poseinv );
            SetPose( i, campose );
        }
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X = pose.apply(X);
            SetPoint( j, X );
        }
    }

    void SfM::Apply( double scale )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.t *= scale;
            campose.P.block<3,1>(0,3) = campose.t;
            SetPose( i, campose );
        }
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X *= scale;
            SetPoint( j, X );
        }
    }

    void SfM::Unapply( const Pose &pose )
    {
        // x = PX
        // X -> poseinv*X
        // x = P' * (poseinv*X)
        // x = (P*pose) * (poseinv*X)
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.postMultiply( pose );
            SetPose( i, campose );
        }
        Pose poseinv = pose.inverse();
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X = poseinv.apply(X);
            SetPoint( j, X );
        }
    }

    Pose SfM::GetPose( int camera )
    {
        if ( !cameras.exists(camera) ) return Pose();

        return Pose( cameras( camera ).head(3), cameras( camera ).tail(3) );
    }

    void SfM::SetPose( int camera, const Pose &pose )
    {
        cameras( camera ).head(3) = pose.t;
        cameras( camera ).tail(3) = pose.r;
    }

    Point SfM::GetPoint( int point )
    {
        if ( !points.exists( point ) ) return Point(0,0,0);
        return points( point );
    }

    void SfM::SetPoint( int point, const Point &position )
    {
        points( point ) = position;
    }

    void SfM::RemovePoint( int point )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            observations.erase(i,point);
        }
        points.erase( point );
        descriptors.erase( point );
    }

    void SfM::RemoveCamera( int camera )
    {
        cameras.erase(camera);
        observations.erase(camera);

        for ( int j = 0; j < numPoints; j++ )
        {
            int i = 0;
            for ( ; i < numCameras; i++ )
            {
                if ( observations.exists(i,j) ) break;
            }
            if ( i == numCameras ) points.erase(j);
        }
    }

    void SfM::WritePoses( const std::string &path, const std::vector<int> &indices )
    {
        assert(indices.size() == numCameras);
        FILE *f = fopen( path.c_str(), "w" );

        for ( int i = 0; i < numCameras; i++ )
        {
            fprintf(f,"%d ",indices[i]);
            Camera camera = cameras(i);
            for ( int j = 0; j < 6; j++ )
            {
                fprintf(f,"%.15lf ",camera(j));
            }
            fprintf(f,"\n");
        }

        fclose(f);
    }

    void SfM::WritePointsOBJ( const std::string &path )
    {
        FILE *f = fopen( path.c_str(), "w" );

        std::vector<int> nobs(numPoints);
        std::vector<double> distances(numPoints);
        for ( int j = 0; j < numPoints; j++ )
        {
            nobs[j] = 0;
        }

        for ( int i = 0; i < numCameras; i++ )
        {
            Pose pose = GetPose(i);
            Eigen::Vector3d center = pose.getCenter();

            for ( int j = 0; j < numPoints; j++ )
            {
                if ( !points.exists(j) ) continue;
                if ( !observations.exists(i,j) ) continue;

                nobs[j]++;
                distances[j] = (GetPoint(j)-center).norm();
            }
        }

        for ( int i = 0; i < numPoints; i++ )
        {
            if ( !points.exists(i) ) continue;

            if ( distances[i] > 2000. ) continue;
            Point X = GetPoint(i);
            if ( X.norm() == 0 ) continue;
            fprintf(f,"v %0.15lf %0.15lf %0.15lf\n", X(0), X(1), X(2) );
        }

        fclose( f );
    }

    void SfM::WriteCameraCentersOBJ( const std::string &path )
    {
        FILE *f = fopen( path.c_str(), "w" );

        for ( int i = 0; i < numCameras; i++ )
        {
            Pose pose = GetPose(i);
            Eigen::Vector3d center = pose.getCenter();
            fprintf(f,"v %0.15lf %0.15lf %0.15lf\n", center(0), center(1), center(2) );
        }

        fclose( f );
    }
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>

using namespace boost;
using namespace std;

int main()
{
/*********************** Graph creation *********************************/
  int N,a,b,j;
  cout << "Enter number of nodes: "<<endl;
  cin >> N;
  j = N;
  // Declares a graph g
  adjacency_list <> g;
  // Adding vertices to graph g
  while(j){
    add_vertex(g);
    j--;
  }

  while(1)
  {
	cout <<"Connect Nodes (x->y) : ";
	cin >> a >> b;
	if( a < 0 || b < 0){
		cout << "Your Graph is ready\n"<< endl;
		break;
	}
	else if(a < N || b < N) {
		cout << "Connected: (" << a << "->" << b <<")"<< endl;
		add_edge(a, b, g);
	}
  }

/**************************** Print neighbors of each vertex *****************************/

  graph_traits < adjacency_list <> >::vertex_iterator i, eend;
  graph_traits < adjacency_list <> >::adjacency_iterator ai, a_end;	// out_edge_iterator can also be used to get the list of adjacency vertices

// A PropertyGraph is a graph that has some property associated with each of the vertices or edges in the graph.
//
// vertex_index_t is the type of property which is being accessed. 'vertex_index' is one of its value.
// Eg. enum vertex_index_t
//     {
//	 vertex_index
//     }
// (https://www.boost.org/doc/libs/1_60_0/libs/graph/doc/property.html)
//
// get(vertex_index, g) returns the property map with the property of type vertex_index

  property_map < adjacency_list <>, vertex_index_t >::type index_map = get(vertex_index, g);

    for (tie(i, eend) = vertices(g); i != eend; i++) {
    cout << "Neighbors of vertex " << (int)get(index_map, *i) << " ==> ";
        for (tie(ai, a_end) = adjacent_vertices(*i, g); ai != a_end; ai++) {
            cout << (int)get(index_map, *ai) << "  ";
        }
    cout << endl;
    }
}


/**
 * @file fixed_point.hpp
 * @author Salvatore Cardamone
 * @brief Largely a replica of the Fixed Point Math Library developed by
 *        Peter Schregle. Pared down superfluous functionality and added some
 *        bits and pieces to make the code a little more useful for the types
 *        of calculation we do in tyche++.
 */
#ifndef __TYCHEPLUSPLUS_FIXED_POINT_HPP
#define __TYCHEPLUSPLUS_FIXED_POINT_HPP

#include <cstddef>
#include <boost/operators.hpp>
#include <boost/type_index.hpp>
#include "utilities/type_promotion.hpp"

namespace tycheplusplus {

/**
 * @class FixedPoint
 * @brief Generic fixed point functionality, allowing us to use this class as
 *        we would any other data type, like float or double. All integer
 *        arithmetic, so if there's no dedicated FPU, using FixedPoint as the
 *        real numerical type is probably advantageous. Furthermore, any
 *        high-level synthesis tools will hopefully pick up on the use of
 *        integer arithmetic and generate efficient implementations.
 *
 *        boost/operators.hpp provides a fairly remarkable set of
 *        functionalities, whereby operators can be automatically generated from
 *        a smaller set of operators. Deriving from the appropriate boost
 *        classes within boost/operators.hpp then gives us access to the full
 *        complement of operators.
 *
 *        For instance, deriving from boost::ordered_field_operators allows us
 *        to explicitly define the += operator, and obtain the + operator for
 *        free, without the need for additional boilerplate.
 *
 * @tparam B Data type used to store the fixed point number. It the type is
 *           signed, then the fixed point representation will be signed too.
 * @tparam I Number of integer bits.
 * @tparam F Number of fractional bits. Automatically determined from number of
 *           integer bits and number of available storage bits.
 */
template<typename B,
         unsigned char I,
         unsigned char F = std::numeric_limits<B>::digits - I>
class FixedPoint
    : boost::ordered_field_operators<FixedPoint<B,I,F>,
      boost::unit_steppable<FixedPoint<B,I,F>,
      boost::shiftable<FixedPoint<B,I,F> > > >
{

public:
  /**
   * @brief Class constructor.
   * @param value Single precision value to initialise with.
   */
  FixedPoint(float value)
      : value_(value * two_power_f_ + (value >= 0 ? 0.5 : -0.5)) {}

  /**
   * @brief Class constructor.
   * @param value Double precision value to initialise with.
   */
  FixedPoint(double value)
      : value_(value * two_power_f_ + (value >= 0 ? 0.5 : -0.5)) {
  }

  /**
   * @brief Class constructor.
   * @param value Single precision value to initialise with.
   */
  FixedPoint<B,I,F>& operator +=(FixedPoint<B,I,F> const& rhs) {
    value_ += rhs.value_;
    return *this;
  }

  /**
   * @brief Subtraction assignment operator.
   * @param rhs Value to subtract from lhs.
   * @retval lhs - rhs.
   */
  FixedPoint<B,I,F>& operator -=(FixedPoint<B,I,F> const& rhs) {
    value_ -= rhs.value_;
    return *this;
  }

  /**
   * @brief Multiplication assignment operator.
   * @param rhs Value to multiply lhs by.
   * @retval lhs * rhs.
   */
  FixedPoint<B,I,F>& operator *=(FixedPoint<B,I,F> const& rhs) {
    value_ = (static_cast<typename TypePromotion<B>::type>
	      (value_) * rhs.value_) >> number_fractional_bits_;
    return *this;
  }

  /**
   * @brief Division assignment operator.
   * @param rhs Value to divide lhs by.
   * @retval lhs / rhs.
   */
  FixedPoint<B,I,F>& operator /=(FixedPoint<B,I,F> const& rhs) {
    value_ = (static_cast<typename TypePromotion<B>::type>
	      (value_) << number_fractional_bits_) / rhs.value_;
    return *this;
  }

  /**
   * @brief Convert the internal value of the fixed point object to a float.
   * @retval Fixed point number cast to float.
   */
  float AsFloat() const {
    return (float)value_ / two_power_f_;
  }

  /**
   * @brief Convert the internal value of the fixed point object to a double.
   * @retval Fixed point number cast to double.
   */
  double AsDouble() const {
    return (double)value_ / two_power_f_;
  }

  /**
   * @brief Print some information about the object.
   */
  void Print(std::ostream& stream) const {
    stream << " *** FixedPoint object" << std::endl
	   << "     "
	   << (int)number_fractional_bits_ << " fractional bits and "
	   << (int)number_integer_bits_ << " integer bits." << std::endl
	   << "     Storage type: "
	   << boost::typeindex::type_id<B>().pretty_name() << std::endl
	   << "     Has Sign Bit: "
	   << std::numeric_limits<B>::is_signed << std::endl
	   << "     Stored Value: " << std::hex << value_ << std::dec << std::endl
	   << "     Floating Point: " << AsDouble() << std::endl;
  }

  /**
   * @brief Compute the exponential of a fixed point number.
   *
   *        This is fairly inefficient, utilising (I+F) integer multiplications.
   *        The exponential is split into its integer and fractional parts:
   *
   *                         exp(i.f) = exp(i) * exp(f)
   *
   *        For the fractional part, we move down the fractional bits of the
   *        argument, lookup the associated value for the exponential of the
   *        fractional bit and multiply-accumulate if the bit is high, otherwise
   *        it makes no contribution. So, for instance, e^{0.625} is equal to:
   *
   *                    1*exp(0.5) * 0*exp(0.25) * 1*exp(0.125)
   *
   *        the values of the exponential for which are already tabulated.
   *        We compute the integer part in a similar fashion using the
   *        integer part lookup table. If the argument is negative, we
   *        divide-accumulate rather than multiply-accumulate.
   *
   *        We should implement a specialised Gaussian function, since the
   *        integer part is only relevant over a much smaller dynamic range.
   * @param arg The argument of the exponential function.
   * @retval exp(arg).
   */
  friend FixedPoint<B,I,F> exp(FixedPoint<B,I,F> const& arg) {

    FixedPoint<B,I,F> result(1.0);

    // We start from the MSB in the fractional part and work our way down the
    // number of fractional bits
    for (int i_frac = F-1; i_frac >= 0; --i_frac) {
      if (arg.value_ & 1ULL<<i_frac) {
	result.value_ =
	  (static_cast<typename TypePromotion<B>::type>(result.value_) *
	   (exp_frac_lut[F-i_frac-1] >> (32-F))) >> F;
      }
    }

    // Need to find out whether we're dividing or multiplying for the
    // integer part
    bool is_negative =
      std::numeric_limits<B>::is_signed && ((1ULL << (I+F-1)) & arg.value_);

    // If the number is negative, we need to do some two's complement to get the
    // integer part then work our way up from the LSB
    if (is_negative) {
      B integer_part = ~(arg.value_ >> F) + 1;
      for (int i_int = 0; i_int<I; ++i_int) {
	if (integer_part & 1ULL<<i_int) {
	  result.value_ =
	    (static_cast<typename TypePromotion<B>::type>(result.value_) << F) /
	    (exp_int_lut[i_int] >> (32-F));
	}
      }
    // If the number is positive, we start from the MSB in the integer part and
    // work our way up the number of integer bits
    } else {
      for (int i_int = F; i_int<(I+F); ++i_int) {
	if (arg.value_ & 1ULL<<i_int) {
	  result.value_ =
	    (static_cast<typename TypePromotion<B>::type>(result.value_) *
	     (exp_int_lut[i_int-F]) >> (32-F)) >> F;
	}
      }
    }

    return result;

  }

private:
  // Alias for the sake of simplifying functions
  B value_;
  static constexpr unsigned char number_integer_bits_ = (unsigned char)I;
  static constexpr unsigned char number_fractional_bits_ = (unsigned char)F;
  static constexpr B two_power_f_ = (1ULL << F);

  // exp[0.5], exp[0.25], exp[0.125], etc... in Q32.32
  static constexpr unsigned long exp_frac_lut[32] = {
    0x00000001a61298e2, 0x0000000148b5e3c4, 0x000000012216045b,
    0x000000011082b578, 0x0000000108205601, 0x0000000104080ab5,
    0x0000000102020156, 0x000000010100802b, 0x0000000100802005,
    0x0000000100400801, 0x0000000100200200, 0x0000000100100080,
    0x0000000100080020, 0x0000000100040008, 0x0000000100020002,
    0x0000000100010001, 0x0000000100008000, 0x0000000100004000,
    0x0000000100002000, 0x0000000100001000, 0x0000000100000800,
    0x0000000100000400, 0x0000000100000200, 0x0000000100000100,
    0x0000000100000080, 0x0000000100000040, 0x0000000100000020,
    0x0000000100000010, 0x0000000100000008, 0x0000000100000004,
    0x0000000100000002, 0x0000000100000001
  };
  // exp[1], exp[2], exp[4], etc... in Q32.32
  static constexpr unsigned long exp_int_lut[32] = {
    0x00000002b7e15163, 0x0000000763992e35, 0x0000003699205c4e,
    0x00000ba4f53ea386, 0x0087975e85400100, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff
  };

};

}

#endif /* #ifndef __TYCHEPLUSPLUS_FIXED_POINT_HPP */


#include <cstdlib>
#include <ctime>
#include <iostream>
#include <armadillo>
#include <random>
#include <functional>

using namespace arma;

class RandDouble
{
  public:
      RandDouble(double low, double high)
      :r(std::bind(std::uniform_real_distribution<>(low,high),std::default_random_engine())){}

      double operator()(){ return r(); }

  private:
      std::function<double()> r;
};

int randInt(int minInt, int  maxInt)
{
  return rand()%maxInt + minInt;
}

template <typename T>
void monteCarlo(const AmoebaParam<T> amoebaParam)
{

  srand(time(NULL));

  vec guess(63);
  for(int i=0; i<63; ++i)
  {
      if(i<37){
        guess(i) = 0.0001;
      }
      else
      {
        guess(i) = 0.000000000001;
      }
  }
  vec newGuess = guess;

  cx_mat X1;

  double tau = 6.28318530718;
  double tol = 0.000001;
  double objectRes = 100;
  double objectResOld;

  double change;

  int entry;
  int iters = 0;
  int maxIters = 10000;
  double scale = 1.0;

  RandDouble randD{-0.00001,0.00001};

  UAmoeba* amoeba = new UAmoeba(amoebaParam.maxAmoebaIters, amoebaParam.nGridPoints, amoebaParam.precision, amoebaParam.matSize, amoebaParam.lieDimension, amoebaParam.basis);
  amoeba->startBoundary = amoebaParam.startBoundary;
  amoeba->endBoundary = amoebaParam.endBoundary;

  cout << "start boundary : " << amoeba->startBoundary << endl;
  cout << "end boundary : " << amoeba->endBoundary << endl;

  X1 = amoeba->curveFunc(guess);
  objectResOld = amoeba->objectFunc(X1);

  cout << "initial weight : " << objectResOld << endl;

  while( iters < maxIters && objectRes > tol)
  {

    iters+=1;
    cout << iters << endl;
    //randomly select a entry
    entry = iters%38;
    //randomly vary the entry mod 2pi
    change = randD();
    newGuess(entry) = fmod((newGuess(entry) + change), tau);

    //recompute geodesic
    X1 = amoeba->curveFunc(newGuess);

    //compute norm
    objectRes = amoeba->objectFunc(X1);

    if(objectRes < objectResOld)
    {
      //new guess is better
      cout << "changed entry :" << entry << endl;
      cout <<"change : " << change << endl;
      cout << "new weight : " << objectRes << endl;
      guess = newGuess;
      objectResOld = objectRes;
    }
    //reset newGuess;
    newGuess = guess;
  }

  amoeba->curvePrint(guess);
  cout << "best guess : " << guess << endl;
}


#pragma once

#include "loader.hpp"
#include <Eigen/Dense>
#include <any>
#include <chrono>
#include <map>
#include <random>
#include <unordered_set>
#include <vector>

class Config;
class SpTensor_List;
class SpTensor_Hash;
class SpTensor_dX;
class DataStream;

class TensorStream;
TensorStream* generateTensorStream(DataStream& paperX, const Config& config); // Generate the tensor stream

class TensorStream {
public:
    TensorStream(DataStream& paperX, const Config& config);
    virtual ~TensorStream(void);

    void updateTensor(const DataStream::Event& e);
    void updateFactor(void);

    double elapsedTime(void) const; // sec

    double find_reconst(const std::vector<int>& coord) const;
    double density(void) const;

    /* Load matrix */
    void saveFactor(std::string fileName) const;

    /* Get errors */
    double rmse(void) const;
    double fitness(void) const;
    double fitness_latest(void) const;
    double error(const std::vector<int>& coord) const; // Error of the given entry

    void updateAtA(void); // Update the AtA when _use_AtA is false

protected:
    const Config* _config;

    std::vector<int> _compute_order;

    virtual void _updateAlgorithm(void) {} // It will change the current updateAlgorithm later

    double _norm_frobenius_reconst(void) const;
    double _innerprod_X_X_reconst(void) const;

    SpTensor_Hash* _X = nullptr;
    SpTensor_dX* _dX = nullptr;
    DataStream* _paperX;

    Eigen::ArrayXd _lambda;
    std::vector<Eigen::MatrixXd> _A;
    std::vector<Eigen::ArrayXXd> _AtA;

    bool _use_AtA = true;

    long long _nextTime;

    std::chrono::nanoseconds _elapsed_time; // Elapsed time

    void _rand_init_A(void); // Randomly initialize factor matrices

    void _als_base(void); // Base code for ALS
    void _unnormalize_A(void); // Unnormalize the factor matrices

    /* Basic factor update algorithms */
    void _als(void);
    void _recurrent_als(void);
};

/* General ALS until the convergence */
class TensorStream_ALS : public TensorStream {
public:
    TensorStream_ALS(DataStream& paperX, const Config& config)
        : TensorStream(paperX, config)
    {
    }
    virtual ~TensorStream_ALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override
    {
        _als();
    }
};

/* ALS using previous factor matrices as a initialization point until the convergence */
class TensorStream_RecurrentALS : public TensorStream {
public:
    TensorStream_RecurrentALS(DataStream& paperX, const Config& config)
        : TensorStream(paperX, config)
    {
    }
    virtual ~TensorStream_RecurrentALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override
    {
        _recurrent_als();
    }
};

/* ALS using previous factor matrices as a initialization point with fixed # of iterations */
class TensorStream_RecurrentALS_iter : public TensorStream_RecurrentALS {
public:
    TensorStream_RecurrentALS_iter(DataStream& paperX, const Config& config);
    virtual ~TensorStream_RecurrentALS_iter(void) {}

protected:
    virtual void _updateAlgorithm(void) override;
    int _numIter;
};

/* SelectiveALS */
class TensorStream_SelectiveALS : public TensorStream {
public:
    TensorStream_SelectiveALS(DataStream& paperX, const Config& config)
        : TensorStream(paperX, config)
    {
    }
    virtual ~TensorStream_SelectiveALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override;
};

class TensorStream_coordSelectiveALS : public TensorStream {
public:
    TensorStream_coordSelectiveALS(DataStream& paperX, const Config& config);
    virtual ~TensorStream_coordSelectiveALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override;
    Eigen::ArrayXd _sqSumProd; // Product of sqSum
    std::vector<Eigen::MatrixXd> _Aprev;
    std::vector<Eigen::ArrayXXd> _AprevtA;
    double _higherBound;
};

/* SamplingALS */
class TensorStream_SamplingALS : public TensorStream {
public:
    TensorStream_SamplingALS(DataStream& paperX, const Config& config);
    virtual ~TensorStream_SamplingALS(void) {}

    /*
        Save sampled entries and their reconstructed value.
        deltidxLists: save changed index, numSel: number of changed idx in each mode.
    */
    void Sampling(const std::vector<std::vector<int>>& deltaIdxLists, const std::vector<int>& numSel);

    typedef std::unordered_map<std::vector<int>, double> samples;
    typedef std::vector<std::vector<samples>> totalSamples;

protected:
    int _numSample; // # of sampling per selected slice
    std::vector<std::vector<int>> _numCand; // Array for sampling candidates
    std::vector<Eigen::ArrayXXd> _AprevtA; // For A_{t-1}^TA_t
    totalSamples _sampledIdx; // Save sampled entries
};

class TensorStream_baseSamplingALS : public TensorStream_SamplingALS {
public:
    TensorStream_baseSamplingALS(DataStream& paperX, const Config& config)
        : TensorStream_SamplingALS(paperX, config)
    {
    }
    virtual ~TensorStream_baseSamplingALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    /* Compute the mttkrp result of one entry */
    void mkpRow(const std::vector<int>& rawIdx, int currMode,
        Eigen::MatrixXd& mkp, int rdx, bool changed, double reconVal = 0.0);
};

/* SamplingALS with coordinate descent */
class TensorStream_coordSamplingALS : public TensorStream_SamplingALS {
public:
    TensorStream_coordSamplingALS(DataStream& paperX, const Config& config);
    virtual ~TensorStream_coordSamplingALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override;
    Eigen::ArrayXd _sqSumProd; // Product of sqSum
    std::vector<Eigen::MatrixXd> _Aprev;
    double _higherBound;
};

class TensorStream_hybridALS : public TensorStream_baseSamplingALS {
public:
    TensorStream_hybridALS(DataStream& paperX, const Config& config);
    virtual ~TensorStream_hybridALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override;
    std::vector<std::vector<int>> selIdxLists;
    std::vector<std::vector<int>> samIdxLists;
};

class TensorStream_coordHybridALS : public TensorStream_coordSamplingALS {
public:
    TensorStream_coordHybridALS(DataStream& paperX, const Config& config);
    virtual ~TensorStream_coordHybridALS(void) {}
    double prodAtA(int numMode, int currMode, const std::vector<Eigen::ArrayXXd>& squareVec,
        int currRow, int currCol);
    double prodA(int numMode, int currMode,
        const std::vector<Eigen::MatrixXd>& factorVec, const std::vector<int>& currIdx, int currCol);
    void updateA_and_AtA(double newVal, int rank, int currMode, int currRow, int currCol);

protected:
    virtual void _updateAlgorithm(void) override;
    std::vector<std::vector<int>> selIdxLists;
    std::vector<std::vector<int>> samIdxLists;
};


/**
 * @file ann_regularizer_test.cpp
 * @author Saksham Bansal
 *
 * Tests the ANN regularizer modules.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/regularizer/regularizer.hpp>

#include <boost/test/unit_test.hpp>
#include "ann_test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ANNRegularizerTest);

BOOST_AUTO_TEST_CASE(GradientL1RegularizerTest)
{
  // Add function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction() :
      factor(0.6),
      reg(factor)
    {
      // Nothing to do here.
    }

    double Output(const arma::mat& weight, size_t i, size_t j)
    {
      return std::abs(weight(i, j)) * factor;
    }

    void Gradient(arma::mat& weight, arma::mat& gradient)
    {
      reg.Evaluate(weight, gradient);
    }

    double factor;
    L1Regularizer reg;
  } function;

  BOOST_REQUIRE_LE(CheckRegularizerGradient(function), 1e-4);
}

BOOST_AUTO_TEST_CASE(GradientL2RegularizerTest)
{
  // Add function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction() :
        factor(0.6),
        reg(factor)
    {
      // Nothing to do here.
    }

    double Output(const arma::mat& weight, size_t i, size_t j)
    {
      return weight(i, j) * weight(i, j) * factor;
    }

    void Gradient(arma::mat& weight, arma::mat& gradient)
    {
      reg.Evaluate(weight, gradient);
    }

    double factor;
    L2Regularizer reg;
  } function;

  BOOST_REQUIRE_LE(CheckRegularizerGradient(function), 1e-4);
}

BOOST_AUTO_TEST_CASE(GradientOrthogonalRegularizerTest)
{
  // Add function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction() :
        factor(0.6),
        reg(factor)
    {
      // Nothing to do here.
    }

    double Output(const arma::mat& weight, size_t /* i */, size_t /* j */)
    {
      arma::mat x = arma::abs(weight * weight.t() -
          arma::eye<arma::mat>(weight.n_rows, weight.n_cols)) * factor;
      return arma::accu(x);
    }

    void Gradient(arma::mat& weight, arma::mat& gradient)
    {
      reg.Evaluate(weight, gradient);
    }

    double factor;
    OrthogonalRegularizer reg;
  } function;

  BOOST_REQUIRE_LE(CheckRegularizerGradient(function), 1e-4);
}

BOOST_AUTO_TEST_SUITE_END();


#include <cellogram/remesh_adaptive.h>
#include <igl/triangle/triangulate.h>
#include <igl/write_triangle_mesh.h>
#include <Eigen/Dense>
#include <iostream>

void isotropic_quad(Eigen::MatrixXd &OV, Eigen::MatrixXi &OF, double area) {
	std::stringstream buf;
	buf.precision(100);
	buf.setf(std::ios::fixed, std::ios::floatfield);

	buf << "Qqa" << area;

	Eigen::MatrixXd V(4,2); V <<
		-1,-1,
		-1,1,
		1,1,
		1,-1;

	Eigen::MatrixXi E(4,2); E <<
		0,1,
		1,2,
		2,3,
		3,0;

	Eigen::MatrixXd H(0,2);
	igl::triangle::triangulate(V, E, H, buf.str(), OV, OF);
}

int main(int argc, char** argv) {
	Eigen::MatrixXd V1, V2;
	Eigen::MatrixXi F1, F2;
	Eigen::VectorXd S;

	isotropic_quad(V1, F1, 0.01);

	S.resize(V1.rows());
	for (int v = 0; v < V1.rows(); ++v) {
		Eigen::RowVector2d p = V1.row(v);
		S(v) = 0.001 + 0.1 * p.squaredNorm();
	}

	cellogram::remesh_adaptive_2d(V1, F1, S, V2, F2);
	igl::write_triangle_mesh("output.obj", V2, F2);

	return 0;
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <limits>
#include <cassert>
#include <map>

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

void testcase()
{
  int n, e, s, source_node, target_node;
  std::cin >> n >> e >> s >> source_node >> target_node;
  assert(n >= 1 && n <= 500 && e >= 1 && s >= 1 && s <= 10);
  assert(source_node >= 0 && source_node < n);
  assert(target_node >= 0 && target_node < n);

  Graph G(n);
  auto shared_weights = boost::get(boost::edge_weight, G);
  int infinite_weight = std::numeric_limits<int>::max();
  std::vector<std::map<Graph::edge_descriptor, int>> weights_by_species(s);
  for (int i = 0; i < e; i++)
  {
    int a, b;
    std::cin >> a >> b;
    Graph::edge_descriptor edge = boost::add_edge(a, b, infinite_weight, G).first;

    for (int j = 0; j < s; j++)
    {
      int w;
      std::cin >> w;
      weights_by_species.at(j).insert(std::make_pair(edge, w));
    }
  }

  for (int i = 0; i < s; i++)
  {
    int hive_location;
    std::cin >> hive_location;
    auto species_weight_map = boost::make_assoc_property_map(weights_by_species.at(i));
    std::vector<Graph::vertex_descriptor> predecessors(n);
    boost::prim_minimum_spanning_tree(G, boost::make_iterator_property_map(predecessors.begin(), boost::get(boost::vertex_index, G)), boost::weight_map(species_weight_map).root_vertex(hive_location));

    for (Graph::vertex_descriptor a = 0; a < Graph::vertex_descriptor(n); a++)
    {
      Graph::vertex_descriptor b = predecessors.at(a);
      if (a == b)
      {
        continue;
      }
      assert(boost::edge(a, b, G).second);
      Graph::edge_descriptor edge = boost::edge(a, b, G).first;
      int &min_weight = shared_weights[edge];
      min_weight = std::min({min_weight, species_weight_map[edge]});
    }
  }

  Graph G_finite(n);
  for (auto edge_iterators = boost::edges(G); edge_iterators.first != edge_iterators.second; edge_iterators.first++)
  {
    auto edge = *edge_iterators.first;
    int w = shared_weights[edge];
    assert(w >= 0);
    if (w != infinite_weight)
    {
      boost::add_edge(edge.m_source, edge.m_target, w, G_finite);
    }
  }

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

  std::cout << distances.at(target_node) << "\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;
}


#include <tagloc/tracking.h>
#include <math.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <iostream>
using std::cout;
using std::endl;
using Eigen::MatrixXd;

#define USE_MATH_DEFINES
int main(int argc, char** argv){

	std::vector<double> px,py,z,R;
	double t[2],c[4];
	double tp[2],cp[4];

	int i=1;
	tp[0] = atof(argv[i++]);
	tp[1] = atof(argv[i++]);
	cp[0] = atof(argv[i++]);
	cp[1] = atof(argv[i++]);
	cp[2] = atof(argv[i++]);
	cp[3] = atof(argv[i++]);

	for(;i<argc-1;i+=3){
		px.push_back(atof(argv[i]));
		py.push_back(atof(argv[i+1]));
		z.push_back(atof(argv[i+2]));
		R.push_back(Rval);
	}

	size_t N = R.size();

	RSN::line_intersection(N, px, py, z, &t[0], &t[1], c);
	RSN::BOT::IWLS_2D(N, px, py, z, R, t, c, true);
	printf("iwls: %0.3f, %0.3f [%0.3f, %0.3f, %0.3f, %0.3f]\n",t[0],t[1],c[0],c[1],c[2],c[3]);

	MatrixXd x0(2,1);
	MatrixXd x1(2,1);
	MatrixXd p0(2,2);
	MatrixXd p1(2,2);
	for (int i=0;i<2;i++){
		x0(i) = tp[i];
		x1(i) = t[i];
		for (int j=0;j<2;j++){
			p0(i,j) = cp[i*2+j];
			p1(i,j) = c[i*2+j];
		}
	}
	cout<<x0<<endl<<endl;
	cout<<x1<<endl<<endl;
	cout<<p0<<endl<<endl;
	cout<<p1<<endl<<endl;

	cout<< (p0.inverse()+p1.inverse()).inverse() * (p0.inverse()*x0 + p1.inverse()*x1) <<endl;
	//printf("iwls+prior: %0.3f, %0.3f [%0.3f, %0.3f, %0.3f, %0.3f]\n",t[0],t[1],cov[0],cov[1],cov[2],cov[3]);

	return 0;
}


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <iostream>
#include <armadillo>
#include "highgui.h"

using namespace std;
using namespace cv;

void detectcan(cv::Mat &frame) {
	int h1 = 100;
	int h2 = 179;
	int s1 = 125;
	int s2 = 255;
	int v1 = 50;
	int v2 = 170;
	cv::Mat hsv;
	cv::cvtColor(frame, hsv, COLOR_BGR2HSV);
	cv::Mat hsv2mask, hsv2;
	cv::inRange(hsv, cv::Scalar(h1, s1, v1), cv::Scalar(h2, s2, v2), hsv2mask);

	arma::mat I;
	cvt_opencv2arma(hsv2mask, I);
	if (arma::accu(I) < 10.0) {
		return;
	}
	arma::rowvec r = arma::sum(I, 0);
	arma::colvec c = arma::sum(I, 1);
	double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
	double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);

	double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
	//double estw = covx;
	//double esth = estw / 0.5;
	//estw *= 3; // strange constants
	//esth *= 3;

	arma::vec cokecanpos = arma::vec({ midx, midy });
	double cokecanwidth = covx * 3;
	cout << "pos: " << cokecanpos << endl;
	cout << "width: " << cokecanwidth << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 8) {
		cout << "usage: ./test img h1 h2 s1 s2 v1 v2\n";
		return 1;
	}
	int h1 = atoi(argv[2]);
	int h2 = atoi(argv[3]);
	int s1 = atoi(argv[4]);
	int s2 = atoi(argv[5]);
	int v1 = atoi(argv[6]);
	int v2 = atoi(argv[7]);

	// convert to hsv
	Mat img = imread(argv[1]);
	Mat hsv;
	cvtColor(img, hsv, COLOR_BGR2HSV);

	// in range masking
	Mat hsv2mask, hsv2;
	inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
	//hsv.copyTo(hsv2, hsv2mask);

	arma::mat I;
	cvt_opencv2arma(hsv2mask, I);
	arma::rowvec r = arma::sum(I, 0);
	arma::colvec c = arma::sum(I, 1);
	double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
	double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
	Vec3b color(255, 0, 0);
	circle(img, Point((int)midx, (int)midy), 4, color, 1);

	double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
	double estw = covx;
	double esth = estw / 0.5;
	estw *= 3; // strange constants
	esth *= 3;

	rectangle(img, Rect(midx-estw/2,midy-esth/2,estw,esth), Scalar(0, 0, 255), 2);

	// use histogram binning to get the center

	// create blob params
	/*SimpleBlobDetector::Params params;

	// Change thresholds
	params.minThreshold = 10;
	params.maxThreshold = 200;

	// Filter by Area.
	params.filterByArea = true;
	params.minArea = 1500;

	// Filter by Circularity
	params.filterByCircularity = true;
	params.minCircularity = 0.1;

	// Filter by Convexity
	params.filterByConvexity = true;
	params.minConvexity = 0.87;

	// Filter by Inertia
	params.filterByInertia = true;
	params.minInertiaRatio = 0.01;

	Ptr<SimpleBlobDetector> blob = SimpleBlobDetector::create(params);
	vector<KeyPoint> kp;
	Mat des;
	Mat hsvd = hsv2mask * -0.75 + 255;
	blob->detect(hsvd, kp);

	cout << "found " << kp.size() << " matches\n";

	Mat kpimg;
	drawKeypoints(img, kp, kpimg, Scalar(0, 0, 255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);*/

	imshow("oldhsv", hsv);
	imshow("newhsv", hsv2mask);
	imshow("img", img);
	waitKey(0);
	return 1;
}


#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp>


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <random>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>

#define LOG_2 0.69314718055994529
#define LOG_PI 1.1447298858494002
#define LOG_2PI 1.8378770664093453
#define LOG_4PI 2.5310242469692907

template<typename T>
inline T logBesselI(T nu, T x)
{
  //TODO link against boost
  // for large values of x besselI \approx exp(x)/sqrt(2 PI x)
//  if(x>100.)  return x - 0.5*log(2.*M_PI*x);
//  return log(std::cyl_bessel_i(nu,x));
  return x - 0.5*LOG_2PI -0.5*log(x);
};

template<typename T>
inline T logxOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 0.;
  else
    return log(x)-log(sinh(x));
}
template<typename T>
inline T xOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 1.;
  else
    return x/sinh(x);
}
template<typename T>
inline T xOverTanPiHalfX(T x) {
  if (fabs(x) < 1e-9)
    return 2./M_PI;
  else
    return x/tan(x*M_PI*0.5);
}

template<typename T>
inline T logSumExp(const Eigen::Matrix<T,Eigen::Dynamic,1>& logX) {
  T logMax = logX.maxCoeff();
  return log((logX.array()-logMax).exp().sum()) + logMax;
}

template <typename T, uint32_t D>
inline T MLEstimateTau(const Eigen::Matrix<T,3,1>& xSum, const
    Eigen::Matrix<T,3,1>& mu, T count) {
  // Need double precision to achive convergence; single is not enough.
  double tau = 1.0;
  double prevTau = 0.;
  double eps = 1e-8;
  double R = xSum.norm()/count;
  while (fabs(tau - prevTau) > eps) {
//    std::cout << "tau " << tau << " R " << R << std::endl;
    double inv_tanh_tau = 1./tanh(tau);
    double inv_tau = 1./tau;
    double f = -inv_tau + inv_tanh_tau - R;
    double df = inv_tau*inv_tau - inv_tanh_tau*inv_tanh_tau + 1.;
    prevTau = tau;
    tau -= f/df;
  }
  return tau;
};

template<typename T, int D>
class vMF
{
public:
  vMF()
    : mu_(0,0,1), tau_(0.), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const Eigen::Matrix<T,D,1>& mu, T tau)
    : mu_(mu), tau_(tau), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const Eigen::Matrix<T,D,1>& tauMu)
    : mu_(tauMu.normalized()), tau_(tauMu.norm()), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const vMF<T,D>& vmf)
    : mu_(vmf.mu_), tau_(vmf.tau_), unif_(0.,1.), gauss_(0.,1.)
  {}

  T logPdf(const Eigen::Matrix<T,D,1>& x) const {
    const T d = static_cast<T>(D);
    if (tau_ < 1e-9) {
      // TODO insert general formula here
      return -LOG_4PI;
    } else {
      return (d/2. -1.)*log(tau_) - (d/2.)*LOG_2PI
        - logBesselI<T>(d/2. -1.,tau_) + tau_*mu_.dot(x);
    }
  }

  /// Use uniform distribution on the sphere as a proposal distribution
  Eigen::Matrix<T,D,1> sample(std::mt19937& rnd) {
//    std::cout << "stating n sampling ------------ " << std::endl;
    // implemented using rejection sampling and proposals from a gaussian
    Eigen::Matrix<T,D,1> x;
    T pdf_g = -LOG_4PI;
    // bound via maximum over vMF at mu
    // TODO: dont know why I need to multiply by 2 here to get the
    // correct samples (as seen in concentration estimates)
    T M = 2.*tau_;
    while(42) {
      // sample from zero mean Gaussian
      for (uint32_t d=0; d<D; d++) x[d] = gauss_(rnd);
      x.normalize();
      // rejection sampling (in log domain)
      T u = log(unif_(rnd));
      T pdf_f = 2.*tau_*x.dot(mu_); //this->logPdf(x);
//      std::cout << pdf_f << " " << pdf_g << " " << M << " " << tau_ << std::endl;
      if(u < pdf_f-(M+pdf_g)) break;
    };
    return x;
  }

  Eigen::Matrix<T,D,1> mu_;
  T tau_;
private:
  std::uniform_real_distribution<T> unif_;
  std::normal_distribution<T> gauss_;
};

template<>
float vMF<float,3>::logPdf(const Eigen::Matrix<float,3,1>& x) const {
  if (tau_ < 1e-9) {
    return -LOG_4PI;
  } else {
    return -LOG_2PI + log(tau_) + tau_*(mu_.dot(x)-1.) - log(1.-exp(-2.*tau_));
//    return 0.5*LOG_PI - 0.5*LOG_2 + tau_*mu_.dot(x) + logxOverSinhX(tau_);
  }
}

template<>
Eigen::Matrix<float,3,1> vMF<float,3>::sample(std::mt19937& rnd) {
  if (tau_ < 1e-10) {
    return Eigen::Vector3f(gauss_(rnd), gauss_(rnd), gauss_(rnd)).normalized();
  }
  // https://www.mitsuba-renderer.org/~wenzel/files/vmf.pdf
  // sample around (0,0,1)
  Eigen::Vector2f v(gauss_(rnd), gauss_(rnd));
  v.normalize();
  const float u = unif_(rnd);
  const float w = 1. + log(u+(1.-u)*exp(-2.*tau_))/tau_;
  const float a = sqrtf(1.-w*w);
  Eigen::Vector3f x(a*v(0), a*v(1), w);

  // rotate to mu
  Eigen::Vector3f axis = Eigen::Vector3f(0,0,1).cross(mu_);
  float angle = acos(mu_[2]);

  if (fabs(angle) <1e-9)
    return x;

  Eigen::Quaternion<float> q(cos(angle*0.5),
      sin(angle*0.5)*axis(0)/axis.norm(),
      sin(angle*0.5)*axis(1)/axis.norm(),
      sin(angle*0.5)*axis(2)/axis.norm());
  return q._transformVector(x);
}


#include <Eigen/Dense>
#include <iostream>

int main(int, char *[]) {
  // AngleAxisf aa = Quaternionf(..);
  Eigen::VectorXd vq(4);
  vq[0] = 1.0;
  vq[1] = 0.0;
  vq(2) = 0.0;
  vq(3) = 0.0;
  Eigen::Matrix<float, 4, 1> coe;
  Eigen::Matrix<float, 3, 3> R;
  Eigen::Quaternionf q(2.0, 0.1, 0.3, 4.0);  // wxyz
  coe = q.coeffs();
  std::cout << coe << std::endl;  // xyzw

  Eigen::Vector3f vec = q.vec();  // xyz
  std::cout << vec << std::endl;

  Eigen::AngleAxisf aa;
  Eigen::Matrix3f mat;
  Eigen::Quaternionf qm(mat);
  Eigen::Quaternionf qa(aa);

  Eigen::Quaternionf qw;
  qw.setIdentity();  // important
  R = qw.toRotationMatrix();
  qw = qw * q;
  qw = qw.normalized();  //规范化
  std::cout << R << std::endl;
  qw = qw.Identity();  // 1 0 0 0 or qw.setIdentity();
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;
  qw = qw.inverse();
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;

  Eigen::Vector3f w1;
  w1 << 1, 2, 3;
  Eigen::Vector3f w2;
  w2 << 2, 3, 4;
  qw = qw.Identity();
  qw = qw.setFromTwoVectors(w1, w2);  //出来的norm=1--姿态四元数
  std::cout << qw.squaredNorm() << std::endl;  // norm^2  .norm()
  q.setIdentity();
  qw = qw.normalized();
  q = q.normalized();
  q = q.inverse();
  std::cout << q.angularDistance(qw) << std::endl;  //必须先规范化
  std::cout << qw.dot(q) << std::endl;              // dot product 内积
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;
  std::cout << q.w() << "  " << q.x() << "  " << q.y() << "  " << q.z()
            << std::endl;

  // std::cout<<q;
  std::cout << std::endl;
  for (int size = 1; size <= 4; ++size) {
    Eigen::MatrixXi m(size, size + 1);    // a (size)x(size+1)-matrix of int's
    for (int j = 0; j < m.cols(); ++j)    // loop over columns
      for (int i = 0; i < m.rows(); ++i)  // loop over rows
        m(i, j) = i + j * m.rows();       // to access matrix coefficients,
    // use operator()(int,int)
    std::cout << m << "\n\n";
  }
  Eigen::VectorXf v(4);  // a vector of 4 float's
  // to access vector coefficients, use either operator () or operator []
  v[0] = 1;
  v[1] = 2;
  v(2) = 3;
  v(3) = 4;
  std::cout << "\nv:\n" << v << std::endl;
}


#include <complex>
#include <fstream>
#include <iostream>
#include <numeric>
#include <vector>
using namespace std;
using namespace std::complex_literals;

#include <boost/math/special_functions/bessel.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <ieompp/algebra/monomial.hpp>
#include <ieompp/algebra/operator.hpp>
#include <ieompp/constants.hpp>
#include <ieompp/lattices/periodic_chain.hpp>
#include <ieompp/models/hubbard_real_space/basis.hpp>
#include <ieompp/models/hubbard_real_space/expectation_value.hpp>
#include <ieompp/openmp.hpp>
namespace hubbard = ieompp::models::hubbard_real_space;

complex<double> minus_i_power(uint64_t power)
{
    switch(power % 4) {
        case 0:
            return 1.;
        case 1:
            return -1.i;
        case 2:
            return -1.;
        default:
            return 1.i;
    }
}

int main()
{
    const uint64_t N = 128;
    ieompp::lattices::PeriodicChain<double> lattice(N, 1.);
    hubbard::Basis1Operator<ieompp::algebra::Monomial<ieompp::algebra::Operator<uint64_t, bool>>>
        basis(lattice);

    const double dt      = 0.01;
    const uint64_t steps = 10000;

    // precompute time dependent prefactors and their complex conjugates
    vector<vector<complex<double>>> h_vals(N), h_vals_conj(N);
#pragma omp parallel for
    for(uint64_t i = 0; i < N; ++i) {
        h_vals[i].resize(steps);
        h_vals_conj[i].resize(steps);
        const auto j                   = lattice.lattice_distance(0, i);
        std::complex<double> prefactor = minus_i_power(j);
        for(uint64_t step = 0; step < steps; ++step) {
            double bess          = boost::math::cyl_bessel_j(j, 2 * step * dt);
            h_vals[i][step]      = bess * prefactor;
            h_vals_conj[i][step] = bess * std::conj(prefactor);
        }
    }

    const hubbard::ExpectationValue1DHalfFilled<double, decltype(lattice)> expectation_value(
        lattice, 1., 0.5);

    vector<vector<complex<double>>> results;
#pragma omp parallel
    {
#pragma omp critical
        {
            results.emplace_back(steps, complex<double>(0.));
        }
    }

#pragma omp parallel for
    for(uint64_t i = 0; i < N; ++i) {
        for(uint64_t j = 0; j < N; ++j) {
            const auto ev = expectation_value(basis[i].front().index1, basis[j].front().index1);
            for(uint64_t step = 0; step < steps; ++step) {
                results[omp_get_thread_num()][step] +=
                    ev * (h_vals[i][step] * h_vals_conj[j][step]);
            }
        }
    }

    ofstream file("theory.txt", ofstream::trunc);
    for(uint64_t step = 0; step < steps; ++step) {
        complex<double> val = 0.;
        for(auto &result : results) {
            val += result[step];
        }
        file << step * dt << '\t' << val.real() << '\t' << val.imag() << '\n';
    }
    file.close();
}


#pragma once

#include <algorithm>
#include <fstream>
#include <iostream>
#include <list>
#include <stdio.h>

#include <Eigen/Dense>

#include "configuration.hpp"
#include "utils/util.hpp"

namespace util {
double SmoothPos(double ini, double end, double moving_duration,
                 double curr_time);
double SmoothVel(double ini, double end, double moving_duration,
                 double curr_time);
double SmoothAcc(double ini, double end, double moving_duration,
                 double curr_time);
void SinusoidTrajectory(double initTime_, const Eigen::VectorXd &midPoint_,
                        const Eigen::VectorXd &amp_,
                        const Eigen::VectorXd &freq_, double evalTime_,
                        Eigen::VectorXd &p_, Eigen::VectorXd &v_,
                        Eigen::VectorXd &a_, double smoothing_dur = 1.0);
double Smooth(double ini, double fin, double rat);
} // namespace util

class HermiteCurve {
public:
  HermiteCurve();
  HermiteCurve(const double &start_pos, const double &start_vel,
               const double &end_pos, const double &end_vel);
  ~HermiteCurve();
  void Initialize(const double &start_pos, const double &start_vel,
                  const double &end_pos, const double &end_vel);
  double Evaluate(const double &s_in);
  double EvaluateFirstDerivative(const double &s_in);
  double EvaluateSecondDerivative(const double &s_in);

private:
  double p1_;
  double v1_;
  double p2_;
  double v2_;

  double s_;
};

class HermiteCurveVec {
public:
  HermiteCurveVec();
  HermiteCurveVec(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  void Initialize(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  ~HermiteCurveVec();
  Eigen::VectorXd Evaluate(const double &s_in);
  Eigen::VectorXd EvaluateFirstDerivative(const double &s_in);
  Eigen::VectorXd EvaluateSecondDerivative(const double &s_in);

private:
  Eigen::VectorXd p1_;
  Eigen::VectorXd v1_;
  Eigen::VectorXd p2_;
  Eigen::VectorXd v2_;

  std::vector<HermiteCurve> curves_;
  Eigen::VectorXd output_;
};

class HermiteQuaternionCurve {
public:
  HermiteQuaternionCurve();
  HermiteQuaternionCurve(const Eigen::Quaterniond &quat_start,
                         const Eigen::Vector3d &angular_velocity_start,
                         const Eigen::Quaterniond &quat_end,
                         const Eigen::Vector3d &angular_velocity_end);
  ~HermiteQuaternionCurve();

  void Initialize(const Eigen::Quaterniond &quat_start,
                  const Eigen::Vector3d &angular_velocity_start,
                  const Eigen::Quaterniond &quat_end,
                  const Eigen::Vector3d &angular_velocity_end);

  // All values are expressed in "world frame"
  void Evaluate(const double &s_in, Eigen::Quaterniond &quat_out);
  void GetAngularVelocity(const double &s_in, Eigen::Vector3d &ang_vel_out);
  void GetAngularAcceleration(const double &s_in, Eigen::Vector3d &ang_acc_out);

private:
  Eigen::Quaterniond qa;   // Starting quaternion
  Eigen::Vector3d omega_a; // Starting Angular Velocity
  Eigen::Quaterniond qb;   // Ending quaternion
  Eigen::Vector3d omega_b; // Ending Angular velocity

  Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
  Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

  void initialize_data_structures();

  void computeBasis(const double &s_in); // computes the basis functions
  void computeOmegas();

  Eigen::Quaterniond q0; // quat0
  Eigen::Quaterniond q1; // quat1
  Eigen::Quaterniond q2; // quat1
  Eigen::Quaterniond q3; // quat1

  double b1; // basis 1
  double b2; // basis 2
  double b3; // basis 3

  double bdot1; // 1st derivative of basis 1
  double bdot2; // 1st derivative of basis 2
  double bdot3; // 1st derivative of basis 3

  double bddot1; // 2nd derivative of basis 1
  double bddot2; // 2nd derivative of basis 2
  double bddot3; // 2nd derivative of basis 3

  Eigen::Vector3d omega_1;
  Eigen::Vector3d omega_2;
  Eigen::Vector3d omega_3;

  Eigen::AngleAxisd omega_1aa;
  Eigen::AngleAxisd omega_2aa;
  Eigen::AngleAxisd omega_3aa;

  // Allocate memory for quaternion operations
  Eigen::Quaterniond qtmp1;
  Eigen::Quaterniond qtmp2;
  Eigen::Quaterniond qtmp3;

  // progression variable
  double s_;
};

class MinJerkCurve {
public:
  // Constructors
  MinJerkCurve();
  MinJerkCurve(const Eigen::Vector3d &init, const Eigen::Vector3d &end,
               const double &time_start, const double &time_end);

  void SetParams(const Eigen::Vector3d &init, const Eigen::Vector3d &end,
                 const double &time_start, const double &time_end);

  void GetPos(const double &time, double &pos);
  void GetVel(const double &time, double &vel);
  void GetAcc(const double &time, double &acc);

  // Destructor
  ~MinJerkCurve();

private:
  Eigen::MatrixXd C_mat;     // Matrix of Coefficients
  Eigen::MatrixXd C_mat_inv; // Inverse of Matrix of Coefficients
  Eigen::VectorXd
      a_coeffs; // mininum jerk coeffs. a = [a0, a1, a2, a3, a4, a5, a6];
  Eigen::VectorXd bound_cond; // boundary conditions x_b = [ x(to), xdot(to),
                              // xddot(to), x(tf), xdot(tf), xddot(tf)]

  Eigen::Vector3d init_cond; // initial pos, vel, acceleration
  Eigen::Vector3d end_cond;  // final pos, vel, acceleration
  double to;                 // Starting time
  double tf;                 // Ending time

  void Initialization();

  // Compute the coefficients
  void compute_coeffs();
};

class MinJerkCurveVec {
public:
  MinJerkCurveVec();
  MinJerkCurveVec(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &start_acc,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel,
                  const Eigen::VectorXd &end_acc, double duration);
  ~MinJerkCurveVec();
  Eigen::VectorXd Evaluate(const double &t_in);
  Eigen::VectorXd EvaluateFirstDerivative(const double &t_in);
  Eigen::VectorXd EvaluateSecondDerivative(const double &t_in);

private:
  double Ts_;

  Eigen::VectorXd p1_;
  Eigen::VectorXd v1_;
  Eigen::VectorXd a1_;

  Eigen::VectorXd p2_;
  Eigen::VectorXd v2_;
  Eigen::VectorXd a2_;

  std::vector<MinJerkCurve> curves_;
  Eigen::VectorXd output_;
};


/**
 * @file
 * @brief Outputs location of global shape functions as managed by a Dofhandler
 * @author Ralf Hiptmair
 * @date   October 2018
 * @copyright MIT License
 */

#include <boost/program_options.hpp>

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include "lf/assemble/assemble.h"
#include "lf/mesh/test_utils/test_meshes.h"
#include "lf/mesh/utils/utils.h"

int main(int argc, char** argv) {
  // The following code is modeled after the example from
  // https://theboostcpplibraries.com/boost.program_options
  // and defines allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
  ("help,h", "--ndof_node <N> --ndof_edge <N> --ndof_tria <N> --ndof_quad <N>")
  ("ndof_node,n", po::value<int>()->default_value(1), "No of dofs on nodes")
  ("ndof_edge,e", po::value<int>()->default_value(2), "No of dofs on edges")
  ("ndof_tria,t", po::value<int>()->default_value(1), "No of dofs on triangles")
  ("ndof_quad,q", po::value<int>()->default_value(4), "Mp of dofs on quadrilaterals");
  // clang-format on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("help") > 0) {
    std::cout << desc << std::endl;
  } else {
    // Retrieve number of degrees of freedom for each entity type from command
    // line arguments
    lf::base::size_type ndof_node = 1;
    if (vm.count("ndof_node") > 0) {
      ndof_node = vm["ndof_node"].as<int>();
    }
    lf::base::size_type ndof_edge = 2;
    if (vm.count("ndof_edge") > 0) {
      ndof_edge = vm["ndof_edge"].as<int>();
    }
    lf::base::size_type ndof_tria = 1;
    if (vm.count("ndof_tria") > 0) {
      ndof_tria = vm["ndof_tria"].as<int>();
    }
    lf::base::size_type ndof_quad = 4;
    if (vm.count("ndof_quad") > 0) {
      ndof_quad = vm["ndof_quad"].as<int>();
    }
    std::cout << "LehrFEM++ demo: assignment of global shape functions"
              << std::endl;
    std::cout << "#dof/vertex = " << ndof_node << std::endl;
    std::cout << "#dof/edge = " << ndof_edge << std::endl;
    std::cout << "#dof/triangle = " << ndof_tria << std::endl;
    std::cout << "#dof/quadrilateral = " << ndof_quad << std::endl;

    // Build a mesh comprising two cells
    std::shared_ptr<lf::mesh::Mesh> mesh_p =
        lf::mesh::test_utils::GenerateHybrid2DTestMesh(2);
    // Output information about the mesh
    lf::mesh::utils::printinfo_ctrl = 100;
    lf::mesh::Entity::output_ctrl_ = 0;
    lf::mesh::utils::PrintInfo(*mesh_p, std::cout);

    // Create a dof handler object describing a uniform distribution
    // of shape functions
    lf::assemble::UniformFEDofHandler dof_handler(
        mesh_p, {{lf::base::RefEl::kPoint(), ndof_node},
                 {lf::base::RefEl::kSegment(), ndof_edge},
                 {lf::base::RefEl::kTria(), ndof_tria},
                 {lf::base::RefEl::kQuad(), ndof_quad}});
    lf::assemble::DofHandler::output_ctrl_ = 30;
    std::cout << dof_handler << std::endl;
  }
}  // end main