#include <frovedis.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>
#include <frovedis/matrix/tsne.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

template <class T>
void call_tsne(const std::string& data_p, const std::string& out_p, double perplexity,
               double early_exaggeration, double min_grad_norm, double learning_rate,
               size_t n_components, size_t max_iter, size_t n_iter_without_progress,
               const std::string& metric, const std::string& method,
               const std::string& init, bool verbose) {
  time_spent load_t(INFO);
  load_t.lap_start();
  auto mat = make_rowmajor_matrix_load<T>(data_p);
  load_t.lap_stop();
  load_t.show_lap("data loading time: ");
  std::cout << "n_samples = " << mat.num_row
            << ", n_features = " << mat.num_col
            << std::endl;
  time_spent tsne_t(INFO);
  tsne_t.lap_start();

  TSNE<T> t1;
  t1.set_perplexity(perplexity).
     set_early_exaggeration(early_exaggeration).
     set_min_grad_norm(min_grad_norm).
     set_learning_rate(learning_rate).
     set_n_components(n_components).
     set_n_iter(max_iter).
     set_n_iter_without_progress(n_iter_without_progress).
     set_metric(metric).
     set_method(method).
     set_init(init).
     set_verbose(verbose);

  auto Y_mat = t1.fit_transform(mat);
  auto n_iter = t1.get_n_iter_();
  auto kl_divergence = t1.get_kl_divergence_();

  tsne_t.lap_stop();
  tsne_t.show_lap("Overall computation time: ");
  Y_mat.save(out_p);
  std::cout << "n_iter_ = " << n_iter << std::endl;
  std::cout << "kl_divergence_ = " << kl_divergence << std::endl;
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
      ("help,h", "produce help message")
      ("input,i" , value<std::string>(), "input data path containing input data for T-SNE")
      ("dtype,t" , value<std::string>(), "input data type (float or double) [default: double]")
      ("output,o" , value<std::string>(), "output data path to save output embeddings")
      ("max_iter,k", value<size_t>(), "maximum no. of iterations (default: 1000)")
      ("perplexity,p", value<double>(), "number of nearest neighbors for each point (default: 30.0)")
      ("early_exaggeration,e", value<double>(), "controls the space between natural clusters in the embedded space (default: 12.0)")
      ("min_grad_norm,g", value<double>(), "gradient norm threshold (default: 1e-7)")
      ("learning_rate,l", value<double>(), "learning rate for t-SNE (default: 200.0)")
      ("n_components,n", value<size_t>(), "dimension of the embedded space (default: 2)")
      ("niter_without_progress", value<size_t>(), "maximum number of iterations without progress before we abort the optimization (default: 300)")
      ("metric,m" , value<std::string>(), "the metric (euclidean or precomputed) to use when calculating distance (default: euclidean)")
      ("method" , value<std::string>(), "the method (exact) to use for TSNE computation (default: exact)")
      ("init" , value<std::string>(), "the init (random) to use for initializing Y mat (default: random)")
      ("verbose", "set loglevel to DEBUG");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  std::string dtype =  "double";
  std::string data_p =  "";
  std::string out_p =  "";
  size_t max_iter = 1000;
  double perplexity = 30.0;
  double early_exaggeration = 12.0;
  double min_grad_norm = 1e-7;
  double learning_rate = 200.0;
  size_t n_components = 2;
  size_t n_iter_without_progress = 300;
  std::string metric = "euclidean"; //possible values = ["euclidean", "precomputed"]
  std::string method = "exact"; //possible values = ["exact"]
  std::string init = "random"; //possible values = ["random"]
  bool verbose = false;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("input")){
    data_p = argmap["input"].as<std::string>();
  } else {
    std::cerr << "input path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("dtype")){
    dtype = argmap["dtype"].as<std::string>();
  }
  if(argmap.count("output")){
    out_p = argmap["output"].as<std::string>();
  } else {
    std::cerr << "output path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("max_iter")){
     max_iter = argmap["max_iter"].as<size_t>();
  }
  if(argmap.count("perplexity")){
     perplexity = argmap["perplexity"].as<double>();
  }
  if(argmap.count("early_exaggeration")){
     early_exaggeration = argmap["early_exaggeration"].as<double>();
  }
  if(argmap.count("min_grad_norm")){
     min_grad_norm = argmap["min_grad_norm"].as<double>();
  }
  if(argmap.count("learning_rate")){
     learning_rate = argmap["learning_rate"].as<double>();
  }
  if(argmap.count("n_components")){
     n_components = argmap["n_components"].as<size_t>();
  }
  if(argmap.count("niter_without_progress")){
     n_iter_without_progress = argmap["niter_without_progress"].as<size_t>();
  }
  if(argmap.count("metric")){
    metric = argmap["metric"].as<std::string>();
  }
  if(argmap.count("method")){
    method = argmap["method"].as<std::string>();
  }
  if(argmap.count("init")){
    init = argmap["init"].as<std::string>();
  }
  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
    verbose = true;
  }

  try {
    if (dtype == "float") {
      call_tsne<float>(data_p, out_p, perplexity, early_exaggeration, min_grad_norm,
                       learning_rate, n_components, max_iter,
                       n_iter_without_progress, metric, method, init, verbose);
    }
    else if (dtype == "double") {
      call_tsne<double>(data_p, out_p, perplexity, early_exaggeration, min_grad_norm,
                        learning_rate, n_components, max_iter,
                        n_iter_without_progress, metric, method, init, verbose);
    }
    else {
      std::cerr << "Supported dtypes are only float and double!\n";
      std::cerr << opt << std::endl;
      exit(1);
    }
  }
  catch(std::exception& e) {
    std::cout << "exception caught: " << e.what() << std::endl;
  }
  return 0;
}


/*
 * Copyright (c) 2019 Nobuyuki Umetani
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <cmath>
#include <iostream>
#include <vector>
#include <chrono>
#include <Eigen/Core>

#include "gtest/gtest.h"
#include "delfem2/view_vectorx.h"
#include "delfem2/lsmats.h"
#include "delfem2/lsilu_mats.h"
#include "delfem2/vecxitrsol.h"
#include "delfem2/femsolidlinear.h"
#include "delfem2/mshuni.h"
#include "delfem2/dtri2_v2dtri.h"
#include "delfem2/dtri.h"
#include "delfem2/eigen/ls_dense.h"
#include "delfem2/eigen/ls_sparse.h"
#include "delfem2/eigen/ls_ilu_sparse.h"
#include "delfem2/lsitrsol.h"

// ------------------------------------------

void MakeMesh(
    std::vector<double>& aXY1,
    std::vector<unsigned int>& aTri1,
    std::vector<int>& aBCFlag,
    double elen,
    unsigned int ndimval)
{
  std::vector< std::vector<double> > aaXY;
  const double len = 1.0;
  {
    aaXY.resize(1);
    aaXY[0].push_back(-len); aaXY[0].push_back(-len);
    aaXY[0].push_back(-len); aaXY[0].push_back(+len);
    aaXY[0].push_back(+len); aaXY[0].push_back(+len);
    aaXY[0].push_back(+len); aaXY[0].push_back(-len);
  }
  std::vector<delfem2::CDynPntSur> aPo2D;
  std::vector<delfem2::CDynTri> aETri;
  std::vector<delfem2::CVec2d> aVec2;
  delfem2::GenMesh(aPo2D,aETri,aVec2,
                   aaXY,elen,elen);
  MeshTri2D_Export(
      aXY1,aTri1,
      aVec2,aETri);
  const unsigned int np = aXY1.size()/2;
  aBCFlag.assign(np*ndimval, 0);
  for(unsigned int ip=0;ip<np;++ip){
//    const double px = aXY1[ip*2+0];
    const double py = aXY1[ip*2+1];
    if( fabs(py-len) > 0.0001 ){ continue; }
    for(unsigned int idim=0;idim<ndimval;++idim) {
      aBCFlag[ip * 2 + idim] = 1;
    }
  }
//  std::cout<<"  ntri;"<<aTri1.size()/3<<"  nXY:"<<aXY1.size()/2<<std::endl;
}

TEST(ls,test1)
{
  namespace dfm2 = delfem2;
  const double epsilon = 1.0e-5;
  //
  std::vector<unsigned int> aTri1;
  std::vector<double> aXY1;
  std::vector<int> aBCFlag;
  MakeMesh(
      aXY1,aTri1,aBCFlag,
      0.08, 2);
  const unsigned int np = aXY1.size()/2;
  const unsigned int nDoF = np*2;
  const std::vector<double> aVal(nDoF,0);
  // -----------
  delfem2::CMatrixSparseBlock<Eigen::Matrix2d,Eigen::aligned_allocator<Eigen::Matrix2d>> Aeig;
  Eigen::VectorXd Veig0(nDoF);
  Eigen::Matrix<double,-1,2,Eigen::RowMajor> Veig1(np,2);
  dfm2::CMatrixSparse<double> Astd;
  std::vector<double> Vstd(nDoF);
  {
    std::vector<unsigned int> psup_ind0, psup0;
    dfm2::JArray_PSuP_MeshElem(
        psup_ind0, psup0,
        aTri1.data(), aTri1.size() / 3, 3,
        aXY1.size() / 2);
    Aeig.Initialize(np);
    Aeig.SetPattern(psup_ind0.data(), psup_ind0.size(), psup0.data(), psup0.size());
    const double myu = 10.0, lambda = 10.0, rho = 1.0, g_x = 0.0, g_y = -3.0;
    Aeig.setZero();
    Veig0.setZero();
    dfm2::MergeLinSys_SolidLinear_Static_MeshTri2D(
        Aeig, Veig0.data(),
        myu, lambda, rho, g_x, g_y,
        aXY1.data(), aXY1.size() / 2,
        aTri1.data(), aTri1.size() / 3,
        aVal.data());
    SetFixedBC_Dia(Aeig, aBCFlag.data(), 1.f);
    SetFixedBC_Col(Aeig, aBCFlag.data());
    SetFixedBC_Row(Aeig, aBCFlag.data());
    delfem2::setZero_Flag(Veig0, aBCFlag, 0);
    // ----------
    for(unsigned int ip=0;ip<np;++ip){
      Veig1(ip,0) = Veig0(ip*2+0);
      Veig1(ip,1) = Veig0(ip*2+1);
    }
    // ----------
    Astd.Initialize(np, 2, true);
    Astd.SetPattern(psup_ind0.data(), psup_ind0.size(), psup0.data(), psup0.size());
    Astd.setZero();
    Vstd.assign(nDoF,0.0);
    dfm2::MergeLinSys_SolidLinear_Static_MeshTri2D(
        Astd, Vstd.data(),
        myu, lambda, rho, g_x, g_y,
        aXY1.data(), aXY1.size() / 2,
        aTri1.data(), aTri1.size() / 3,
        aVal.data());
    Astd.SetFixedBC(aBCFlag.data());
    dfm2::setRHS_Zero(Vstd, aBCFlag, 0);
  }
  // ---------------
  double conv_ratio = 1.0e-6;
  int iteration = 10000;
  {
    const unsigned int max_itr = 10;
    const auto time0 = std::chrono::system_clock::now();
    unsigned int nitr1 = 0;
    for(int itr=0;itr<max_itr;++itr){ // CG method std::vector
      const std::size_t n = Vstd.size();
      std::vector<double> tmp0(n), tmp1(n), vx1(n), vB1(n);
      vB1 = Vstd;
      std::vector<double> aConv = Solve_CG(
          dfm2::ViewAsVectorXd(vB1),
          dfm2::ViewAsVectorXd(vx1),
          dfm2::ViewAsVectorXd(tmp0),
          dfm2::ViewAsVectorXd(tmp1),
          conv_ratio, iteration, Astd);
      nitr1 = aConv.size();
    }
    const auto time1 = std::chrono::system_clock::now();
    // ---------------
    unsigned int nitr0 = 0;
    for(int itr=0;itr<max_itr;++itr){ // CG method with eigen
      const std::size_t n = Veig0.size();
      Eigen::VectorXd tmp0(n), tmp1(n), vx0(n), vB0(n);
      vB0 = Veig0;
      std::vector<double> aConv = delfem2::Solve_CG(
          vB0, vx0, tmp0, tmp1,
          conv_ratio, iteration, Aeig);
      nitr0 = aConv.size();
    }
    const auto time2 = std::chrono::system_clock::now();
    // ---------------
    unsigned int nitr2 = 0;
    for(int itr=0;itr<max_itr;++itr){ // CG method with eigen
      Eigen::Matrix<double,-1,2,Eigen::RowMajor> tmp0(np,2), tmp1(np,2), vx0(np,2), vB0(np,2);
      vB0 = Veig1;
      std::vector<double> aConv = delfem2::Solve_CG(
          vB0, vx0, tmp0, tmp1,
          conv_ratio, iteration, Aeig);
      nitr2 = aConv.size();
    }
    const auto time3 = std::chrono::system_clock::now();
    EXPECT_NEAR(nitr0,nitr1,10);
    EXPECT_NEAR(nitr1,nitr2,10);
    long long elapsed01 = std::chrono::duration_cast<std::chrono::milliseconds>(time1 - time0).count();
    long long elapsed12 = std::chrono::duration_cast<std::chrono::milliseconds>(time2 - time1).count();
    long long elapsed23 = std::chrono::duration_cast<std::chrono::milliseconds>(time3 - time2).count();
    std::cout << "cg std::vector: " << elapsed01 << " " << nitr0 << std::endl;
    std::cout << "cg eigen vec: " << elapsed12 << " " << nitr1 << std::endl;
    std::cout << "cg eigen mat: " << elapsed23 << " " << nitr2 << std::endl;
  }
  delfem2::CILU_SparseBlock<Eigen::Matrix2d,Eigen::aligned_allocator<Eigen::Matrix2d>> ilu0;
  delfem2::CPreconditionerILU<double> ilu1;
  { // LU
    ilu1.SetPattern0(Astd);
    ilu1.CopyValue(Astd);
    ilu1.Decompose();
    delfem2::ILU_SetPattern0(ilu0, Aeig);
    delfem2::ILU_CopyValue(ilu0, Aeig);
    delfem2::ILU_Decompose(ilu0);
    // check if the entry is the same
    for(unsigned int icrs=0;icrs<ilu0.valCrs.size();++icrs) {
      EXPECT_NEAR(ilu0.valCrs[icrs](0, 0), ilu1.valCrs[icrs * 4 + 0], epsilon);
      EXPECT_NEAR(ilu0.valCrs[icrs](0, 1), ilu1.valCrs[icrs * 4 + 1], epsilon);
      EXPECT_NEAR(ilu0.valCrs[icrs](1, 0), ilu1.valCrs[icrs * 4 + 2], epsilon);
      EXPECT_NEAR(ilu0.valCrs[icrs](1, 1), ilu1.valCrs[icrs * 4 + 3], epsilon);
    }
    for(unsigned int iblk=0;iblk<ilu0.valDia.size();++iblk) {
      EXPECT_NEAR(ilu0.valDia[iblk](0, 0), ilu1.valDia[iblk * 4 + 0], epsilon);
      EXPECT_NEAR(ilu0.valDia[iblk](0, 1), ilu1.valDia[iblk * 4 + 1], epsilon);
      EXPECT_NEAR(ilu0.valDia[iblk](1, 0), ilu1.valDia[iblk * 4 + 2], epsilon);
      EXPECT_NEAR(ilu0.valDia[iblk](1, 1), ilu1.valDia[iblk * 4 + 3], epsilon);
    }
  }
  {
    const auto time0 = std::chrono::system_clock::now();
    const unsigned int max_itr = 10;
    unsigned int nitr1 = 0;
    for (int itr = 0; itr < max_itr; ++itr) { // solve with ILU-CG std::vector
      ilu1.CopyValue(Astd);
      ilu1.Decompose();
      const std::size_t n = Vstd.size();
      std::vector<double> tmp0(n), tmp1(n), vx1(n), vB1(n);
      vB1 = Vstd;
      std::vector<double> aConv = Solve_PCG(
          dfm2::ViewAsVectorXd(vB1),
          dfm2::ViewAsVectorXd(vx1),
          dfm2::ViewAsVectorXd(tmp0),
          dfm2::ViewAsVectorXd(tmp1),
          conv_ratio, iteration, Astd, ilu1);
      nitr1 = aConv.size();
    }
    const auto time1 = std::chrono::system_clock::now();
    unsigned int nitr0 = 0;
    for (int itr = 0; itr < max_itr; ++itr) { // solve with ILU-CG Eigen
      delfem2::ILU_CopyValue(ilu0, Aeig);
      delfem2::ILU_Decompose(ilu0);
      const std::size_t n = Vstd.size();
      Eigen::VectorXd tmp0(n), tmp1(n), vx0(n), vB0(n);
      vB0 = Veig0;
      std::vector<double> aConv = Solve_PCG(
          vB0, vx0, tmp0, tmp1,
          conv_ratio, iteration, Aeig, ilu0);
      nitr0 = aConv.size();
    }
    const auto time2 = std::chrono::system_clock::now();
    unsigned int nitr2 = 0;
    for (int itr = 0; itr < max_itr; ++itr) { // solve with ILU-CG Eigen
      delfem2::ILU_CopyValue(ilu0, Aeig);
      delfem2::ILU_Decompose(ilu0);
      Eigen::Matrix<double,-1,2,Eigen::RowMajor> tmp0(np,2), tmp1(np,2), vx0(np,2), vB0(np,2);
      vB0 = Veig1;
      std::vector<double> aConv = Solve_PCG(
          vB0, vx0, tmp0, tmp1,
          conv_ratio, iteration, Aeig, ilu0);
      nitr2 = aConv.size();
    }
    const auto time3 = std::chrono::system_clock::now();
    EXPECT_NEAR(nitr0,nitr1,10);
    EXPECT_NEAR(nitr1,nitr2,10);
    double elapsed01 = std::chrono::duration_cast<std::chrono::milliseconds>(time1 - time0).count();
    double elapsed12 = std::chrono::duration_cast<std::chrono::milliseconds>(time2 - time1).count();
    double elapsed23 = std::chrono::duration_cast<std::chrono::milliseconds>(time3 - time2).count();
    std::cout << "ilu-cg std::vector: " << elapsed01 << " " << nitr0 << std::endl;
    std::cout << "ilu-cg eigen vec: " << elapsed12 << " " << nitr1 << std::endl;
    std::cout << "ilu-cg eigen mat: " << elapsed23 << " " << nitr2 << std::endl;
  }
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-exponential
    Function object implementing nthroot capabilities

    nth root function: \f$\sqrt[n]{x}\f$
    \arg n must be of integer type
    \arg if n is even and x negative the result is @ref Nan
    \arg if x is null the result is @ref Zero
    \arg if x is one  the result is @ref One

    @par Semantic:

    For every parameters of  floating type T and integral type N:

    @code
    T r = nthroot(x, n);
    @endcode

    is similar to:

    @code
    T r = n >= 0 ? pow(x, rec(tofloat(n))) : Nan;
    @endcode

    @par Note:
    nthroot is more expansive than pow(x, rec(tofloat(n))) because
    it takes care of some limits issues that @ref pow does not mind of.

    See if it suits you better.

    @see pow, rec, sqrt, cbrt

  **/
  const boost::dispatch::functor<tag::nthroot_> nthroot = {};
} }
#endif

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

#endif


/*
 * 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 <boost/numeric/ublas/matrix.hpp>
#include <math.h>       /* sqrt */
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

/**
*
* ublas binding for gsl_eigen_symmv
* note that the eigenvalues/eigenvectors are UNSORTED
*
*/
bool linalg_eigenvalues_symmetric( ub::symmetric_matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	//gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_ABS_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symmv
* input matrix type general matrix!
* wrapping gsl_eigen_symmv
*
*/
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symmv
* input matrix type general matrix single precision!
* wrapping gsl_eigen_symmv
*
*/
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle symmetric_matrix and floats, so this is super stupid
        ub::matrix<double> _A( N,N );
        _A = A;
        ub::vector<double> _E(N);
        ub::matrix<double> _V(N,N);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&_E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&_V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	//E.resize(N, false);
	//V.resize(N, N, false);
        E = _E;
        V = _V;

	return (status != 0);
};

bool linalg_eigenvalues(  ub::vector<float> &E, ub::matrix<float> &V)
{
        /* on input V is the matrix that shall be diagonalized
         * GSL does not provide an in-place routine, so we wrap
         * gsl_eigen_symmv for compatibility
         */

         // make a copy of E
         ub::matrix<float> A = V;

         // now call wrapper for gsl_eigen_symmv
         bool status = linalg_eigenvalues( A , E, V );
	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symm
* input matrix type general matrix!
* wrapping gsl_eigen_symm leaves input matrix
*
*/
bool linalg_eigenvalues( ub::vector<double> &E, ub::matrix<double> &V)
{
        /* on input V is the matrix that shall be diagonalized
         * GSL does not provide an in-place routine, so we wrap
         * gsl_eigen_symmv for compatibility
         */

         // make a copy of E
         ub::matrix<double> A = V;

         // now call wrapper for gsl_eigen_symmv
         bool status = linalg_eigenvalues( A , E, V );

         return status;
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V , int nmax)
{
    throw std::runtime_error("linalg_eigenvalues is not compiled-in due to disabling of MKL - recompile Votca Tools with MKL support");
}

/*
 * use expert routine to calculate only a subrange of eigenvalues single precision
 */
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V , int nmax)
{
    // now call wrapper for gsl_eigen_symmv
    bool status = linalg_eigenvalues( A , E, V );

    return status;
}

bool linalg_eigenvalues_general( ub::matrix<double> &A,ub::matrix<double> &B, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl destroys A so use copy
        ub::matrix<double> _A( N,N );
        _A = A;

        ub::matrix<double> _B( N,N );
        _B=B;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
        gsl_matrix_view B_view = gsl_matrix_view_array(&_B(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_gensymmv_workspace *w = gsl_eigen_gensymmv_alloc(N);

	int status = gsl_eigen_gensymmv(&A_view.matrix,&B_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_gensymmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_gensymmv_free(w);
	gsl_set_error_handler(handler);

        ub::matrix<double> _temp= ub::prod(B,V);
        ub::matrix<double> n=ub::prod(ub::trans(V),_temp);
      /*
        for (int i=0;i<n.size1();i++){

                for (int j=0;j<n.size2();j++){
                cout <<"n("<< i << ":"<< j <<")= " <<n(i,j)<< endl;
                }}

       */

        for (unsigned int i=0;i<n.size1();i++){
            ub::matrix_range<ub::matrix<double> > column=ub::subrange( V, 0, V.size2(),i, i+1 );
            //cout <<"n("<< i << ":"<< i <<")= " <<n(i,i) <<":" <<sqrt(n(i,i))<< endl;
            //for (int j=0;j<column.size1();j++){

            //cout <<"V("<<i<<":"<<j<<")="<<column(j,0)<< endl;

              //}
            column=column/sqrt(n(i,i));

        }

	return (status != 0);
};

}}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/format.hpp>

typedef boost::numeric::ublas::matrix<float> Matrix44;

void Out(const char *const message)
{

}

int main()
{
	Matrix44 matrix = boost::numeric::ublas::identity_matrix<float> (4);

	Out(std::cout << matrix);
	//std::cout << matrix << std::endl;

	return 0;
}


#include <iostream>

#include <Eigen/Core>

#include "lib/utilities.h"
#include "lib/pose_estimation.h"

#define DEBUG

using namespace std;

bool testQuaternionFromMatrix() {
  Eigen::Matrix4f mat;
  mat << 0, 0, 1, 0,
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 0, 1;
  Eigen::Quaternion<float> q;
  Utilities::quaternionFromMatrix(mat, q);
  cout << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " " << endl;
}

bool testCVRotatedRect() {
  PointCloudMono::Ptr hull(new PointCloudMono);
  hull->points.resize(4);
//  pcl::PointXYZ v0{1, 2, 0};
//  pcl::PointXYZ v1{1, -2, 0};
//  pcl::PointXYZ v2{-1, -2, 0};
//  pcl::PointXYZ v3{-1, 2, 0};

  // 70 deg
  pcl::PointXYZ v0{-1.5373651,  1.62373291, 0};
  pcl::PointXYZ v1{2.22140538, 0.25565233, 0};
  pcl::PointXYZ v2{1.5373651, -1.62373291, 0};
  pcl::PointXYZ v3{-2.22140538, -0.25565233, 0};

  hull->points[0] = v0;
  hull->points[1] = v1;
  hull->points[2] = v2;
  hull->points[3] = v3;

  vector<pcl::PointXY> rect;
  pcl::PointXY center{};
  pcl::PointXY edge_center{};
  float width, height, rotation;
  Utilities::getRotatedRect2D(hull, rect,center,edge_center,width,height,rotation);
}

int main(int argc, char **argv)
{
  PointCloudMono::Ptr contour(new PointCloudMono);
  contour->points.resize(4);
  contour->points[0].x = 1;
  contour->points[0].y = 1;
  contour->points[0].z = -1;
  contour->points[1].x = 1;
  contour->points[1].y = -1;
  contour->points[1].z = -1;
  contour->points[2].x = -1;
  contour->points[2].y = -1;
  contour->points[2].z = -1;
  contour->points[3].x = -1;
  contour->points[3].y = 1;
  contour->points[3].z = -1;

  pcl::PointXY p{0.9, 0};
  bool ok = Utilities::isInContour(contour, p);
  cout << "res 1: " << ok << endl;  // should be true

  p.x = 2;
  p.y = 0;
  ok = Utilities::isInContour(contour, p);
  cout << "res 2: " << ok << endl;  // should be false

  p.x = 1;
  p.y = 0;
  ok = Utilities::isInContour(contour, p);
  cout << "res 3: " << ok << endl;  // should be true

  testQuaternionFromMatrix();

  testCVRotatedRect();

//  float dsp_th = 0.005f;
//  PoseEstimation *pe = new PoseEstimation(dsp_th);
//  std::string scene_path = "/home/dzp/scene.pcd";
//  PointCloudN::Ptr scene_cloud(new PointCloudN);
//  pcl::io::loadPCDFile<PointN>(scene_path, *scene_cloud);
//
//  Eigen::Matrix4f trans;
//  pe->estimate(scene_cloud, trans, true);

  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
  Matrix3f m = Matrix3f::Random();
  std::ptrdiff_t i, j;
  float minOfM = m.minCoeff(&i,&j);
  cout << "Here is the matrix m:\n" << m << endl;
  cout << "Its minimum coefficient (" << minOfM
       << ") is at position (" << i << "," << j << ")\n\n";

  RowVector4i v = RowVector4i::Random();
  int maxOfV = v.maxCoeff(&i);
  cout << "Here is the vector v: " << v << endl;
  cout << "Its maximum coefficient (" << maxOfV
       << ") is at position " << i << endl;

}
  return 0;
}


//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/config.hpp>
#include <set>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edge_connectivity.hpp>

using namespace boost;

int
main()
{
  const int N = 8;
  typedef adjacency_list<vecS, vecS, undirectedS> UndirectedGraph;
  UndirectedGraph g(N);

  add_edge(0, 1, g);
  add_edge(0, 2, g);
  add_edge(0, 3, g);
  add_edge(1, 2, g);
  add_edge(1, 3, g);
  add_edge(2, 3, g);
  add_edge(3, 4, g);
  add_edge(3, 7, g);
  add_edge(4, 5, g);
  add_edge(4, 6, g);
  add_edge(4, 7, g);
  add_edge(5, 6, g);
  add_edge(5, 7, g);
  add_edge(6, 7, g);

  typedef graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
  typedef graph_traits<UndirectedGraph>::degree_size_type degree_size_type;
  std::vector<edge_descriptor> disconnecting_set;

  degree_size_type c = edge_connectivity(g, std::back_inserter(disconnecting_set));

  std::cout << "The edge connectivity is " << c << "." << std::endl;
  std::cout << "The disconnecting set is {";

  std::copy(disconnecting_set.begin(), disconnecting_set.end(),
            std::ostream_iterator<edge_descriptor>(std::cout, " "));
  std::cout << "}." << std::endl;

  return 0;
}


#ifndef KRAVERAGER_H
#define KRAVERAGER_H

#include <iostream>

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

/**
 * @class KRAverager
 */
class KRAverager {
    public:
        KRAverager();

        /**
         * @param threshold
         */
        KRAverager(int threshold);

        /**
         * @param force_constant
         * @param range
         */
        virtual void add_force_constant_tuple(double force_constant, double range);

        /**
         * @param force_constants
         * @param ranges
         */
        virtual void add_force_constant_vector(const Eigen::VectorXd &force_constants, const Eigen::VectorXd &ranges);

        /**
         * @return the average range
         */
        double get_average_range() const;

        /**
         * @return the average force constant
         */
        double get_average_force_constant() const;

        /**
         * @return first error
         */
        double get_error_1() const;

        /**
         * @return second error
         */
        double get_error_2() const;
    protected:
        int threshold;
        bool use_threshold = false;
        double r_sum = 0;
        double k_sum = 0;
        double error_sum = 0;
        long count = 0;
};

/**
 * @class KRAveragerCis
 */
class KRAveragerCis : public KRAverager {
    public:
        KRAveragerCis() : KRAverager() {};
        KRAveragerCis(int threshhold) : KRAverager(threshhold) {};

        /**
         * @param force_constant
         * @param range
         */
        void add_force_constant_tuple(double k, double r);

        /**
         * @param force_constants
         * @param ranges
         */
        void add_force_constant_vector(const Eigen::VectorXd & ks, const Eigen::VectorXd & rs);

        /**
         * @return range cis
         */
        double get_range_cis() const;

        /**
         * @return force constant cis
         */
        double get_force_constant_cis() const;
    private:
        double cis_count = 0;
        double r_cis_sum = 0;
        double k_cis_sum = 0;
};

#endif

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4


#include <array>
#include <vector>
#include "plane3d.hpp"
#include "found_path.hpp"
#include "mesh_components.hpp"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

#ifndef MESH_HPP
#define MESH_HPP

class Mesh {
    vector<Vertex3d> vertices;
    vector<Tetrahedron> tetrahedrons;
    vector<Face> faces;
    Vector3d target;
    unsigned long int targetTetId;
    //used for testing
    vector<int> sliceIds;
  public:
    Mesh (const int numVertices, const int numFaces, const int numCells);
    void setVertices(const vector<array<double, 3>> & _vertices);
    void addTetrahedron(const int id, const array<int, 4> vertexIds, const vector<unsigned long int> neighborIds, const double weight, const int label);
    void addFace(const array<int, 3> vertexIds, const int tetId);
    bool setTarget(const array<double, 3> _target);
    vector<Shape3d> slice(Plane3d plane, vector<int> &tetsChecked);
    vector<FoundPath> shortestPaths(const int epsilon, const int numThreads, double distBound);
    vector<Shape3d> computeSliceComponent(Plane3d plane, vector<int> &tetsChecked, unsigned long int initTet);
    vector<Shape3d> sliceIndv(array<double, 2> rotation);
    vector<FoundPath> findPaths(vector<Plane3d> planes, double distBound);
    //these functions are exposed to Python for testing purposes; while it is possible that a python application may require them, they are not required for any other purpose in this application
    unsigned long int getTargetTetId();
};

#endif


///////////////////////////////////////////////////////////////////////////////
// standard_error_iid.hpp                                                    //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_STANDARD_ERROR_IID_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_STANDARD_ERROR_IID_HPP_ER_2008_04
#include <cmath>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
//#include <boost/assert.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/accumulators/statistics/acv0.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // standard_error_iid
    template<typename T,typename I>
    class standard_error_iid_impl
      : public accumulator_base
    {
    public:
        typedef T result_type;

        standard_error_iid_impl(dont_care)
        {}

        template<typename Args>
        void operator()(Args const &args)
        {
            T acv0_val = acv0<I>(args[accumulator]);
            T n = (T)(count(args));
            val = static_cast<T>(0);
            if((acv0_val>static_cast<T>(0)) && (n>static_cast<T>(0))){val = sqrt(acv0_val/n);}
        }

        result_type result(dont_care) const
        {
            return val;
        }
    private:
        T val;
    };

} // namespace impl
///////////////////////////////////////////////////////////////////////////////
// tag::standard_error_iid
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct standard_error_iid
      : depends_on<count,acv0<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::standard_error_iid_impl<
            mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::standard_error_iid
//

namespace extract
{

//  extractor<tag::standard_error_iid<> >
//    const standard_error_iid = {};

  // see acvf about default_delay_discriminator
  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::standard_error_iid<I>
    >::type::result_type
  standard_error_iid(AccumulatorSet const& acc){
    typedef tag::standard_error_iid<I> the_tag;
    return extract_result<the_tag>(acc);
  }

//  TODO
//  overload (default) see acvf

}

using extract::standard_error_iid;

}}

#endif


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef std::vector<double>                                    knot_container;
typedef std::vector<vector_type>                               point_container;

typedef OpenTissue::spline::NUBSpline<knot_container, point_container> spline_type;

BOOST_AUTO_TEST_SUITE(opentissue_spline_make_cubic_interpolation);

BOOST_AUTO_TEST_CASE(test_make_cubic_interpolation)
{
  point_container X;

  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 0.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 0.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;

  X.push_back(p0);
  X.push_back(p1);
  X.push_back(p2);
  X.push_back(p3);

  knot_container U;

  OpenTissue::spline::compute_chord_length_knot_vector(4, X, U);

  spline_type spline = OpenTissue::spline::make_cubic_interpolation( U, X );

  BOOST_CHECK( spline.get_order() == 4 );

  BOOST_CHECK( spline.get_knot_container().size() == 10 );
  BOOST_CHECK( spline.get_knot_container().size() == (spline.get_control_container().size()+4) );
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include <Eigen/Core>
#include <array>
#include <csim/type.hpp>
#include <string>

#ifdef __GNUC__
#if __GNUC__ >= 8
using namespace std::complex_literals;
#endif
#endif

// random
static UINT rand_int(UINT max) { return rand() % max; }

static double rand_real() { return (rand() % RAND_MAX) / ((double)RAND_MAX); }

// obtain single dense matrix
static Eigen::MatrixXcd get_eigen_matrix_single_Pauli(UINT pauli_id) {
    Eigen::MatrixXcd mat(2, 2);
    if (pauli_id == 0)
        mat << 1, 0, 0, 1;
    else if (pauli_id == 1)
        mat << 0, 1, 1, 0;
    else if (pauli_id == 2)
        mat << 0, -1.i, 1.i, 0;
    else if (pauli_id == 3)
        mat << 1, 0, 0, -1;
    return mat;
}

static Eigen::MatrixXcd get_eigen_matrix_random_single_qubit_unitary() {
    Eigen::MatrixXcd Identity, X, Y, Z;
    Identity = get_eigen_matrix_single_Pauli(0);
    X = get_eigen_matrix_single_Pauli(1);
    Y = get_eigen_matrix_single_Pauli(2);
    Z = get_eigen_matrix_single_Pauli(3);

    double icoef, xcoef, ycoef, zcoef, norm;
    icoef = rand_real();
    xcoef = rand_real();
    ycoef = rand_real();
    zcoef = rand_real();
    norm = sqrt(icoef * icoef + xcoef + xcoef + ycoef * ycoef + zcoef * zcoef);
    icoef /= norm;
    xcoef /= norm;
    ycoef /= norm;
    zcoef /= norm;
    return icoef * Identity + 1.i * xcoef * X + 1.i * ycoef * Y +
           1.i * zcoef * Z;
}

static Eigen::VectorXcd get_eigen_diagonal_matrix_random_multi_qubit_unitary(
    UINT qubit_count) {
    ITYPE dim = (1ULL) << qubit_count;
    auto vec = Eigen::VectorXcd(dim);
    for (ITYPE i = 0; i < dim; ++i) {
        double angle = rand_real() * 2 * 3.14159;
        vec[i] = cos(angle) + 1.i * sin(angle);
    }
    return vec;
}

// expand matrix
static Eigen::MatrixXcd kronecker_product(
    const Eigen::MatrixXcd& lhs, const Eigen::MatrixXcd& rhs) {
    Eigen::MatrixXcd result(lhs.rows() * rhs.rows(), lhs.cols() * rhs.cols());
    for (int i = 0; i < lhs.cols(); i++) {
        for (int j = 0; j < lhs.rows(); j++) {
            result.block(i * rhs.rows(), j * rhs.cols(), rhs.rows(),
                rhs.cols()) = lhs(i, j) * rhs;
        }
    }
    return result;
}

static Eigen::MatrixXcd get_expanded_eigen_matrix_with_identity(
    UINT target_qubit_index, const Eigen::MatrixXcd& one_qubit_matrix,
    UINT qubit_count) {
    const ITYPE left_dim = 1ULL << target_qubit_index;
    const ITYPE right_dim = 1ULL << (qubit_count - target_qubit_index - 1);
    auto left_identity = Eigen::MatrixXcd::Identity(left_dim, left_dim);
    auto right_identity = Eigen::MatrixXcd::Identity(right_dim, right_dim);
    return kronecker_product(
        kronecker_product(right_identity, one_qubit_matrix), left_identity);
}

// get expanded matrix

static Eigen::MatrixXcd get_eigen_matrix_full_qubit_pauli(
    std::vector<UINT> pauli_ids) {
    Eigen::MatrixXcd result = Eigen::MatrixXcd::Identity(1, 1);
    for (UINT i = 0; i < pauli_ids.size(); ++i) {
        result = kronecker_product(
            get_eigen_matrix_single_Pauli(pauli_ids[i]), result)
                     .eval();
    }
    return result;
}

static Eigen::MatrixXcd get_eigen_matrix_full_qubit_pauli(
    std::vector<UINT> index_list, std::vector<UINT> pauli_list,
    UINT qubit_count) {
    std::vector<UINT> whole_pauli_ids(qubit_count, 0);
    for (UINT i = 0; i < index_list.size(); ++i) {
        whole_pauli_ids[index_list[i]] = pauli_list[i];
    }
    return get_eigen_matrix_full_qubit_pauli(whole_pauli_ids);
}

static Eigen::MatrixXcd get_eigen_matrix_full_qubit_CNOT(
    UINT control_qubit_index, UINT target_qubit_index, UINT qubit_count) {
    ITYPE dim = 1ULL << qubit_count;
    Eigen::MatrixXcd result = Eigen::MatrixXcd::Zero(dim, dim);
    for (ITYPE ind = 0; ind < dim; ++ind) {
        if (ind & (1ULL << control_qubit_index)) {
            result(ind, ind ^ (1ULL << target_qubit_index)) = 1;
        } else {
            result(ind, ind) = 1;
        }
    }
    return result;
}

static Eigen::MatrixXcd get_eigen_matrix_full_qubit_CZ(
    UINT control_qubit_index, UINT target_qubit_index, UINT qubit_count) {
    ITYPE dim = 1ULL << qubit_count;
    Eigen::MatrixXcd result = Eigen::MatrixXcd::Zero(dim, dim);
    for (ITYPE ind = 0; ind < dim; ++ind) {
        if ((ind & (1ULL << control_qubit_index)) != 0 &&
            (ind & (1ULL << target_qubit_index)) != 0) {
            result(ind, ind) = -1;
        } else {
            result(ind, ind) = 1;
        }
    }
    return result;
}

static Eigen::MatrixXcd get_eigen_matrix_full_qubit_SWAP(
    UINT target_qubit_index1, UINT target_qubit_index2, UINT qubit_count) {
    ITYPE dim = 1ULL << qubit_count;
    Eigen::MatrixXcd result = Eigen::MatrixXcd::Zero(dim, dim);
    for (ITYPE ind = 0; ind < dim; ++ind) {
        bool flag1, flag2;
        flag1 = (ind & (1ULL << target_qubit_index1)) != 0;
        flag2 = (ind & (1ULL << target_qubit_index2)) != 0;
        if (flag1 ^ flag2) {
            result(ind, ind ^ (1ULL << target_qubit_index1) ^
                            (1ULL << target_qubit_index2)) = 1;
        } else {
            result(ind, ind) = 1;
        }
    }
    return result;
}

// utils
static std::string convert_CTYPE_array_to_string(
    const CTYPE* state, ITYPE dim) {
    std::string str = "";
    for (ITYPE ind = 0; ind < dim; ++ind) {
        str += "(" + std::to_string(_creal(state[ind])) + "," +
               std::to_string(_cimag(state[ind])) + ") ";
    }
    return str;
}

static Eigen::VectorXcd convert_CTYPE_array_to_eigen_vector(
    const CTYPE* state, ITYPE dim) {
    Eigen::VectorXcd vec(dim);
    for (ITYPE i = 0; i < dim; ++i) vec[i] = state[i];
    return vec;
}

static void state_equal(const CTYPE* state, const Eigen::VectorXcd& test_state,
    ITYPE dim, std::string gate_string) {
    const double eps = 1e-14;
    Eigen::VectorXcd vec = convert_CTYPE_array_to_eigen_vector(state, dim);
    for (ITYPE ind = 0; ind < dim; ++ind) {
        ASSERT_NEAR(abs(vec[ind] - test_state[ind]), 0, eps)
            << gate_string << " at " << ind << std::endl
            << "Eigen : " << test_state.transpose() << std::endl
            << "CSIM : " << convert_CTYPE_array_to_string(state, dim)
            << std::endl;
    }
}

#define _CHECK_NEAR(val1, val2, eps) \
    _check_near(val1, val2, eps, #val1, #val2, #eps, __FILE__, __LINE__)
static std::string _check_near(double val1, double val2, double eps,
    std::string val1_name, std::string val2_name, std::string eps_name,
    std::string file, UINT line) {
    double diff = std::abs(val1 - val2);
    if (diff <= eps) return "";
    std::stringstream error_message_stream;
    error_message_stream << file << ":" << line << " Failure\n"
                         << "The difference between " << val1_name << " and "
                         << val2_name << " is " << diff << ", which exceeds "
                         << eps_name << ", where\n"
                         << val1_name << " evaluates to " << val1 << ",\n"
                         << val2_name << " evaluates to " << val2 << ", and\n"
                         << eps_name << " evaluates to " << eps << ".\n";
    return error_message_stream.str();
}


// unit test file acosh.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <functional>
#include <iomanip>
#include <iostream>

#include <boost/math/special_functions/acosh.hpp>

#include <boost/test/unit_test.hpp>

template<typename T>
T    acosh_error_evaluator(T x)
{
    using    ::std::abs;
    using    ::std::sinh;
    using    ::std::cosh;

    using    ::std::numeric_limits;

    using    ::boost::math::acosh;

    static T const    epsilon = numeric_limits<float>::epsilon();

    T                y = cosh(x);
    T                z = acosh(y);

    T                absolute_error = abs(z-abs(x));
    T                relative_error = absolute_error*abs(sinh(x));
    T                scaled_error = relative_error/epsilon;

    return(scaled_error);
}

template<typename T>
void    acosh_test(const char * more_blurb)
{
    BOOST_MESSAGE("Testing acosh in the real domain for "
        << more_blurb << ".");

    for    (int i = 0; i <= 100; i++)
    {
        T    x = static_cast<T>(i-50)/static_cast<T>(5);

        BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
            (
                acosh_error_evaluator(x),
                static_cast<T>(4)
            ));
    }
}

void    acosh_manual_check()
{
    BOOST_MESSAGE("acosh");

    for    (int i = 0; i <= 100; i++)
    {
        float        xf = static_cast<float>(i-50)/static_cast<float>(5);
        double       xd = static_cast<double>(i-50)/static_cast<double>(5);
        long double  xl =
                static_cast<long double>(i-50)/static_cast<long double>(5);

        BOOST_MESSAGE(  ::std::setw(15)
                     << acosh_error_evaluator(xf)
                     << ::std::setw(15)
                     << acosh_error_evaluator(xd)
                     << ::std::setw(15)
                     << acosh_error_evaluator(xl));
    }

    BOOST_MESSAGE(" ");
}


#ifndef WAGNER_COMMON_H_
#define WAGNER_COMMON_H_

#include <cmath>

#ifndef WAGNER_NOBOOST
  #include <boost/container/flat_set.hpp>
  #include <boost/container/flat_map.hpp>
#else
  #include <set>
  #include <map>
#endif

namespace wagner {

#ifndef WAGNER_NOBOOST
  template<typename Key>
  using set = boost::container::flat_set<Key>;

  template<typename Key, typename Value>
  using map = boost::container::flat_map<Key, Value>;
#else
  template<typename Key>
  using set = std::set<Key>;

  template<typename Key, typename Value>
  using map = std::map<Key, Value>;
#endif

constexpr size_t wagner_version = 2;
constexpr size_t wagner_revision = 0;

/** Mathematical constant e. */
#define math_e 2.71828182845904523536

/** Mathematical constant pi. */
#define math_pi 3.14159265358979323846

/** Log 2 (very important in information theory). */
#define log2(x) (log(x) / log(2.0))

/** Check if a number is a power of two. */
#define power_of_two(n) (((n) != 0) && !((n) & ((n)-1)))

/** Cubic root. */
#define cbrt(x) (pow((x), 1.0 / 3.0))

/** Max of two values. */
#define max2(a, b) ((a) > (b) ? (a) : (b))

/** Min of two values. */
#define min2(a, b) ((a) < (b) ? (a) : (b))

/** Max of three values. */
#define max3(a, b, c) (max2(a, b) > (c) ? max2(a, b) : (c))

/** Min of three values. */
#define min3(a, b, c) (min2(a, b) < (c) ? min2(a, b) : (c))

/** Max of four values. */
#define max4(a, b, c, d) (max3(a, b, c) > (d) ? max3(a, b, c) : (d))

/** Min of four values. */
#define min4(a, b, c, d) (min3(a, b, c) < (d) ? min3(a, b, c) : (d))

/** Max of five values. */
#define max5(a, b, c, d, e) (max4(a, b, c, d) > (e) ? max4(a, b, c, d) : (e))

/** Min of five values. */
#define min5(a, b, c, d, e) (min4(a, b, c, d) < (e) ? min4(a, b, c, d) : (e))

}

#endif


#include <Eigen/Core>
#include <Eigen/Array>
USING_PART_OF_NAMESPACE_EIGEN
using namespace std;

// define a custom template unary functor
template<typename Scalar>
struct CwiseClampOp {
  CwiseClampOp(const Scalar& inf, const Scalar& sup) : m_inf(inf), m_sup(sup) {}
  const Scalar operator()(const Scalar& x) const { return x<m_inf ? m_inf : (x>m_sup ? m_sup : x); }
  Scalar m_inf, m_sup;
};

int main(int, char**)
{
  Matrix4d m1 = Matrix4d::Random();
  cout << m1 << endl << "becomes: " << endl << m1.unaryExpr(CwiseClampOp<double>(-0.5,0.5)) << endl;
  return 0;
}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of kernel_parameter_optimizer class.
 */
#include "num_collect/interp/kernel/impl/kernel_parameter_optimizer.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "is_finite.h"
#include "num_collect/interp/kernel/calc_kernel_mat.h"
#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"
#include "num_collect/interp/kernel/impl/auto_regularizer_wrapper.h"
#include "num_collect/interp/kernel/rbf_kernel.h"

TEST_CASE("num_collect::interp::kernel::impl::kernel_parameter_optimizer") {
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::rbf_kernel;
    using num_collect::interp::kernel::impl::auto_regularizer_wrapper;
    using num_collect::interp::kernel::impl::kernel_parameter_optimizer;

    const auto vars = std::vector<double>{0.0, 0.1, 0.2, 0.4, 0.6, 1.0};
    const auto data = Eigen::VectorXd{{0.0, 0.2, 0.4, 0.7, 1.0, 2.0}};

    using kernel_type =
        rbf_kernel<euclidean_distance<double>, gaussian_rbf<double>>;
    auto kernel = kernel_type();

    auto interpolator = auto_regularizer_wrapper<double>();

    SECTION("compute") {
        auto optimizer =
            kernel_parameter_optimizer<kernel_type>(interpolator, kernel);

        optimizer.compute(vars, data);

        kernel.kernel_param(optimizer.opt_param());
        const Eigen::MatrixXd kernel_mat = calc_kernel_mat(kernel, vars);

        interpolator.compute(kernel, vars, data);
        Eigen::VectorXd coeff;
        interpolator.solve(coeff);

        const Eigen::VectorXd retrieved_data = kernel_mat * coeff;
        constexpr double tol_error = 1e-4;
        REQUIRE_THAT(retrieved_data, eigen_approx(data, tol_error));
    }
}


#include <igl/colon.h>
#include <igl/directed_edge_orientations.h>
#include <igl/directed_edge_parents.h>
#include <igl/forward_kinematics.h>
#include <igl/PI.h>
#include <igl/partition.h>
#include <igl/mat_max.h>
#include <igl/lbs_matrix.h>
#include <igl/slice.h>
#include <igl/deform_skeleton.h>
#include <igl/dqs.h>
#include <igl/lbs_matrix.h>
#include <igl/columnize.h>
#include <igl/readDMAT.h>
#include <igl/readOBJ.h>
#include <igl/arap.h>
#include <igl/arap_dof.h>
#include <igl/opengl/glfw/Viewer.h>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <iostream>

#include "tutorial_shared_path.h"

typedef
  std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
  RotationList;

const Eigen::RowVector3d sea_green(70./255.,252./255.,167./255.);
Eigen::MatrixXd V,U,M;
Eigen::MatrixXi F;
Eigen::VectorXi S,b;
Eigen::MatrixXd L;
Eigen::RowVector3d mid;
double anim_t = 0.0;
double anim_t_dir = 0.03;
double bbd = 1.0;
bool resolve = true;
igl::ARAPData arap_data,arap_grouped_data;
igl::ArapDOFData<Eigen::MatrixXd,double> arap_dof_data;
Eigen::SparseMatrix<double> Aeq;

enum ModeType
{
  MODE_TYPE_ARAP = 0,
  MODE_TYPE_ARAP_GROUPED = 1,
  MODE_TYPE_ARAP_DOF = 2,
  NUM_MODE_TYPES = 4
} mode = MODE_TYPE_ARAP;

bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(resolve)
  {
    MatrixXd bc(b.size(),V.cols());
    VectorXd Beq(3*b.size());
    for(int i = 0;i<b.size();i++)
    {
      bc.row(i) = V.row(b(i));
      switch(i%4)
      {
        case 2:
          bc(i,0) += 0.15*bbd*sin(0.5*anim_t);
          bc(i,1) += 0.15*bbd*(1.-cos(0.5*anim_t));
          break;
        case 1:
          bc(i,1) += 0.10*bbd*sin(1.*anim_t*(i+1));
          bc(i,2) += 0.10*bbd*(1.-cos(1.*anim_t*(i+1)));
          break;
        case 0:
          bc(i,0) += 0.20*bbd*sin(2.*anim_t*(i+1));
          break;
      }
      Beq(3*i+0) = bc(i,0);
      Beq(3*i+1) = bc(i,1);
      Beq(3*i+2) = bc(i,2);
    }
    switch(mode)
    {
      default:
        assert("unknown mode");
      case MODE_TYPE_ARAP:
        igl::arap_solve(bc,arap_data,U);
        break;
      case MODE_TYPE_ARAP_GROUPED:
        igl::arap_solve(bc,arap_grouped_data,U);
        break;
      case MODE_TYPE_ARAP_DOF:
      {
        VectorXd L0 = L;
        arap_dof_update(arap_dof_data,Beq,L0,30,0,L);
        const auto & Ucol = M*L;
        U.col(0) = Ucol.block(0*U.rows(),0,U.rows(),1);
        U.col(1) = Ucol.block(1*U.rows(),0,U.rows(),1);
        U.col(2) = Ucol.block(2*U.rows(),0,U.rows(),1);
        break;
      }
    }
    viewer.data().set_vertices(U);
    viewer.data().set_points(bc,sea_green);
    viewer.data().compute_normals();
    if(viewer.core.is_animating)
    {
      anim_t += anim_t_dir;
    }else
    {
      resolve = false;
    }
  }
  return false;
}

bool key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int mods)
{
  switch(key)
  {
    case '0':
      anim_t = 0;
      resolve = true;
      return true;
    case '.':
      mode = (ModeType)(((int)mode+1)%((int)NUM_MODE_TYPES-1));
      resolve = true;
      return true;
    case ',':
      mode = (ModeType)(((int)mode-1)%((int)NUM_MODE_TYPES-1));
      resolve = true;
      return true;
    case ' ':
      viewer.core.is_animating = !viewer.core.is_animating;
      if(viewer.core.is_animating)
      {
        resolve = true;
      }
      return true;
  }
  return false;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/armadillo.obj",V,F);
  U=V;
  MatrixXd W;
  igl::readDMAT(TUTORIAL_SHARED_PATH "/armadillo-weights.dmat",W);
  igl::lbs_matrix_column(V,W,M);

  // Cluster according to weights
  VectorXi G;
  {
    VectorXi S;
    VectorXd D;
    igl::partition(W,50,G,S,D);
  }

  // vertices corresponding to handles (those with maximum weight)
  {
    VectorXd maxW;
    igl::mat_max(W,1,maxW,b);
  }

  // Precomputation for FAST
  cout<<"Initializing Fast Automatic Skinning Transformations..."<<endl;
  // number of weights
  const int m = W.cols();
  Aeq.resize(m*3,m*3*(3+1));
  vector<Triplet<double> > ijv;
  for(int i = 0;i<m;i++)
  {
    RowVector4d homo;
    homo << V.row(b(i)),1.;
    for(int d = 0;d<3;d++)
    {
      for(int c = 0;c<(3+1);c++)
      {
        ijv.push_back(Triplet<double>(3*i + d,i + c*m*3 + d*m, homo(c)));
      }
    }
  }
  Aeq.setFromTriplets(ijv.begin(),ijv.end());
  igl::arap_dof_precomputation(V,F,M,G,arap_dof_data);
  igl::arap_dof_recomputation(VectorXi(),Aeq,arap_dof_data);
  // Initialize
  MatrixXd Istack = MatrixXd::Identity(3,3+1).replicate(1,m);
  igl::columnize(Istack,m,2,L);

  // Precomputation for ARAP
  cout<<"Initializing ARAP..."<<endl;
  arap_data.max_iter = 1;
  igl::arap_precomputation(V,F,V.cols(),b,arap_data);
  // Grouped arap
  cout<<"Initializing ARAP with grouped edge-sets..."<<endl;
  arap_grouped_data.max_iter = 2;
  arap_grouped_data.G = G;
  igl::arap_precomputation(V,F,V.cols(),b,arap_grouped_data);

  // bounding box diagonal
  bbd = (V.colwise().maxCoeff()- V.colwise().minCoeff()).norm();

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(U, F);
  viewer.data().add_points(igl::slice(V,b,1),sea_green);
  viewer.data().show_lines = false;
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core.is_animating = false;
  viewer.core.animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle animation."<<endl<<
    "Press '0' to reset pose."<<endl<<
    "Press '.' to switch to next deformation method."<<endl<<
    "Press ',' to switch to previous deformation method."<<endl;
  viewer.launch();
}


#pragma once

#include <Eigen/Core>
#include <cppad/cppad.hpp> // the CppAD package http://www.coin-or.org/CppAD/
#include <vector>

template <typename Type, typename Derived>
std::vector<Type> Eigen2AD(const Eigen::MatrixBase<Derived>& M)
{
    std::vector<Type> out;
    out.resize(M.size());

    Eigen::Index p = 0;
    for (Eigen::Index j = 0; j < M.cols(); ++j)
        for (Eigen::Index i = 0; i < M.rows(); ++i)
            out[p++] = M(i, j);

    return out;
}

template <typename Type>
std::vector<Type> SumAD(const std::vector<Type>& M1, const std::vector<Type>& M2)
{
    std::vector<Type> out(M1.size());

    for (size_t i = 0; i < M1.size(); ++i)
        out[i] = M1[i] + M2[i];

    return out;
}

template <typename Type>
std::vector<Type> SubAD(const std::vector<Type>& M1, const std::vector<Type>& M2)
{
    std::vector<Type> out(M1.size());

    for (size_t i = 0; i < M1.size(); ++i)
        out[i] = M1[i] - M2[i];

    return out;
}

template <typename Type>
std::vector<Type> ProductAD(const std::vector<Type>& M1, const std::vector<Type>& M2, size_t rowsOut, size_t colsOut = 1)
{
    std::vector<Type> out(rowsOut * colsOut, Type(0));

    size_t m = rowsOut;
    size_t n = M1.size() / rowsOut;
    size_t p = colsOut;
    for (size_t j = 0; j < p; ++j)
        for (size_t i = 0; i < m; ++i)
            for (size_t k = 0; k < n; ++k)
                out[i + j * p] += M1[i + k * m] * M2[k + j * p];

    return out;
}

template <typename Type>
Type DotProductAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    Type sum;

    sum = 0.;
    for (size_t i = 0; i < size_t(v1.size()); ++i)
        sum += v1[i] * v2[i];

    return sum;
}

template <typename Type>
std::vector<Type> CrossAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(3);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0];
    return out;
}

template <typename Type>
std::vector<Type> Cross6AD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(6);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0];
    out[3] = v1[4] * v2[2] - v1[5] * v2[1] + v1[1] * v2[5] - v1[2] * v2[4];
    out[4] = v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5];
    out[5] = v1[3] * v2[1] - v1[4] * v2[0] + v1[0] * v2[4] - v1[1] * v2[3];

    return out;
}

template <typename Type>
std::vector<Type> Cross6DAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(6);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1] + v1[4] * v2[5] - v1[5] * v2[4];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0] + v1[3] * v2[4] - v1[4] * v2[3];
    out[3] = v1[1] * v2[5] - v1[2] * v2[4];
    out[4] = v1[2] * v2[3] - v1[0] * v2[5];
    out[5] = v1[0] * v2[4] - v1[1] * v2[3];

    return out;
}

template <typename Type>
std::vector<Type> CrossAD(const std::vector<Type>& v)
{
    std::vector<Type> out(9);
    out[0] = Type(0);
    out[1] = v[2];
    out[2] = -v[1];
    out[3] = -v[2];
    out[4] = Type(0);
    out[5] = v[0];
    out[6] = v[1];
    out[7] = -v[0];
    out[8] = Type(0);
    return out;
}

template <typename Type>
std::vector<Type> TransposeAD(const std::vector<Type>& M, int rows)
{
    std::vector<Type> out(M.size());
    size_t cols = M.size() / rows;
    size_t p = 0;
    for (size_t j = 0; j < cols; ++j)
        for (size_t i = 0; i < rows; ++i)
            out[p++] = M[j + cols * i];

    return out;
}

template <typename Type>
std::vector<Type> IdMatAD(size_t size)
{
    std::vector<Type> out(size * size, Type(0));
    for (size_t i = 0; i < size; ++i)
        out[i + i * size] = Type(1);

    return out;
}

template <typename Type>
std::vector<Type> exp3x3AD(const std::vector<Type>& ax)
{
    std::vector<Type> out(9, Type(0));
    Type angle = CppAD::sqrt(DotProductAD(ax, ax));
    if (CppAD::abs(angle) < std::numeric_limits<double>::epsilon()) {
        out[0] = 1;
        out[4] = 1;
        out[8] = 1;
    } else {
        Type x = ax[0] / angle;
        Type y = ax[1] / angle;
        Type z = ax[2] / angle;
        Type sa = CppAD::sin(angle);
        Type ca = CppAD::cos(angle);

        out[0] = ca + (1 - ca) * x * x;
        out[1] = (1 - ca) * y * x + sa * z;
        out[2] = (1 - ca) * z * x - sa * y;
        out[3] = (1 - ca) * x * y - sa * z;
        out[4] = ca + (1 - ca) * y * y;
        out[5] = (1 - ca) * z * y + sa * x;
        out[6] = (1 - ca) * x * z + sa * y;
        out[7] = (1 - ca) * y * z - sa * x;
        out[8] = ca + (1 - ca) * z * z;
    }

    return out;
}

template <template <class> typename ADType, typename Type>
std::vector<Type> CastOutAD(const std::vector<ADType<Type>>& in)
{
    size_t s = in.size();
    std::vector<Type> out(s);
    for (size_t i = 0; i < s; ++i)
        out[i] = CppAD::Value(in[i]);

    return out;
}


#pragma once
#include <Eigen/Dense>
#include <random>

// Macro for silencing  unused parameters on demand.
#define SUPPRESS_WARNING(a) (void) a

/**
 * Defining an alias for double. Used so that if we ever want to change
 * this to float (for improved speed, reduced accuracy), this is done
 * one place, as apposed to a project wide refactor.
 * \typedef
 */
using Real = double;

constexpr Real PI = 3.14159265358979323846; /**< Circle constant. */

constexpr Real NUMMERIC_DIFF_STEP
    = 0.001; /**< Default step used in numerical differentiation. */

/**
 * Define various linear algebra types. Note that row-major storage order
 * is used. Eigen should be equally performing with both, but row-major is
 * compatible with NumPy and hence bindings are easy to make.
 */
using Matrix    = Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Array     = Eigen::Array<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Vector    = Eigen::Matrix<Real, Eigen::Dynamic, 1>;
using RowVector = Eigen::Matrix<Real, 1, Eigen::Dynamic>;

/**
 * Corresponding Eigen::Ref bindings for each linear algebra type.
 * Most computations can be used as one of these types, useful for writing
 * generic functions without having to use templates (which can make it
 * difficult writing bindings for).
 */
using MatrixRef    = Eigen::Ref<Matrix>;
using ArrayRef     = Eigen::Ref<Array>;
using VectorRef    = Eigen::Ref<Vector>;
using RowVectorRef = Eigen::Ref<RowVector>;

/**
 * Generic template for computing x * x, for any type. Useful
 * when x is some expression which we do not want to compute twice.
 * @param x Value to multiply with itself.
 * @return Result of `x * x`.
 */
template <typename T>
constexpr auto square(T&& x)
{
    return x * x;
}
template <typename T>
constexpr auto square(const T& x)
{
    return x * x;
}

/** Type safe sign(x). */
template <typename T>
inline constexpr int sign(T x, std::false_type)
{
    return T(0) < x;
}
template <typename T>
inline constexpr int sign(T x, std::true_type)
{
    return (T(0) < x) - (x < T(0));
}
template <typename T>
inline constexpr int sign(T x)
{
    return sign(x, std::is_signed<T>());
}

// Random number generation.
extern std::mt19937_64 rand_gen; /**< Random number generator used. */
extern std::uniform_real_distribution<Real>
    unif; /**< Uniform random distribution (0, 1). */
extern std::uniform_real_distribution<Real>
                                      centered; /**< Uniform random distribution (-.5, .5). */
extern std::normal_distribution<Real> rnorm; /**< N(0, 1) random number distribution. */
extern std::normal_distribution<Real>
    rnorm_small; /**< N(0, 0.1) random number distribution. */

inline auto unif_func()
{
    return unif(rand_gen);
}
inline auto centered_func()
{
    return centered(rand_gen);
}
inline auto rnorm_func()
{
    return rnorm(rand_gen);
}
inline auto rnorm_small_func()
{
    return rnorm_small(rand_gen);
}


#ifndef TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP
#define TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP

#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed Gaussian distribution of exponentials integral
/// (from 0 to time_p <= time)
template <typename T = double>
T pulsed_gauss_dist_exp_integral(T time, T time_p, T nuclear_lifetime,
                                 T slr_rate, T sigma) {
  // make sure that
  assert(time >= time_p);
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    // translated from Maxima
    T expo1 = std::exp(
        boost::math::constants::half<T>() * time * time * sigma * sigma +
        boost::math::constants::half<T>() * t_p * t_p * sigma * sigma +
        t_p * slr_rate);
    T expo2 = std::exp(-time / nuclear_lifetime + t_p / nuclear_lifetime -
                       t_p * time * sigma * sigma);
    T expo3 = std::exp(time * slr_rate);
    return -expo2 *
           (expo1 * std::erf(((time - t_p) * sigma * sigma - slr_rate) /
                             boost::math::constants::root_two<T>() / sigma) -
            expo1) /
           (expo3 * std::erf(slr_rate / sigma /
                             boost::math::constants::root_two<T>()) +
            expo3);
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed Gaussian distribution of exponentials
template <typename T = double>
T pulsed_gauss_dist_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                        T slr_rate, T sigma) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_gauss_dist_exp_integral(time, time, nuclear_lifetime,
                                          slr_rate, sigma) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_gauss_dist_exp_integral(time, pulse_length, nuclear_lifetime,
                                           slr_rate, sigma) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed Gaussian distribution of exponentials (ROOT)
template <typename T = double>
T pulsed_gauss_dist_exp(const T *x, const T *par) {
  return pulsed_gauss_dist_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP


#include "mesh_reader.hpp"
#include "newton_raphson.hpp"
#include "constants.hpp"

#include <tuple>
#include <iostream>
#include <math.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include <chrono>

using namespace boost::numeric::ublas;

/* Configure boost-numeric-bindings in /usr/local to be able to use the compile statement on the next line. */
/* COMPILE WITH: g++ -Wall -std=c++14 -O3 -lstdc++ -o fem.o fem.cpp
mesh_reader.hpp needs those two flags for reading files */

class F {
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    matrix<double> Au_;
    matrix<double> Av_;
    matrix<double> B_;
    matrix<double> C_;
    vector<double> D_;

  public:
    F(matrix<double> Au, matrix<double> Av, matrix<double> B, matrix<double> C, vector<double> D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {

    }

    vector<double> operator()(vector<double> x) {
      /* x = [uT vT]T */
      int n = Au_.size1();
      vector<double> u = project(x,range(0,n));
      vector<double> v = project(x,range(n,2*n));
      vector<double> func(2*n);
      project(func,range(0,n)) = prod(Au_,u) + prod(B_,Ru(u,v)) + hu*(prod(C_,u) - D_*uamb);
      project(func,range(n,2*n)) = -prod(Av_,v) + prod(B_,Rv(u,v)) - hv*(prod(C_,v) - D_*vamb);
      return func;
    }
};

class J {
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    matrix<double> Au_;
    matrix<double> Av_;
    matrix<double> B_;
    matrix<double> C_;
    vector<double> D_;

  public:
    J(matrix<double> Au, matrix<double> Av, matrix<double> B, matrix<double> C, vector<double> D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {

    }

    matrix<double> operator()(vector<double> x) {
      /* x = [uT vT]T */
      int n = Au_.size1();
      vector<double> u = project(x,range(0,n));
      vector<double> v = project(x,range(n,2*n));
      matrix<double> JAC(2*n, 2*n);
      project(JAC,range(0,n),range(0, n)) = Au_ + prod(B_,dRudu(u,v)) + hu*C_;
      project(JAC,range(n,2*n),range(0,n)) = prod(B_,dRudv(u,v));
      project(JAC,range(n,2*n),range(0,n)) = prod(B_,dRvdu(u,v));
      project(JAC,range(n,2*n),range(n,2*n)) = -1*Av_ + prod(B_,dRvdv(u,v)) - hv*C_;
      return JAC;
    }
};

int main() {
  /* Read and store mesh information */
  auto t1 = std::chrono::high_resolution_clock::now();
  matrix<double> vertices = mesh::read_vertices();
  matrix<double> triangles = mesh::read_triangles(vertices);
  matrix<int> boundaries = mesh::read_boundaries(vertices);
  auto t2 = std::chrono::high_resolution_clock::now();
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate righthand side vector integrals */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_B(vertices.size1(), vertices.size1());
  symmetric_adaptor<matrix<double>, lower> B(init_B);
  B = set_zero(B);
  for (unsigned t = 0; t < triangles.size1(); ++t) {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    //std::cout << std::endl;
    B(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    //std::cout<< B(a,a) << std::endl;
    B(b, a) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(c, a) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    //std::cout << "B(b,c): " << B(b,c) << "deel1: "<< area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0)) << std::endl;
    B(b, c) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
    //std::cout << B << std::endl;
  }
  //std::cout << "before division: " << B << std::endl;
  B *= (1./60.);
  //std::cout << "after divison: " << B << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate first part of stiffness matrix A */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_A_U(vertices.size1(), vertices.size1());
  matrix<double> init_A_V(vertices.size1(), vertices.size1());
  matrix<double> I_U(2,2);
  matrix<double> I_V(2,2);
  matrix<double> G(3, 2);
  matrix<double> GGT_U(3, 3);
  matrix<double> GGT_V(3, 3);
  matrix<double> temp(3, 3);
  symmetric_adaptor<matrix<double>, lower> A_U(init_A_U);
  symmetric_adaptor<matrix<double>, lower> A_V(init_A_V);
  I_U = set_zero(I_U);
  I_V = set_zero(I_V);
  A_U = set_zero(A_U);
  A_V = set_zero(A_V);
  I_U(0,0) = DU_R;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  for (unsigned t = 0; t < triangles.size1(); ++t) {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    temp = prod(G,I_U);
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*prod(temp, trans(G));
    temp = prod(G,I_V);
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*prod(temp, trans(G));
    // std::cout << "factor: " << (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6) << std::endl;
    // std::cout << "G: " << G << std::endl;
    // std::cout << "temp: " << temp << std::endl;
    // std::cout << "tempGT: " << prod(temp, trans(G)) << std::endl;
    // std::cout << "GGT_V: " << GGT_V << std::endl;
    A_U(a, a) += GGT_U(0, 0);
    A_U(b, a) += GGT_U(1, 0);
    A_U(c, a) += GGT_U(2, 0);
    A_U(b, b) += GGT_U(1, 1);
    A_U(b, c) += GGT_U(1, 2);
    A_U(c, c) += GGT_U(2, 2);
    A_V(a, a) += GGT_V(0, 0);
    A_V(b, a) += GGT_V(1, 0);
    A_V(c, a) += GGT_V(2, 0);
    A_V(b, b) += GGT_V(1, 1);
    A_V(b, c) += GGT_V(1, 2);
    A_V(c, c) += GGT_V(2, 2);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Boundary condition integrals: second part of A and a constant vector term */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_C(vertices.size1(), vertices.size1());
  symmetric_adaptor<matrix<double>, lower> C(init_C);
  C = set_zero(C);
  vector<double> D(vertices.size1());
  D = set_zero(D);
  // std::cout << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  for (unsigned b = 0; b < boundaries.size1(); ++b) {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    // Nakijken -> 1 vergeten
    D(boundaries(b,0)) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    // std::cout << "D(boundaries(b,0)): " << len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.) << std::endl;
    // std::cout << "b: " << b << std::endl;
    D(boundaries(b,1)) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
    // std::cout << "D(boundaries(b,1)): " << len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.) << std::endl;
    // std::cout << "b: " << len << std::endl;
  }
  // std::cout << "C: "<< C << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Root finding for nonlinear system of equations */
  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  std::cout << "A_U: " << A_U << std::endl;
  std::cout << std::endl;
  std::cout << "A_V: " << A_V << std::endl;
  std::cout << std::endl;
  std::cout << "B: " << B << std::endl;
  std::cout << std::endl;
  std::cout << "C: " << C << std::endl;
  std::cout << std::endl;
  std::cout << "D: " << D << std::endl;
  std::cout << std::endl;
  vector<double> guess = vector<double>(vertices.size1()*2,5.);
  matrix<double> inverse_u_0 (vertices.size1(),vertices.size1());
  inverse_u_0 = set_zero(inverse_u_0);
  InvertMatrix<double>(A_U+(Vmu/Kmu)*B+hu*C, inverse_u_0);
  std::cout << "V/K" << Vmu/Kmu << std::endl;
  std::cout << std::endl;
  std::cout << "hu" << hu << std::endl;
  std::cout << std::endl;
  vector<double> u_0 = prod(inverse_u_0, hu*D*uamb);

  std::cout << std::endl;
  std::cout << std::endl;
  matrix<double> inverse_v_0 (vertices.size1(),vertices.size1());
  inverse_v_0 = set_zero(inverse_v_0);
  InvertMatrix<double>(A_V+hv*C, inverse_v_0);
  vector<double> v_0 = prod(inverse_v_0, rq*(Vmu/Kmu)*prod(B,u_0)+hv*vamb*D);
  std::cout << "v_0" << v_0 << std::endl;
  std::cout << std::endl;
  std::cout << "inverse_v_0" << inverse_v_0 << std::endl;
  std::cout << std::endl;
  std::cout << "second factor" << rq*(Vmu/Kmu)*prod(B,u_0)+hv*vamb*D << std::endl;
  std::cout << std::endl;
  std::cout << std::endl;
  std::cout << "uamb: " << uamb << std::endl;
  std::cout << std::endl;
  std::cout << std::endl;

  project(guess,range(0,vertices.size1())) = u_0;
  project(guess,range(vertices.size1(),2*vertices.size1())) = v_0;
  std::cout << "Initial guess" << guess << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  newton_raphson(F_funct,J_funct,guess,pow(10,-15));
  std::cout << guess << std::endl;

  return 0;
}


#include <cstdio>
#include <iostream>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/tools.h>

using namespace std;
using namespace NTL;

#define REPORT_INTERVAL	100000
#define MAX_Q_BITS 255

/*
  This code modifies [Algorithm 1, BN06] to search for candidate parameters of
  Barreto-Naehrig curves with high 2-adicity. (The curve itself can be explicitly
  constructed by following the second half of [Algorithm 1, BN06].)

  This code was used to find the BN curve in [BCTV13].

  [BCTV13] = "Succinct Non-Interactive Arguments for a von Neumann Architecture"
  [BN06]   = "Pairing-Friendly Elliptic Curves of Prime Order"
*/

int main(int argc, char **argv)
{
    if (argc != 4)
    {
        cout << "usage: " << argv[0] << " [rand_seed even_offset wanted_two_adicity]\n";
        return 0;
    }

    /* Collect inputs */
    ZZ seed;
    conv(seed, atoi(argv[1]));
    long even_offset = atoi(argv[2]);
    long wanted_two_adicity = atoi(argv[3]);

    /* |x| ~ 64 bits so that |q| = 4 * |x| ~ 256 bits */
    long num_x_bits = 63;

    long num_iters = 0;
    long num_found = 0;

    SetSeed(seed);
    while (1)
    {
        ++num_iters;

        /* Sample x */
        ZZ x = RandomLen_ZZ(num_x_bits);

        /**
         * Make x even and divisible by a large power of 2 to improve two adicity.
         * The resulting q is s.t. -1 is a square in Fq.
         */
        x = ((x >> even_offset) << even_offset);

        /* Uncomment to make x odd and ensure that -1 is a nonsquare in Fq. */
        // SetBit(x, 0);

        /**
         * Compute candidate BN parameters using the formulas:
         * t = 6*x^2 + 1,
         * q = 36*x^4 + 36*x^3 + 24*x^2 + 6*x + 1
         * r = q - t + 1
         * (see [BN06])
         */
        ZZ x2 = x * x;
        ZZ x3 = x2 * x;
        ZZ x4 = x3 * x;
        ZZ t = 6 * x2 + 1;
        ZZ q = 36 * x4 + 36 * x3 + 24 * x2 + 6 * x + 1;
        ZZ r = q - t + 1;

        long num_q_bits = NumBits(q);
        long two_adicity = NumTwos(r-1);

        if (num_q_bits > MAX_Q_BITS)
        {
            continue;
        }

        if (ProbPrime(r) && ProbPrime(q) && (two_adicity >= wanted_two_adicity))
        {
            cout << "x = " << x << "\n";
            cout << "q = " << q << "\n";
            cout << "r = " << r << "\n";
            cout << "log2(q) =" << num_q_bits << "\n";
            cout << "ord_2(r-1) = " << two_adicity << "\n";
            cout.flush();
            ++num_found;
        }

        if (num_iters % REPORT_INTERVAL == 0)
        {
            printf("[ num_iters = %ld , num_found = %0.2f per %d ]\n", num_iters, 1.*REPORT_INTERVAL*num_found/num_iters, REPORT_INTERVAL);
            fflush(stdout);
        }
    }
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include "Algorithms.hh"
#include "IsPrimeGenerator.hh"
#include "Typedefs.hh"

namespace cml {

struct RsaPublicKey {
    UnboundedInt e{ 0 };
    UnboundedInt n{ 0 };
};

struct RsaPrivateKey {
    UnboundedInt d{ 0 };
    UnboundedInt n{ 0 };
};

template <typename PrimeGeneratorType>
struct RsaProtocol {
    static_assert(IsPrimeGenerator<PrimeGeneratorType>::value,
                  "Invalid template argument for cml::RsaProtocol: PrimeGeneratorType "
                  "interface is not suitable");

    using PublicKey      = RsaPublicKey;
    using PrivateKey     = RsaPrivateKey;
    using PrimeGenerator = PrimeGeneratorType;

    RsaProtocol();
    explicit RsaProtocol(const PrimeGenerator& primeGenerator);

    void generate();

    UnboundedInt encrypt(const Uint64& source, const PublicKey& anotherPublicKey);
    std::vector<UnboundedInt> encrypt(const std::vector<Uint64>& source, const PublicKey& anotherPublicKey);

    Uint64 decrypt(const UnboundedInt& source);
    std::vector<Uint64> decrypt(const std::vector<UnboundedInt>& source);

    PublicKey publicKey{};
    PrivateKey privateKey{};
    PrimeGenerator primeGenerator{};
};

template <typename PrimeGeneratorType>
RsaProtocol<PrimeGeneratorType>::RsaProtocol() = default;

template <typename PrimeGeneratorType>
RsaProtocol<PrimeGeneratorType>::RsaProtocol(const PrimeGenerator& primeGenerator) : primeGenerator(primeGenerator)
{}

template <typename PrimeGeneratorType>
void RsaProtocol<PrimeGeneratorType>::generate()
{
    UnboundedInt p{ primeGenerator() };
    UnboundedInt q{ primeGenerator() };

    // Compute phi
    UnboundedInt phi = (p - 1) * (q - 1);

    // Compute 'n'
    UnboundedInt n = p * q;
    publicKey.n    = n;
    privateKey.n   = n;

    // Mersenne prime number
    publicKey.e = 65537;

    // Compute 'd'
    privateKey.d = invmod(publicKey.e, phi);
}

template <typename PrimeGeneratorType>
UnboundedInt RsaProtocol<PrimeGeneratorType>::encrypt(const Uint64& source, const PublicKey& anotherPublicKey)
{
    return modexp<UnboundedInt>(UnboundedInt{ source }, anotherPublicKey.e, anotherPublicKey.n);
}

template <typename PrimeGeneratorType>
std::vector<UnboundedInt> RsaProtocol<PrimeGeneratorType>::encrypt(const std::vector<Uint64>& source,
                                                                   const PublicKey& anotherPublicKey)
{
    std::vector<UnboundedInt> result{};
    result.resize(source.size());

    for (std::size_t i = 0; i < source.size(); ++i) {
        result[i] = encrypt(source[i], anotherPublicKey);
    }

    return result;
}

template <typename PrimeGeneratorType>
Uint64 RsaProtocol<PrimeGeneratorType>::decrypt(const UnboundedInt& source)
{
    return static_cast<Uint64>(modexp<UnboundedInt>(source, privateKey.d, privateKey.n));
}

template <typename PrimeGeneratorType>
std::vector<Uint64> RsaProtocol<PrimeGeneratorType>::decrypt(const std::vector<UnboundedInt>& source)
{
    std::vector<Uint64> result{};
    result.resize(source.size());

    for (std::size_t i = 0; i < source.size(); ++i) {
        result[i] = decrypt(source[i]);
    }

    return result;
}

} // namespace cml


#include "catch.hpp"
#include "timer.hpp"

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <fmt/format.h>
#include <functional>
#include <iostream>

using boost::math::cyl_bessel_j;
using namespace boost::math::quadrature;
using namespace std::placeholders;

double f1(double x, double r) {
  return 1.0 / (pow(x, 2) + 1.0) * cyl_bessel_j(0.0, r * x) * x +
         1.0 / (pow(x, 2) + 2.0) * cyl_bessel_j(1.0, r * x) * x;
}

double f2(double x) { return pow(x, 2); }

TEST_CASE("Guass-Kronrod (f1)", "[mW transform Integral]") {
  double kmin = 1.0;
  double kmax = 2.0;
  double r = 100.0;

  std::function<double(double)> func = std::bind(f1, _1, r);
  double error = 0;
  double estimate1 = 0;
  int nk = 4;
  Timer timer;
  timer.start();
  double dk = (kmax - kmin) / nk;
  for (int i = 0; i < nk; ++i) {
    double k1 = kmin + i * dk;
    double k2 = k1 + dk;
    estimate1 +=
        gauss_kronrod<double, 15>::integrate(func, k1, k2, 0, 0, &error);
  }
  timer.stop();
  fmt::print("Milliseconds: {:f}\n", timer.elapsedMilliseconds());
  timer.start();
  double estimate2 =
      gauss_kronrod<double, 61>::integrate(func, kmin, kmax, 0, 0, &error);
  timer.stop();
  fmt::print("Milliseconds: {:f}\n", timer.elapsedMilliseconds());

  fmt::print("e2 - e1: {:15.8e} - {:15.8e} = {:15.8e}\n", estimate2, estimate1,
             estimate2 - estimate1);
  REQUIRE(estimate1 == Approx(0.000289257995979269));
}

TEST_CASE("Guass-Kronrod (f2)", "[mW transform Integral]") {
  double kmin = 1.0;
  double kmax = 2.0;

  double error;
  double estimate =
      gauss_kronrod<double, 15>::integrate(f2, kmin, kmax, 0, 0, &error);

  REQUIRE(estimate == Approx(2.3333333333333335));
}


#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
#include "metrics.h"
#include <iostream>

using namespace Eigen;

namespace dmaps
{
	f_type rmsd(const vector_t& ri, const vector_t& rj, const vector_t& w)
	{
		// Copy vectors for manipulation.
		vector_t r1 = ri, r2 = rj;

		// Get subset of weights just to make sure the size is proper.
		const vector_t& ws = w.segment(0, ri.size()/3);

		// Maps for ease of use.
		Map<matrix3_t> xi(r1.data(), r1.size()/3, 3);
		Map<matrix3_t> xj(r2.data(), r2.size()/3, 3);

		// Subtract out centers of mass.
		f_type wtot = ws.sum();
		vector3_t comi = (ws.asDiagonal()*xi).colwise().sum()/wtot;
		vector3_t comj = (ws.asDiagonal()*xj).colwise().sum()/wtot;
		xi.rowwise() -= comi.transpose();
		xj.rowwise() -= comj.transpose();

		// SVD of covariance matrix.
		matrix_t cov = xi.transpose()*ws.asDiagonal()*xj;
		JacobiSVD<matrix_t> svd(cov, ComputeThinU | ComputeThinV);

		// Find rotation.
		f_type d = (svd.matrixV()*svd.matrixU().transpose()).determinant() > 0 ? 1 : -1;

		matrix33_t eye = matrix33_t::Identity(3, 3);
		eye(2, 2) = d;
		matrix33_t R = svd.matrixV()*eye*svd.matrixU().transpose();

		// Return rmsd.
		return std::sqrt((ws.asDiagonal()*(xi - xj*R).array().square().matrix()).sum()/wtot);
	}

	f_type euclidean(const vector_t& ri, const vector_t& rj, const vector_t& w)
	{
		return std::sqrt((w.array()*(ri-rj).array().square()).sum());
	}

	f_type contact_map(const vector_t& ri, const vector_t& rj, const vector_t&)
	{
		Map<const matrix3_t> xi(ri.data(), ri.size()/3, 3);
		Map<const matrix3_t> xj(rj.data(), rj.size()/3, 3);

		int irows = xi.rows(), jrows = xj.rows();
		vector_t dxi(irows*irows/2 - irows/2), dxj(jrows*jrows/2 - jrows/2);

		// Compute pairwise distances.
		int k = 0;
		for(int i = 0; i < irows - 1; ++i)
		{
			int nrows = irows - i - 1;
			dxi.segment(k, nrows) = (xi.bottomRows(nrows).rowwise() - xi.row(i)).matrix().rowwise().norm();
			k += nrows;
		}

		k = 0;
		for(int i = 0; i < jrows - 1; ++i)
		{
			int nrows = jrows - i - 1;
			dxj.segment(k, nrows) = (xj.bottomRows(nrows).rowwise() - xj.row(i)).matrix().rowwise().norm();
			k += nrows;
		}

		// Calculate distance metric normalization constants.
		f_type r0 = 0.35, n = 8, m = 12;

		dxi.array() = (1. - (dxi.array()/r0).pow(n))/(1. - (dxi.array()/r0).pow(m));
		dxj.array() = (1. - (dxj.array()/r0).pow(n))/(1. - (dxj.array()/r0).pow(m));

		f_type norm = std::sqrt(dxi.sum()*dxj.sum());

		return std::sqrt(1.0/norm*(dxi - dxj).array().square().sum());
	}
}


#ifndef MYGRAPH_H
#define MYGRAPH_H

#include <vector>
#include <queue>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using std::vector;
using Eigen::MatrixXf;

class MyGraph{

public:
	MyGraph(Eigen::MatrixXf A);
	~MyGraph();
	void findConnComps();
	unsigned getNumConnComps();
	vector<vector<unsigned>> getConnComps();

private:
	Eigen::MatrixXi A_;
	vector<vector<unsigned>> ConnComps; // each vector is a connected component

};

#endif


#include <igl/writeDMAT.h>
#include <Eigen/Core>

void write_transformation(const std::string filename, const std::vector<Eigen::Matrix4d> Transformations){
    Eigen::MatrixXd bigT;
    bigT.resize(4*Transformations.size(),4);
    for (int i = 0; i<Transformations.size(); i++) {
        bigT.block(4*i,0,4,4) = Transformations[i];
    }
    igl::writeDMAT(filename,bigT);
}