#ifndef CAMERA_H
#define CAMERA_H

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

#include "math.h"

using namespace Eigen;

namespace SnowSimulator {

#define PI (3.14159265358979323)
#define EPS_D (0.00000000001)
#define EPS_F (0.00001f)
#define INF_D (std::numeric_limits<double>::infinity())
#define INF_F (std::numeric_limits<float>::infinity())

/*
  Takes any kind of number and converts from degrees to radians.
*/
template <typename T> inline T radians(T deg) { return deg * (PI / 180); }

/*
  Takes any kind of number and converts from radians to degrees.
*/
template <typename T> inline T degrees(T rad) { return rad * (180 / PI); }

/*
  Takes any kind of number, as well as a lower and upper bound, and clamps the
  number to be within the bound.
  NOTE: x, lo, and hi must all be the same type or compilation will fail. A
        common mistake is to pass an int for x and size_ts for lo and hi.
*/
template <typename T> inline T clamp(T x, T lo, T hi) {
  return std::min(std::max(x, lo), hi);
}

class Camera {
public:
  /*
    Sets the field of view to match screen screenW/H.
    NOTE: data and screenW/H will almost certainly disagree about the aspect
          ratio. screenW/H are treated as the source of truth, and the field
          of view is expanded along whichever dimension is too narrow.
    NOTE2: info.hFov and info.vFov are expected to be in DEGREES.
  */
  void configure(double nearClip, double farClip, double hFov, double vFov,
                 size_t screenW, size_t screenH);

  /*
    Phi and theta are in RADIANS.
  */
  void place(const Vector3d &targetPos, const double phi, const double theta,
             const double r, const double minR, const double maxR);

  std::string param_string() { return ""; }

  /*
    Copies just placement data from the other camera.
  */
  void copy_placement(const Camera &other);

  /*
    Updates the screen size to be the specified size, keeping screenDist
    constant.
  */
  void set_screen_size(const size_t screenW, const size_t screenH);

  /*
    Translates the camera such that a value at distance d directly in front of
    the camera moves by (dx, dy). Note that dx and dy are in screen coordinates,
    while d is in world-space coordinates (like pos/dir/up).
  */
  void move_by(const double dx, const double dy, const double d);

  /*
    Move the specified amount along the view axis.
  */
  void move_forward(const double dist);

  /*
    Rotate by the specified amount around the target.
  */
  void rotate_by(const double dPhi, const double dTheta);

  Vector3d position() const { return pos; }
  Vector3d view_point() const { return targetPos; }
  Vector3d up_dir() const { return c2w.col(1); }

  double v_fov() const { return vFov; }
  double aspect_ratio() const { return ar; }

  double near_clip() const { return nearClip; }
  double far_clip() const { return farClip; }

  // virtual void dump_settings(std::string filename);
  // virtual void load_settings(std::string filename);

private:
  // Computes pos, screenXDir, screenYDir from target, r, phi, theta.
  void compute_position();

  // Field of view aspect ratio, clipping planes.
  double hFov, vFov, ar, nearClip, farClip;

  // Current position and target point (the point the camera is looking at).
  Vector3d pos, targetPos;

  // Orientation relative to target, and min & max distance from the target.
  double phi, theta, r, minR, maxR;

  // camera-to-world rotation matrix (note: also need to translate a
  // camera-space point by 'pos' to perform a full camera-to-world
  // transform)
  Matrix3d c2w;

  // Info about screen to render to; it corresponds to the camera's full field
  // of view at some distance.
  size_t screenW, screenH;
  double screenDist;
};

} // namespace SnowSimulator

#endif // CAMERA_H


#include <boost/graph/king_ordering.hpp>


#include <Eigen/Dense>

#include <ancse/config.hpp>
#include <ancse/cfl_condition.hpp>
#include <ancse/fvm_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

static int n_vars = 1;

template<class F>
Eigen::MatrixXd ic(const F &f, const Grid &grid) {
    Eigen::MatrixXd u0(n_vars, grid.n_cells);
    for(int i = 0; i < grid.n_cells; ++i) {
        u0.col(i) = f(cell_center(grid, i));
    }

    return u0;
}

TimeLoop make_fvm(const nlohmann::json &config,
                  const Grid &grid,
                  std::shared_ptr<Model> &model)
{
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto n_vars = model->get_nvars();

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto fvm_rate_of_change
            = make_fvm_rate_of_change(config, grid, model,
                                      simulation_time);
    auto boundary_condition
            = make_boundary_condition(n_ghost,
                                      config["boundary_condition"]);
    auto time_integrator = make_runge_kutta(config,
                                            fvm_rate_of_change,
                                            boundary_condition,
                                            n_vars, n_cells);
    auto cfl_condition = make_cfl_condition(grid, model, cfl_number);
    auto snapshot_writer = std::make_shared< JSONSnapshotWriter<FVM> >
            (grid, model, simulation_time,
             std::string(config["output_dir"]),
             std::string(config["output_file"]));

    return TimeLoop(simulation_time, time_integrator,
                    cfl_condition, snapshot_writer);
}

void shock_test(const nlohmann::json &config)
{
    std::shared_ptr<Model> model = std::make_shared<Burgers>();

    auto fn = [](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 1;
        } else {        // right state
            u(0) = 0;
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

void rarefaction_test(const nlohmann::json &config)
{
    std::shared_ptr<Model> model = std::make_shared<Burgers>();

    auto fn = [](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 0;
        } else {        // right state
            u(0) = 1;
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

int main(int argc, char* const argv[])
{
    nlohmann::json config;
    std::string fileName;
    if (argc == 2) {
        fileName = argv[1];
    } else {
        fileName = "../config.json";
    }
    config = get_config (fileName);

    std::string ic_key = config["initial_conditions"];
    if (ic_key == "shock") {
        shock_test(config);
    } else if (ic_key == "rarefaction") {
        rarefaction_test(config);
    }

    return 0;
}


#define BOOST_TEST_MODULE TestStaticMatrix
#include <boost/test/unit_test.hpp>

#include <amgcl/value_type/static_matrix.hpp>

BOOST_AUTO_TEST_SUITE( test_static_matrix )

BOOST_AUTO_TEST_CASE( sum ) {
    amgcl::static_matrix<int, 2, 2> a = {{1, 2, 3, 4}};
    amgcl::static_matrix<int, 2, 2> b = {{4, 3, 2, 1}};
    amgcl::static_matrix<int, 2, 2> c = a + b;

    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            BOOST_CHECK_EQUAL(c(i,j), 5);
}

BOOST_AUTO_TEST_CASE( minus ) {
    amgcl::static_matrix<int, 2, 2> a = {{5, 5, 5, 5}};
    amgcl::static_matrix<int, 2, 2> b = {{4, 3, 2, 1}};
    amgcl::static_matrix<int, 2, 2> c = a - b;

    for(int i = 0; i < 4; ++i)
        BOOST_CHECK_EQUAL(c(i), i+1);
}

BOOST_AUTO_TEST_CASE( product ) {
    amgcl::static_matrix<int, 2, 2> a = {{2, 1, 1, 2}};
    amgcl::static_matrix<int, 2, 2> c = a * a;

    BOOST_CHECK_EQUAL(c(0,0), 5);
    BOOST_CHECK_EQUAL(c(0,1), 4);
    BOOST_CHECK_EQUAL(c(1,0), 4);
    BOOST_CHECK_EQUAL(c(1,1), 5);
}

BOOST_AUTO_TEST_CASE( scale ) {
    amgcl::static_matrix<int, 2, 2> a = {{1, 2, 3, 4}};
    amgcl::static_matrix<int, 2, 2> c = 2 * a;

    for(int i = 0; i < 4; ++i)
        BOOST_CHECK_EQUAL(c(i), 2 * (i+1));
}

BOOST_AUTO_TEST_CASE( inner_product ) {
    amgcl::static_matrix<int, 2, 1> a = {{1, 2}};
    int c = amgcl::math::inner_product(a, a);

    BOOST_CHECK_EQUAL(c, 5);
}

BOOST_AUTO_TEST_CASE( inverse ) {
    amgcl::static_matrix<double, 2, 2> a = {{2.0, -1.0, -1.0, 2.0}};
    amgcl::static_matrix<double, 2, 2> b = amgcl::math::inverse(a);
    amgcl::static_matrix<double, 2, 2> c = b * a;

    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            BOOST_CHECK_SMALL(c(i,j) - (i == j), 1e-8);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  C++ sensor fusion/filtering code that uses flexkalman
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#ifndef __cplusplus
#error "This header is C++-only."
#endif

#include "tracking/t_lowpass.hpp"
#include "tracking/t_lowpass_vector.hpp"
#include "math/m_api.h"
#include "util/u_time.h"
#include "util/u_debug.h"

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

#include "flexkalman/EigenQuatExponentialMap.h"

DEBUG_GET_ONCE_BOOL_OPTION(simple_imu_debug, "SIMPLE_IMU_DEBUG", false)

#define SIMPLE_IMU_DEBUG(MSG)                                                  \
	do {                                                                   \
		if (debug_) {                                                  \
			printf("SimpleIMU(%p): " MSG "\n",                     \
			       (const void *)this);                            \
		}                                                              \
	} while (0)

namespace xrt_fusion {
class SimpleIMUFusion
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	/*!
	 * @param gravity_rate Value in [0, 1] indicating how much the
	 * accelerometer should affect the orientation each second.
	 */
	explicit SimpleIMUFusion(double gravity_rate = 0.9)
	    : gravity_scale_(gravity_rate),
	      debug_(debug_get_bool_option_simple_imu_debug())
	{
		SIMPLE_IMU_DEBUG("Creating instance");
	}

	bool
	valid() const noexcept
	{
		return started_;
	}

	Eigen::Quaterniond
	getQuat() const
	{
		return quat_;
	}

	Eigen::Quaterniond
	getPredictedQuat(timepoint_ns timestamp) const
	{
		timepoint_ns state_time =
		    std::max(last_accel_timestamp_, last_gyro_timestamp_);
		time_duration_ns delta_ns =
		    (state_time == 0) ? 1e6 : timestamp - state_time;
		float dt = time_ns_to_s(delta_ns);
		return quat_ * flexkalman::util::quat_exp(angVel_ * dt * 0.5);
	}

	Eigen::Vector3d
	getRotationVec() const
	{
		return flexkalman::util::quat_ln(quat_);
	}

	//! in world space
	Eigen::Vector3d const &
	getAngVel() const
	{
		return angVel_;
	}

	bool
	handleGyro(Eigen::Vector3d const &gyro, timepoint_ns timestamp)
	{
		if (!started_) {

			SIMPLE_IMU_DEBUG(
			    "Discarding gyro report before first usable accel "
			    "report");
			return false;
		}
		time_duration_ns delta_ns =
		    (last_gyro_timestamp_ == 0)
		        ? 1e6
		        : timestamp - last_gyro_timestamp_;
		if (delta_ns > 1e10) {

			SIMPLE_IMU_DEBUG("Clamping integration period");
			// Limit integration to 1/10th of a second
			// Does not affect updating the last gyro timestamp.
			delta_ns = 1e10;
		}
		float dt = time_ns_to_s(delta_ns);
		last_gyro_timestamp_ = timestamp;
		Eigen::Vector3d incRot = gyro * dt;
		// Crude handling of "approximately zero"
		if (incRot.squaredNorm() < 1.e-8) {

			SIMPLE_IMU_DEBUG(
			    "Discarding gyro data that is approximately zero");
			return false;
		}

		angVel_ = gyro;

		// Update orientation
		quat_ = quat_ * flexkalman::util::quat_exp(incRot * 0.5);

		return true;
	}

	/*!
	 * Returns a coefficient to correct the scale of the accelerometer
	 * reading.
	 */
	double
	getAccelScaleFactor() const
	{
		return MATH_GRAVITY_M_S2 / gravity_filter_.getState();
	}

	bool
	handleAccel(Eigen::Vector3d const &accel, timepoint_ns timestamp)
	{
		uint64_t delta_ns = (last_accel_timestamp_ == 0)
		                        ? 1e6
		                        : timestamp - last_accel_timestamp_;
		float dt = time_ns_to_s(delta_ns);
		if (!started_) {
			auto diff = std::abs(accel.norm() - MATH_GRAVITY_M_S2);
			if (diff > 1.) {
				// We're moving, don't start it now.

				SIMPLE_IMU_DEBUG(
				    "Can't start tracker with this accel "
				    "sample: we're moving too much.");
				return false;
			}

			// Initially, just set it to totally trust gravity.
			started_ = true;
			quat_ = Eigen::Quaterniond::FromTwoVectors(
			    accel.normalized(), Eigen::Vector3d::UnitY());
			accel_filter_.addSample(accel, timestamp);
			gravity_filter_.addSample(accel.norm(), timestamp);
			last_accel_timestamp_ = timestamp;

			SIMPLE_IMU_DEBUG("Got a usable startup accel report");
			return true;
		}
		last_accel_timestamp_ = timestamp;
		accel_filter_.addSample(accel, timestamp);
		gravity_filter_.addSample(accel.norm(), timestamp);

		// Adjust scale of accelerometer
		Eigen::Vector3d adjusted_accel =
		    accel_filter_.getState() * getAccelScaleFactor();
		auto diff = std::abs(adjusted_accel.norm() - MATH_GRAVITY_M_S2);
		auto scale = 1. - diff;
		if (scale <= 0) {
			// Too far from gravity to be useful/trusted.
			SIMPLE_IMU_DEBUG(
			    "Too far from gravity to be useful/trusted.");
			return false;
		}

		// This should match the global gravity vector if the rotation
		// is right.
		Eigen::Vector3d measuredGravityDirection =
		    (quat_ * adjusted_accel).normalized();
		auto incremental = Eigen::Quaterniond::FromTwoVectors(
		    measuredGravityDirection, Eigen::Vector3d::UnitY());

		double alpha = scale * gravity_scale_ * dt;
		Eigen::Quaterniond scaledIncrementalQuat =
		    Eigen::Quaterniond::Identity().slerp(alpha, incremental);

		// Update orientation
		quat_ = scaledIncrementalQuat * quat_;

		return true;
	}

	/*!
	 * Use this to obtain the residual, world-space acceleration not
	 * associated with gravity, after incorporating a measurement.
	 */
	Eigen::Vector3d
	getCorrectedWorldAccel(Eigen::Vector3d const &accel) const
	{
		Eigen::Vector3d adjusted_accel = accel * getAccelScaleFactor();
		return (quat_ * adjusted_accel) -
		       (Eigen::Vector3d::UnitY() * MATH_GRAVITY_M_S2);
	}

	Eigen::Matrix3d
	getRotationMatrix() const
	{
		return quat_.toRotationMatrix();
	}

	void
	postCorrect()
	{
		quat_.normalize();
	}

private:
	Eigen::Vector3d angVel_{Eigen::Vector3d::Zero()};
	Eigen::Quaterniond quat_{Eigen::Quaterniond::Identity()};
	double gravity_scale_;
	LowPassIIRVectorFilter<3, double> accel_filter_{
	    200 /* hz cutoff frequency */};
	LowPassIIRFilter<double> gravity_filter_{1 /* hz cutoff frequency */};
	uint64_t last_accel_timestamp_{0};
	uint64_t last_gyro_timestamp_{0};
	bool started_{false};
	bool debug_{false};
};
} // namespace xrt_fusion


#pragma once

#include <random>

#include <Eigen/Dense>
#include <data/data.hpp>
#include <utils/math/kalman_multivariate.hpp>
#include <utils/system.hpp>

namespace hyped {
namespace navigation {

class KalmanFilter {
 public:
  KalmanFilter(uint32_t n = 3, uint32_t m = 1, uint32_t k = 0);
  void setup();
  void updateStateTransitionMatrix(data::nav_t dt);
  void updateMeasurementCovarianceMatrix(const data::nav_t var);
  data::nav_t filter(data::nav_t z);
  // transfer estimate to NavigationVector
  data::nav_t getEstimate();
  // transfer estimate variances to NavigationVector
  const data::nav_t getEstimateVariance();

 private:
  // state dimensionality
  uint32_t n_;
  // measurement dimensionality
  uint32_t m_;
  // control dimensionality default = 0
  uint32_t k_;
  utils::math::KalmanMultivariate kalmanFilter_;

  // covariance matrix variances
  static constexpr float kInitialErrorVariance          = 0.5;
  static constexpr float kStateTransitionVariance       = 0.02;
  static constexpr float kTrackMeasurementVariance      = 0.001;
  static constexpr float kElevatorMeasurementVariance   = 0.12;
  static constexpr float kStationaryMeasurementVariance = 0.04;

  // create initial error covariance matrix P
  const Eigen::MatrixXf createInitialErrorCovarianceMatrix() const;

  // create state transition matrix A
  Eigen::MatrixXf createStateTransitionMatrix(data::nav_t dt) const;

  // create measurement matrix H
  Eigen::MatrixXf createMeasurementMatrix() const;

  // create state transition coveriance matrix Q
  const Eigen::MatrixXf createStateTransitionCovarianceMatrix() const;

  // create measurement covariance matrices R
  const Eigen::MatrixXf createTrackMeasurementCovarianceMatrix() const;
  const Eigen::MatrixXf createElevatorMeasurementCovarianceMatrix() const;
  const Eigen::MatrixXf createStationaryMeasurementCovarianceMatrix() const;
};
}  // namespace navigation
}  // namespace hyped


#include <boost/test/unit_test.hpp>
#include "common/equality.hpp"
#include "algorithms/math/calc_angle_btw_clock_hands.hpp"

BOOST_AUTO_TEST_SUITE(CalcAngleBetweenClockHands)

BOOST_AUTO_TEST_CASE(invalid_input)
{
    BOOST_CHECK(equal(CalcAngleBtwClockHands(-1, 4), 0.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(10, -90), 0.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(13, -3600), 0.0));
}

BOOST_AUTO_TEST_CASE(valid_input)
{
    BOOST_CHECK(equal(CalcAngleBtwClockHands(0, 0), 0.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(12, 60), 0.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(1, 0), 30.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(0, 1), 5.5));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(5, 24), 18.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(2, 20), 50.0));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(4, 15), 37.5));
    BOOST_CHECK(equal(CalcAngleBtwClockHands(10, 43), 63.5));
}

BOOST_AUTO_TEST_SUITE_END()


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Sebastian Schlenkrich

*/

#ifndef quantlib_templateauxilliaries_solver1d_hpp
#define quantlib_templateauxilliaries_solver1d_hpp

#include <ql/types.hpp>
#include <boost/function.hpp>

namespace TemplateAuxilliaries {

    //! Template for 1-D solution f(x) = 0, s.t. x \in [a,b] via secant method
    template <class Type>
    Type solve1d( const boost::function<Type (Type)>& f, Type xTol, Type a, Type b, size_t nTrials = 10 ) {
        Type fa = f(a);
        Type fb = f(b);
        if (fa*fb>0) {  // we need new arguments enclosing the solution
            b = (a + b);
            a = b / 4.0;
            for (size_t k=0; k<nTrials; ++k) {
                fa = f(a);
                fb = f(b);
                if (fa*fb<=0) break;
                a = a/2.0;
                b = b*2.0;
            }
        }
        QL_REQUIRE(fa*fb<=0,"Solve1d: Can't find intervall enclosing a solution");
        if (a>b) { // swap a <-> b
            Type tmp = a; a = b; b = tmp;
            tmp = fa; fa = fb; fb = tmp;
        }
        Type m = (fb - fa)/(b-a);
        Type x1 = a;
        Type y1 = fa;
        Type s = - y1/m;
        while (fabs(s)>xTol) {
            Type x0=x1, y0=y1;
            // find a new solution
            x1 = x0 + s;
            if ((x1<a)||(x1>b)) x1 = (a + b)/2.0;
            y1 = f(x1);
            m  = (y1-y0)/(x1-x0);
            s  = -y1/m;
            // update intervalls
            if (fa*y1>=0) {
                a  = x1;
                fa = y1;
            } else {
                b  = x1;
                fb = y1;
            }
        }
        return x1;
    }

}

#endif  /* ifndef quantlib_solve1d_hpp */


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

#define BOOST_TEST_MODULE exp_sinh_quadrature_test

#include <complex>
#include <boost/multiprecision/cpp_complex.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/type_traits/is_class.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/complex128.hpp>
#endif

using std::exp;
using std::cos;
using std::tan;
using std::log;
using std::sqrt;
using std::abs;
using std::sinh;
using std::cosh;
using std::pow;
using std::atan;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_bin_float_quad;
using boost::math::constants::pi;
using boost::math::constants::half_pi;
using boost::math::constants::two_div_pi;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::catalan;
using boost::math::constants::ln_two;
using boost::math::constants::root_two;
using boost::math::constants::root_two_pi;
using boost::math::constants::root_pi;
using boost::math::quadrature::exp_sinh;

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3) && !defined(TEST4) && !defined(TEST5) && !defined(TEST6) && !defined(TEST7) && !defined(TEST8)
#  define TEST1
#  define TEST2
#  define TEST3
#  define TEST4
#  define TEST5
#  define TEST6
#  define TEST7
#  define TEST8
#endif

#ifdef BOOST_MSVC
#pragma warning (disable:4127)
#endif

//
// Coefficient generation code:
//
template <class T>
void print_levels(const T& v, const char* suffix)
{
   std::cout << "{\n";
   for (unsigned i = 0; i < v.size(); ++i)
   {
      std::cout << "      { ";
      for (unsigned j = 0; j < v[i].size(); ++j)
      {
         std::cout << v[i][j] << suffix << ", ";
      }
      std::cout << "},\n";
   }
   std::cout << "   };\n";
}

template <class T>
void print_levels(const std::pair<T, T>& p, const char* suffix = "")
{
   std::cout << "   static const std::vector<std::vector<Real> > abscissa = ";
   print_levels(p.first, suffix);
   std::cout << "   static const std::vector<std::vector<Real> > weights = ";
   print_levels(p.second, suffix);
}

template <class Real, class TargetType>
std::pair<std::vector<std::vector<Real>>, std::vector<std::vector<Real>> > generate_constants(unsigned max_rows)
{
   using boost::math::constants::half_pi;
   using boost::math::constants::two_div_pi;
   using boost::math::constants::pi;
   auto g = [](Real t)->Real { return exp(half_pi<Real>()*sinh(t)); };
   auto w = [](Real t)->Real { return cosh(t)*half_pi<Real>()*exp(half_pi<Real>()*sinh(t)); };

   std::vector<std::vector<Real>> abscissa, weights;

   std::vector<Real> temp;

   Real tmp = (Real(boost::math::tools::log_min_value<TargetType>()) + log(Real(boost::math::tools::epsilon<TargetType>())))*0.5f;
   Real t_min = asinh(two_div_pi<Real>()*tmp);
   // truncate t_min to an exact binary value:
   t_min = floor(t_min * 128) / 128;

   std::cout << "m_t_min = " << t_min << ";\n";

   // t_max is chosen to make g'(t_max) ~ sqrt(max) (g' grows faster than g).
   // This will allow some flexibility on the users part; they can at least square a number function without overflow.
   // But there is no unique choice; the further out we can evaluate the function, the better we can do on slowly decaying integrands.
   const Real t_max = log(2 * two_div_pi<Real>()*log(2 * two_div_pi<Real>()*sqrt(Real(boost::math::tools::max_value<TargetType>()))));

   Real h = 1;
   for (Real t = t_min; t < t_max; t += h)
   {
      temp.push_back(g(t));
   }
   abscissa.push_back(temp);
   temp.clear();

   for (Real t = t_min; t < t_max; t += h)
   {
      temp.push_back(w(t * h));
   }
   weights.push_back(temp);
   temp.clear();

   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = t_min + h; t < t_max; t += 2 * h)
         temp.push_back(g(t));
      abscissa.push_back(temp);
      temp.clear();
   }
   h = 1;
   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = t_min + h; t < t_max; t += 2 * h)
         temp.push_back(w(t));
      weights.push_back(temp);
      temp.clear();
   }

   return std::make_pair(abscissa, weights);
}

template <class Real>
const exp_sinh<Real>& get_integrator()
{
   static const exp_sinh<Real> integrator(14);
   return integrator;
}

template <class Real>
Real get_convergence_tolerance()
{
   return boost::math::tools::root_epsilon<Real>();
}

template<class Real>
void test_right_limit_infinite()
{
    std::cout << "Testing right limit infinite for tanh_sinh in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    // Example 12
    const auto f2 = [](const Real& t)->Real { return exp(-t)/sqrt(t); };
    Q = integrator.integrate(f2, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = root_pi<Real>();
    Real tol_mult = 1;
    // Multiprecision type have higher error rates, probably evaluation of f() is less accurate:
    if (std::numeric_limits<Real>::digits10 > std::numeric_limits<long double>::digits10)
       tol_mult = 12;
    else if (std::numeric_limits<Real>::digits10 > std::numeric_limits<double>::digits10)
       tol_mult = 5;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol * tol_mult);
    // The integrand is strictly positive, so it coincides with the value of the integral:
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol * tol_mult);

    auto f3 = [](Real t)->Real { Real z = exp(-t); if (z == 0) { return z; } return z*cos(t); };
    Q = integrator.integrate(f3, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    Q = integrator.integrate(f3, 10, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("-6.6976341310426674140007086979326069121526743314567805278252392932e-6");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 10 * tol);
    // Integrating through zero risks precision loss:
    Q = integrator.integrate(f3, -10, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("-15232.3213626280525704332288302799653087046646639974940243044623285817777006");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, std::numeric_limits<Real>::digits10 > 30 ? 1000 * tol : tol);

    auto f4 = [](Real t)->Real { return 1/(1+t*t); };
    Q = integrator.integrate(f4, 1, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>()/4;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
    Q = integrator.integrate(f4, 20, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("0.0499583957219427614100062870348448814912770804235071744108534548299835954767");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
    Q = integrator.integrate(f4, 500, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("0.0019999973333397333150476759363217553199063513829126652556286269630");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
}

template<class Real>
void test_left_limit_infinite()
{
    std::cout << "Testing left limit infinite for 1/(1+t^2) on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    // Example 11:
    auto f1 = [](const Real& t)->Real { return 1/(1+t*t);};
    Q = integrator.integrate(f1, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), 0, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
    Q = integrator.integrate(f1, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), -20, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("0.0499583957219427614100062870348448814912770804235071744108534548299835954767");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
    Q = integrator.integrate(f1, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), -500, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::lexical_cast<Real>("0.0019999973333397333150476759363217553199063513829126652556286269630");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
}

// Some examples of tough integrals from NR, section 4.5.4:
template<class Real>
void test_nr_examples()
{
    using std::sin;
    using std::cos;
    using std::pow;
    using std::exp;
    using std::sqrt;
    std::cout << "Testing type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;
    auto integrator = get_integrator<Real>();

    auto f0 = [] (Real)->Real { return (Real) 0; };
    Q = integrator.integrate(f0, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = 0;
    BOOST_CHECK_CLOSE_FRACTION(Q, 0.0f, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, 0.0f, tol);

    auto f = [](const Real& x)->Real { return 1/(1+x*x); };
    Q = integrator.integrate(f, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    auto f1 = [](Real x)->Real {
        Real z1 = exp(-x);
        if (z1 == 0)
        {
            return (Real) 0;
        }
        Real z2 = pow(x, -3*half<Real>())*z1;
        if (z2 == 0)
        {
            return (Real) 0;
        }
        return sin(x*half<Real>())*z2;
    };

    Q = integrator.integrate(f1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = sqrt(pi<Real>()*(sqrt((Real) 5) - 2));

    // The integrand is oscillatory; the accuracy is low.
    Real tol_mul = 1;
    if (std::numeric_limits<Real>::digits10 > 40)
       tol_mul = 500000;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol_mul * tol);

    auto f2 = [](Real x)->Real { return x > boost::math::tools::log_max_value<Real>() ? Real(0) : Real(pow(x, -(Real) 2/(Real) 7)*exp(-x*x)); };
    Q = integrator.integrate(f2, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half<Real>()*boost::math::tgamma((Real) 5/ (Real) 14);
    tol_mul = 1;
    if (std::numeric_limits<Real>::is_specialized == false)
       tol_mul = 6;
    else if (std::numeric_limits<Real>::digits10 > 40)
       tol_mul = 100;
    else
       tol_mul = 3;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol_mul * tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol_mul * tol);

    auto f3 = [](Real x)->Real { return (Real) 1/ (sqrt(x)*(1+x)); };
    Q = integrator.integrate(f3, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>();

    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 10*boost::math::tools::epsilon<Real>());
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, 10*boost::math::tools::epsilon<Real>());

    auto f4 = [](const Real& t)->Real { return  t > boost::math::tools::log_max_value<Real>() ? Real(0) : Real(exp(-t*t*half<Real>())); };
    Q = integrator.integrate(f4, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = root_two_pi<Real>()/2;
    tol_mul = 1;
    if (std::numeric_limits<Real>::digits10 > 40)
       tol_mul = 5000;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol_mul * tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol_mul * tol);

    auto f5 = [](const Real& t)->Real { return 1/cosh(t);};
    Q = integrator.integrate(f5, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol * 12);   // Fails at float precision without higher error rate
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol * 12);
}

// Definite integrals found in the CRC Handbook of Mathematical Formulas
template<class Real>
void test_crc()
{
    using std::sin;
    using std::pow;
    using std::exp;
    using std::sqrt;
    using std::log;
    using std::cos;
    std::cout << "Testing integral from CRC handbook on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;
    auto integrator = get_integrator<Real>();

    auto f0 = [](const Real& x)->Real { return x > boost::math::tools::log_max_value<Real>() ? Real(0) : Real(log(x)*exp(-x)); };
    Q = integrator.integrate(f0, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = -boost::math::constants::euler<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // Test the integral representation of the gamma function:
    auto f1 = [](Real t)->Real { Real x = exp(-t);
        if(x == 0)
        {
            return (Real) 0;
        }
        return pow(t, (Real) 12 - 1)*x;
    };

    Q = integrator.integrate(f1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::math::tgamma(12.0f);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // Integral representation of the modified bessel function:
    // K_5(12)
    auto f2 = [](Real t)->Real {
        Real x = 12*cosh(t);
        if (x > boost::math::tools::log_max_value<Real>())
        {
            return (Real) 0;
        }
        return exp(-x)*cosh(5*t);
    };
    Q = integrator.integrate(f2, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = boost::math::cyl_bessel_k<int, Real>(5, 12);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    // Laplace transform of cos(at)
    Real a = 20;
    Real s = 1;
    auto f3 = [&](Real t)->Real {
        Real x = s * t;
        if (x > boost::math::tools::log_max_value<Real>())
        {
            return (Real) 0;
        }
        return cos(a * t) * exp(-x);
    };

    // Since the integrand is oscillatory, we increase the tolerance:
    Real tol_mult = 10;
    // Multiprecision type have higher error rates, probably evaluation of f() is less accurate:
    if (!boost::is_class<Real>::value)
    {
       // For high oscillation frequency, the quadrature sum is ill-conditioned.
       Q = integrator.integrate(f3, get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = s/(a*a+s*s);
       if (std::numeric_limits<Real>::digits10 > std::numeric_limits<double>::digits10)
          tol_mult = 5000; // we should really investigate this more??
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol_mult*tol);
    }

    //
    // This one doesn't pass for real_concept..
    //
    if (std::numeric_limits<Real>::is_specialized)
    {
       // Laplace transform of J_0(t):
       auto f4 = [&](Real t)->Real {
          Real x = s * t;
          if (x > boost::math::tools::log_max_value<Real>())
          {
             return (Real)0;
          }
          return boost::math::cyl_bessel_j(0, t) * exp(-x);
       };

       Q = integrator.integrate(f4, get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = 1 / sqrt(1 + s*s);
       tol_mult = 3;
       // Multiprecision type have higher error rates, probably evaluation of f() is less accurate:
       if (std::numeric_limits<Real>::digits10 > std::numeric_limits<long double>::digits10)
          tol_mult = 750;
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol_mult * tol);
    }
    auto f6 = [](const Real& t)->Real { return t > boost::math::tools::log_max_value<Real>() ? Real(0) : Real(exp(-t*t)*log(t));};
    Q = integrator.integrate(f6, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = -boost::math::constants::root_pi<Real>()*(boost::math::constants::euler<Real>() + 2*ln_two<Real>())/4;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // CRC Section 5.5, integral 591
    // The parameter p allows us to control the strength of the singularity.
    // Rapid convergence is not guaranteed for this function, as the branch cut makes it non-analytic on a disk.
    // This converges only when our test type has an extended exponent range as all the area of the integral
    // occurs so close to 0 (or 1) that we need abscissa values exceptionally small to find it.
    // "There's a lot of room at the bottom".
    // This version is transformed via argument substitution (exp(-x) for x) so that the integral is spread
    // over (0, INF).
    tol *= boost::math::tools::digits<Real>() > 100 ? 100000 : 75;
    for (Real pn = 99; pn > 0; pn -= 10) {
       Real p = pn / 100;
       auto f = [&](Real x)->Real
       {
          return x > 1000 * boost::math::tools::log_max_value<Real>() ? Real(0) : Real(exp(-x * (1 - p) + p * log(-boost::math::expm1(-x))));
       };
       Q = integrator.integrate(f, get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = 1 / boost::math::sinc_pi(p*pi<Real>());
       /*
       std::cout << std::setprecision(std::numeric_limits<Real>::max_digits10) << p << std::endl;
       std::cout << std::setprecision(std::numeric_limits<Real>::max_digits10) << Q << std::endl;
       std::cout << std::setprecision(std::numeric_limits<Real>::max_digits10) << Q_expected << std::endl;
       std::cout << fabs((Q - Q_expected) / Q_expected) << std::endl;
       */
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    }
    // and for p < 1:
    for (Real p = -0.99; p < 0; p += 0.1) {
       auto f = [&](Real x)->Real
       {
          return x > 1000 * boost::math::tools::log_max_value<Real>() ? Real(0) : Real(exp(-p * log(-boost::math::expm1(-x)) - (1 + p) * x));
       };
       Q = integrator.integrate(f, get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = 1 / boost::math::sinc_pi(p*pi<Real>());
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    }
}

template<class Complex>
void test_complex_modified_bessel()
{
    std::cout << "Testing complex modified Bessel function on type " << boost::typeindex::type_id<Complex>().pretty_name() << "\n";
    typedef typename Complex::value_type Real;
    Real tol = 100 * boost::math::tools::epsilon<Real>();
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    // Integral Representation of Modified Complex Bessel function:
    // https://en.wikipedia.org/wiki/Bessel_function#Modified_Bessel_functions
    Complex z{2, 3};
    const auto f = [&z](const Real& t)->Complex
    {
        using std::cosh;
        using std::exp;
        Real cosht = cosh(t);
        if (cosht > boost::math::tools::log_max_value<Real>())
        {
            return Complex{0, 0};
        }
        Complex arg = -z*cosht;
        Complex res = exp(arg);
        return res;
    };

    Complex K0 = integrator.integrate(f, get_convergence_tolerance<Real>(), &error, &L1);

    // Mathematica code: N[BesselK[0, 2 + 3 I], 140]
    Real K0_x_expected = boost::lexical_cast<Real>("-0.08296852656762551490517953520589186885781541203818846830385526187936132191822538822296497597191327722262903004145527496422090506197776994");
    Real K0_y_expected = boost::lexical_cast<Real>("0.027949603635183423629723306332336002340909030265538548521150904238352846705644065168365102147901993976999717171115546662967229050834575193041");
    BOOST_CHECK_CLOSE_FRACTION(K0.real(), K0_x_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(K0.imag(), K0_y_expected, tol);
}

BOOST_AUTO_TEST_CASE(exp_sinh_quadrature_test)
{
   //
   // Uncomment to generate the coefficients:
   //

   /*
   std::cout << std::scientific << std::setprecision(8);
   print_levels(generate_constants<cpp_bin_float_100, float>(8), "f");
   std::cout << std::setprecision(18);
   print_levels(generate_constants<cpp_bin_float_100, double>(8), "");
   std::cout << std::setprecision(35);
   print_levels(generate_constants<cpp_bin_float_100, cpp_bin_float_quad>(8), "L");
   */

#ifdef TEST1
    test_left_limit_infinite<float>();
    test_right_limit_infinite<float>();
    test_nr_examples<float>();
    test_crc<float>();
#endif
#ifdef TEST2
    test_left_limit_infinite<double>();
    test_right_limit_infinite<double>();
    test_nr_examples<double>();
    test_crc<double>();
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST3
    test_left_limit_infinite<long double>();
    test_right_limit_infinite<long double>();
    test_nr_examples<long double>();
    test_crc<long double>();
#endif
#endif
#ifdef TEST4
    test_left_limit_infinite<cpp_bin_float_quad>();
    test_right_limit_infinite<cpp_bin_float_quad>();
    test_nr_examples<cpp_bin_float_quad>();
    test_crc<cpp_bin_float_quad>();
#endif

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST5
    test_left_limit_infinite<boost::math::concepts::real_concept>();
    test_right_limit_infinite<boost::math::concepts::real_concept>();
    test_nr_examples<boost::math::concepts::real_concept>();
    test_crc<boost::math::concepts::real_concept>();
#endif
#endif
#ifdef TEST6
    test_left_limit_infinite<boost::multiprecision::cpp_bin_float_50>();
    test_right_limit_infinite<boost::multiprecision::cpp_bin_float_50>();
    test_nr_examples<boost::multiprecision::cpp_bin_float_50>();
    test_crc<boost::multiprecision::cpp_bin_float_50>();
#endif
#ifdef TEST7
    test_left_limit_infinite<boost::multiprecision::cpp_dec_float_50>();
    test_right_limit_infinite<boost::multiprecision::cpp_dec_float_50>();
    test_nr_examples<boost::multiprecision::cpp_dec_float_50>();
    //
    // This one causes stack overflows on the CI machine, but not locally,
    // assume it's due to resticted resources on the server, and <shrug> for now...
    //
#if ! BOOST_WORKAROUND(BOOST_MSVC, == 1900)
    test_crc<boost::multiprecision::cpp_dec_float_50>();
#endif
#endif
#ifdef TEST8
    test_complex_modified_bessel<std::complex<float>>();
    test_complex_modified_bessel<std::complex<double>>();
    test_complex_modified_bessel<std::complex<long double>>();
    #ifdef BOOST_HAS_FLOAT128
        test_complex_modified_bessel<boost::multiprecision::complex128>();
    #endif
    test_complex_modified_bessel<boost::multiprecision::cpp_complex_quad>();
#endif
}


/*
 * Copyright Nick Thompson, 2019
 * 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 <numeric>
#include <utility>
#include <random>
#include <boost/core/demangle.hpp>
#include <boost/math/interpolators/whittaker_shannon.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::whittaker_shannon;

template<class Real>
void test_trivial()
{
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    std::vector<Real> v{1.5};
    std::vector<Real> v_copy = v;
    auto ws = whittaker_shannon<decltype(v)>(std::move(v), t0, h);

    Real expected = 0;
    if(!CHECK_MOLLIFIED_CLOSE(expected, ws.prime(0), 10*std::numeric_limits<Real>::epsilon())) {
        std::cerr << "  Problem occurred at abscissa " << 0 << "\n";
    }

    expected = -v_copy[0]/h;
    if(!CHECK_MOLLIFIED_CLOSE(expected, ws.prime(h), 10*std::numeric_limits<Real>::epsilon())) {
        std::cerr << "  Problem occurred at abscissa " << 0 << "\n";
    }
}

template<class Real>
void test_knots()
{
    Real t0 = 0;
    Real h = Real(1)/Real(16);
    size_t n = 512;
    std::vector<Real> v(n);
    std::mt19937 gen(323723);
    std::uniform_real_distribution<Real> dis(1.0, 2.0);

    for(size_t i = 0;  i < n; ++i) {
      v[i] = static_cast<Real>(dis(gen));
    }
    auto ws = whittaker_shannon<decltype(v)>(std::move(v), t0, h);

    size_t i = 0;
    while (i < n) {
      Real t = t0 + i*h;
      Real expected = ws[i];
      Real computed = ws(t);
      CHECK_ULP_CLOSE(expected, computed, 16);
      ++i;
    }
}

template<class Real>
void test_bump()
{
    using std::exp;
    using std::abs;
    using std::sqrt;
    auto bump = [](Real x) { if (abs(x) >= 1) { return Real(0); } return exp(-Real(1)/(Real(1)-x*x)); };

    auto bump_prime = [&bump](Real x) { Real z = 1-x*x; return -2*x*bump(x)/(z*z); };

    Real t0 = -1;
    size_t n = 2049;
    Real h = Real(2)/Real(n-1);

    std::vector<Real> v(n);
    for(size_t i = 0; i < n; ++i) {
        Real t = t0 + i*h;
        v[i] = bump(t);
    }

    std::vector<Real> v_copy = v;
    auto ws = whittaker_shannon<decltype(v)>(std::move(v), t0, h);

    // Test the knots:
    for(size_t i = v_copy.size()/4; i < 3*v_copy.size()/4; ++i) {
        Real t = t0 + i*h;
        Real expected = v_copy[i];
        Real computed = ws(t);
        if(!CHECK_MOLLIFIED_CLOSE(expected, computed, 10*std::numeric_limits<Real>::epsilon())) {
            std::cerr << "  Problem occurred at abscissa " << t << "\n";
        }

        Real expected_prime = bump_prime(t);
        Real computed_prime = ws.prime(t);
        if(!CHECK_MOLLIFIED_CLOSE(expected_prime, computed_prime, 1000*std::numeric_limits<Real>::epsilon())) {
            std::cerr << "  Problem occurred at abscissa " << t << "\n";
        }

    }

    std::mt19937 gen(323723);
    std::uniform_real_distribution<long double> dis(-0.85, 0.85);

    size_t i = 0;
    while (i++ < 1000)
    {
        Real t = static_cast<Real>(dis(gen));
        Real expected = bump(t);
        Real computed = ws(t);
        if(!CHECK_MOLLIFIED_CLOSE(expected, computed, 10*std::numeric_limits<Real>::epsilon())) {
            std::cerr << "  Problem occurred at abscissa " << t << "\n";
        }

        Real expected_prime = bump_prime(t);
        Real computed_prime = ws.prime(t);
        if(!CHECK_MOLLIFIED_CLOSE(expected_prime, computed_prime, sqrt(std::numeric_limits<Real>::epsilon()))) {
            std::cerr << "  Problem occurred at abscissa " << t << "\n";
        }
    }
}

int main()
{
    test_knots<float>();
    test_knots<double>();
    test_knots<long double>();

    test_bump<double>();
    test_bump<long double>();

    test_trivial<float>();
    test_trivial<double>();
    return boost::math::test::report_errors();
}


#include "square_subset_finder.h"

#include <vector>

#include <NTL/vec_vec_GF2.h>

#include "util.h"

namespace {
    using namespace NTL;
    using namespace std;
}

namespace gnfs {
    template <typename RowGenerator>
    vector<pair<typename SquareSubsetFinder<RowGenerator>::Index, vec_GF2>> SquareSubsetFinder<RowGenerator>::get() {
        long rows = matrix_.NumRows();
        long cols = rg_.num_cols();
        // If matrix_ doesn't have more rows than columns,
        // we need to generate more rows.
        // But our loop below will generate the (cols+1)th row,
        // so we stop this loop when there's a square matrix.
        while (rows < cols) {
            matrix_.SetDims(rows + 1, cols);
            rows++;
            pair<Index, vec_GF2> new_row = rg_.get();
            indices_.push_back(new_row.first);
            matrix_[rows - 1] = new_row.second;
        }

        // Overall course of events:
        // basis_cursor_ is a bitfield representing which elements
        // of basis_ to add together for the next subset we return.
        // We iterate through all values of basis_cursor_
        // until it reaches 2^(basis.NumRows()),
        // at which point we request a new row from rg_.
        // Then we iterate through the new basis,
        // suppressing returned sets that do not include the new row.
        //
        // This loop is a shoddy equivalent to an infinite generator.
        // Each iteration through the loop produces a candidate
        // that we can return.
        // Either we return it,
        // saving tons of state so we know where we left off,
        // or we continue; and produce a new candidate.
        while (true) {
            basis_cursor_++;
            if (basis_cursor_ == 0 || NumBits(basis_cursor_) >= basis_.NumRows()) {
                // Request a new row for the matrix.
                matrix_.SetDims(rows + 1, cols);
                rows++;
                pair<Index, vec_GF2> new_row = rg_.get();
                indices_.push_back(new_row.first);
                matrix_[rows - 1] = new_row.second;

                // Generate a new basis.
                kernel(basis_, matrix_);

                // Reset the cursor.
                suppress_ = (basis_cursor_ != 0);
                // Skip 0 because returning the empty set is stupid.
                basis_cursor_ = 1;
            }

            // Compute the sum of the indicated rows in the basis.
            vec_GF2 basis_rows_to_sum = bitfield(basis_cursor_);
            basis_rows_to_sum.SetLength(basis_.NumRows());
            vec_GF2 their_sum = basis_rows_to_sum * basis_;
            // Now their_sum is a vector of length rows,
            // indicating which rows are in the square subset
            // that we're about to return.

            // Check suppression.
            if (suppress_ && their_sum[rows - 1] == 0) continue;

            // For each 1 in sum, add the corresponding entry of matrix_
            // to the returned vector.
            vector<pair<Index, vec_GF2>> ret;
            for (int i = 0; i < their_sum.length(); i++) {
                if (their_sum[i] != 0) {
                    Index first = indices_[i];
                    vec_GF2 second = matrix_[i];
                    ret.emplace_back(first, second);
                }
            }

            // Make sure we're not returning the empty vector.
            if (!ret.size()) continue;

            return ret;
        }
    }
}


#ifndef SPLX_INTERNAL_TYPES_HPP
#define SPLX_INTERNAL_TYPES_HPP

#include <Eigen/Dense>

namespace splx {

template<typename T>
using Row = Eigen::Matrix<T, 1, Eigen::Dynamic>;

template<typename T>
using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template<typename T, unsigned int DIM>
using VectorDIM = Eigen::Matrix<T, DIM, 1>;

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

template<typename T, unsigned int DIM>
using Hyperplane = Eigen::Hyperplane<T, DIM>;

using Index = Eigen::Index;

template<typename T, unsigned int DIM>
using AlignedBox = Eigen::AlignedBox<T, DIM>;

template<typename T>
struct Constraint {
    using _Row = Row<T>;

    Constraint(const _Row& c, T l, T u, bool sc = false, T sw = T(1)):
        coeff(c), lb(l), ub(u), soft_convertible(sc), soft_weight(sw) {

    }

    Constraint(const _Row&& c, T l, T u, bool sc = false, T sw = T(1)):
        coeff(std::move(c)), lb(l), ub(u), soft_convertible(sc), soft_weight(sw) {

    }

    _Row coeff;
    T lb;
    T ub;
    bool soft_convertible;
    T soft_weight;
};

}

#endif


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

  Implementation of mean.

  Copyright (c) 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

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

#ifndef MEAN_HPP_BB12C8E0_E48D_11E2_83DA_9FFD99890B3C
#define MEAN_HPP_BB12C8E0_E48D_11E2_83DA_9FFD99890B3C

#include <vector>
#include <algorithm>
#include <functional>
#include <stdexcept>

// Suppress annoying MSVC's C4244 conversion warnings popping out from boost::accumulators
// library (boost::numeric::functional namespace) mainly due to division operation
// in mean computation. Suppress MSVC's C4512 warning for boost auxiliary classes as well.
#ifdef _MSC_VER
#   pragma warning(push)
#   pragma warning(disable:4244)
#   pragma warning(disable:4512)
#endif // _MSC_VER

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>

#ifdef _MSC_VER
#   pragma warning(pop)
#endif // _MSC_VER

namespace bo {
namespace math {

// Returns the mean of the given samples.
template <typename SampleType>
SampleType mean(const std::vector<SampleType>& data)
{
    // The mean of an empty set is meaningless.
    if (data.size() == 0)
        throw std::logic_error("The mean of an empty set is meaningless.");

    // Initialize boost accumulator.
    namespace accs = boost::accumulators;
    typedef accs::accumulator_set<SampleType, accs::stats<accs::tag::mean> > Acc;
    Acc acc;

    // Fill accumulator with data.
    acc = std::for_each(data.begin(), data.end(), acc);

    // Request and return mean.
    return accs::mean(acc);
}

} // namespace math
} // namespace bo

#endif // MEAN_HPP_BB12C8E0_E48D_11E2_83DA_9FFD99890B3C


// Copyright Aleksey Gurtovoy 2003-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: numeric_ops.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/arithmetic.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/long.hpp>
#include <boost/mpl/aux_/test.hpp>

struct complex_tag : int_<10> {};

template< typename Re, typename Im > struct complex
{
    typedef complex_tag tag;
    typedef complex type;
    typedef Re real;
    typedef Im imag;
};

template< typename C > struct real : C::real {};
template< typename C > struct imag : C::imag {};

namespace boost { namespace mpl {

template<> struct BOOST_MPL_AUX_NUMERIC_CAST< integral_c_tag,complex_tag >
{
    template< typename N > struct apply
        : complex< N, integral_c< typename N::value_type, 0 > >
    {
    };
};

template<>
struct plus_impl< complex_tag,complex_tag >
{
    template< typename N1, typename N2 > struct apply
        : complex<
              plus< typename N1::real, typename N2::real >
            , plus< typename N1::imag, typename N2::imag >
            >
    {
    };
};

template<>
struct times_impl< complex_tag,complex_tag >
{
    template< typename N1, typename N2 > struct apply
        : complex<
              minus<
                  times< typename N1::real, typename N2::real >
                , times< typename N1::imag, typename N2::imag >
                >
            , plus<
                  times< typename N1::real, typename N2::imag >
                , times< typename N1::imag, typename N2::real >
                >
            >
    {
    };
};

template<>
struct equal_to_impl< complex_tag,complex_tag >
{
    template< typename N1, typename N2 > struct apply
        : and_<
              equal_to< typename N1::real, typename N2::real >
            , equal_to< typename N1::imag, typename N2::imag >
            >
    {
    };
};

}}

typedef int_<2> i;
typedef complex< int_<5>, int_<-1> > c1;
typedef complex< int_<-5>, int_<1> > c2;

MPL_TEST_CASE()
{
    typedef plus<c1,c2>::type r1;
    MPL_ASSERT_RELATION( real<r1>::value, ==, 0 );
    MPL_ASSERT_RELATION( imag<r1>::value, ==, 0 );

    typedef plus<c1,c1>::type r2;
    MPL_ASSERT_RELATION( real<r2>::value, ==, 10 );
    MPL_ASSERT_RELATION( imag<r2>::value, ==, -2 );

    typedef plus<c2,c2>::type r3;
    MPL_ASSERT_RELATION( real<r3>::value, ==, -10 );
    MPL_ASSERT_RELATION( imag<r3>::value, ==, 2 );

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    typedef plus<c1,i>::type r4;
    MPL_ASSERT_RELATION( real<r4>::value, ==, 7 );
    MPL_ASSERT_RELATION( imag<r4>::value, ==, -1 );

    typedef plus<i,c2>::type r5;
    MPL_ASSERT_RELATION( real<r5>::value, ==, -3 );
    MPL_ASSERT_RELATION( imag<r5>::value, ==, 1 );
#endif
}

MPL_TEST_CASE()
{
    typedef times<c1,c2>::type r1;
    MPL_ASSERT_RELATION( real<r1>::value, ==, -24 );
    MPL_ASSERT_RELATION( imag<r1>::value, ==, 10 );

    typedef times<c1,c1>::type r2;
    MPL_ASSERT_RELATION( real<r2>::value, ==, 24 );
    MPL_ASSERT_RELATION( imag<r2>::value, ==, -10 );

    typedef times<c2,c2>::type r3;
    MPL_ASSERT_RELATION( real<r3>::value, ==, 24 );
    MPL_ASSERT_RELATION( imag<r3>::value, ==, -10 );

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    typedef times<c1,i>::type r4;
    MPL_ASSERT_RELATION( real<r4>::value, ==, 10 );
    MPL_ASSERT_RELATION( imag<r4>::value, ==, -2 );

    typedef times<i,c2>::type r5;
    MPL_ASSERT_RELATION( real<r5>::value, ==, -10 );
    MPL_ASSERT_RELATION( imag<r5>::value, ==, 2 );
#endif
}

MPL_TEST_CASE()
{
    MPL_ASSERT(( equal_to<c1,c1> ));
    MPL_ASSERT(( equal_to<c2,c2> ));
    MPL_ASSERT_NOT(( equal_to<c1,c2> ));

    MPL_ASSERT(( equal_to<c1, complex< long_<5>, long_<-1> > > ));

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    MPL_ASSERT_NOT(( equal_to<c1,i> ));
    MPL_ASSERT_NOT(( equal_to<i,c2> ));
#endif
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testGaussianBayesNet.cpp
 * @brief   Unit tests for GaussianBayesNet
 * @author  Frank Dellaert
 */

// STL/C++
#include <iostream>
#include <sstream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>

#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <gtsam/base/Testable.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <tests/smallExample.h>

using namespace std;
using namespace gtsam;
using namespace example;

static const Index _x_=0, _y_=1, _z_=2;

/* ************************************************************************* */
TEST( GaussianBayesNet, constructor )
{
  // small Bayes Net x <- y
  // x y d
  // 1 1 9
  //   1 5
  Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0);
  Matrix                         R22 = Matrix_(1,1,1.0);
  Vector d1(1), d2(1);
  d1(0) = 9; d2(0) = 5;
  Vector sigmas(1);
  sigmas(0) = 1.;

  // define nodes and specify in reverse topological sort (i.e. parents last)
  GaussianConditional x(_x_,d1,R11,_y_,S12, sigmas), y(_y_,d2,R22, sigmas);

  // check small example which uses constructor
  GaussianBayesNet cbn = createSmallGaussianBayesNet();
  EXPECT( x.equals(*cbn[_x_]) );
  EXPECT( y.equals(*cbn[_y_]) );
}

/* ************************************************************************* */
TEST( GaussianBayesNet, matrix )
{
  // Create a test graph
  GaussianBayesNet cbn = createSmallGaussianBayesNet();

  Matrix R; Vector d;
  boost::tie(R,d) = matrix(cbn); // find matrix and RHS

  Matrix R1 = Matrix_(2,2,
		      1.0, 1.0,
		      0.0, 1.0
    );
  Vector d1 = Vector_(2, 9.0, 5.0);

  EXPECT(assert_equal(R,R1));
  EXPECT(assert_equal(d,d1));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, optimize )
{
  GaussianBayesNet cbn = createSmallGaussianBayesNet();
  VectorValues actual = optimize(cbn);

  VectorValues expected(vector<size_t>(2,1));
  expected[_x_] = Vector_(1,4.);
  expected[_y_] = Vector_(1,5.);

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, optimize2 )
{

	// Create empty graph
	GaussianFactorGraph fg;
	SharedDiagonal noise = noiseModel::Unit::Create(1);

	fg.add(_y_, eye(1), 2*ones(1), noise);

	fg.add(_x_, eye(1),_y_, -eye(1), -ones(1), noise);

	fg.add(_y_, eye(1),_z_, -eye(1), -ones(1), noise);

	fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise);

  VectorValues actual = *GaussianSequentialSolver(fg).optimize();

  VectorValues expected(vector<size_t>(3,1));
  expected[_x_] = Vector_(1,1.);
  expected[_y_] = Vector_(1,2.);
  expected[_z_] = Vector_(1,3.);

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, optimize3 )
{
	// y=R*x, x=inv(R)*y
	// 9 = 1 1   4
	// 5     1   5
	// NOTE: we are supplying a new RHS here
  GaussianBayesNet cbn = createSmallGaussianBayesNet();

  VectorValues expected(vector<size_t>(2,1)), x(vector<size_t>(2,1));
  expected[_x_] = Vector_(1, 4.);
  expected[_y_] = Vector_(1, 5.);

  // test functional version
  VectorValues actual = optimize(cbn);
  EXPECT(assert_equal(expected,actual));

  // test imperative version
  optimizeInPlace(cbn,x);
  EXPECT(assert_equal(expected,x));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, backSubstituteTranspose )
{
	// x=R'*y, y=inv(R')*x
	// 2 = 1    2
	// 5   1 1  3
  GaussianBayesNet cbn = createSmallGaussianBayesNet();

  VectorValues y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
  x[_x_] = Vector_(1,2.);
  x[_y_] = Vector_(1,5.);
  y[_x_] = Vector_(1,2.);
  y[_y_] = Vector_(1,3.);

  // test functional version
  VectorValues actual = backSubstituteTranspose(cbn,x);
  EXPECT(assert_equal(y,actual));
}

/* ************************************************************************* */
// Tests computing Determinant
TEST( GaussianBayesNet, DeterminantTest )
{
	GaussianBayesNet cbn;
	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
					0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ),
					1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0),
					ones(2)));

	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
					1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ),
					2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0),
					ones(2)));

	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
			3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ),
			ones(2)));

	double expectedDeterminant = 60;
	double actualDeterminant = determinant(cbn);

  EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class PoissonTestRig : public VectorIntRNGTestRig {
 public:
  PoissonTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.1, 4.99},
                            {1, 2, 3}, {-3.0, -2.0, 0.0}, {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& lambda, const T2&, const T3&,
                        T_rng& rng) const {
    return stan::math::poisson_rng(lambda, rng);
  }

  template <typename T1>
  double pmf(int y, T1 lambda, double, double) const {
    return std::exp(stan::math::poisson_lpmf(y, lambda));
  }
};

TEST(ProbDistributionsPoisson, errorCheck) {
  check_dist_throws_all_types(PoissonTestRig());
}

TEST(ProbDistributionsPoisson, distributionCheck) {
  check_counts_real(PoissonTestRig());
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acotpi capabilities

    inverse cotangent in pi multiples.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = acotpi(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[0, 1[\f$ such that <tt>cotpi(r) == x</tt>.

    @see acot, acotd, cotpi

  **/
  Value acotpi(Value const & v0);
} }
#endif

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

#endif


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

#include "get_fitzgibbon_cvpr_2001.hpp"
#include <float.h>  // For DBL_MAX
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include "posedata.hpp"
#include "radial.hpp"

namespace HomLib {
namespace FitzgibbonCVPR2001 {
    inline Eigen::Matrix3d vec2asym(const Eigen::Vector3d& t);

    HomLib::PoseData get(const Eigen::MatrixXd& x1n, const Eigen::MatrixXd& x2n) {
        // This is a five point method
        int n_points = 5;

        // Make homogenous
        Eigen::MatrixXd x1 = x1n.colwise().homogeneous();
        Eigen::MatrixXd x2 = x2n.colwise().homogeneous();

        // Compute the distance to center point
        Eigen::MatrixXd z1(3, n_points);
        z1 << Eigen::MatrixXd::Zero(2, n_points), x1n.colwise().squaredNorm();
        Eigen::MatrixXd z2(3, n_points);
        z2 << Eigen::MatrixXd::Zero(2, n_points), x2n.colwise().squaredNorm();

        // Initialize D0, D1 and D2
        Eigen::MatrixXd D0(9, 9);
        D0.setZero();
        Eigen::MatrixXd D1(9, 9);
        D1.setZero();
        Eigen::MatrixXd D2(9, 9);
        D2.setZero();

        Eigen::Matrix3d Bx2, Bz2;
        Eigen::Matrix3d e1, e2;
        Eigen::Vector3d tmp;

        for (int k = 0; k < n_points; k++) {
            tmp = x2.col(k);
            Bx2 = HomLib::FitzgibbonCVPR2001::vec2asym(tmp);
            tmp = z2.col(k);
            Bz2 = HomLib::FitzgibbonCVPR2001::vec2asym(tmp);

            // D0
            e1 = Bx2.row(0).transpose() * x1.col(k).transpose();
            e2 = Bx2.row(1).transpose() * x1.col(k).transpose();
            D0.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {  // Assure it is 9x9
                D0.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }
            // D1
            e1 = Bx2.row(0).transpose() * z1.col(k).transpose() + Bz2.row(0).transpose() * x1.col(k).transpose();
            e2 = Bx2.row(1).transpose() * z1.col(k).transpose() + Bz2.row(1).transpose() * x1.col(k).transpose();
            D1.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {
                D1.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }

            // D2
            e1 = Bz2.row(0).transpose() * z1.col(k).transpose();
            e2 = Bz2.row(1).transpose() * z1.col(k).transpose();
            D2.row(2*k) = Eigen::Map<Eigen::VectorXd>(e1.data(), 9);
            if (k < 4) {
                D2.row(2*k + 1) = Eigen::Map<Eigen::VectorXd>(e2.data(), 9);
            }
        }

        // Create generalized eigenvalue problem
        Eigen::MatrixXd A(18, 18);
        Eigen::MatrixXd B(18, 18);
        A.setZero();
        B.setZero();

        A.topLeftCorner(9, 9) = -D0;
        A.bottomRightCorner(9, 9) = Eigen::MatrixXd::Identity(9, 9);
        B.topLeftCorner(9, 9) = D1;
        B.topRightCorner(9, 9) = D2;
        B.bottomLeftCorner(9, 9) = Eigen::MatrixXd::Identity(9, 9);

        Eigen::GeneralizedEigenSolver<Eigen::MatrixXd> ges;
        ges.compute(A, B, true);
        Eigen::VectorXcd l;
        Eigen::MatrixXd X;
        l = ges.eigenvalues();
        Eigen::MatrixXcd eigvecs;
        eigvecs = ges.eigenvectors();
        X = eigvecs.real().topRows(9);

        // Extract correct solution
        Eigen::Matrix3d Htmp;
        Eigen::MatrixXd z(3, 5);
        double res;
        double minres = DBL_MAX;
        HomLib::PoseData posedata;
        double ltmp;
        Eigen::Array<bool, 1, 18> is_ok;
        is_ok = l.array().isFinite() && l.array().imag() == 0;

        for (int k = 0; k < 18; k++) {
            if (is_ok(k)) {
                ltmp = l(k).real();
                Htmp = Eigen::Map<Eigen::Matrix3d>(X.col(k).data(), 3, 3);
                z = Htmp * radialundistort(x1.colwise().hnormalized(), ltmp).colwise().homogeneous();
                res = (x2.colwise().hnormalized() - radialdistort(z.colwise().hnormalized(), ltmp)).squaredNorm();
                if (res < minres) {
                    minres = res;
                    posedata.homography = Htmp;
                    posedata.distortion_parameter = ltmp;
                }
            }
        }

        return posedata;
    }

    // TODO(marcusvaltonen): Refactor -> helpers when necessary
    inline Eigen::Matrix3d vec2asym(const Eigen::Vector3d& t) {
        Eigen::Matrix3d t_hat;
        t_hat << 0, -t(2), t(1),
                 t(2), 0, -t(0),
                -t(1), t(0), 0;
        return t_hat;
    }
}  // namespace FitzgibbonCVPR2001
}  // namespace HomLib


#include "tbb/parallel_for.h"
#include <iostream>
#include <mex.h>
#include <omp.h>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
// #include <Eigen/StdVector>

// #include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

// #include "storage_aliases.h"

// #define ngp int(*mxGetPr(prhs[0]))
#define Hdo prhs[0]
#define inv_Hdo prhs[1]
#define sign int(*mxGetPr(prhs[2]))
#define C_operator double(*mxGetPr(prhs[3]))
#define I_t prhs[4]
#define d_alpha_in prhs[5]
#define alpha_in prhs[6]
#define delta_in prhs[7]

#define output plhs[0]

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  auto Hdo_Dimensions = mxGetDimensions(Hdo);
  auto alpha_Dimensions = mxGetDimensions(alpha_in);

  size_t const ncomp = 6; // TODO 3D only
  size_t const ngp = Hdo_Dimensions[2];
  size_t const ntp = Hdo_Dimensions[3];
  size_t const nmodes = alpha_Dimensions[1];

  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;
  using matrix6 = Eigen::Matrix<double, 6, 6>;

  Eigen::Map<Eigen::MatrixXd> Int_time(mxGetPr(I_t), ntp, ntp);

  auto const tensor_shift = ncomp * ncomp;
  auto const delta_shift = ncomp;

  Matrix A = Matrix::Zero(nmodes * ntp, nmodes * ntp);
  Vector b = Vector::Zero(ntp * nmodes);

  Vector alpha = Eigen::Map<Vector>(mxGetPr(alpha_in), ntp);
  Vector d_alpha = Eigen::Map<Vector>(mxGetPr(d_alpha_in), ntp);

  // A
  Vector int_C_C_alpha_alpha =
      pow(C_operator, 2) * Int_time * (alpha.cwiseProduct(alpha));
  double int_C_alpha_d_alpha =
      C_operator * alpha.transpose() * (Int_time * d_alpha);
  Vector int_d_alpha_d_alpha = (Int_time * d_alpha.cwiseProduct(d_alpha));

  // b
  Vector int_C_alpha = C_operator * (Int_time * alpha);
  Vector int_d_alpha = (Int_time * d_alpha);

  Vector space_mode = Vector::Zero(ngp * ncomp);
  Matrix a2 = 2 * int_C_alpha_d_alpha * Matrix::Identity(ncomp, ncomp);

  // #pragma omp parallel for
  for (size_t i = 0; i < ngp; i++) {
    Matrix a1 = Matrix::Zero(ncomp, ncomp);
    Matrix a3 = Matrix::Zero(ncomp, ncomp);
    Matrix A = Matrix::Zero(ncomp, ncomp);

    Vector b1 = Vector::Zero(ncomp);
    Vector b2 = Vector::Zero(ncomp);
    Vector b = Vector::Zero(ncomp);

    for (size_t j = 0; j < ntp; j++) {

      int const delta_idx = (j * ngp + i) * delta_shift;
      int const tensor_idx = (j * ngp + i) * tensor_shift;

      matrix6 H = Eigen::Map<matrix6>(mxGetPr(Hdo) + tensor_idx);
      matrix6 iH = Eigen::Map<matrix6>(mxGetPr(inv_Hdo) + tensor_idx);
      Vector delta = Eigen::Map<Vector>(mxGetPr(delta_in) + delta_idx, ncomp);

      a1 += H * int_C_C_alpha_alpha(j);
      a3 += iH * int_d_alpha_d_alpha(j);
      b1 += delta * int_C_alpha(j);
      b2 += (iH * delta) * int_d_alpha(j);
    }

    A = a1 - sign * a2 + a3;
    b = b1 - sign * b2;

#pragma omp critical
    if (b.norm() > 0)
      space_mode.segment(i * ncomp, ncomp) = A.partialPivLu().solve(b);
    else
      space_mode.segment(i * ncomp, ncomp) = b;
  }

  output = mxCreateDoubleMatrix(ngp * ncomp, 1, mxREAL);
  auto out_pt = mxGetPr(output);
  Eigen::Map<Eigen::MatrixXd> mapp(out_pt, ngp * ncomp, 1);
  mapp = space_mode; // copy
  // std::copy(x.begin(), x.end(), mxGetPr(output));
  return;
}

// mxArray *cpp_to_MexArray(const std::vector<double> &v) {
//   mxArray *mx = mxCreateDoubleMatrix(1, v.size(), mxREAL);
//   std::copy(v.begin(), v.end(), mxGetPr(mx));
//
//   return mx;
// }


// BLAS level 1
// std::vector<>, std::valarray<>, boost::array<>, C array

// element type: float or double
#ifdef F_FLOAT
typedef float real_t;
#else
typedef double real_t;
#endif

#include <iostream>
#include <iterator>
#include <algorithm>
#include <complex>

#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/bindings/std/valarray.hpp>
#include <boost/numeric/bindings/boost/array.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>

#include "utils.h"

namespace blas = boost::numeric::bindings::blas;

using std::cout;
using std::endl;
using std::size_t;

int main() {

  int n = 10;

  cout << endl;
  cout << "std::vector" << endl;
  std::vector<real_t> sv (n);
  init_v (sv, kpp (1));
  print_v (sv, "sv");
  cout << "std::valarray" << endl;
  std::valarray<real_t> va (n);
  blas::set (0.1, va);
  print_v (va, "va");
  cout << endl;

  cout << "dot(): sv^t va: ";
  real_t d = 0;
  for (int i = 0; i < n; ++i)
    d += sv[i] * va[i];

  cout << "is " << d << " == " << blas::dot (sv, va) << " ?" << endl;
  cout << endl;

#ifdef F_FLOAT
  cout << "sdsdot(): 10 + sv^T va = " << blas::sdsdot (10, sv, va) << endl;
  cout << endl;
#endif

  blas::scal (real_t(2), sv);
  print_v (sv, "scal(): 2 sv");

  cout << endl;

  std::random_shuffle (sv.begin(), sv.end());
  cout << "shuffled sv: ";
  std::copy (sv.begin(), sv.end(), std::ostream_iterator<real_t> (cout, " "));
  cout << endl;
  int i = blas::iamax (sv);
  cout << "iamax():\n  index of max el = " << i
       << "; max el = " << sv[i] << endl;
  cout << endl;

  cout << "asum():\n  ||sv||_1 =  " << blas::asum (sv)
       << "; ||va||_1 = " << blas::asum (va) << endl;
  cout << "nrm2():\n  ||sv||_2 = " << blas::nrm2 (sv)
       << "; ||va||_2 = " << blas::nrm2 (va) << endl;
  cout << endl;

  cout << "boost::array" << endl;
  boost::array<double, 10> ba;
  blas::set (0.1, ba);
  print_v (ba, "ba");
  cout << "C array" << endl;
  typedef double double_array[10];
  double_array ca;
  blas::set (1., ca);
  print_v (ca, "ca");
  cout << endl;

  blas::axpy (0.1, ba, ca);
  print_v (ca, "axpy(): 0.1 ba + ca");

//  blas::axpby (0.1, ba, 2., ca);
//  print_v (ca, "axpby(): 0.1 ba + 2.0 ca");

  cout << endl;
}


#include <Engine/MeshEdit/Simulate.h>
#include <windows.h>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

using namespace Ubpa;
using namespace std;
using namespace Eigen;

void Simulate::SetFast(){
	isfast = true;
}

void Simulate::Clear() {
	this->positions.clear();
}

bool Simulate::Init() {
	isfast = true;
	m = positions.size(); // number of vertices
	s = edgelist.size() / 2;  // number of springs
	g = 9.8;
	iteration = 10;
	stiff = 1e5;

	this->velocity.resize(positions.size());
	for (int i = 0; i < positions.size(); i++)
		for (int j = 0; j < 3; j++)
			this->velocity[i][j] = 0;

	// init l
	for (int i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = positions[index1];
		pointf3 v2 = positions[index2];
		l.push_back((v1 - v2).norm());
	}

	mass.resize(3 * positions.size());
	f_int.resize(3 * positions.size());
	for (int i = 0; i < 3 * positions.size(); i++) {
		f_int[i] = 0.0;
		mass[i] = 1;
	}

	x.resize(3 * m); // vector x, initialized to be y
	y.resize(3 * m); // vector y = 2q_n - q_n-1
	x_pre.resize(3 * m);
	for (int i = 0; i < m; i++) {
		x.segment(3 * i, 3) << positions[i][0], positions[i][1], positions[i][2];
		y.segment(3 * i, 3) = x.segment(3 * i, 3);
		x_pre.segment(3 * i, 3) = x.segment(3 * i, 3);
	}

	// init Mass
	M = MatrixXd::Identity(m * 3, m * 3);

	// init f_ext, i.e. gravity
	f_ext.resize(3 * m);
	for (int i = 0; i < m; i++)
		f_ext.segment(3 * i, 3) = Vector3d(0, -mass[i] * g, 0);

	L = MatrixXd::Zero(m * 3, m * 3);
	buildL();
	J = MatrixXd::Zero(m * 3, s * 3);
	buildJ();

	FixPoint();
	buildK();
	getb();

	// prefactorization
	MatrixXd A_;
	A_.resize(K.rows(), K.rows());
	A_ = K * (M + h * h * L) * K.transpose();

	A = A_.sparseView();
	LLT_.compute(A);

	return true;
}

void Simulate::SetLeftFix() {
	// 固定网格x坐标最小点
	fixed_id.clear();
	double x = 100000;
	for (int i = 0; i < positions.size(); i++)
	{
		if (positions[i][0] < x)
		{
			x = positions[i][0];
		}
	}

	for (int i = 0; i < positions.size(); i++)
	{
		if (abs(positions[i][0] - x) < 1e-5)
		{
			fixed_id.push_back(i);
		}
	}

	Init();
}

void Simulate::FixPoint() {
	fixed_id.push_back(10);
 	fixed_id.push_back(120);
}

void Simulate::buildK() {
	K = MatrixXd::Zero(m * 3 - 3 * fixed_id.size(), m * 3);
	set<int> fix(fixed_id.begin(), fixed_id.end());
	for (int i = 0, j = 0; i < m * 3; i++) {
		if (fix.find(i / 3) == fix.end()) {
			K(j++, i) = 1;
		}
	}
}

void Simulate::buildL() {
	// L is 3m * 3m matrix
	MatrixXd temp = MatrixXd::Zero(m, m);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		VectorXd Ai = VectorXd::Zero(m);
		Ai(index1) += 1;
		Ai(index2) -= 1;
		temp += stiff * Ai * Ai.transpose();
	}

	// kronecker product, L = kronecker(temp, I3)
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < m; i++)
		for (int j = 0; j < m; j++)
			L.block(i * 3, j * 3, 3, 3) = temp(i, j) * I3;
}

void Simulate::buildJ() {
	// J is 3m * 3s matrix
	MatrixXd temp = MatrixXd::Zero(m, s);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		VectorXd Ai = VectorXd::Zero(m);
		VectorXd Si = VectorXd::Zero(s);
		Ai(index1) += 1;
		Ai(index2) -= 1;
		Si(i) += 1;
		temp += stiff * Ai * Si.transpose();
	}

	// kronecker product, J = kronecker(temp, I3)
	Matrix3d I3 = Matrix3d::Identity();
	for (int i = 0; i < m; i++)
		for (int j = 0; j < s; j++)
			J.block(i * 3, j * 3, 3, 3) = temp(i, j) * I3;
}

void Simulate::local() {
	d = VectorXd::Ones(s * 3);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		Vector3d p1 = x.segment(3 * index1, 3);
		Vector3d p2 = x.segment(3 * index2, 3);

		d.segment(3 * i, 3) = l[i] * (p1 - p2) / (p1 - p2).norm();
	}

	cout << "Local Step" << endl;
}

void Simulate::global() {
	VectorXd RHS = K * (h * h * J * d + M * y + h * h * f_ext - (M + h * h * L) * b);
	VectorXd xf = LLT_.solve(RHS);
	x = K.transpose() * xf + b;
	cout << "Global Step" << endl;
}

void Simulate::getb() {
	b = x - K.transpose() * K * x;
}

void Simulate::UpdatePos() {
	for (int i = 0; i < m * 3; i++)
		positions[i / 3][i % 3] = x(i);
}

void Simulate::SimulateOnce() {
	if (!isfast) {
		buildX();
		buildV();
	} else {
		//update y, y = 2q_n - q_n-1
		y = 2 * x - x_pre;
		x_pre = x;
		size_t step = 0;
		while (step++ < iteration) {
			local();
			global();
		}
	}
	UpdatePos();
	cout << "Simulate Once" << endl;
}

bool Simulate::Run() {
	SimulateOnce();
	return true;
}

void Simulate::buildX() {
	std::vector<double> y(m * 3);
	for (int i = 0; i < x.size(); i++)
		y[i] = x(i) + h * velocity[i / 3][i % 3] + h * h / mass[i] * f_ext[i];
	xk = y;
	int i = 0;
	do {
		CalForce();
		GetGX();
		CalDiff();
		CalGxM();
		Eigen::MatrixXd t = G_inverse * gx_m;
		xk_1.clear();
		xk_1.resize(gx.size());
		for (int i = 0; i < gx.size(); i++)
			xk_1[i] = xk[i] - t(i, 0);
		i++;
		UpdateX();
		xk.resize(3 * m);
		for (int i = 0; i < m * 3; i++)
			xk[i] = x(i);
	} while (!isConv() && i <= 10);
}

void Simulate::UpdateX() {
	Eigen::MatrixXd xt;
	xt.resize((xk_1.size()), 1);
	for (int i = 0; i < xk_1.size(); i++)
		xt(i, 0) = xk_1[i];

	Eigen::MatrixXd t = K.transpose() * xt;
	for (int i = 0; i < x.size(); i++)
		x(i) = t(i, 0) + b[i];
}

void Simulate::CalGxM() {
	gx_m.resize(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		gx_m(i, 0) = gx[i];
}

bool Simulate::isConv() {
	double delta = 0.01;
	std::vector<double> zero(gx.size(), delta);
	for (int i = 0; i < gx.size(); i++)
		if (abs(gx[i]) > zero[i])
			return false;
	return true;
}

void Simulate::GetGX() {
	std::vector<double> y(xk.size());
	gx.resize(xk.size());

	for (int i = 0; i < xk.size(); i++)
		y[i] = xk[i] + h * velocity[i / 3][i % 3] + h * h / mass[i] * f_ext[i];

	for (int i = 0; i < xk.size(); i++)
		gx[i] = mass[i] * (xk[i] - y[i]) - h * h * f_int[i];

	Eigen::MatrixXd t(gx.size(), 1);
	for (int i = 0; i < gx.size(); i++)
		t(i, 0) = gx[i];

	t = K * t;

	gx.clear();
	gx.resize(t.rows());
	for (int i = 0; i < gx.size(); i++)
		gx[i] = t(i, 0);

	Eigen::MatrixXd xt;
	xt.resize((xk.size()), 1);

	for (int i = 0; i < xk.size(); i++)
		xt(i, 0) = xk[i];

	t = K * xt;
	xk.clear();
	xk.resize(t.rows());
	for (int i = 0; i < xk.size(); i++)
		xk[i] = t(i, 0);
}

void Simulate::CalDiff() {
	std::vector<Eigen::Triplet<double> > triple;
	Eigen::MatrixXd I = MatrixXd::Identity(3, 3);
	for (int i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = pointf3(x(3 * index1 + 0), x(3 * index1 + 1), x(3 * index1 + 2));
		pointf3 v2 = pointf3(x(3 * index2 + 0), x(3 * index2 + 1), x(3 * index2 + 2));
		vecf3 r = v1 - v2;
		Vector3d t;
		t << r[0], r[1], r[2];

		Eigen::MatrixXd dif;
		dif.resize(3, 3);
		dif = stiff * (l[i] / r.norm() - 1) * I - stiff * l[i]
			/ ((r.norm()) * (r.norm()) * (r.norm())) * t * t.transpose();
		for (int j = 0; j < 3; j++)
			for (int k = 0; k < 3; k++) {
				triple.push_back(Eigen::Triplet<double>(3 * index1 + j, 3 * index1 + k, -h * h * dif(j, k)));
				triple.push_back(Eigen::Triplet<double>(3 * index2 + k, 3 * index2 + j, -h * h * dif(j, k)));
			}
	}

	for (int i = 0; i < m * 3; i++)
		triple.push_back(Eigen::Triplet<double>(i, i, mass[i]));

	Eigen::MatrixXd diff_;
	Eigen::SparseLU<Eigen::SparseMatrix<double>> LU_;

	diff.setZero();
	diff.resize(m * 3, m * 3);
	diff.setFromTriplets(triple.begin(), triple.end());

	diff_ = K * diff * K.transpose();
	diff = diff_.sparseView();

	I = MatrixXd::Identity(gx.size(), gx.size());
	LU_.analyzePattern(diff);
	LU_.factorize(diff);
	G_inverse = LU_.solve(I);
}

void Simulate::buildV() {
	for (int i = 0; i < x.size(); i++)
		velocity[i / 3][i % 3] = (x(i) - positions[i / 3][i % 3]) / h;
}

void Simulate::CalForce() {
	f_int.clear();
	f_int.resize(3 * positions.size());
	for (int i = 0; i < edgelist.size() / 2; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 v1 = pointf3(x(3 * index1 + 0), x(3 * index1 + 1), x(3 * index1 + 2));
		pointf3 v2 = pointf3(x(3 * index2 + 0), x(3 * index2 + 1), x(3 * index2 + 2));
		vecf3 r = v1 - v2;
		for (int j = 0; j < 3; j++) {
			f_int[3 * index1 + j] += -stiff * (r.norm() - l[i]) * r[j] / r.norm();
			f_int[3 * index2 + j] += stiff * (r.norm() - l[i]) * r[j] / r.norm();
		}
	}
}


//-std=c++17 -static -m64 -Wall -Os

#include <boost/random.hpp>
#include <iostream>

int main(int argc,char *argv[])
{
  boost::mt19937 gen; //伪随机数发生器（generator）
  boost::uniform_int<>dist(1,10000000);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<>>die(gen,dist);

  for(auto i = 0; i < 1000; ++i)
  {
	std::cout << die() << "  ";
  }
  std::cout << "\n";
}


// Copyright 2004-5 The Trustees of Indiana University.
// Copyright 2002 Brad King and Douglas Gregor

// Use, modification and distribution is 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_PAGE_RANK_HPP
#define BOOST_GRAPH_PAGE_RANK_HPP

#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <vector>

namespace boost { namespace graph {

struct n_iterations
{
  explicit n_iterations(std::size_t n) : n(n) { }

  template<typename RankMap, typename Graph>
  bool
  operator()(const RankMap&, const Graph&)
  {
    return n-- == 0;
  }

 private:
  std::size_t n;
};

namespace detail {
  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      incidence_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type rank_type;

    // Set new rank maps
    BGL_FORALL_VERTICES_T(v, g, Graph) put(to_rank, v, rank_type(1 - damping));

    BGL_FORALL_VERTICES_T(u, g, Graph) {
      rank_type u_rank_out = damping * get(from_rank, u) / out_degree(u, g);
      BGL_FORALL_ADJ_T(u, v, g, Graph)
        put(to_rank, v, get(to_rank, v) + u_rank_out);
    }
  }

  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      bidirectional_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type damping_type;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      typename property_traits<RankMap>::value_type rank(0);
      BGL_FORALL_INEDGES_T(v, e, g, Graph)
        rank += get(from_rank, source(e, g)) / out_degree(source(e, g), g);
      put(to_rank, v, (damping_type(1) - damping) + damping * rank);
    }
  }
} // end namespace detail

template<typename Graph, typename RankMap, typename Done, typename RankMap2>
void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n,
          RankMap2 rank_map2)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  rank_type initial_rank = rank_type(rank_type(1) / n);
  BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, initial_rank);

  bool to_map_2 = true;
  while ((to_map_2 && !done(rank_map, g)) ||
         (!to_map_2 && !done(rank_map2, g))) {
    typedef typename graph_traits<Graph>::traversal_category category;

    if (to_map_2) {
      detail::page_rank_step(g, rank_map, rank_map2, damping, category());
    } else {
      detail::page_rank_step(g, rank_map2, rank_map, damping, category());
    }
    to_map_2 = !to_map_2;
  }

  if (!to_map_2) {
    BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, get(rank_map2, v));
  }
}

template<typename Graph, typename RankMap, typename Done>
void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  std::vector<rank_type> ranks2(num_vertices(g));
  page_rank(g, rank_map, done, damping, n,
            make_iterator_property_map(ranks2.begin(), get(vertex_index, g)));
}

template<typename Graph, typename RankMap, typename Done>
inline void
page_rank(const Graph& g, RankMap rank_map, Done done,
          typename property_traits<RankMap>::value_type damping = 0.85)
{
  page_rank(g, rank_map, done, damping, num_vertices(g));
}

template<typename Graph, typename RankMap>
inline void
page_rank(const Graph& g, RankMap rank_map)
{
  page_rank(g, rank_map, n_iterations(20));
}

// TBD: this could be _much_ more efficient, using a queue to store
// the vertices that should be reprocessed and keeping track of which
// vertices are in the queue with a property map. Baah, this only
// applies when we have a bidirectional graph.
template<typename MutableGraph>
void
remove_dangling_links(MutableGraph& g)
{
  typename graph_traits<MutableGraph>::vertices_size_type old_n;
  do {
    old_n = num_vertices(g);

    typename graph_traits<MutableGraph>::vertex_iterator vi, vi_end;
    for (tie(vi, vi_end) = vertices(g); vi != vi_end; /* in loop */) {
      typename graph_traits<MutableGraph>::vertex_descriptor v = *vi++;
      if (out_degree(v, g) == 0) {
        clear_vertex(v, g);
        remove_vertex(v, g);
      }
    }
  } while (num_vertices(g) < old_n);
}

} } // end namespace boost::graph

#endif // BOOST_GRAPH_PAGE_RANK_HPP


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

#include <SymEigsSolver.h>
#include <MatOp/DenseGenMatProd.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

template <int SelectionRule>
void run_test(const Matrix &mat, int k, int m)
{
    // Eigen::SelfAdjointEigenSolver<MatrixXd> eig(mat);
    // std::cout << "all eigenvalues = \n" << eig.eigenvalues().transpose() << "\n";

    DenseGenMatProd<double> op(mat);
    SymEigsSolver<double, SelectionRule, DenseGenMatProd<double>> eigs(&op, k, m);
    eigs.init();
    int nconv = eigs.compute();
    int niter = eigs.num_iterations();
    int nops = eigs.num_operations();

    REQUIRE( nconv > 0 );

    Vector evals = eigs.eigenvalues();
    Matrix evecs = eigs.eigenvectors();

    // std::cout << "computed eigenvalues D = \n" << evals.transpose() << "\n";
    // std::cout << "computed eigenvectors U = \n" << evecs << "\n\n";
    Matrix err = mat * evecs - evecs * evals.asDiagonal();

    INFO( "nconv = " << nconv );
    INFO( "niter = " << niter );
    INFO( "nops = " << nops );
    INFO( "||AU - UD||_inf = " << err.array().abs().maxCoeff() );
    REQUIRE( err.array().abs().maxCoeff() == Approx(0.0) );
}

void run_test_sets(const Matrix &mat, int k, int m)
{
    SECTION( "Largest Magnitude" )
    {
        run_test<LARGEST_MAGN>(mat, k, m);
    }
    SECTION( "Largest Value" )
    {
        run_test<LARGEST_ALGE>(mat, k, m);
    }
    SECTION( "Smallest Magnitude" )
    {
        run_test<SMALLEST_MAGN>(mat, k, m);
    }
    SECTION( "Smallest Value" )
    {
        run_test<SMALLEST_ALGE>(mat, k, m);
    }
    SECTION( "Both Ends" )
    {
        run_test<BOTH_ENDS>(mat, k, m);
    }
}

TEST_CASE("Eigensolver of symmetric real matrix [10x10]", "[eigs_sym]")
{
    srand(123);

    Matrix A = Eigen::MatrixXd::Random(10, 10);
    Matrix M = A + A.transpose();
    int k = 3;
    int m = 6;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of symmetric real matrix [100x100]", "[eigs_sym]")
{
    srand(123);

    Matrix A = Eigen::MatrixXd::Random(100, 100);
    Matrix M = A + A.transpose();
    int k = 10;
    int m = 20;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of symmetric real matrix [1000x1000]", "[eigs_sym]")
{
    srand(123);

    Matrix A = Eigen::MatrixXd::Random(1000, 1000);
    Matrix M = A + A.transpose();
    int k = 20;
    int m = 50;

    run_test_sets(M, k, m);
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/significants.hpp>
#include <boost/simd/function/simd/enumerate.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
namespace bd =  boost::dispatch;
template < int N >
struct signif
{
  template<class T> T operator()(const T & a) const
  {
    using i_t = bd::as_integer_t<T>;
    return bs::significants(a, bs::enumerate<i_t>(0, N));
  }
};

DEFINE_SCALAR_BENCH(scalar_significantsp, signif< 1>());
DEFINE_SCALAR_BENCH(scalar_significantsn, signif<-1>());

DEFINE_BENCH_MAIN() {
  nsb::for_each<scalar_significantsn, NS_BENCH_IEEE_TYPES>(-10, 10);
  nsb::for_each<scalar_significantsp, NS_BENCH_IEEE_TYPES>(-10, 10);
}