/*
 * Copyright (c) 2016 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include <gtest/gtest.h>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include <boost/math/constants/constants.hpp>

// Given inertia expressed at frame (p, a) and rotation matrix from
// basis b to basis a, return the inertia expressed at frame (p, b)
Eigen::Matrix3d rotateInertia(const Eigen::Matrix3d &inertia_p_a,
                              const Eigen::Matrix3d &R_a_b) {
  return R_a_b.transpose() * inertia_p_a * R_a_b;
}

// Return the inertia of a point mass located at p
// (expressed in the same frame as the p coordinates).
Eigen::Matrix3d pointMassInertia(double mass, const Eigen::Vector3d &p) {
  return mass * (p.array().square().sum() * Eigen::Matrix3d::Identity() -
                 (p * p.transpose()));
}

TEST(BodyMass, solidworks) {
  // std::ofstream os("/tmp/out");
  auto &os = std::cout;

  // a format which plays nicely with sphinx.
  Eigen::IOFormat rst(4, 0, ", ", "\n", "  [", "]", "\n", "\n");
  // a format which plays nicely with urdf.
  Eigen::IOFormat urdf(Eigen::StreamPrecision, Eigen::DontAlignCols);
  Eigen::Array3d half_extents(0.2, 0.3, 0.4);
  double mass = 2.;
  double pi = boost::math::constants::pi<double>();
  Eigen::Array3d half_extents2 = half_extents.square();
  Eigen::Vector3d moments = mass / 3 * (half_extents2.sum() - half_extents2);

  os << "Let consider a box of mass " << mass << " kg and half-lengths (m)::\n"
     << half_extents.transpose().format(rst) << "\n"
     << "its principal moments of inertia are (kg m^2)::\n"
     << moments.transpose().format(rst) << " \n\n";

  os << "Let name \"a\" the basis aligned with the box axis, and \"c\""
        " the box center (which is also its center of mass).\n"
        "The box rotational inertia matrix at center of mass, aligned with "
        "basis a (ie. in the (c,a) frame) is::\n";
  Eigen::Matrix3d inertia_c_a = moments.asDiagonal();
  os << inertia_c_a.format(rst) << "\n\n";

  os << "let name \"b\" the basis defined by rotating of pi/6 the basis a "
        "around the z-axis.\n"
     << "The corresponding rotation matrix is::\n";
  Eigen::Matrix3d R_a_b;
  double yaw_angle = pi / 6;
  R_a_b = Eigen::AngleAxisd(yaw_angle, Eigen::Vector3d::UnitZ());
  os << R_a_b.format(rst) << "\n\n";

  os << "The box rotational inertia matrix at center of mass, aligned with "
        "basis b (ie. in the (c,b) frame) is::\n";
  Eigen::Matrix3d inertia_c_b = rotateInertia(inertia_c_a, R_a_b);
  os << inertia_c_b.format(rst) << "\n"
     << "The principal axis of inertia in basis b are the columns::\n"
     << R_a_b.transpose().format(rst) << "\n\n";

  Eigen::Vector3d p_c_a = half_extents;
  os << "Let name \"p\" a corner of the box.\n"
     << "It's coordinates in the (c, a) frame are::\n"
     << p_c_a.transpose().format(rst) << "\n"
     << "The box rotational inertia matrix at p, aligned with basis a is::\n";
  Eigen::Matrix3d inertia_p_a = pointMassInertia(mass, p_c_a) + inertia_c_a;
  os << inertia_p_a.format(rst) << "\n\n";

  os << "The coordinates of p in the (c, b) frame are::\n";
  Eigen::Vector3d p_c_b = R_a_b.transpose() * p_c_a;
  os << p_c_b.transpose().format(rst) << "\n\n";

  os << "The box rotational inertia matrix in the (p, b) frame can be "
        "computed from the one at (p, a)::\n";
  Eigen::Matrix3d inertia_p_b = rotateInertia(inertia_p_a, R_a_b);
  os << inertia_p_b.format(rst) << "\n\n"
     << "...or from the one at (c, b)::\n";
  Eigen::Matrix3d inertia_p_b_bis = pointMassInertia(mass, p_c_b) + inertia_c_b;
  os << inertia_p_b_bis.format(rst) << "\n\n";

  std::stringstream sv_c_a;
  sv_c_a << "      <visual>\n"
         << "          <origin xyz=\"" << (-p_c_b.transpose()).format(urdf)
         << "\" rpy=\"0 0 " << -yaw_angle << "\"/>\n"
         << "          <geometry>\n"
         << "              <box size=\"" << (2 * half_extents).transpose()
         << "\"/>\n"
         << "          </geometry>\n"
         << "      </visual>\n";

  std::stringstream si_c_b;
  si_c_b << "      <inertial>\n"
         << "          <mass value=\"" << mass << "\"/>\n"
         << "          <origin xyz=\"" << (-p_c_b.transpose()).format(urdf)
         << "\"/>\n"
         << "          <inertia"
         << " ixx=\"" << inertia_c_b(0, 0) << "\""
         << " ixy=\"" << inertia_c_b(0, 1) << "\""
         << " ixz=\"" << inertia_c_b(0, 2) << "\""
         << " iyy=\"" << inertia_c_b(1, 1) << "\""
         << " iyz=\"" << inertia_c_b(1, 2) << "\""
         << " izz=\"" << inertia_c_b(2, 2) << "\" />\n"
         << "      </inertial>\n";

  std::stringstream si_c_a;
  si_c_a << "      <inertial>\n"
         << "          <mass value=\"" << mass << "\"/>\n"
         << "          <origin xyz=\"" << (-p_c_b.transpose()).format(urdf)
         << "\" rpy=\"0 0 " << -yaw_angle << "\"/>\n"
         << "          <inertia"
         << " ixx=\"" << inertia_c_a(0, 0) << "\""
         << " ixy=\"" << inertia_c_a(0, 1) << "\""
         << " ixz=\"" << inertia_c_a(0, 2) << "\""
         << " iyy=\"" << inertia_c_a(1, 1) << "\""
         << " iyz=\"" << inertia_c_a(1, 2) << "\""
         << " izz=\"" << inertia_c_a(2, 2) << "\" />\n"
         << "      </inertial>\n";

  os << "Let now assume one wants to model this box as an URDF link, using"
        " frame (p, b) as the link origin.\n"
        "Our box visual can be displayed using the URDF \"box\" element,"
        " placed in frame (c, a)::\n\n" << sv_c_a.str() << "\n";

  os << "Let now assume one wants to model this box as an URDF link, using"
        " frame (p, b) as the link origin.\n"
        "The inertial URDF element can be defined in frame (c, b), resulting"
        " in::\n\n" << si_c_b.str() << "\n\n";

  os << "...or in frame (c, a), resulting in::\n\n" << si_c_a.str() << "\n";

  os << "The full URDF file being::\n\n"
     << "  <robot name=\"MyBox\">\n"
     << "  <link name=\"Box\">\n" << sv_c_a.str() << si_c_b.str()
     << "  </link>\n"
     << "  </robot>\n";

  EXPECT_TRUE(inertia_p_b_bis.isApprox(inertia_p_b));
}


#include <cmath>
#include <iostream>
#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/special_functions/hypergeometric_2F0.hpp>
#include "quicksvg/graph_fn.hpp"
#include "quicksvg/plot_time_series.hpp"

using boost::math::hypergeometric_1F1;
using boost::math::hypergeometric_2F0;

int main() {

    double a = -3.14159;
    double b = 3.14159;
    std::string title = "sin(𝑥) and cos(𝑥)";
    std::string filename = "examples/sine_and_cosine.svg";
    auto f = [](double x)->double { return std::sin(x); };
    auto g = [](double x)->double { return std::cos(x); };
    quicksvg::graph_fn sin_graph(a, b, title, filename);

    sin_graph.add_fn(f);
    sin_graph.add_fn(g, "green");

    sin_graph.write_all();

    auto onef1_1 = [](double x)->double { return hypergeometric_1F1(3, 7, x); };
    a = -3;
    b = 2;
    title = "\u2081F\u2081(3, 7, 𝑥)";
    filename = "examples/1F1_1.svg";
    quicksvg::graph_fn onef1_1graph(a, b, title, filename);
    onef1_1graph.add_fn(onef1_1);
    onef1_1graph.write_all();

    auto onef1_2 = [](double x)->double { return hypergeometric_1F1(-2, 3, x); };
    a = -3;
    b = 2;
    title = "\u2081F\u2081(-2, 3, 𝑥)";
    filename = "examples/1F1_2.svg";
    quicksvg::graph_fn onef1_2graph(a, b, title, filename);
    onef1_2graph.add_fn(onef1_2);
    onef1_2graph.write_all();

    auto onef1_3 = [](double x)->double { return hypergeometric_1F1(2, -2.5, x); };
    a = -2;
    b = 1;
    title = "\u2081F\u2081(2, -2.5, 𝑥)";
    filename = "examples/1F1_3.svg";

    quicksvg::graph_fn onef1_3graph(a, b, title, filename);
    onef1_3graph.add_fn(onef1_3);
    onef1_3graph.write_all();

    auto onef1_4 = [](double x)->double { return hypergeometric_1F1(-2, -2.5, x); };
    a = -4;
    b = 1;
    title = "\u2081F\u2081(-2, -2.5, 𝑥)";
    filename = "examples/1F1_4.svg";
    quicksvg::graph_fn onef1_4graph(a, b, title, filename);
    onef1_4graph.add_fn(onef1_4);
    onef1_4graph.write_all();

    std::vector<double> v(50);
    std::vector<double> u(50);
    double start_time = 0;
    double time_step = 0.25;
    for (size_t i = 0; i < v.size(); ++i) {
      v[i] = std::sin(start_time + i*time_step);
      u[i] = std::cos(start_time + i*time_step);
    }

    title = "sine and cosine time series";
    filename = "examples/sin_cos_time_series.svg";

    quicksvg::plot_time_series pts(start_time, time_step, title, filename);
    pts.add_dataset(v);
    pts.add_dataset(u, false, "lime", "lightgreen");
    pts.write_all();

}


#include <MishMesh/PolyMesh.h>

#ifdef HAS_EIGEN
#include <Eigen/Eigen>
#endif

/*
 * Fit vertices to a plane defined by a set of points with normals as described in
 * Ju, Tao, et al. "Dual contouring of hermite data." Proceedings of the 29th annual conference on Computer graphics and interactive techniques. 2002.
 * and
 * Lindstrom, P. (2000, July). Out-of-core simplification of large polygonal models. In Proceedings of the 27th annual conference on Computer graphics and interactive techniques (pp. 259-262).
 */

namespace MishMesh {
	namespace Meshing {
		enum SVDCutoffType {
			RELATIVE,
			ABSOLUTE
		};
		constexpr double SVD_CUTOFF_FACTOR = 0.1;

		/**
		 * Fit a point to the average of a set of points and clip it to a bounding box.
		 * @param The mesh.
		 * @param vh The vertex handle for the point to fit.
		 * @param points a set of points, that is used to calculcate the average.
		 */
		void fit_vertex_average(MishMesh::PolyMesh &mesh,
		                        const MishMesh::PolyMesh::VertexHandle vh,
		                        const MishMesh::BBox<OpenMesh::Vec3d, 3> &bbox,
		                        const std::vector<OpenMesh::Vec3d> &points) {
			OpenMesh::Vec3d x = std::accumulate(points.begin(), points.end(), OpenMesh::Vec3d{0, 0, 0}) / points.size();
			mesh.set_point(vh, OpenMesh::Vec3d(x.data()));
		}

#ifdef HAS_EIGEN
		/**
		 * Fit a point to a plane defined by a set of points with normals.
		 * @param The mesh.
		 * @param vh The vertex handle for the point to fit.
		 * @param bbox a bounding box that is used to clip the point when it lies outside.
		 * @param points a set of points, that is used to calculcate the average.
		 * @param normals The normals at the points in the points array.
		 * @param clip If clip is true, the result will be clipped on the bounding box.
		 * @param svd_cutoff_type For absolute cutoff, singular values smaller than svd_cutoff_factor
		 *        are truncated and for relative cutoff singular values S_i with S_i/S_0 smaller than
		 *        svd_cutoff_factor are truncated.
		 * @param svd_cutoff_factor Threshold which singular values are truncated to avoid instable results,
		 *        when determining the plane of the points.
		 */
		void fit_vertex_svd(MishMesh::PolyMesh &mesh,
		                    const MishMesh::PolyMesh::VertexHandle vh,
		                    const MishMesh::BBox<OpenMesh::Vec3d, 3> &bbox,
		                    const std::vector<OpenMesh::Vec3d> &points,
		                    const std::vector<OpenMesh::Vec3d> &normals,
		                    const bool clip,
		                    const SVDCutoffType svd_cutoff_type,
		                    const double svd_cutoff_factor) {
			Eigen::Matrix3d ATA;
			Eigen::Vector3d ATb;
			ATA.setZero();
			ATb.setZero();
			assert(points.size() == normals.size());
			MishMesh::BBox<Eigen::Vector3d, 3> cube_bbox_eigen{Eigen::Vector3d(bbox.ltf.data()), Eigen::Vector3d(bbox.rbn.data())};
			for(int i = 0; i < points.size(); i++) {
				Eigen::Vector3d p_i(points[i].data());
				Eigen::Matrix<double, 3, 1> n_i(normals[i].data());
				assert(1.0 - n_i.norm() < FLT_EPSILON);
				ATA += n_i * n_i.transpose();
				assert(cube_bbox_eigen.contains(p_i));
				ATb += n_i * (n_i.transpose().dot(p_i));
			}

			Eigen::Vector3d cell_center = (cube_bbox_eigen.rbn + cube_bbox_eigen.ltf) / 2.0;
			Eigen::JacobiSVD<Eigen::Matrix3d> svd(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
			svd.compute(ATA);
			Eigen::MatrixXd S = svd.singularValues().asDiagonal();
			// Invert the singular values diagonal matrix to calculate the pseudo inverse,
			// but cut off small singular values.
			for(short j = 0; j < 3; j++) {
				if(svd_cutoff_type == SVDCutoffType::RELATIVE && S(j, j) / S(0, 0) < svd_cutoff_factor) {
					// Relative cutoff like in Lindstrom 2000
					S(j, j) = 0.0;
				} else if(svd_cutoff_type == SVDCutoffType::ABSOLUTE && S(j, j) < svd_cutoff_factor) {
					// Absolute cutoff like in Ju et al. 2002
					S(j, j) = 0.0;
				} else {
					S(j, j) = 1.0 / S(j, j);
				}
			}
			Eigen::MatrixXd ATA_pinv = svd.matrixV() * S * svd.matrixU().transpose();
			Eigen::Vector3d x = cell_center + ATA_pinv * (ATb - ATA * cell_center);
			if(clip) {
				x = cube_bbox_eigen.clip(x, 0.2);
			}

			mesh.set_point(vh, OpenMesh::Vec3d(x.data()));
		}
#endif
	}
}


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

#pragma once

#include <boost/math/distributions.hpp>
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {

namespace tools_stats {

//! @brief Density function of the Standard normal distribution.
//!
//! @param x evaluation points.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated densities.
inline Eigen::MatrixXd dnorm(const Eigen::MatrixXd &x)
{
    boost::math::normal dist;
    auto f = [&dist](double y) { return boost::math::pdf(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

//! @brief Distribution function of the Standard normal distribution.
//!
//! @param x evaluation points.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated probabilities.
inline Eigen::MatrixXd pnorm(const Eigen::MatrixXd &x)
{
    boost::math::normal dist;
    auto f = [&dist](double y) { return boost::math::cdf(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

//! @brief Quantile function of the Standard normal distribution.
//!
//! @param x evaluation points.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated quantiles.
inline Eigen::MatrixXd qnorm(const Eigen::MatrixXd &x)
{
    boost::math::normal dist;
    auto f = [&dist](double y) { return boost::math::quantile(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

//! @brief Density function of the Student t distribution.
//!
//! @param x evaluation points.
//! @param nu degrees of freedom parameter.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated densities.
inline Eigen::MatrixXd dt(const Eigen::MatrixXd &x, double nu)
{
    boost::math::students_t dist(nu);
    auto f = [&dist](double y) { return boost::math::pdf(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

//! @brief Distribution function of the Student t distribution.
//!
//! @param x evaluation points.
//! @param nu degrees of freedom parameter.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated probabilities.
inline Eigen::MatrixXd pt(const Eigen::MatrixXd &x, double nu)
{
    boost::math::students_t dist(nu);
    auto f = [&dist](double y) { return boost::math::cdf(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

//! @brief Quantile function of the Student t distribution.
//!
//! @param x evaluation points.
//! @param nu degrees of freedom parameter.
//!
//! @return An \f$ n \times d \f$ matrix of evaluated quantiles.
inline Eigen::MatrixXd qt(const Eigen::MatrixXd &x, double nu)
{
    boost::math::students_t dist(nu);
    auto f = [&dist](double y) { return boost::math::quantile(dist, y); };
    return tools_eigen::unaryExpr_or_nan(x, f);
}

Eigen::MatrixXd simulate_uniform(const size_t& n, const size_t& d,
                                 std::vector<int> seeds = std::vector<int>());

Eigen::MatrixXd simulate_uniform(const size_t& n, const size_t& d, bool qrng,
                                  std::vector<int> seeds = std::vector<int>());

Eigen::VectorXd to_pseudo_obs_1d(Eigen::VectorXd x,
                                 std::string ties_method = "average");

Eigen::MatrixXd to_pseudo_obs(Eigen::MatrixXd x,
                              std::string ties_method = "average");

double pairwise_mcor(const Eigen::Matrix<double, Eigen::Dynamic, 2>& x,
                     const Eigen::VectorXd &weights = Eigen::VectorXd());

Eigen::MatrixXd dependence_matrix(const Eigen::MatrixXd &x,
                                  const std::string &measure);

Eigen::MatrixXd ghalton(const size_t& n, const size_t& d,
                        std::vector<int> seeds = std::vector<int>());

Eigen::MatrixXd sobol(const size_t& n, const size_t& d,
                      std::vector<int> seeds = std::vector<int>());

Eigen::VectorXd pbvt(const Eigen::Matrix<double, Eigen::Dynamic, 2> &z,
                     int nu, double rho);

Eigen::VectorXd pbvnorm(const Eigen::Matrix<double, Eigen::Dynamic, 2> &z,
                        double rho);
}

}

#include <vinecopulib/misc/implementation/tools_stats.ipp>


#pragma once

#include <armadillo>

namespace random_sinks {
    arma::mat fwht(arma::mat X, bool apply_to_rows=true);

    template<class T> inline T randchi(double df, uint m, uint n) { // T must be an armadillo Row, Col, or Mat type
        return arma::sqrt(arma::chi2rnd<T>(df, m, n));
    }

    template<class T> inline T randchi(double df, uint n) { // T must be an armadillow Row or Col type
        return arma::sqrt(arma::chi2rnd<T>(df, n));
    }

    inline double randchi(double df) {
        return std::sqrt(arma::chi2rnd<double>(df));
    }

    class RFF {
        private:
        arma::mat _w;
        arma::rowvec _b;
        double _gamma;
        int _nfs, _dim;

        public:
        const arma::mat& weights;
        const arma::rowvec& bias;

        RFF(double gamma=1, int n_feats=100, int seed=-1);
        void fit(const arma::mat& X);
        arma::mat get_features(const arma::mat& X);
    };

    class fastfood {
        private:
        arma::rowvec _B, _G, _S, _b;
        arma::uvec _P;
        int _dim, _nfs, _d;
        double _gamma;

        public:
        fastfood(double gamma=1, int n_feats=128, int seed=-1);
        void fit(const arma::mat& X);
        arma::mat get_features(arma::mat X);
    };

    class SORF {
        private:
        arma::rowvec _B1, _B2, _B3, _b;
        int _dim, _d, _nfs;
        double _gamma;

        public:
        SORF(double gamma=1, int n_feats=128, int seed=-1);
        void fit(const arma::mat& X);
        arma::mat get_features(arma::mat X);
    };

    void rSVD(arma::mat& U, arma::vec& s, arma::mat& V, const arma::mat& X, uint n_comps);
    double powerSVD(arma::mat& U, arma::vec& s, arma::mat& V, const arma::mat& X, uint n_comps, uint max_iter=10, double tol=1e-8);

    class tPCA {
        private:
        arma::mat _V;
        arma::vec _s;
        arma::rowvec _means;
        int _dim, _n_comps;
        bool _use_rSVD;

        public:
        const arma::mat& components;
        const arma::vec& cov_eigenvals;
        const int& n_components;

        tPCA(uint n_components, std::string method="rSVD");
        void fit(arma::mat X, uint max_iter=10, double tol=1e-8, bool zero_mean=false);
        arma::mat get_features(arma::mat X);
        arma::mat get_projection(arma::mat X);
        arma::mat inv_features(arma::mat X);
    };

    class gauss_nystrom {
        private:
        arma::mat _data, _V, _U;
        arma::vec _s;
        arma::rowvec  _stds, _means;
        double _gamma;
        int _dim, _nfs;
        arma::mat _eval_kernel(arma::mat X);
        arma::mat _eval_kernel();

        public:
        const arma::vec& kernel_singular_vals;
        const arma::mat& kernel_left_vecs;
        const arma::mat& kernel_right_vecs;
        const int& n_features;
        gauss_nystrom(uint n_feats=128, double gamma=1, int seed=-1);
        void fit(arma::mat X, std::string method="powerSVD", uint max_iter=20, double tol=1e-10);
        arma::mat get_features(arma::mat X);
    };
}


//####### Test module for gamma functions #############################

//Define Module name
 #define BOOST_TEST_MODULE "phys/gamma_functions"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <picsar_qed/physics/gamma_functions.hpp>

#include <picsar_qed/physics/unit_conversion.hpp>
#include <picsar_qed/math/vec_functions.hpp>

using namespace picsar::multi_physics::phys;
using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-6;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

const double me_c = electron_mass<double> * light_speed<double>;
const double lambda = 800.0e-9;
const double omega = 2.0*pi<double>*light_speed<double>/lambda;

// ------------- Tests --------------

//***Test normalized energy calculation for photons

template<typename RealType, unit_system UnitSystem>
void test_gamma_photons(
    vec3<double> t_p, double t_exp_res, double t_ref_quant = 1.0)
{
    const auto p =  vec3<RealType>{
        static_cast<RealType>(t_p[0]),
        static_cast<RealType>(t_p[1]),
        static_cast<RealType>(t_p[2])}*
        conv<quantity::momentum, unit_system::SI, UnitSystem, RealType>::fact();

    const auto res = compute_gamma_photon<RealType, UnitSystem>(p, t_ref_quant);

    const auto exp_res = static_cast<RealType>(t_exp_res);

    if(exp_res != static_cast<RealType>(0.0))
        BOOST_CHECK_SMALL((res-exp_res)/exp_res, tolerance<RealType>());
    else
        BOOST_CHECK_SMALL(res, tolerance<RealType>());
}

void test_gamma_photons_all_cases(
    vec3<double> p, double gamma_exp)
{
    test_gamma_photons<double, unit_system::SI>(p, gamma_exp);
    test_gamma_photons<double, unit_system::norm_lambda>(p, gamma_exp, lambda);
    test_gamma_photons<double, unit_system::norm_omega>(p, gamma_exp, omega);
    test_gamma_photons<double, unit_system::heaviside_lorentz>(p, gamma_exp, 1.0);
    test_gamma_photons<float, unit_system::SI>(p, gamma_exp);
    test_gamma_photons<float, unit_system::norm_lambda>(p, gamma_exp, lambda);
    test_gamma_photons<float, unit_system::norm_omega>(p, gamma_exp, omega);
    test_gamma_photons<float, unit_system::heaviside_lorentz>(p, gamma_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( gamma_photons_1 )
{
    const double px = 83.759*me_c;
    const double py = 139.311*me_c;
    const double pz = -230.553*me_c;

    const double gamma_exp = 282.09539275039566;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_photons_2 )
{
    const double px = 9.2627*me_c;
    const double py = -25.4575*me_c;
    const double pz = -10.2246*me_c;

    const double gamma_exp = 28.955558407670193;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_photons_3 )
{
    const double px = -2314.45*me_c;
    const double py = -2356.30*me_c;
    const double pz = 546.28*me_c;

    const double gamma_exp = 3347.723156251126;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_photons_4 )
{
    const double px = 0*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;

    const double gamma_exp = 0.0;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_photons_5 )
{
    const double px = 0.001*me_c;
    const double py = -0.002*me_c;
    const double pz = 0.003*me_c;

    const double gamma_exp = 0.0037416573867739412;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_photons_6 )
{
    const double px = -2314.45*me_c;
    const double py = 0.0*me_c;
    const double pz = 0.0*me_c;

    const double gamma_exp = 2314.45;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_photons_all_cases(p, gamma_exp);
}

//*******************************

//***Test Lorentz factor calculation for electrons and positrons

template<typename RealType, unit_system UnitSystem>
void test_gamma_ele_pos(
    vec3<double> t_p, double t_exp_res, double t_ref_quant = 1.0)
{
    const auto p =  vec3<RealType>{
        static_cast<RealType>(t_p[0]),
        static_cast<RealType>(t_p[1]),
        static_cast<RealType>(t_p[2])}*
        conv<quantity::momentum, unit_system::SI, UnitSystem, RealType>::fact();

    const auto res = compute_gamma_ele_pos<RealType, UnitSystem>(p, t_ref_quant);

    const auto exp_res = static_cast<RealType>(t_exp_res);

    if(exp_res != static_cast<RealType>(0.0))
        BOOST_CHECK_SMALL((res-exp_res)/exp_res, tolerance<RealType>());
    else
        BOOST_CHECK_SMALL(res, tolerance<RealType>());
}

void test_gamma_ele_pos_all_cases(
    vec3<double> p, double gamma_exp)
{
    test_gamma_ele_pos<double, unit_system::SI>(p, gamma_exp);
    test_gamma_ele_pos<double, unit_system::norm_lambda>(p, gamma_exp, lambda);
    test_gamma_ele_pos<double, unit_system::norm_omega>(p, gamma_exp, omega);
    test_gamma_ele_pos<double, unit_system::heaviside_lorentz>(p, gamma_exp, 1.0);
    test_gamma_ele_pos<float, unit_system::SI>(p, gamma_exp);
    test_gamma_ele_pos<float, unit_system::norm_lambda>(p, gamma_exp, lambda);
    test_gamma_ele_pos<float, unit_system::norm_omega>(p, gamma_exp, omega);
    test_gamma_ele_pos<float, unit_system::heaviside_lorentz>(p, gamma_exp, 1.0);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_1 )
{
    const double px = 24.3752*me_c;
    const double py = -11.5710*me_c;
    const double pz = -10.0841*me_c;

    const double gamma_exp = 28.822343569703;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_2 )
{
    const double px = 4.015*me_c;
    const double py = 197.287*me_c;
    const double pz = 141.705*me_c;

    const double  gamma_exp = 242.93947315946826;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_3 )
{
    const double px = -2534.83*me_c;
    const double py = 1011.54*me_c;
    const double pz = -793.04*me_c;

    const double gamma_exp = 2842.0924935863713;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_4 )
{
    const double px = 0*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;

    const double gamma_exp = 1.0;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_5 )
{
    const double px = 0.001*me_c;
    const double py = -0.002*me_c;
    const double pz = 0.003*me_c;

    const double gamma_exp = 1.0000069999755001;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

BOOST_AUTO_TEST_CASE( gamma_ele_pos_6 )
{
    const double px = -2534.83*me_c;
    const double py = 0*me_c;
    const double pz = 0*me_c;

    const double gamma_exp = 2534.830197251879;

    const auto p = vec3<double>{px,py,pz};

    test_gamma_ele_pos_all_cases(p, gamma_exp);
}

//*******************************


#include <iostream>

#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() {
	MatrixXf A(4, 4);

	float data[4][4] = {
		{1, 0, 0, 0},
		{0, 1, 0, 0},
		{0, 0, 1, 0},
		{0, 0, 0, 1}
	};

	for (size_t row = 0; row < 4; row++) {
		for (size_t col = 0; col < 4; col++)
			A(row, col) = data[row][col];
	}

	MatrixXf b(4, 2);
	for (size_t row = 0; row < 4; row++) {
		b(row, 0) = 1;
		b(row, 1) = 2;
	}

	MatrixXf x = A.colPivHouseholderQr().solve(b);

	cout << x << endl;

	return 0;
}


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <map>
#include <cassert>
#include <sstream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

struct Point {
    double x;
    double y;
    string word;
};

size_t read_file(string filename, MatrixXd& points, vector<string>& words)
{
    vector<Point> output;
    Point tmp;
    string line;
    ifstream infile(filename);
    assert(infile.is_open());

    while(getline(infile, line)) {
        // read line in

        // read parts of line
        istringstream iss(line);
        iss >> tmp.x >> tmp.y >> tmp.word;

        // add point to list
        output.push_back(tmp);
    }

    points.resize(2, output.size());
    words.resize(output.size());
    for(unsigned int ii=0; ii<output.size(); ii++) {
        words[ii] = output[ii].word;
        points(0, ii) = output[ii].x;
        points(1, ii) = output[ii].y;
    }

    return output.size();
}

/**
 * @brief Computes the pseudo inverse with the SVD:
 *
 * A = USV*
 * A^-1 = VS^-1U*
 *
 * @param inmat
 * @param pinvtoler
 */
MatrixXd pseudo_inverse(MatrixXd& inmat, double pinvtoler=1.e-6)
{
    JacobiSVD<MatrixXd> svd(inmat, ComputeThinU | ComputeThinV);

    VectorXd s_inv = svd.singularValues();
    cerr << endl << s_inv << endl;
    for(unsigned int ii = 0; ii < s_inv.rows(); ii++) {
        if(s_inv[ii] > pinvtoler) {
            s_inv[ii] = 1.0/s_inv[ii];
        } else {
            s_inv[ii] = 0;
        }
    }
    return svd.matrixV()*s_inv.asDiagonal()*svd.matrixU().transpose();
}

/**
 * @brief Solves
 *
 * YW = AXW
 * A = YW (XW)^1
 *
 * where columns of R are points in the reference (fixed) point set, A is an
 * affine matrix (2x3, where column 3 is the shift), M is an augmented matrix
 * with 1' in the bottom row and the top two rows containing coordinates in the
 * moving dataset -- one point per column and W being a correspondance matrix
 * such that correponding points in the R and M matrixes 1 is
 *
 * @param ref
 * @param moving
 * @param weights
 *
 * @return
 */
void solve(const MatrixXd& ref_y, const MatrixXd& mov_x,
        const VectorXd& weights, Matrix<double, 2, 3>& affine)
{
    // Create augmented moving matrix, with lowest row of 1s
    MatrixXd aug_mov_x = MatrixXd::Ones(mov_x.rows() + 1, mov_x.cols());
    aug_mov_x.topRows(2) = mov_x;

    MatrixXd tmp = aug_mov_x*weights.asDiagonal();
    MatrixXd pinv = pseudo_inverse(tmp);
    affine = ref_y*weights.asDiagonal()*pinv;

    // enforce rigid + scaling ???

}

int main(int argc, char** argv)
{
    if(argc != 3) {
        cerr << "Need to provide two input files!" << endl;
        return -1;
    }

    vector<string> ref_words;
    MatrixXd ref_points;
    read_file(argv[1], ref_points, ref_words);

    vector<string> move_words;
    MatrixXd move_points;
    read_file(argv[2], move_points, move_words);

    map<string, int> word_counts;
    for(auto& word: ref_words) {
        auto ret = word_counts.insert(pair<string,int>(word,0));
        if(ret.second) {
            // element not inserted, so it exists, already existed so just
            // increment the old count
            ret.first->second++;
        }
    }

    for(auto& v: word_counts) {
        cerr << v.first << ":" << v.second << endl;
    }

    // Reorder points / duplicate / remove points that don't match. Also
    // initialize weights to down-weight repeated matches
    VectorXd weights = VectorXd::Ones(ref_points.cols());
    MatrixXd matches(move_points.cols(), ref_points.cols());
    for(unsigned int cc = 0; cc < ref_words.size(); cc++) {
        for(unsigned int rr = 0; rr< move_words.size(); rr++) {
            cerr << ref_words[cc] << " vs " << move_words[cc] << endl;
            if(ref_words[cc] == move_words[rr]) {
                matches(rr, cc) = 1;
                //weights[cc] /= 2.;
            } else {
                matches(rr, cc) = 0;
            }
        }
    }
    MatrixXd move_points_match = move_points*matches;
    MatrixXd err(ref_points.rows(), ref_points.cols());

    cerr << "Ref" << endl << ref_points << endl;
    cerr << "Moving" << endl << move_points_match << endl;
    cerr << "Matching" << endl << matches << endl;
    cerr << "Weights" << endl << weights << endl;

    Matrix<double, 2, 3>  affine(2, 3);
    affine << 1, 0, 0,
              0, 1, 0;

    for(int ii = 0; ii < 10; ii++) {
        MatrixXd adjusted = affine.leftCols(2)*move_points +
            affine.col(2)*MatrixXd::Ones(1, move_points.cols());
        cerr << "Adjusted " << endl << adjusted << endl;

        // Re-weight based on norms
        err = ref_points - adjusted;
        for(size_t rr = 0; rr < weights.rows(); rr++) {
            weights[rr] = 1./max(0.0001, err.col(rr).norm());
        }
        cerr << "Weights" << endl << weights << endl;

        cerr << "Affine" << endl << affine << endl;
        solve(ref_points, move_points_match, weights, affine);

    }
}


/*!=======================================================
  |                                                     |
  |                     tensor.cpp                      |
  |                                                     |
  -------------------------------------------------------
  | The source file for a class definition that allows  |
  | n-dimensional tensors. The data for the tensor is   |
  | stored in a 2 dimensional array and there is logic  |
  | which allows referencing to the correct indices to  |
  | occur.                                              |
  =======================================================
  | Dependencies:                                       |
  | Eigen: An implementation of various matrix          |
  |        commands. The implementation of the data     |
  |        matrix uses such a matrix.                   |
  =======================================================*/

#include <functional>
#include <iostream>
#include <numeric>
#include <vector>
#include <Eigen/Dense>
#include <tensor.h>

namespace tensor{
    //!==
    //!|
    //!| Functions
    //!|
    //!==

    Tensor23 eye(){
        /*!Return the second order identity tensor

        Populate the second order identity tensor.
        This function is useful so that a consistent identity tensor can be used
        with different storage schemes.*/

        //Initialize the matrix

        Tensor23 I({3,3});

        for(int i=0; i<3; i++){
            I(i,i) = 1.;
        }
        return I;
    }
    Tensor43 FOT_eye(){
        /*!Return the fourth order identity tensor

        Populate the fourth order identity tensor.
        This function is useful so that a consistent identity tensor can be used
        with different storage schemes.*/

        //Initialize the tensor
        Tensor43 FOTI({3,3,3,3});
        Tensor23 I    = eye(); //Get the second order identity tensor

        for(int i=0; i<3; i++){
            for(int j=0; j<3; j++){
                for(int k=0; k<3; k++){
                    for(int l=0; l<3; l++){
                        FOTI(i,j,k,l) = I(i,k)*I(j,l);
                    }
                }
            }
        }

        return FOTI;
    }
}


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include "bayesian/graph.hpp"
#include "bayesian/inference/belief_propagation.hpp"

// Pearl's Belief Propagation Algorithm -> Examples of application of algorithm
// http://www.cse.unsw.edu.au/~cs9417ml/Bayes/Pages/PearlPropagation.html
// 2 Samples
bn::graph_t make_pearl_belief_propagation_graph()
{
    bn::graph_t graph;
    auto vertex_r = graph.add_vertex();
    auto vertex_s = graph.add_vertex();
    auto vertex_w = graph.add_vertex();
    auto vertex_h = graph.add_vertex();
    auto edge_rw = graph.add_edge(vertex_r, vertex_w);
    auto edge_rh = graph.add_edge(vertex_r, vertex_h);
    auto edge_sh = graph.add_edge(vertex_s, vertex_h);

    {
        vertex_r->id = 1;
        vertex_r->selectable_num = 2;
        vertex_r->cpt.assign({}, vertex_r);

        bn::condition_t const cond;
        vertex_r->cpt[cond].second = {0.2, 0.8};
    }
    {
        vertex_s->id = 2;
        vertex_s->selectable_num = 2;
        vertex_s->cpt.assign({}, vertex_s);

        bn::condition_t const cond;
        vertex_s->cpt[cond].second = {0.1, 0.9};
    }
    {
        vertex_w->id = 3;
        vertex_w->selectable_num = 2;
        vertex_w->cpt.assign({vertex_r}, vertex_w);

        bn::condition_t const cond0 = {{vertex_r, 0}};
        bn::condition_t const cond1 = {{vertex_r, 1}};
        vertex_w->cpt[cond0].second = {1.0, 0.0};
        vertex_w->cpt[cond1].second = {0.2, 0.8};
    }
    {
        vertex_h->id = 4;
        vertex_h->selectable_num = 2;
        vertex_h->cpt.assign({vertex_r, vertex_s}, vertex_h);

        bn::condition_t const cond00 = {{vertex_r, 0}, {vertex_s, 0}};
        bn::condition_t const cond01 = {{vertex_r, 0}, {vertex_s, 1}};
        bn::condition_t const cond10 = {{vertex_r, 1}, {vertex_s, 0}};
        bn::condition_t const cond11 = {{vertex_r, 1}, {vertex_s, 1}};
        vertex_h->cpt[cond00].second = {1.0, 0.0};
        vertex_h->cpt[cond01].second = {1.0, 0.0};
        vertex_h->cpt[cond10].second = {0.9, 0.1};
        vertex_h->cpt[cond11].second = {0.0, 1.0};
    }

    return graph;
}

BOOST_AUTO_TEST_CASE( belief_propagation_pearl_part1 )
{
    bn::graph_t graph = make_pearl_belief_propagation_graph();
    auto const vertex = graph.vertex_list();
    std::vector<std::vector<double>> const teacher_r = {
        { 0.200, 0.800 },
        { 0.100, 0.900 },
        { 0.360, 0.640 },
        { 0.272, 0.728 }
    };

    bn::inference::belief_propagation bp(graph);
    auto const result = bp();

    for(std::size_t i = 0; i < vertex.size(); ++i)
    {
        matrix_type const data = result.at(vertex[i]);

        BOOST_CHECK(data.height() == 1);

        for(std::size_t j = 0; j < data.width(); ++j)
        {
            BOOST_CHECK_CLOSE(data[0][j], teacher_r[i][j], 0.01);
        }
    }
}

// 多分なんか違う
BOOST_AUTO_TEST_CASE( belief_propagation_pearl_part2 )
{
    bn::graph_t graph = make_pearl_belief_propagation_graph();
    auto const vertex = graph.vertex_list();
    std::vector<std::vector<double>> const teacher_r = {
        { 0.7353, 0.2647 },
        { 0.3382, 0.6618 },
        { 0.7882, 0.2118 },
        { 1.0000, 0.0000 }
    };

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[3]].resize(1, 2, 0);
    precondition[vertex[3]][0][0] = 1;

    bn::inference::belief_propagation bp(graph);
    auto const result = bp(precondition);

    for(std::size_t i = 0; i < vertex.size(); ++i)
    {
        matrix_type const data = result.at(vertex[i]);

        BOOST_CHECK(data.height() == 1);

        for(std::size_t j = 0; j < data.width(); ++j)
        {
            BOOST_CHECK_CLOSE(data[0][j], teacher_r[i][j], 0.1);
        }
    }
}

//
// resume14:
// 5 samples
//
bn::graph_t make_resume_graph()
{
    bn::graph_t graph;
    auto const vertex_A = graph.add_vertex();
    auto const vertex_B = graph.add_vertex();
    auto const vertex_C = graph.add_vertex();
    auto const vertex_D = graph.add_vertex();
    auto const edge_AB = graph.add_edge(vertex_A, vertex_B);
    auto const edge_BC = graph.add_edge(vertex_B, vertex_C);
    auto const edge_CD = graph.add_edge(vertex_C, vertex_D);

    {
        bn::condition_t const cond;

        vertex_A->id = 1;
        vertex_A->selectable_num = 3;
        vertex_A->cpt.assign({}, vertex_A);
        vertex_A->cpt[cond].second = { 0.30, 0.60, 0.10 };
    }
    {
        bn::condition_t const cond0 = {{vertex_A, 0}};
        bn::condition_t const cond1 = {{vertex_A, 1}};
        bn::condition_t const cond2 = {{vertex_A, 2}};

        vertex_B->id = 2;
        vertex_B->selectable_num = 3;
        vertex_B->cpt.assign({vertex_A}, vertex_B);
        vertex_B->cpt[cond0].second = { 0.20, 0.30, 0.50 };
        vertex_B->cpt[cond1].second = { 0.30, 0.30, 0.40 };
        vertex_B->cpt[cond2].second = { 0.80, 0.10, 0.10 };
    }
    {
        bn::condition_t const cond0 = {{vertex_B, 0}};
        bn::condition_t const cond1 = {{vertex_B, 1}};
        bn::condition_t const cond2 = {{vertex_B, 2}};

        vertex_C->id = 3;
        vertex_C->selectable_num = 2;
        vertex_C->cpt.assign({vertex_B}, vertex_C);
        vertex_C->cpt[cond0].second = { 0.50, 0.50 };
        vertex_C->cpt[cond1].second = { 0.70, 0.30 };
        vertex_C->cpt[cond2].second = { 0.40, 0.60 };
    }
    {
        bn::condition_t const cond0 = {{vertex_C, 0}};
        bn::condition_t const cond1 = {{vertex_C, 1}};

        vertex_D->id = 4;
        vertex_D->selectable_num = 3;
        vertex_D->cpt.assign({vertex_C}, vertex_D);
        vertex_D->cpt[cond0].second = { 0.40, 0.30, 0.30 };
        vertex_D->cpt[cond1].second = { 0.20, 0.60, 0.20 };
    }

    return graph;
}

BOOST_AUTO_TEST_CASE( belief_propagation_resume_ex )
{
    std::vector<double> const teacher_C = { 0.570, 0.430 };

    bn::graph_t graph = make_resume_graph();
    auto const vertex = graph.vertex_list();

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[1]].resize(1, 3);
    precondition[vertex[3]].resize(1, 3);
    precondition[vertex[1]][0] = { 0.0, 0.0, 1.0 };
    precondition[vertex[3]][0] = { 1.0, 0.0, 0.0 };

    bn::inference::belief_propagation func(graph);
    auto const result = func(precondition);

    auto const data = result.at(vertex[2]);
    BOOST_CHECK(data.height() == 1);

    for(std::size_t i = 0; i < data.width(); ++i)
    {
        BOOST_CHECK_CLOSE(data[0][i], teacher_C[i], 3);
    }
}

BOOST_AUTO_TEST_CASE( belief_propagation_resume_sample1 )
{
    std::vector<double> const teacher_B = { 0.330, 0.170, 0.500 };

    bn::graph_t graph = make_resume_graph();
    auto const vertex = graph.vertex_list();

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[2]].resize(1, 2);
    precondition[vertex[2]][0] = { 0.0, 1.0 };

    bn::inference::belief_propagation func(graph);
    auto const result = func(precondition);

    auto const data = result.at(vertex[1]);
    BOOST_CHECK(data.height() == 1);

    for(std::size_t i = 0; i < data.width(); ++i)
    {
        BOOST_CHECK_CLOSE(data[0][i], teacher_B[i], 3);
    }
}

BOOST_AUTO_TEST_CASE( belief_propagation_resume_sample2 )
{
    std::vector<double> const teacher_B = { 0.310, 0.190, 0.500 };

    bn::graph_t graph = make_resume_graph();
    auto const vertex = graph.vertex_list();

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[0]].resize(1, 3);
    precondition[vertex[2]].resize(1, 2);
    precondition[vertex[0]][0] = { 0.0, 1.0, 0.0 };
    precondition[vertex[2]][0] = { 0.0, 1.0 };

    bn::inference::belief_propagation func(graph);
    auto const result = func(precondition);

    auto const data = result.at(vertex[1]);
    BOOST_CHECK(data.height() == 1);

    for(std::size_t i = 0; i < data.width(); ++i)
    {
        BOOST_CHECK_CLOSE(data[0][i], teacher_B[i], 3);
    }
}

BOOST_AUTO_TEST_CASE( belief_propagation_resume_sample3 )
{
    std::vector<double> const teacher_A = { 0.300, 0.600, 0.100 };

    bn::graph_t graph = make_resume_graph();
    auto const vertex = graph.vertex_list();

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[3]].resize(1, 3);
    precondition[vertex[3]][0] = { 0.0, 0.0, 1.0 };

    bn::inference::belief_propagation func(graph);
    auto const result = func(precondition);

    auto const data = result.at(vertex[0]);
    BOOST_CHECK(data.height() == 1);

    for(std::size_t i = 0; i < data.width(); ++i)
    {
        BOOST_CHECK_CLOSE(data[0][i], teacher_A[i], 3);
    }
}

BOOST_AUTO_TEST_CASE( belief_propagation_resume_sample4 )
{
    std::vector<double> const teacher_B = { 0.200, 0.300, 0.500 };

    bn::graph_t graph = make_resume_graph();
    auto const vertex = graph.vertex_list();

    std::unordered_map<bn::vertex_type, bn::matrix_type> precondition;
    precondition[vertex[0]].resize(1, 3);
    precondition[vertex[0]][0] = { 1.0, 0.0, 0.0 };

    bn::inference::belief_propagation func(graph);
    auto const result = func(precondition);

    auto const data = result.at(vertex[1]);
    BOOST_CHECK(data.height() == 1);

    for(std::size_t i = 0; i < data.width(); ++i)
    {
        BOOST_CHECK_CLOSE(data[0][i], teacher_B[i], 3);
    }
}


/*
author: Kris Gonzalez
date: 200118
objective: using cmake, compile code that uses eigen, opencv and gainput

stat | descrip
done | hello world
done | compile with eigen
done | compile with opencv
done | compile with gainput

*/

#include <iostream>
#include <Eigen/Dense>                  // eigen library
#include <opencv2/core/core.hpp>        // cpp libraries
#include <opencv2/imgcodecs.hpp>        // ...
#include <opencv2/highgui/highgui.hpp>  // ...
#include <opencv2/core/mat.hpp>         // ...
#include <gainput/gainput.h>    // gainput library

using Eigen::MatrixXd;

int main(){
    std::printf("Eigen Matrix\n");
    MatrixXd m(2,2);
    m(0,0) = 3;
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);
    std::cout << m << std::endl;

    std::printf("OpenCV Matrix\n");
    float a[3][3] = {{1,2,1},{7,9,6},{1,5,7}};
    cv::Mat A(3,3,CV_32F);
    A = cv::Mat(3,3,CV_32F,a);
    std::cout << A.t() << '\n'; // transpose operation here

    std::printf("Initialize gainput lib\n");

}


#include <iostream>
#include <armadillo>
#include <cmath>
#include <time.h>
#include <fstream>
using namespace std;
using namespace arma;

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN); //rho, kind of like a linespace
                                                 //A, Tridiagonal matrix
void Maxoff(mat &A, int N,int &k, int &l, double &max);        //Finds the max element of a matrix

int main(){
    int N = 100; //matrix size; N x N
    double rhoN = 6;
    double max;
    int k,l;

    mat A = mat(N,N,fill::zeros); //indexes go from (0) to (N-1)
    mat S = mat(N,N,fill::eye);
    vec rho = vec(N,fill::zeros);
    fvec eigen = fvec(N);

    RHO_A_FILL(rho,A,N,rhoN);
    Maxoff(A,N,k,l,max);
    //A.print();
    int iterations = 0;

    double eps = 1E-10;

    double tau, t, s, c, il, ik, kk, ll, s_ik, s_il;
    double start, finish;

    start = clock();                      //clock value before eigen solve
    while(max > eps){
        tau = (A(l,l)-A(k,k))/(2.*A(k,l));
        if(tau>0){
             t = 1.0/(tau + sqrt(1.0 + tau*tau));}
        else{t = -1.0/( -tau + sqrt(1.0 + tau*tau));}

        //cosine and sine
        c = 1./sqrt(1.+t*t);
        s = t*c;

        //Jacobi rotating A round theta in N-dim space
        for(int i = 0; i<N; i++){
            if ((i != k) && (i !=l)){
                ik = A(i,k)*c - A(i,l)*s;
                il = A(i,l)*c + A(i,k)*s;
                A(i,k) = ik;
                A(k,i) = ik;
                A(i,l) = il;
                A(l,i) = il;
                }
            s_ik = S(i,k);
            s_il = S(i,l);
            S(i,k) = c*s_ik - s*s_il;
            S(i,l) = c*s_il + s*s_ik;
            }

        kk = A(k,k)*c*c - 2.*A(k,l)*c*s + A(l,l)*s*s;
        ll = A(l,l)*c*c + 2.*A(k,l)*c*s + A(k,k)*s*s;

        A(k,k) = kk;
        A(l,l) = ll;
        A(k,l) = 0;
        A(l,k) = 0;

        iterations++;
        Maxoff(A,N,k,l,max);

        } //end of while
    finish = clock();                    //clock value after eigen solve

    //[eigen] is now eigenvalues
    for(int i = 0; i<N;i++){
        eigen(i) = A(i,i);
        }

    eigen = sort(eigen);
    eigen.print();

    double time = (finish -start)/CLOCKS_PER_SEC/iterations;

    fstream outfile;
    outfile.open("eigenvectors.dat",ios::out);

    for (int i = 0; i<N; i++){
        for (int j = 0; j<N; j++){
            if(j%N == 0){outfile<<endl;}
            outfile << S(i,j)<<" ";
            }
        }
    outfile.close();

    //fstream Outfile;
    //Outfile.open("steps.dat",ios::out);
    //Outfile << "N     epsilon    steps  step_time"<<endl;

    //Outfile <<N <<" "<<eps<<" "<<iterations<<" "<<time;
    //Outfile<<endl;

    //Outfile.close();

} //end of main

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN){
    double h = rhoN/(N);
    for(int i = 0; i < N;i++){
        rho(i) = i*h;
        A(i,i) = 2./(h*h)+(rho(i)*rho(i));
        if(i<N-1){
            A(i+1,i) = -1./(h*h);
            A(i,i+1) = -1./(h*h);
            }
        }
    }

void Maxoff(mat &A, int N,int &k, int &l, double &max){
    max = 0;
    for(int i = 0; i < N ; i++){
        for(int j = i+1; j < N ; j++){
            if ( A(i,j)*A(i,j) > max){
                max =A(i,j)*A(i,j);
                k = i;
                l = j;
                }
            }
        }

    }


//---------------------------------Spheral++----------------------------------//
// QuadraticInterpolator
//
// Encapsulates the algorithm and data for parabolic interpolation in 1D
// Assumes the results is interpolated as y_interp = a + b*x + c*x^2
//
// Created by JMO, Fri Dec  4 14:28:08 PST 2020
//----------------------------------------------------------------------------//
#include "QuadraticInterpolator.hh"

#include <Eigen/Dense>

namespace Spheral {

//------------------------------------------------------------------------------
// Default constructor
//------------------------------------------------------------------------------
QuadraticInterpolator::QuadraticInterpolator():
  mN1(),
  mXmin(),
  mXmax(),
  mXstep(),
  mcoeffs() {
}

//------------------------------------------------------------------------------
// Construct with tabulated data
//------------------------------------------------------------------------------
QuadraticInterpolator::QuadraticInterpolator(const double xmin,
                                             const double xmax,
                                             const std::vector<double>& yvals):
  mN1(),
  mXmin(),
  mXmax(),
  mXstep(),
  mcoeffs() {
  this->initialize(xmin, xmax, yvals);
}

//------------------------------------------------------------------------------
// Initialize the interpolation to fit the given data
// Note, because we're doing a parabolic fit there are 2 fewer sets of coeficients
// than the size of the table we're fitting.
//------------------------------------------------------------------------------
void
QuadraticInterpolator::initialize(const double xmin,
                                  const double xmax,
                                  const std::vector<double>& yvals) {
  const auto n = yvals.size();
  REQUIRE(n > 2);      // Need at least 3 points to fit a parabola
  REQUIRE(xmax > xmin);

  mN1 = n - 3;  // Maximum index into arrays
  mXmin = xmin;
  mXmax = xmax;
  mXstep = (xmax - xmin)/(n - 1);
  mcoeffs.resize(3*(n - 2));

  typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> EMatrix;
  typedef Eigen::Matrix<double, 3, 1> EVector;

  // We use simple least squares fitting for each 3-point interval, giving us an
  // exact parabolic fit for those three points.
  // Find the coefficient fits.
  double x0, x1, x2;
  EMatrix A;
  EVector B, C;
  for (auto i0 = 0u; i0 < n - 2; ++i0) {
    const auto i1 = i0 + 1u;
    const auto i2 = i0 + 2u;
    CHECK(i2 < n);
    x0 = xmin + i0*mXstep;
    x1 = x0 + mXstep;
    x2 = x1 + mXstep;
    A << 1.0, x0, x0*x0,
         1.0, x1, x1*x1,
         1.0, x2, x2*x2;
    B << yvals[i0], yvals[i1], yvals[i2];
    C = A.inverse()*B;
    mcoeffs[3*i0    ] = C(0);
    mcoeffs[3*i0 + 1] = C(1);
    mcoeffs[3*i0 + 2] = C(2);
  }
}

//------------------------------------------------------------------------------
// Destructor
//------------------------------------------------------------------------------
QuadraticInterpolator::~QuadraticInterpolator() {
}

}


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

using namespace bglex;

BOOST_AUTO_TEST_CASE(test1) {
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS>
      Graph;
  Graph G(5);
  boost::add_edge(0, 1, G);
  boost::add_edge(1, 2, G);
  boost::add_edge(1, 4, G);
  boost::add_edge(3, 4, G);

  vector<vector<int>> reference = {
      {0, 1, 2, 3, 4}, {0, 1, 3, 4, 2}, {0, 1, 3, 2, 4}, {0, 3, 1, 2, 4},
      {0, 3, 1, 4, 2}, {3, 0, 1, 2, 4}, {3, 0, 1, 4, 2}};

  std::set<string> reference_str;
  for (auto r : reference) {
    string token = "";
    for (auto v : r) {
      token += to_string(v) + ",";
      reference_str.insert(token);
    }
  }

  vector<vector<Graph::vertex_descriptor>> ts;
  ts = bglex::all_topological_sorts<Graph>(G);

  BOOST_CHECK_EQUAL(ts.size(), reference.size());
  for (auto n = 0; n < ts.size(); n++) {
    BOOST_CHECK_EQUAL(ts[n].size(), 5);
    string token = "";
    for (auto m = 0; m < ts[n].size(); m++) {
      token += to_string(ts[n][m]) + ",";
    }
    BOOST_CHECK(reference_str.find(token) != reference_str.end());
  }
}


#include "advent.hpp"

#include <Eigen/Core>
#include <algorithm>
#include <array>
#include <fmt/ranges.h>
#include <fstream>
#include <functional>
#include <gsl/gsl_util>
#include <iostream>
#include <numeric>
#include <ranges>
#include <scn/scn.h>
#include <vector>

using std::vector;
using std::ranges::sort;
using std::views::iota;
using std::views::transform;

auto day07(int argc, char** argv) -> int
{
    if (argc < 2) {
        fmt::print("Error: no input.");
        return 1;
    }

    std::ifstream infile(argv[1]); // NOLINT
    std::string str;

    std::getline(infile, str);
    vector<i64> pos; // positions
    auto res = scn::scan_list(str, pos, ',');
    ENSURE(res);
    sort(pos);
    auto const sz = std::ssize(pos);
    auto med = sz % 2 == 0 ? pos[sz / 2 - 1] : gsl::narrow<i64>(std::midpoint(pos[sz / 2 - 1], pos[sz / 2]));
    auto mean = std::accumulate(pos.begin(), pos.end(), double { 0 }) / gsl::narrow<double>(sz);
    fmt::print("mean: {}, median: {}\n", mean, med);

    // part 1
    auto costs1 = pos | transform([&](auto p) { return std::abs(med - p); });
    fmt::print("part 1: {} (pos = {})\n", std::accumulate(costs1.begin(), costs1.end(), i64 { 0 }), med);

    // part 2
    auto sum = [](auto n) { return n * (n + 1) / 2; };

    i64 min_cost = std::numeric_limits<i64>::max();
    i64 min_pos = 0;
    for (auto i : iota(gsl::narrow<i64>(std::floor(mean-1)), gsl::narrow<i64>(std::ceil(mean+1)))) {
        auto costs2 = pos | transform([&](auto p) { return sum(std::abs(i - p)); });
        auto cost = std::accumulate(costs2.begin(), costs2.end(), i64 { 0 });
        if (min_cost > cost) {
            min_cost = cost;
            min_pos = i;
        }
    }
    fmt::print("part 2: {} (pos = {})\n", min_cost, min_pos);

    return 0;
}


/*
 * Copyright (C) 2009 The Android Open Source Project
 *
 * 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 <jni.h>
#include "HelloEigen.h"

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

using namespace Eigen;

std::string helloMatrix(){
  std::stringstream ss;
  ss << __PRETTY_FUNCTION__ << std::endl;
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  ss << "Here is the matrix m:\n" << m << std::endl;
  VectorXd v(2);
  v(0) = 4;
  v(1) = v(0) - 1;
  ss << "Here is the vector v:\n" << v << std::endl;
  return ss.str();
}
JNIEXPORT jstring JNICALL Java_com_theveganrobot_cmake_HelloEigen_helloMatrix
  (JNIEnv * env, jobject)
{
    return env->NewStringUTF( helloMatrix().c_str());
}


#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include "../incl/bellman_ford_test.h"
#include "../incl/bellman_ford.h"
#include <iostream>

/**
 * Running tests for the following graphs:
 * - regular small graph
 * - small graph with positive edges
 * - small graph with negative cycle
 * - random graph with 1000 nodes and 20*1000*log(1000) edges
 * - grid graph 100x100
 * Using my CHECK_BELLMAN_FORD() checker
*/
void testAll()
{
    std::cout << "[i] Running all tests" << std::endl;
    std::cout << "[i] Testing Graph with positive weights" << std::endl;
    testBF(createTestGraph());
    std::cout << "[i] Testing Graph with positive and negative weights" << std::endl;
    testBF(createPositiveTestGraph());
    std::cout << "[i] Testing Graph with negative cycle" << std::endl;
    testBF(createTestGraphWithNegativeCycle());
    std::cout << "[i] Random Graph" << std::endl;
    testBF(createRandomGraph(1000, 20*1000*log10(1000), -100, 10000));
    std::cout << "[i] Grid Graph" << std::endl;
    testBF(createGridGraph(100,-100, 10000));
}

/**
 * Running my bellman ford algorithm and the checker afterwords.
 * If problem occur program terminates.
 * Prints the appropriate message.
*/
void testBF(Graph G)
{
    const int INF = (std::numeric_limits < int >::max)();
    int i;
    // init
    unsigned long  n_vertices = num_vertices(G);
    std::vector<unsigned long> pred(n_vertices);
    std::vector<long> dist(n_vertices, INF);
    WeightMap weight = get(&EdgeProperties::weight, G);
    // Running
    bool r_rafa = bellmanFord(G, 0, weight, pred, dist);
    CHECK_BELLMAN_FORD(G, 0, weight, pred, dist);
    std::cout << "[+] Test OK!" << std::endl;
}

/**
 * Given a graph runs the 3 algorithms
 * (boost's, leda's and mine) @param(times) times.
 * On each iteration the algorithms are running from different start node.
 * If times=1 then the start node is always the firstone.
 * Prints the average times, and if a negative cycle detected.
*/
void benchmark(Graph g, int times)
{
    std::cout << "[i] " << times << " time(s)" << std::endl;
    int i;
    // setup
    unsigned long  N = num_vertices(g);

    // **************************************************** BOOST
    // init
    WeightMap weight_pmap = get(&EdgeProperties::weight, g);
    std::vector<int> b_dist(N, (std::numeric_limits < short >::max)());
    std::vector<std::size_t> b_pred(N);
    // run
    bool r_boost;
    clock_t begin = clock();
    for(i = 0; i<times ; ++i){
        for (unsigned long i = 0; i < N; ++i) b_pred[i] = i;
        int s = (times == 1) ? 0 : randomRange(0, (int)N);
        b_dist[s] = 0;
        r_boost = boost::bellman_ford_shortest_paths(
            g, int(N), weight_pmap,
            &b_pred[0], &b_dist[0],
            boost::closed_plus<int>(),
            std::less<int>(),
            boost::default_bellman_visitor());
        if(!r_boost) break;
    }
    clock_t end = clock();
    double elapsed_secs_boost = double(end-begin) / CLOCKS_PER_SEC / (i+1);
    // Info
    std::cout << "      Average time boost: " << elapsed_secs_boost << " seconds";
    if(!r_boost) std::cout << " (negative cycle)" << std::endl;
    else std::cout << std::endl;

    // **************************************************** RAFA
    // init
    std::vector<unsigned long> pred(N);
    std::vector<long> dist(N, (std::numeric_limits < short >::max)());
    // run
    begin = clock();
    bool r_rafa;
    for(i = 0; i<times ; ++i) {
        int s = (times == 1) ? 0 : randomRange(0, (int)N);
        r_rafa = bellmanFord(g, s, weight_pmap, pred, dist);
    }
    end = clock();
    double elapsed_secs_rafa = double(end - begin) / CLOCKS_PER_SEC / times;
    // Info
    std::cout << "      Average time rafa: " << elapsed_secs_rafa << " seconds";
    if(!r_rafa) std::cout << " (negative cycle)" << std::endl;
    else std::cout << std::endl;
}


/**
* \file random-matrix-example.cc
*
* \author Tobias Waurick
* \date 21.06.17
*
*
*/
#include "ns3/random-matrix.h"
#include "ns3/core-module.h"
#include <iostream>
#include <armadillo>
#include <stdint.h>

using namespace std;
using namespace arma;

int main(int argc, char *argv[])
{
	CommandLine cmd;
	uint32_t seed1 = 1,
			seed2 = 1;
	cmd.AddValue("seed1", "first seed", seed1);
	cmd.AddValue("seed2", "second", seed2);
	cmd.Parse(argc, argv);

	cout << "########### 10x20 RANDOM IDENTITY MATRIX ###########" << endl;
	IdentRandomMatrix ranMat(10, 20);
	Col<double> x = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20};
	Row<double> y = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
	mat test = ranMat;

	cout << "-Seed: " << seed1 << endl;
	ranMat.Generate(seed1);
	cout << ranMat;

	cout << "-Seed: " << seed2 << endl;
	ranMat.Generate(seed2);
	cout << ranMat;

	cout << "-Multiplication: Mat * [1 2 3 ... 20]T" << endl;
	cout << (ranMat * x);

	cout << "-Multiplication: [1 2 3 .. 10]*Mat" << endl;
	cout << (y * ranMat);

	cout << "Rescaling to 5x10" << endl;
	ranMat.SetSize(5, 10);
	cout << ranMat;
	cout << "########### 10x20 RANDOM GAUSSIAN MATRIX with mean 0 var 1###########" << endl;
	GaussianRandomMatrix ranMat2(0, 1, 10, 20);

		cout << "-Seed: " << seed1 << endl;
	ranMat2.Generate(seed1);
	cout << ranMat2;

	cout << "-Seed: " << seed2 << endl;
	ranMat2.Generate(seed2);
	cout << ranMat2;

	cout << "-Mean :" << endl;
	cout << mean(mean(mat(ranMat2))) << endl;
	cout << "-Variance :" << endl;
	test = mat(ranMat2);
	test.reshape(200, 1);
	cout << var(test) << endl;

	cout << "Normalized" << endl;
	ranMat2.NormalizeToM();
	cout << ranMat2;
	cout << "-Mean :" << endl;
	cout << mean(mean(mat(ranMat2))) << endl;
	cout << "-Variance :" << endl;
	test = mat(ranMat2);
	test.reshape(200, 1);
	cout << var(test) << endl;

	cout << "Rescaling to 5x10" << endl;
	ranMat2.SetSize(5, 10);
	cout << ranMat2;
	cout << "########### 10x20 RANDOM BERNOULLIMATRIX###########" << endl;
	BernRandomMatrix ranMat3(10, 20);
		cout << "-Seed: " << seed1 << endl;
	ranMat3.Generate(seed1);
	cout << ranMat3;

	cout << "-Seed: " << seed2 << endl;
	ranMat3.Generate(seed2);
	cout << ranMat3;

	cout << "Normalized" << endl;
	ranMat3.NormalizeToM();
	cout << ranMat3;

	cout << "-Mean :" << endl;
	cout << mean(mean(mat(ranMat3))) << endl;
}


#include "MatrixSolver.hpp"
#include <Eigen/Dense>
#include <cassert>
#include <iostream>

using namespace Eigen;

MatrixSolver::MatrixSolver(DecompositionType decompositionType)
    : _decompositionType(decompositionType) {}

void MatrixSolver::solve(const Eigen::MatrixXd &A, const Eigen::VectorXd &b,
                         Eigen::VectorXd &x)
{
  if (_decompositionType == LU) {
    PartialPivLU<MatrixXd> LU;
    LU.compute(A);
    x = LU.solve(b);
  } else if (_decompositionType == QR) {
    FullPivHouseholderQR<MatrixXd> QR;
    QR.compute(A);
    x = QR.solve(b);
  } else if (_decompositionType == LU2) {
    PartialPivLU<MatrixXd> LU;
    LU.compute(A);
    x = LU.solve(b);
    VectorXd residual =
        b -
        A * x; // only really effective if we could go to higher precision here
    x = x + LU.solve(residual);
  } else {
    assert(false); // Unsupported decomposition type, should be caught in configuration
                   // already
  }
}


/*
 * EigenDecomposition.hpp
 *
 *  Created on: 11/01/2011
 *      Author: jimali
 */

#ifndef EIGENDECOMPOSITION_HPP_
#define EIGENDECOMPOSITION_HPP_

#if !defined(SWIG)
#include <Eigen/Core>
#include <vector>
#endif
namespace rw { namespace math {
    //! @brief Type representing a set of eigen values and eigen vectors.
    template< class T = double > class EigenDecomposition
    {
      public:
        /**
         * @brief Construct new decomposition.
         * @param vectors [in] the eigen vectors as columns in a matrix.
         * @param values [in] the corresponding eigen values.
         */
        EigenDecomposition (Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > vectors,
                            Eigen::Matrix< T, Eigen::Dynamic, 1 > values) :
            _vectors (vectors),
            _values (values)
        {}

        /**
         * @brief returns all eigenvectors as columns in a matrix
         * @return reference to the matrix.
         */
        const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic >& getEigenVectors ()
        {
            return _vectors;
        }

        /**
         * @brief returns the i'th eigenvector
         * @return the eigen vector.
         */
        Eigen::Matrix< T, Eigen::Dynamic, 1 > getEigenVector (size_t i) { return _vectors.col (i); }

        /**
         * @brief return all eigenvalues
         * @return the eigen values.
         */
        const Eigen::Matrix< T, Eigen::Dynamic, 1 >& getEigenValues () { return _values; }

        /**
         * @brief returns the i'th eigenvalue
         * @return the eigenvalue.
         */
        T getEigenValue (size_t i) { return _values (i); }

        //! @brief Sort function for ordering of eigen values and vectors.
        struct MapSort
        {
          private:
            Eigen::Matrix< T, Eigen::Dynamic, 1 >& _values;

          public:
            /**
             * @brief Construct new sort function struct.
             * @param values [in] the eigen values.
             */
            MapSort (Eigen::Matrix< T, Eigen::Dynamic, 1 >& values) : _values (values) {}

            /**
             * @brief Compare the eigen values with the given indices.
             * @param i0 [in] index of first eigenvalue.
             * @param i1 [in] index of second eigenvalue.
             * @return true if first eigenvalue comes before the second (it is smaller), false
             * otherwise.
             */
            bool operator() (const int& i0, const int& i1)
            {
                using namespace rw::math;
                return _values (i0) < _values (i1);
            }
        };

        /**
         * @brief sorts the eigen vectors according to their eigen value. The vector with smallest
         * eigen value has index 0
         */
        void sort ()
        {
            std::vector< int > map (_values.size ());
            for (size_t i = 0; i < map.size (); i++)
                map[i] = (int) i;

            std::sort (map.begin (), map.end (), MapSort (_values));
            // now the mapping determines how the new vectors are to be layed out
            Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > vectors = _vectors;
            Eigen::Matrix< T, Eigen::Dynamic, 1 > values               = _values;
            for (size_t i = 0; i < map.size (); i++) {
                _vectors.col (i) = vectors.col (map[i]);
                _values (i)      = values (map[i]);
            }
        }

      private:
        Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > _vectors;
        Eigen::Matrix< T, Eigen::Dynamic, 1 > _values;
    };

#if !defined(SWIG)
    using EigenDecompositiond = EigenDecomposition< double >;
    using EigenDecompositionf = EigenDecomposition< float >;
#else
#if SWIG_VERSION < 0x040000
    SWIG_DECLARE_TEMPLATE (EigenDecomposition_d, rw::math::EigenDecomposition< double >);
    ADD_DEFINITION (EigenDecomposition_d, EigenDecomposition)
#else
    SWIG_DECLARE_TEMPLATE (EigenDecomposition, rw::math::EigenDecomposition< double >);
#endif
    SWIG_DECLARE_TEMPLATE (EigenDecomposition_f, rw::math::EigenDecomposition< float >);
#endif
}}    // namespace rw::math

#endif /* EIGENDECOMPOSITION_HPP_ */


#include <mpp.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <functional>
#include <random>
#include <cstddef>

using namespace mpp::hamiltonian;
using namespace mpp::chains;
using namespace boost::numeric::ublas;

double log_posterior(vector<double> const & q){
    double log_post_val(0);
    for(std::size_t i=0;i<q.size();++i) {
        log_post_val -= q(i)*q(i);
    }
    return log_post_val;
}

vector<double> grad_log_posterior(vector<double> const & q) {
    vector<double> dq(q.size());
    for(std::size_t i=0;i<q.size();++i) {
        dq(i) = -q(i);
    }
    return dq;
}

int main() {
    std::size_t const num_dims(10);
    std::size_t const max_num_steps(10);
    double const max_eps(1);
    vector<double> inv_mass_mat(num_dims);
    for(std::size_t i=0;i<num_dims;++i) {
        inv_mass_mat(i) = double(1);
    }

    std::function<double(vector<double> const &)> lp = log_posterior;
    std::function<
        vector<double> (vector<double> const &) > glp = grad_log_posterior;

    hmc_sampler<double>  hmc_spr(
        lp,
        glp,
        num_dims,
        max_num_steps,
        max_eps,
        inv_mass_mat
    );

    std::size_t const num_samples(1000);
    std::mt19937 rng;
    std::normal_distribution<double> nrm_dist;
    vector<double> q_0(num_dims);
    for(size_t i=0;i<num_dims;++i) {
        q_0(i) = nrm_dist(rng);
    }
    mcmc_chain<double> chn = hmc_spr.run_sampler(num_samples,q_0);
    std::string chn_file_name("./eg_classic_hmc.chain");
    chn.write_samples_to_csv(chn_file_name);

    return 0;
}


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

#include "math_unit_test.hpp"
#include <boost/math/tools/luroth_expansion.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::tools::luroth_expansion;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::constants::pi;

template<class Real>
void test_integral()
{
    for (int64_t i = -20; i < 20; ++i) {
        Real ii = i;
        auto luroth = luroth_expansion<Real>(ii);
        auto const & a = luroth.digits();
        CHECK_EQUAL(size_t(1), a.size());
        CHECK_EQUAL(i, a.front());
    }
}

template<class Real>
void test_halves()
{
    // x = n + 1/k => lur(x) = ((n; k - 1))
    // Note that this is a bit different that Kalpazidou (examine the half-open interval of definition carefully).
    // One way to examine this definition is correct for rationals (it never happens for irrationals)
    // is to consider i + 1/3. If you follow Kalpazidou, then you get ((i, 3, 0)); a zero digit!
    // That's bad since it destroys uniqueness and also breaks the computation of the geometric mean.
    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(1)/Real(2);
        auto luroth = luroth_expansion<Real>(x);
        auto const & a = luroth.digits();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i, a.front());
        CHECK_EQUAL(int64_t(1), a.back());
    }

    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(1)/Real(4);
        auto luroth = luroth_expansion<Real>(x);
        auto const & a = luroth.digits();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i, a.front());
        CHECK_EQUAL(int64_t(3), a.back());
    }

    for (int64_t i = -20; i < 20; ++i) {
        Real x = i + Real(1)/Real(8);
        auto luroth = luroth_expansion<Real>(x);
        auto const & a = luroth.digits();
        CHECK_EQUAL(size_t(2), a.size());
        CHECK_EQUAL(i, a.front());
        CHECK_EQUAL(int64_t(7), a.back());
    }
    // 1/3 is a pain because it's not representable:
    Real x = Real(1)/Real(3);
    auto luroth = luroth_expansion<Real>(x);
    auto const & a = luroth.digits();
    CHECK_EQUAL(size_t(2), a.size());
    CHECK_EQUAL(int64_t(0), a.front());
    CHECK_EQUAL(int64_t(2), a.back());
}

int main()
{
    test_integral<float>();
    test_integral<double>();
    test_integral<long double>();
    test_integral<cpp_bin_float_100>();

    test_halves<float>();
    test_halves<double>();
    test_halves<long double>();
    test_halves<cpp_bin_float_100>();

    #ifdef BOOST_HAS_FLOAT128
    test_integral<float128>();
    test_halves<float128>();
    #endif
    return boost::math::test::report_errors();
}


#include <stdio.h>
#include <malloc.h>

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"

#define VUL_DEFINE
#include "vul_timer.h"

#include <Eigen/SVD>

using namespace Eigen;

int main( int argc, char **argv )
{
   if( argc < 3 ) {
      printf( "Usage: %s [path to image] [rank to reconstruct from] [iterations (optional)]\n", argv[ 0 ] );
      exit(1);
   }

   int w, h, n;
   unsigned char *data = stbi_load( argv[ 1 ], &w, &h, &n, 0 );

   // @TODO(thynn): Don't average into grayscale, do something useful (or each channel separately)
	MatrixXf A = MatrixXf( h, w );
   for( int y = 0; y < h; ++y ) {
      for( int x = 0; x < w; ++x ) {
         float v = ( float )data[ ( y * w + x ) * n ] / 255.f;
         for( int c = 1; c < n; ++c ) {
            v += ( float )data[ ( y * w + x ) * n + c ] / 255.f;
         }
			A( y, x ) = v / ( float )n;
      }
   }

   char *eptr;
   int rank = strtol( argv[ 2 ], &eptr, 10 );
   int wrank = rank;

   printf( "Computing SVD of %s (%dx%d)\n", argv[ 1 ], w, h );
   vul_timer *t = vul_timer_create( );
	JacobiSVD< MatrixXf > svd( A, ComputeThinU | ComputeThinV );
   uint64_t mms = vul_timer_get_micros( t );
   printf( "Completed in %lu.%lus\n", mms / 1000000, mms % 1000000 );

	VectorXf sigma = svd.singularValues();
   for( int i = 0; i < svd.rank(); ++i ) {
      printf( "S[%d]: %f\n", i, sigma[i] );
   }

   printf( "Rank of decomposition %d, wanted at most %d\n", svd.rank(), wrank );

	MatrixXf B = svd.matrixU() * svd.singularValues().asDiagonal() * svd.matrixV().transpose();

   unsigned char *odata = ( unsigned char* )malloc( sizeof( unsigned char ) * w * h * 3 );
   for( int y = 0; y < h; ++y ) {
      for( int x = 0; x < w; ++x ) {
         float v = B( y, x ) * 255.f;
         v = v < 0.f   ? 0.f
           : v > 255.f ? 255.f
           : v;
         odata[ ( y * w + x ) * 3     ] = v;
         odata[ ( y * w + x ) * 3 + 1 ] = v;
         odata[ ( y * w + x ) * 3 + 2 ] = v;
      }
   }
   stbi_write_bmp( "out.bmp", w, h, 3, odata );
   printf( "Wrote output to out.bmp.\n");
   for( int y = 0; y < h; ++y ) {
      for( int x = 0; x < w; ++x ) {
         float v = A( y, x ) * 255.f;
         v = v < 0.f   ? 0.f
           : v > 255.f ? 255.f
           : v;
         odata[ ( y * w + x ) * 3     ] = v;
         odata[ ( y * w + x ) * 3 + 1 ] = v;
         odata[ ( y * w + x ) * 3 + 2 ] = v;
      }
   }
   stbi_write_bmp( "source.bmp", w, h, 3, odata );
   printf( "Wrote input to source.bmp.\n");

   free( odata );
   free( data );
}


#include <iostream>
#include <vector>
#include <limits>
#include <cmath>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/pending/disjoint_sets.hpp>

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

using namespace std;

// Essentially, the tasks asks for the cost of the second best spanning tree.
// Leia performs Prims MST algorithm. We need at least one edge different to the MST,
// therefore, the best we can do is computing the second best spanning tree.
void solve() {
  // Tatooine is not needed for anything
  int n, tatooine;
  cin >> n >> tatooine;

  // Compute a MST (with any algorithm)
  weighted_graph G(n);

  int cost;
  vector<vector<int>> costs(n, vector<int>(n));
  for (int j = 0; j < n - 1; ++j) {
    for (int k = 0; k < n - j - 1; ++k) {
      int u = j;
      int v = j + k + 1;
      cin >> cost;
      costs[u][v] = cost;
      costs[v][u] = cost;
      boost::add_edge(u, v, cost, G);
    }
  }

  // Use Kurskal (we could also use Prim...does not make any difference)
  std::vector<edge_desc> mst;
  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst));

  // Extract neighbors from MST and compute MST Value
  int mstValue = 0;
  vector<vector<int>> neighbors(n);
  vector<vector<int>> isMstEdge(n, vector<int>(n, false));
  for (auto edge : mst) {
    int u = boost::source(edge, G);
    int v = boost::target(edge, G);
    mstValue += costs[u][v];
    neighbors[u].push_back(v);
    neighbors[v].push_back(u);
    isMstEdge[u][v] = true;
    isMstEdge[v][u] = true;
  }

  // Compute the most expensive edge for every u-v path in the MST
  vector<vector<int>> uvMaxEdge(n, vector<int>(n, 0));

  for (int u = 0; u < n; ++u) {
    vector<int> stack;
    stack.push_back(u);
    vector<int> visited(n, false);
    visited[u] = true;

    while (!stack.empty()) {
      int v = *stack.rbegin();
      stack.pop_back();

      for (auto w : neighbors[v]) {
        if (!visited[w]) {
          visited[w] = true;
          uvMaxEdge[u][w] = max(costs[v][w], uvMaxEdge[u][v]);
          stack.push_back(w);
        }
      }
    }
  }

  // For all edges e = {u, v}, that are not in the MST, we will try adding e to the MST.
  // This will introduce a cycle. We remove the most expensive edge of the cycle that is
  // part of the MST. For this we can simply query the most expensive edge from u to v in the MST
  // and remove it. The cost of the MST changes by delta = cost[u][v] - cost[most expensive edge].
  // We simply find the best delta.
  int minVal = numeric_limits<int>::max();
  for (int u = 0; u < n; ++u) {
    for (int v = u + 1; v < n; ++v) {
      if (!isMstEdge[u][v]) {
        int val = costs[u][v] - uvMaxEdge[u][v];
        minVal = min(val, minVal);
      }
    }
  }

  cout << mstValue + minVal << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  while(t--) {
    solve();
  }
}


#pragma once

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <array>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multi_array.hpp>
#include <vector>

#include <cmath>
#include <fstream>
#include <iostream>

#include "mpfr/import_std_math.hpp"
#include "quadrature/qhermitew.hpp"
#include "spectral/hermitenw.hpp"
#include "spectral/lagrange_polynomial.hpp"

namespace boltzmann {

/**
 * @brief Nodal <-> Hermite transformation matrices in 1d.
 *
 * @remark
 * the nodal basis consists of Lagrange polynomials at Hermite quadrature nodes.
 * The underlying Lagrange polynomials satisfy:
 *
 * \f$ l_i(x_j) = 0 \quad \textrm{for } i  \neq j \f$
 *
 * and
 *
 * \f$ l_i(x_i) = 1/sqrt(w_i) \f$
 *
 * where \f$w_i\f$ are the Gauss-Hermite quadrature weights.
 *
 */
template <typename NUMERIC_T = double>
class H2N_1d
{
 public:
  typedef NUMERIC_T numeric_t;
  //  typedef Eigen::Matrix<numeric_t, Eigen::Dynamic, Eigen::Dynamic> matrix_t;

 public:
  /**
   *
   *
   * @param H2N  hermite to nodal matrix
   * @param N2H  nodal to hermite matrix
   * @param K    max. polynomial degree
   */
  template <typename MATRIX>
  static void create(MATRIX& H2N, MATRIX& N2H, const int K);
};

// --------------------------------------------------------------------------------
template <typename NUMERIC_T>
template <typename MATRIX>
void
H2N_1d<NUMERIC_T>::create(MATRIX& H2N, MATRIX& N2H, const int K)
{
  N2H.resize(K, K);
  H2N.resize(K, K);
  //  N2H_.resize(K,K);
  QHermiteW quad(1, K);
  HermiteNW<numeric_t> hermw(K);
  hermw.compute(quad.pts());

  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      const double wi = quad.wts(i);
      H2N(i, j) = hermw.get(j)[i] * ::math::sqrt(wi);
    }
  }

  N2H = H2N.transpose();
}

// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
/**
 * @brief Nodal <-> Hermite transformation matrices in 1d, nodes of the nodal basis can be
 * arbitrary.
 *
 * The nodes of the nodal basis are located at the nodes of the Gauss-Hermite quadrature rule
 * with weight function  \f$ exp(-\alpha r^2) \f$.
 *
 * @remark
 * ATTENTION: H2N(a=1) is orthonormal, but this is not the case when \f$ a \neq 1\f$!
 *
 *
 * If this class is used together with @a Hermite2Nodal (and alpha!=1), the transformation
 * `N->H` will give wrong results! Only use H->N.  TODO: update code such that BOTH forward AND
 * BACKWARD transformation works.
 *
 */
class H2NG_1d
{
 public:
  template <typename MATRIX>
  static void create(MATRIX& H2N, MATRIX& N2H, const int K, const double alpha)
  {
    typedef Eigen::VectorXd vec_t;

    H2N.resize(K, K);

    QHermiteW quad4nodes(alpha, K);
    QHermiteW quad(1, K);

    // Lagrange Poly
    vec_t xi = Eigen::Map<const vec_t>(quad4nodes.points_data(), K);
    vec_t li(K);  // lambda's (barycentric interpolation)
    vec_t yi(K);
    vec_t x = Eigen::Map<const vec_t>(quad.points_data(), K);
    vec_t w = Eigen::Map<const vec_t>(quad.weights_data(), K);
    vec_t y(K);
    // prepare barycentric interpol. weights
    LagrangePolynomial<>::compute_weights(li, xi);
    // evaluate hermite polynomials at quadrature points
    HermiteNW<double> hermw(K);
    hermw.compute(quad.pts());
    const double ah4 = std::pow(alpha, 0.25);

    Eigen::MatrixXd T(K, K);
    for (int i = 0; i < K; ++i) {
      yi.setZero();
      yi[i] = 1.0;
      LagrangePolynomial<>::evaluate(y, x, xi, yi, li);
      const double sqrtwi = std::sqrt(w[i]);
      for (int j = 0; j < K; ++j) {
        double sum = 0;
        // quadrature
        for (int q = 0; q < K; ++q) {
          sum += hermw.get(j)[q] * std::exp((x[i] * x[i] - x[q] * x[q]) / 2) * ah4 * y[q] * w[q] /
                 sqrtwi;
        }
        T(i, j) = sum;
      }
    }

    Eigen::MatrixXd M(K, K);
    vec_t y1(K);
    vec_t y2(K);
    const double ah2 = std::sqrt(alpha);
    // mass matrix (cf. notes in yellow notebook)
    for (int i = 0; i < K; ++i) {
      yi.setZero();
      yi[i] = 1.0;
      LagrangePolynomial<>::evaluate(y1, x, xi, yi, li);
      for (int j = 0; j < K; ++j) {
        yi.setZero();
        yi[j] = 1.0;
        LagrangePolynomial<>::evaluate(y2, x, xi, yi, li);
        double sum = 0;
        for (int q = 0; q < K; ++q) {
          sum += std::exp((x[i] * x[i] + x[j] * x[j]) / 2 - x[q] * x[q]) * y1[q] * y2[q] /
                 std::sqrt(w[i] * w[j]) * w[q];
        }
        M(i, j) = ah2 * sum;
      }
    }
    H2N = M.inverse() * T;
    N2H = T.transpose();
  }
};

}  // end namespace boltzmann


/*
* 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.
*/

/**
 * @brief demo for visualizing eigenmodes
 */

#include <Eigen/Eigenvalues>
#if defined(_WIN32) // windows
#  define NOMINMAX   // to remove min,max macro
#  include <windows.h>  // should put before glfw3.h
#endif
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>

#include "delfem2/eigen/ls_dense.h"
#include "delfem2/lsitrsol.h"
#include "delfem2/mshprimitive.h"
#include "delfem2/mshuni.h"
#include "delfem2/points.h"
#include "delfem2/femsolidlinear.h"
#include "delfem2/glfw/viewer3.h"
#include "delfem2/glfw/util.h"
#include "delfem2/opengl/old/mshuni.h"

namespace dfm2 = delfem2;

void ShowEigen_SolidLinear_MeshQuad2(
    const std::vector<double>& aXY,
    const std::vector<unsigned int>& aQuad,
    double elen)
{
  const unsigned int np = aXY.size() / 2;
  Eigen::MatrixXd A(np * 2, np * 2);
  A.setZero();

  double emat[4][4][2][2];
  dfm2::EMat_SolidLinear2_QuadOrth_GaussInt(emat, elen, elen, 1.0, 1.0, 2);
  std::vector<unsigned int> tmp_buffer;
  for (unsigned int iq = 0; iq < aQuad.size() / 4; ++iq) {
    const unsigned int aIp[4] = {
        aQuad[iq * 4 + 0], aQuad[iq * 4 + 1], aQuad[iq * 4 + 2], aQuad[iq * 4 + 3]};
    delfem2::Merge<4,4,2,2,double>(A,aIp,aIp,emat,tmp_buffer);
  }

  {
    auto B = A;
    B(0,0) += 1.0;
    B(1,1) += 1.0;
    B(2,2) += 1.0;
    B(3,3) += 1.0;
    Eigen::VectorXd r(np*2), u(np*2), Ap(np*2), p(np*2);
    r.setRandom();
    std::vector<double> aConv = dfm2::Solve_CG(r,u,Ap,p,1.0e-5,300,B);
    std::cout << aConv.size() << std::endl;
  }

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(A);
  const auto& evec = s.eigenvectors(); // somehow this takes time

  //
  delfem2::glfw::CViewer3 viewer;
  delfem2::glfw::InitGLOld();
  viewer.OpenWindow();

  std::vector<double> aDisp(np*2,0.0);

  for(unsigned int iframe=0;iframe<10;++iframe){
    std::cout << s.eigenvalues()(iframe) << std::endl;
    for(unsigned int i=0;i<np*2;++i){
      aDisp[i] = evec(i,iframe);
    }
    for(unsigned int i=0;i<100;++i) {
      viewer.DrawBegin_oldGL();
      dfm2::opengl::DrawMeshQuad2D_EdgeDisp(
          aXY.data(), aXY.size()/2,
          aQuad.data(), aQuad.size()/4,
          aDisp.data(),sin(i*0.1));
      viewer.SwapBuffers();
      glfwPollEvents();
    }
  }
  glfwDestroyWindow(viewer.window);
  glfwTerminate();
}

void ShowEigen_SolidLinear_MeshTri2(
    const std::vector<double>& aXY,
    const std::vector<unsigned int>& aTri)
{
  const unsigned int np = aXY.size()/2;
  Eigen::MatrixXd A(np*2, np*2);
  A.setZero();

  std::vector<double> aDisp(np*2,0.0);
  std::vector<unsigned int> tmp_buffer;
  for(unsigned int it=0;it<aTri.size()/3;++it){
    const unsigned int aIp[3] = {aTri[it*3+0], aTri[it*3+1], aTri[it*3+2]};
    double disp[3][2]; dfm2::FetchData<3,2>(disp,aIp,aDisp.data());
    double coord[3][2]; dfm2::FetchData<3,2>(coord,aIp,aXY.data());
    double emat[3][3][2][2], eres[3][2];
    dfm2::EMat_SolidStaticLinear_Tri2D(
        eres,emat,
        1,1,0,0,0,
        disp,coord);
    delfem2::Merge<3,3,2,2,double>(A,aIp,aIp,emat,tmp_buffer);
  }

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(A);
  const auto& evec = s.eigenvectors();
  //
  delfem2::glfw::CViewer3 viewer;
  delfem2::glfw::InitGLOld();
  viewer.OpenWindow();
  for(unsigned int iframe=0;iframe<10;++iframe){
    std::cout << s.eigenvalues()(iframe) << std::endl;
    for(unsigned int i=0;i<np*2;++i){
      aDisp[i] = evec(i,iframe);
    }
    for(unsigned int i=0;i<100;++i) {
      viewer.DrawBegin_oldGL();
      dfm2::opengl::DrawMeshTri2D_EdgeDisp(
          aXY.data(), aXY.size()/2,
          aTri.data(), aTri.size()/3,
          aDisp.data(),sin(i*0.1));
      viewer.SwapBuffers();
      glfwPollEvents();
    }
  }
  glfwDestroyWindow(viewer.window);
  glfwTerminate();
}

int main()
{
  std::cout << "Available :SIMD Instructions: "<< Eigen::SimdInstructionSetsInUse() << std::endl;
  //
  std::vector<double> aXY;
  std::vector<unsigned int> aQuad;
  unsigned int nx = 10;
  unsigned int ny = 5;
  dfm2::MeshQuad2D_Grid(
      aXY,aQuad,
      nx,ny);
  const double elen = 0.1;
  {
    dfm2::Scale_Points(aXY.data(), aXY.size() / 2, 2, elen);
    dfm2::Translate_Points2(aXY,-0.5*nx*elen, -0.5*ny*elen);
  }
  ShowEigen_SolidLinear_MeshQuad2(aXY, aQuad, elen);
  //
  std::vector<unsigned int> aTri;
  dfm2::convert2Tri_Quad(
      aTri,
      aQuad);
  ShowEigen_SolidLinear_MeshTri2(aXY,aTri);
}


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/internal/scopedHostPtr.h>
#include <gauss/linalg.h>
#include <gauss/polynomial.h>

#include <Eigen/Eigenvalues>

using namespace Eigen;

constexpr auto EPSILON = 1e-8;

namespace {

af::array vandermonde(const af::array &x, int order, bool ascending) {
    af::array result = af::array(x.dims(0), order, x.type());
    gfor(af::seq i, order) { result(af::span, i) = af::pow(x, i); }
    if (!ascending) {
        result = af::flip(result, 1);
    }
    return result;
}

}  // namespace

af::array gauss::polynomial::polyfit(const af::array &x, const af::array &y, int deg) {
    int order = deg + 1;
    af::array lhs = vandermonde(x, order, false);
    const af::array &rhs = y;

    af::array scale = af::max(af::sqrt(af::sum(lhs * lhs, 0)), EPSILON);

    lhs /= af::tile(scale, static_cast<unsigned int>(lhs.dims(0)));

    af::array c = gauss::linalg::lls(lhs, rhs);
    c = af::transpose(c);
    c /= af::tile(scale, static_cast<unsigned int>(c.dims(0)));
    c = af::transpose(c);

    return c;
}

af::array gauss::polynomial::roots(const af::array &pp) {
    af::array result = af::array(pp.dims(0) - 1, pp.dims(1), af::dtype::c32);
    for (int i = 0; i < pp.dims(1); i++) {
        af::array p = pp(af::span, i);
        // Strip leading and trailing zeros
        p = p(p != 0);

        p = (-1 * p(af::seq(1, static_cast<double>(p.dims(0)) - 1), af::span)) /
            af::tile(p(0, af::span), static_cast<unsigned int>(p.dims(0)) - 1);

        auto coeffs = gauss::utils::makeScopedHostPtr(p.as(af::dtype::f32).host<float>());

        Eigen::VectorXf vec = Eigen::VectorXf::Ones(p.dims(0));
        Eigen::MatrixXf diag = vec.asDiagonal();

        Eigen::MatrixXf diag2(diag.rows(), diag.cols());
        int rest = static_cast<int>(diag.rows()) - 1;
        diag2.topRows(1) = Eigen::Map<Eigen::MatrixXf>(coeffs.get(), 1, p.dims(0));
        diag2.bottomRows(rest) = diag.topRows(rest);

        Eigen::VectorXcf eivals = diag2.eigenvalues();

        result(af::span, i) = af::array(p.dims(0), (af::cfloat *)eivals.data());
    }

    return result;
}


#ifndef RANDOMNUMBERGENERATOR_HPP_
#define RANDOMNUMBERGENERATOR_HPP_

// for random sampling
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions.hpp>
#include <cstdlib>
#include <mutex>
#include <Eigen/Core>

template<typename Dtype>
class RandomNumberGenerator {

 public:

  RandomNumberGenerator() {
  }

  ~RandomNumberGenerator() {
  }

  /* mean =0, std = 1*/
  Dtype sampleNormal() {
    auto dist = boost::random::normal_distribution<Dtype>(0.0, 1.0);
    return dist(rngGenerator);
  }

  /* from -1 to 1*/
  Dtype sampleUniform() {
    auto dist = boost::uniform_real<Dtype>(-1, 1);
    return dist(rngGenerator);
  }

  /* from 0 to 1*/
  Dtype sampleUniform01() {
    auto dist = boost::uniform_real<Dtype>(0, 1);
    return dist(rngGenerator);
  }

  bool forXPercent(float epsilon) {
    auto dist = boost::uniform_real<Dtype>(0, 1);
    return dist(rngGenerator) < epsilon;
  }

  int intRand(const int &min, const int &max) {
    {
      std::uniform_int_distribution<int> distribution(min, max);
      return distribution(rngGenerator);
    }
  }

  /* weighted random sampling where the weights are given by 1, r, r^2, ... */
  int intWeightedRand (const int &max, Dtype weightDecayFtr) {
    Dtype sum = (Dtype(1) - std::pow(weightDecayFtr, max+1)) / (Dtype(1)-weightDecayFtr);
    return std::ceil(log(Dtype(1) - sampleUniform01() * sum * (Dtype(1) - weightDecayFtr)) / log(weightDecayFtr)) - 1;
  }

  template<int dim>
  void sampleVectorInNormalUniform(Dtype *vector) {
    auto dist = boost::uniform_real<Dtype>(-1, 1);
    for (int i = 0; i < dim; i++)
      vector[i] = dist(rngGenerator);
  }

  template<int dim>
  void sampleInUnitSphere(Dtype *vector) {
    sampleVectorInNormalUniform<dim>(vector);
    Dtype sum = 0.0f;

    for (int i = 0; i < dim; i++)
      sum += vector[i] * vector[i];

    Dtype amplitudeOverSum = pow(std::abs(sampleUniform()), Dtype(1.0) / Dtype(dim)) / sqrtf(sum);

    for (int i = 0; i < dim; i++)
      vector[i] = vector[i] * amplitudeOverSum;
  }

  template<int dim>
  void sampleOnUnitSphere(Dtype *vector) {
    sampleVectorInNormalUniform<dim>(vector);
    Dtype sum = 0.0f;

    for (int i = 0; i < dim; i++)
      sum += vector[i] * vector[i];

    for (int i = 0; i < dim; i++)
      vector[i] = vector[i] / sqrtf(sum);
  }

  template<typename Derived>
  void shuffleSTDVector(std::vector<Derived> &order) {
    boost::variate_generator<boost::mt19937 &, boost::uniform_int<> >
        random_number_shuffler(rngGenerator, boost::uniform_int<>());
    std::random_shuffle(order.begin(), order.end(), random_number_shuffler);
  }

  /* this method is opitmized for memory use. The column should be dynamic size */
  template<typename Derived, int Rows, int Cols>
  void shuffleColumns(Eigen::Matrix<Derived, Rows, Cols> &matrix) {
    int colSize = int(matrix.cols());

    /// sampling the order
    std::vector<int> order;
    std::vector<bool> needSuffling(colSize, true);

    order.resize(colSize);
    for (int i = 0; i < colSize; i++) order[i] = i;
    shuffleSTDVector(order);
    Eigen::Matrix<Derived, Rows, 1> memoryCol(matrix.rows());

    int colID;

    for (int colStartID = 0; colStartID < colSize; colStartID++) {
      if (order[colStartID] == colStartID || !needSuffling[colStartID]) continue;

      colID = colStartID;
      memoryCol = matrix.col(colID);
      do {
        matrix.col(colID) = matrix.col(order[colID]);
        needSuffling[colID] = false;
        colID = order[colID];
      } while (colStartID != order[colID]);
      matrix.col(colID) = memoryCol;
      needSuffling[colID] = false;
    }
  }

  std::vector<unsigned> getNrandomSubsetIdx (unsigned nOfElem, unsigned nOfSubElem) {
    std::vector<unsigned> memoryIdx(nOfSubElem);
    ///// randomly sampling memory indeces
    for (unsigned i = 0; i < nOfSubElem; i++) {
      memoryIdx[i] = intRand(0, nOfElem - 1);
      for (unsigned j = 0; j < i; j++) {
        if (memoryIdx[i] == memoryIdx[j]) {
          i--;
          break;
        }
      }
    }
    return memoryIdx;
  }

  /*you can use this method to make the random samples the same*/
  void seed(uint32_t seed) {
    rngGenerator.seed(seed);
  }

 private:
  boost::random::mt19937 rngGenerator;

};

#endif /* RANDOMNUMBERGENERATOR_HPP_ */


#include <iostream>
#include <armadillo>
#include <test_class_one.h>

using namespace std;
using namespace arma;

int main(int argc, char** argv) {
	TestClassOne cls;
	cout << cls.show() << endl;

	cout << "Armadillo version: " << arma_version::as_string() << endl;
	mat A = randn<mat>(4,4);
	mat B = randn<mat>(4,4);
	mat C = A * B;
	A.print("A:");
	B.print("B:");
	C.print("AxB:");
}


#pragma once
#include "shape.hpp"
#include <Eigen/Core>
#include <tuple>

//! The gradient of the shape function (on the reference element) for LINEAR FEM
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
	std::ignore = x;
	std::ignore = y;

	return Eigen::Vector2d(-1 + (i > 0) + (i == 1),
	                       -1 + (i > 0) + (i == 2));
	return Eigen::Vector2d(0, 0); //remove when implemented
}

//----------------gradshapefunBegin----------------
//! The gradient of the shape function (on the reference element) for QUADRATIC FEM
//!
//! We have six shape functions
//!
//! @param i integer between 0 and 5 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientShapefun(const int i, double x, double y) {
	// Eigen::Vector2d value;
	// (write your solution here)
	assert(0 <= i && i <= 5);
	switch (i) {
		case 0:
			return Eigen::Vector2d(-3 + 4 * x + 4 * y, -3 + 4 * x + 4 * y);
		case 1:
			return Eigen::Vector2d(4 * x - 1, 0);
		case 2:
			return Eigen::Vector2d(0, 4 * y - 1);
		case 3:
			return Eigen::Vector2d(4 - 8 * x - 4 * y, -4 * x);
		case 4:
			return Eigen::Vector2d(4 * y, 4 * x);
		case 5:
			return Eigen::Vector2d(-4 * y, 4 - 4 * x - 8 * y);
		default:
			throw std::domain_error("i not in {0,1,2,3,4,5}");
	}
	//return value;
}
//----------------gradshapefunEnd----------------


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date aug-21
 * @license GPLv3, see 'LICENSE' file
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Fft1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_fft1, t_real, t_types)
{
	using namespace tl2_ops;

	using t_cplx = std::complex<t_real>;

	std::vector<t_cplx> invec{{
		1., 2., 3., 4., 5., 6. ,7. ,8 }};

	auto outvec_fft = tl2::fft<t_real, t_cplx, std::vector>(invec, 0, 0);
	auto outvec_dft = tl2::dft<t_real, t_cplx, std::vector>(invec, 0, 0);

	BOOST_TEST((outvec_fft.size() == outvec_dft.size()));
	for(std::size_t i=0; i<std::min(outvec_dft.size(), outvec_fft.size()); ++i)
	{
		const t_cplx& c1 = outvec_fft[i];
		const t_cplx& c2 = outvec_dft[i];
		//std::cout << c1 << "  " << c2 << std::endl;
		BOOST_TEST((tl2::equals<t_cplx>(c1, c2, 1e-4)));
	}
}