/*
    File Name:      main.cpp
    ROS module:     simpl_ekf
    Description:    Implements an extended Kalman Filter for attitude estimation.
                    Only uses the accelerometer measurements with a simplified
                    model to compute pitch and roll of the device.
    Author:         Thomas Lew
    Last update:    2017.03.13
    Comments:       -Assumptions: - No movement (linear & angular) of the device
                                  - The model is therefore overly simplified.
                    -Known bug: The pitch value is always negative regardless of
                      (solved)  the rotation around y-axis. The cause is a wrong
                                z-value of vector "y" in update function.
                                This problem is solved in publishOdometryResult()
                                by using accel-x (ax) to determine the direction
                                of the pitch angle.
                                                                                */
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>

//  Library for linear algebra
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <math.h>

using namespace std;
using namespace Eigen;

//  Debugging boolean for printing EKF variables during execution
#define EKF_DEBUG 1

//  Number of IMU measurements used for calibration
#define IMU_COUNT_CALIBRATION 30

//  Euler angles values limits for debugging
#define PITCH_MAX M_PI/2
#define ROLL_MAX  M_PI/2
#define YAW_MAX   M_PI/2

/*  ------------------------------------------------------------
    Parameters of the Extended Kalman Filter. Depends on the IMU
    ------------------------------------------------------------                        */
/*//  Python Script simulated IMU measurements:
#define FREQ_HZ         50      // 50Hz for python Script
#define PERIOD_SEC      0.02    // 1/50 for python Script
#define SIGMA_SENSORS   0.1     // Variance of python script IMU data (imu_publisher)   */

//  Nvdia Shield Tablet IMU (only accelerometer) measurements:
#define FREQ_HZ       50        // 50Hz for Nvdia Shield Tablet
#define PERIOD_SEC    0.02      // 1/50 for Nvdia Shield Tablet
//  Computed with imu_variance.py : Variance_Accelerometer = 0.0004
#define SIGMA_SENSORS 0.02      // Variance of Nvdia Shield Tablet IMU                  */

/*  -----------------------------------------------------------------------------
    ROS Topics used for publishing the results and receiving the IMU measurements
    -----------------------------------------------------------------------------       */
ros::Publisher pub_odometry;
ros::Subscriber sub_imu;

const char imu_topic_name[] = "/imu_3dm_gx4/imu";
//const char imu_topic_name[] = "/python_IMU";

/*  ----------------------------------
    State vector

    Representation using Euler angles:

                [ Yaw ]
            x = [Pitch]
                [Roll ]

    ----------------------------------   */
VectorXd x(VectorXd::Zero(3));

//  Bias of sensors
const MatrixXd Q(PERIOD_SEC * PERIOD_SEC * MatrixXd::Identity(3,3));

//  Noise of sensors. We assume same value for accelerometer and gyroscope
const MatrixXd R(SIGMA_SENSORS * MatrixXd::Identity(3,3));

/*  --------------------
    Prediction variables
    --------------------    */

/*  Transition matrix A
    We assume a linear model with no motion, no rotation.   */
MatrixXd A(MatrixXd::Identity(3,3));

//  Covariance estimate matrix P
MatrixXd P(MatrixXd::Zero(3,3));

/*  --------------------
    Update variables
    --------------------    */

/*  Form of the measurements z:

        [accel-x]
    z = [accel-y]   with (-1 <= z <= 1)
        [accel-z]
    Note. These accelerometer measurements values are normalised.   */

/*  Measurement prediction vector H(x). It depends on the current
    Yaw, Pitch and Roll values of the state.                        */
VectorXd H(VectorXd::Zero(3));
/*  Jacobian matrix of H in regard of x                             */
MatrixXd J(MatrixXd::Zero(3,3));

/*  Accelerometer and gyroscope measurement for IMU calibration.
    These values are not used in the EKF but can be useful for
    further implementations.                                        */
double gx = 0;
double gy = 0;
double gz = 0;  //9.81

double bias_gyro_x = 0;
double bias_gyro_y = 0;
double bias_gyro_z = 0;

/*  IMU variables set at the start of the execution of the program.
    Used for calibration.                                           */
double current_t = 0;
bool is_time_init = false;
bool is_imu_init = false;
int imu_count = 0;

/*  Initialisation of the State initialisation.                     */
void state_init(){
    x.segment<3>(0) = Vector3d(0,0,0);
}

/*  Jacobian matrix Initialisation                                  */
void jacobian_init(){
    // block use: Selects the .block(1st value location, size matrix)
    /*J.block(0,3,3,3) = MatrixXd::Identity(3,3);
    J.block(0,6,3,3) = MatrixXd::Identity(3,3);*/
}

/* Covariance matrix Q initialisation                               */
void my_Q_covariance_matrix_init(){
    /*//  Values for the Nvidia shield tablet
    Q.block(0,0,3,3) = 0.032 * 0.032 * MatrixXd::Identity(3,3);
    Q.block(3,3,3,3) = 0.1 * 0.032 * MatrixXd::Identity(3,3);
    Q.block(6,6,3,3) = 0.03 * 0.032 * MatrixXd::Identity(3,3);  //  */
    /*//  Values for the test python publisher
    //  0.02 ) 1/50 with 50hz
    /*double delta_t = 1 / double(FREQ_HZ);
    Q = delta_t * delta_t * MatrixXd::Identity(3,3);*/
    /*Q.block(3,3,3,3) = 0.1 * 0.02 * MatrixXd::Identity(3,3);
    Q.block(6,6,3,3) = 0.03 * 0.02 * MatrixXd::Identity(3,3);   //  */
}

/*  Computes the original orientation and gyroscope bias.
    These values are not used in the EKF but can be useful for
    further implementations.                                        */
void imu_init(double ax, double ay, double az,
              double rx, double ry, double rz){
    gx += ax;
    gy += ay;
    gz += az;

    bias_gyro_x += rx;
    bias_gyro_y += ry;
    bias_gyro_z += rz;

    if(imu_count == IMU_COUNT_CALIBRATION){
        gx /= imu_count;
        gy /= imu_count;
        gz /= imu_count;

        bias_gyro_x /= imu_count;
        bias_gyro_y /= imu_count;
        bias_gyro_z /= imu_count;

        cout << "calibrated gx: " << gx << endl;
        cout << "calibrated gy: " << gy << endl;
        cout << "calibrated gz: " << gz << endl;

        cout << "bias_gyro_x: " << bias_gyro_x << endl;
        cout << "bias_gyro_y: " << bias_gyro_y << endl;
        cout << "bias_gyro_z: " << bias_gyro_z << endl;
    }
}

Vector3d get_euler_angles()
{
    return x.segment<3>(0);
}

void publishOdometryResult(double time_imu, double ax)
{
    //  Get current state variables
    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    //  Odometry results are published
    nav_msgs::Odometry odometry;
    odometry.header.stamp = ros::Time::now();
    odometry.header.frame_id = "world";
    odometry.pose.pose.position.x = 0;
    odometry.pose.pose.position.y = 0;
    odometry.pose.pose.position.z = 0;
    odometry.pose.pose.orientation.x = 0;
    odometry.pose.pose.orientation.y = 0;
    odometry.pose.pose.orientation.z = 0;
    odometry.pose.pose.orientation.w = 1;
    odometry.twist.twist.linear.x = 0;
    odometry.twist.twist.linear.y = 0;
    odometry.twist.twist.linear.y = 0;
    odometry.twist.twist.angular.z = angles(0);
    /*  Fix for the pitch angular being always negative */
    if(ax >= 0)
        odometry.twist.twist.angular.y = abs(angles(1));
    else
        odometry.twist.twist.angular.y = -abs(angles(1));
    odometry.twist.twist.angular.x = angles(2);

    //  Publish EKF Results
    pub_odometry.publish(odometry);

    /*  For Debugging purposes:
        The pitch, roll and yaw values are displayed on the terminal                */
    if(EKF_DEBUG){
        cout << "--------------------------------------------" << endl;
        cout << "End of kalman filtering for this imu dataset" << endl;
        cout << "--------------------------------------------" << endl;
        ROS_INFO("Odometry results published after IMU sample time: %lf", time_imu);
        cout << "Yaw: " <<      yaw     << endl;
        cout << "Pitch: " <<    pitch   << endl;
        cout << "Roll: " <<     roll    << endl;
        cout << "                     " << endl;

        //  We limit the pitch and roll values to stop a non converging Kalman Filter
        if(pitch > PITCH_MAX || pitch < -PITCH_MAX || roll > ROLL_MAX || roll < -ROLL_MAX){
            cout << "pitch or roll value too high " << endl;
            abort();
        }
    }
}

/*  Extended Kalman Filter prediction function. */
void predict(double dt){
    if(EKF_DEBUG){
        cout << "                       " << endl;
        cout << " --------------------- " << endl;
        cout << "  NEW STATE PREDICTION " << endl;
        cout << " --------------------- " << endl;
        cout << "                       " << endl;
    }

    /*  -----------------------------
        Predict the prediction matrix
        -----------------------------
            Prediction matrix A
                    {1 0 0}
                A = {0 1 0}
                    {0 0 1}         */

    MatrixXd A(MatrixXd::Identity(3,3));

    x = A * x;

    /*  The Yaw cannot be computed with this model.
        To avoid a drift of this angle, it is set to zero.              */
    x[0] = 0;

    //  Covariance matrix prediction
    P = A * P * A.transpose() + Q;

    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    if(EKF_DEBUG){
        cout << "predicted pitch   yaw   roll "         << endl;
        cout << pitch << ";  "  << yaw << ";  " << roll << endl;
        cout << "                 "                     << endl;

        cout << "Value of P after prediction"   << endl;
        cout << P                               << endl;
        cout << "                 "             << endl;

        cout << "Value of Q : "                 << endl;
        cout << Q                               << endl;
        cout << "                 "             << endl;
    }
}

/*  For Debugging purposes:
    the state vector, measurements and EKF matrices
    are displayed on the terminal.                              */
void debug_ekf_cout(double pitch, double yaw, double roll,
                    VectorXd z, Vector3d H, Matrix<double, 3, 3> J,
                    VectorXd y, Matrix<double, 3, 3> S,
                    Matrix<double, 3, 3> K, MatrixXd P){
    cout << "Old value of Pitch, yaw and Roll " << endl;
    cout << pitch << "  " << yaw << "  " << roll << endl;
    cout << "                 " << endl;
    cout << "Value of z (ax, ay, az) :" << endl;
    cout << z << endl;
    cout << "                 " << endl;
    cout << "H vector updated: " << endl;
    cout << H             << endl;
    cout << "           " << endl;
    cout << "J matix updated: " << endl;
    cout << J << endl;
    cout << "                 " << endl;
    cout << "Value of y " << endl;
    cout << y << endl;
    cout << "                 " << endl;
    /*cout << "S  updated: "<< endl;
    cout << S << endl;
    cout << "                 " << endl;
    cout << "S inverse updated: "<< endl;
    cout << S.inverse() << endl;
    cout << "                 " << endl;*/
    cout << "K updated: " << endl;
    cout << K << endl;
    cout << "                 " << endl;
    cout << "K * y updated: " << endl;
    cout << K * y << endl;
    cout << "                 " << endl;
    /*cout << "P updated: "<< endl;
    cout << P << endl;
    cout << "                 " << endl;*/
}

/*  Extended Kalman Filter update function. */
void update(double ax, double ay, double az, double rx, double ry, double rz, double dt)
{
    if(EKF_DEBUG){
        cout << "                 " << endl;
        cout << "---------------- " << endl;
        cout << " NEW UPDATE n. "   << imu_count << endl;
        cout << "---------------- " << endl;
        cout << "                 " << endl;
    }

    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    //  Measurement Vector
    VectorXd z(3);
    z << ax, ay, az;

    /*  ---------------------------------------------------
        -                Measurement Model                -
        ---------------------------------------------------   */

    /*          --------------------------
                Measurement vector z model
                --------------------------
                               T  (0)            -1       T
                z = H(x) = Rzyx * (0),  with Rzyx   = Rzyx
                                  (1)   and Rzyx = Rz*Ry*Rx = R = ...

    [cos(yaw) -sin(yaw) 0][cos(pitch) 0 sin(pitch)][1     0          0    ]
    [sin(yaw) cos(yaw)  0][    0      1      0    ][0 cos(roll) -sin(roll)]
    [   0         0     1][-sin(pitch)0 cos(pitch)][0 sin(roll)  cos(roll)]

            --------------------------------------
            Jacobian matrix from measurement model
            --------------------------------------
                    J = d/dx (H(x))
                                                                        */

    //  Theoretical Measurement from state estimate:
    Vector3d H(3);
    H <<    -sin(pitch),
            cos(pitch)*sin(roll),
            cos(pitch)*cos(roll);

            /*cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll),
            sin(yaw)*sin(pitch)*sin(roll) - cos(yaw)*sin(roll),
            cos(pitch)*cos(roll);*/

    //  Jacobian matrix J computation
    Matrix<double, 3, 3> J;
    J <<    0,  -cos(pitch),            0,
            0,  -sin(pitch)*sin(roll),  cos(pitch)*cos(roll),
            0,  -sin(pitch)*cos(roll),  -cos(pitch)*sin(roll);

    /*
    -sin(yaw)*sin(pitch)*cos(roll)+cos(yaw)*sin(roll), cos(yaw)*cos(pitch)*cos(roll), -cos(yaw)*sin(pitch)*sin(roll)+sin(yaw)*cos(roll),
    cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll),  sin(yaw)*cos(pitch)*cos(roll), -sin(yaw)*sin(pitch)*sin(roll)-cos(yaw)*cos(roll),
    0,                                                 -sin(pitch)*cos(roll),         -cos(pitch)*sin(roll);*/

    /*  -------------------------------------
        Update step of Extended Kalman Filter
        -------------------------------------  */

    //  Innovation vector
    VectorXd y(3);
    y = z - H;

    //  Innovation covariance
    MatrixXd S(3,3);
    S = J * P * J.transpose() + R;

    //  Kalman gain
    MatrixXd K(3,3);
    K = P * J * S.inverse();

    //  State estimate update
    x = x + K * y;

    //  Covariance estimate update
    P = (MatrixXd::Identity(3, 3) - K * J) * P;

    //  If Debug Mode: Print all variables computed by this update
    if(EKF_DEBUG){
        debug_ekf_cout(pitch, yaw, roll, z, H, J, y, S, K, P);
    }
}

/*  Extended Kalman Filter global function.             */
void kalman_filtering(double ax, double ay, double az,
                      double rx, double ry, double rz,
                      double dt){
    predict(dt);
    update(ax, ay, az, rx, ry, rz, dt);
}

/*  IMU callback function.
    Uses the IMU measurements to compute the new state variables using an EKF. */
void imu_callback(const sensor_msgs::Imu& imu_msg)
{
    ROS_INFO("Time of IMU sample: %lf", imu_msg.header.stamp.toSec());

    /*  Accelerometer and Gyroscope measurements.
        This version doesn't use the angular velocity values */
    double ax = imu_msg.linear_acceleration.x;
    double ay = imu_msg.linear_acceleration.y;
    double az = imu_msg.linear_acceleration.z;
    double rx = imu_msg.angular_velocity.x;
    double ry = imu_msg.angular_velocity.y;
    double rz = imu_msg.angular_velocity.z;

    /*  ----------------------------------------------------------------------
        Calibrate Inertial Measurement Unit (IMU) at the start of the program.
        Can be useful in case of an initial spatial frame which is moving.
        Uses the IMU_COUNT_CALIBRATION first IMU values.
        ----------------------------------------------------------------------  */
    imu_count++;
    if(!is_imu_init){
        imu_init(ax,ay,az,rx,ry,rz);
        if(imu_count == IMU_COUNT_CALIBRATION){
            is_imu_init = true;
            imu_count = 0;
        }
        return;
    }

    //  After imu calibration, set the IMU time
    if(!is_time_init){
        current_t = imu_msg.header.stamp.toSec();
        is_time_init = true;
        return;
    }

    /*  --------------------------------
        Accelerometer data normalisation
        --------------------------------  */
    double norm_accel = sqrt(ax * ax + ay * ay + az * az);
    ax = ax / norm_accel;
    ay = ay / norm_accel;
    az = az / norm_accel;

    /*  ---------------
        IMU Time update
        ---------------  */
    double new_time = imu_msg.header.stamp.toSec();
    double dt = new_time - current_t;
    current_t = new_time;

    /*  --------------------------------------------------------------
        Extended Kalman Filtering.

        In this version, only the accelerometer measurements are used.
        --------------------------------------------------------------  */
    kalman_filtering(ax, ay, az, rx, ry, rz, dt);

    /*  -------------------------------------------------
        Results of the EKF are pusblished in a ROS topic.
        -------------------------------------------------  */
    publishOdometryResult(imu_msg.header.stamp.toSec(), ax);
}

void setupROS()
{
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);

    //  Define ROS topics to publish the results
    pub_odometry     = n.advertise<nav_msgs::Odometry>("odometry", 1000);

    //  Define IMU Measurements ROS topic.
    sub_imu = n.subscribe(imu_topic_name, 1000, imu_callback);
}

int main(int argc, char **argv)
{
    state_init();
    jacobian_init();
    my_Q_covariance_matrix_init();

    ros::init(argc, argv, "simpl_ekf");

    setupROS();
    ros::spin();

    return 0;
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/biconnected_components.hpp>

using namespace std;

struct topological {
  bool operator() (const pair<int, int> t0, const pair<int, int> t1) {
    return (t0.first < t1.first) || (t0.first == t1.first && t0.second < t1.second);
  }
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      graph;
typedef boost::property_map<graph, boost::edge_weight_t>::type edge_map;
typedef boost::graph_traits<graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<graph>::vertex_descriptor          vertex_desc;
typedef boost::graph_traits<graph>::edge_iterator                         edge_it;

void important_bridges(graph &G) {
  edge_map component = boost::get(boost::edge_weight, G);
  int ncc = boost::biconnected_components(G, component);

  vector<pair<int, int>> imp_bridges(0);
  edge_it ebeg, eend;
  vector<int> size_ncc(ncc, 0);
  for (boost::tie(ebeg, eend) = boost::edges(G); ebeg != eend; ++ebeg) {
    size_ncc[component[*ebeg]]++;
  }

  for (boost::tie(ebeg, eend) = boost::edges(G); ebeg != eend; ++ebeg) {
    int u = boost::source(*ebeg, G), v = boost::target(*ebeg, G);
    if (size_ncc[component[*ebeg]] == 1) imp_bridges.push_back({min(u,v),max(u,v)});
  }
  std::sort(imp_bridges.begin(), imp_bridges.end(), topological());
  cout << imp_bridges.size() << endl;
  for(auto e : imp_bridges)
    cout << e.first <<  " " << e.second << endl;

}

void testcase()
{
  int n; cin >> n;
  int m; cin >> m;
  graph G(n);
  for(int i = 0; i < m; i++) {
    int e1, e2; cin >> e1; cin >> e2;
    boost::add_edge(e1, e2, G);
  }

  important_bridges(G);
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) testcase();
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "MathTools.h"

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

#include "Point3d.h"

namespace MathLib
{

double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
                                 Point3d const& pb, double& lambda, double& d0)
{
    auto const a = Eigen::Map<Eigen::Vector3d const>(pa.getCoords());
    auto const b = Eigen::Map<Eigen::Vector3d const>(pb.getCoords());
    auto const p = Eigen::Map<Eigen::Vector3d const>(pp.getCoords());

    // g(lambda) = a + lambda v, v = b-a
    Eigen::Vector3d const v = b - a;

    // orthogonal projection: (p - g(lambda))^T * v = 0
    // <=> (a-p - lambda (b-a))^T * (b-a) = 0
    // <=> (a-p)^T * (b-a) = lambda (b-a)^T ) (b-a)
    lambda = (((p - a).transpose() * v) / v.squaredNorm())(0, 0);

    // compute projected point
    Eigen::Vector3d const proj_pnt = a + lambda * v;

    d0 = (proj_pnt - a).norm();

    return (p - proj_pnt).norm();
}

double getAngle(Point3d const& p0, Point3d const& p1, Point3d const& p2)
{
    auto const a = Eigen::Map<Eigen::Vector3d const>(p0.getCoords());
    auto const b = Eigen::Map<Eigen::Vector3d const>(p1.getCoords());
    auto const c = Eigen::Map<Eigen::Vector3d const>(p2.getCoords());
    Eigen::Vector3d const v0 = a - b;
    Eigen::Vector3d const v1 = c - b;

    // apply Cauchy Schwarz inequality
    return std::acos(
        (v0.transpose() * v1 / (v0.norm() * v1.norm()))(0, 0));
}

double scalarTriple(Eigen::Vector3d const& u, Eigen::Vector3d const& v,
                    Eigen::Vector3d const& w)
{
    return u.cross(v).dot(w);
}

}  // namespace MathLib


#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

struct SimpleHarmonic {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = - x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 0.;
		x(1) = 1.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 100.;
	}
};

struct Lorenz {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(3);

		y(0) = 10. * ( x(1) - x(0) );
		y(1) = 28. * x(0) - x(1) - x(0) * x(2);
		y(2) = (-8./3.) * x(2) + x(0) * x(1);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(3);
		x(0) = 15.;
		x(1) = 15.;
		x(2) = 36.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct VdP {
	double mu;

	VdP(double mu = 0.25) : mu(mu) {}

	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = mu * (1. - x(0)*x(0))*x(1) - x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 1.;
		x(1) = 1.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct Nobi {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) - x(0)*x(0)*x(0);

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(2);
		x(0) = 0.;
		x(1) = 4.;
		// x(0) = kv::interval<T>(-0.05, 0.05);
		// x(1) = kv::interval<T>(3.95, 4.05);
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = "3.3";
	}
};

struct QuadTest1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(1);

		y(0) = t * t;

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(1);
		x(0) = 0.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};

struct QuadTest2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x, T t){
		ub::vector<T> y(1);

		y(0) = 0.;

		return y;
	}

	template <class T>
	void initial_value(ub::vector< kv::interval<T> >&  x) {
		x.resize(1);
		x(0) = 0.;
	}

	template <class T>
	void start_time(kv::interval<T>& x) {
		x = 0.;
	}

	template <class T>
	void stop_time(kv::interval<T>& x) {
		x = 20.;
	}
};


//  Copyright (c) 2014 Anton Bikineev
//  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)
//
//  Computes test data for the derivatives of the
//  various bessel functions. Results of derivatives
//  are generated by the relations between the derivatives
//  and Bessel functions, which actual implementation
//  doesn't use. Results are printed to ~ 50 digits.
//
#include <fstream>

#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>

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

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;
using namespace boost::multiprecision;

template <class T>
T bessel_j_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_j(v, x) - boost::math::cyl_bessel_j(v+1, x);
}

template <class T>
T bessel_y_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_neumann(v, x) - boost::math::cyl_neumann(v+1, x);
}

template <class T>
T bessel_i_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_i(v, x) + boost::math::cyl_bessel_i(v+1, x);
}

template <class T>
T bessel_k_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_k(v, x) - boost::math::cyl_bessel_k(v+1, x);
}

template <class T>
T sph_bessel_j_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_bessel(1, x);
   return boost::math::sph_bessel(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_bessel(itrunc(v), x);
}

template <class T>
T sph_bessel_y_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_neumann(1, x);
   return boost::math::sph_neumann(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_neumann(itrunc(v), x);
}

enum
{
   func_J = 0,
   func_Y,
   func_I,
   func_K,
   func_j,
   func_y
};

int cpp_main(int argc, char*argv [])
{
   typedef number<mpfr_float_backend<200> > bignum;

   parameter_info<bignum> arg1, arg2;
   test_data<bignum> data;

   int functype = 0;
   std::string letter = "J";

   if(argc == 2)
   {
      if(std::strcmp(argv[1], "--Y") == 0)
      {
         functype = func_Y;
         letter = "Y";
      }
      else if(std::strcmp(argv[1], "--I") == 0)
      {
         functype = func_I;
         letter = "I";
      }
      else if(std::strcmp(argv[1], "--K") == 0)
      {
         functype = func_K;
         letter = "K";
      }
      else if(std::strcmp(argv[1], "--j") == 0)
      {
         functype = func_j;
         letter = "j";
      }
      else if(std::strcmp(argv[1], "--y") == 0)
      {
         functype = func_y;
         letter = "y";
      }
      else
         assert(0);
   }

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the Bessel " << letter << " function derivative\n\n";
   do{
      if(0 == get_user_parameter_info(arg1, "a"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "b"))
         return 1;

      bignum (*fp)(bignum, bignum) = 0;
      if(functype == func_J)
         fp = bessel_j_derivative_bare;
      else if(functype == func_I)
         fp = bessel_i_derivative_bare;
      else if(functype == func_K)
         fp = bessel_k_derivative_bare;
      else if(functype == func_Y)
         fp = bessel_y_derivative_bare;
      else if(functype == func_j)
         fp = sph_bessel_j_derivative_bare;
      else if(functype == func_y)
         fp = sph_bessel_y_derivative_bare;
      else
         assert(0);

      data.insert(fp, arg2, arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=bessel_j_derivative_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "bessel_j_derivative_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific << std::setprecision(50);
   write_code(ofs, data, line.c_str());

   return 0;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <cmath>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    double T,
    double tilde_q,
    unsigned int seed
) noexcept :
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    T_(T),
    tilde_q_(tilde_q),
    gauss_rnd_sample_([&] { return (distribution_)(generator_); })
{
    F_ << 1.0,  T_, 0.0, 0.0,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0,  T_,
          0.0, 0.0, 0.0, 1.0;

    double q11 = 1.0/3.0 * std::pow(T_, 3.0);
    double q2  = 1.0/2.0 * std::pow(T_, 2.0);
    Q_ << q11,  q2, 0.0, 0.0,
           q2,  T_, 0.0, 0.0,
          0.0, 0.0, q11,  q2,
          0.0, 0.0,  q2,  T_;
    Q_ *= tilde_q;

    LDLT<Matrix4d> chol_ldlt(Q_);
    sqrt_Q_ = (chol_ldlt.transpositionsP() * Matrix4d::Identity()).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
}

WhiteNoiseAcceleration::WhiteNoiseAcceleration(double T, double tilde_q) noexcept :
    WhiteNoiseAcceleration(T, tilde_q, 1)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration() noexcept :
    WhiteNoiseAcceleration(1.0, 1.0, 1)
{ }

WhiteNoiseAcceleration::~WhiteNoiseAcceleration() noexcept
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(const WhiteNoiseAcceleration& wna) :
    generator_(wna.generator_),
    distribution_(wna.distribution_),
    T_(wna.T_),
    F_(wna.F_),
    Q_(wna.Q_),
    tilde_q_(wna.tilde_q_),
    sqrt_Q_(wna.sqrt_Q_),
    gauss_rnd_sample_(wna.gauss_rnd_sample_)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(WhiteNoiseAcceleration&& wna) noexcept :
    generator_(std::move(wna.generator_)),
    distribution_(std::move(wna.distribution_)),
    T_(wna.T_),
    F_(std::move(wna.F_)),
    Q_(std::move(wna.Q_)),
    tilde_q_(wna.tilde_q_),
    sqrt_Q_(std::move(wna.sqrt_Q_)),
    gauss_rnd_sample_(std::move(wna.gauss_rnd_sample_))
{
    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(const WhiteNoiseAcceleration& wna)
{
    WhiteNoiseAcceleration tmp(wna);
    *this = std::move(tmp);

    return *this;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(WhiteNoiseAcceleration&& wna) noexcept
{
    T_       = wna.T_;
    F_       = std::move(wna.F_);
    Q_       = std::move(wna.Q_);
    tilde_q_ = wna.tilde_q_;

    sqrt_Q_           = std::move(wna.sqrt_Q_);
    generator_        = std::move(wna.generator_);
    distribution_     = std::move(wna.distribution_);
    gauss_rnd_sample_ = std::move(wna.gauss_rnd_sample_);

    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;

    return *this;
}

MatrixXd WhiteNoiseAcceleration::getNoiseSample(const std::size_t num)
{
    MatrixXd rand_vectors(4, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = gauss_rnd_sample_();

    return sqrt_Q_ * rand_vectors;
}

MatrixXd WhiteNoiseAcceleration::getNoiseCovarianceMatrix()
{
    return Q_;
}

MatrixXd WhiteNoiseAcceleration::getStateTransitionMatrix()
{
    return F_;
}

VectorXd WhiteNoiseAcceleration::getTransitionProbability(const Ref<const MatrixXd>& prev_states, Ref<MatrixXd> cur_states)
{
    VectorXd probabilities(prev_states.cols());
    MatrixXd differences = cur_states - prev_states;

    std::size_t size = differences.rows();
    for (std::size_t i = 0; i < prev_states.cols(); i++)
    {
        probabilities(i) = (-0.5 * static_cast<double>(size) * log(2.0 * M_PI) + -0.5 * log(Q_.determinant()) -0.5 * (differences.col(i).transpose() * Q_.inverse() * differences.col(i)).array()).exp().coeff(0);
    }

    return probabilities;
}

std::pair<std::size_t, std::size_t> WhiteNoiseAcceleration::getOutputSize() const
{
    return std::make_pair(4, 0);
}


#include <iostream>

#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

void usage(char *progname) {
	cout << "This program returns the factors p and q of "
		"n if p - q < n^(1/4)."
		<< endl;
	cout << "Usage: " << progname << " n" << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 2) { usage(argv[0]); return 3; }

	ZZ n, a, b, p, q;
	n = conv<ZZ>(argv[1]);

	// p = floor(SqrRoot(n))
	SqrRoot(a, n);
	// q = p*p - n
	sqr(b, a);
	b -= n;

	while(b < 0 || sqr(SqrRoot(b)) != b) {
		a++;
		sqr(b, a);
		b -= n;
	}

	p = a - SqrRoot(b);
	q = n / p;

	cout << p << endl << q << endl;
}


#pragma once

#include "../../Primitives/SignalTraits.hpp"
#include "../../Utility/Numbers.hpp"

#include <Eigen/Dense>
#include <Eigen/QR>

namespace dspbb::fir {

namespace impl {

	template <class T>
	auto CoefficientMatrix(size_t filterLength, size_t gridSize) {
		Eigen::Matrix<T, Eigen::Dynamic, 1> col;
		Eigen::Matrix<T, 1, Eigen::Dynamic> row;

		col.resize(gridSize);
		row.resize(filterLength);

		for (size_t i = 0; i < gridSize; ++i) {
			col(i) = T(i);
		}
		col *= T(1) / T(gridSize - 1) * pi_v<T>;

		for (size_t i = 0; i < filterLength; ++i) {
			row(i) = T(i);
		}

		Eigen::MatrixX<T> coefficientMatrix = col * row;

		for (size_t row = 0; row < gridSize; ++row) {
			coefficientMatrix(row, 0) = T(1);
			for (size_t col = 1; col < filterLength; ++col) {
				coefficientMatrix(row, col) = 2 * std::cos(coefficientMatrix(row, col));
			}
		}

		return coefficientMatrix;
	}

	template <class T, class Func>
	auto WeightMatrix(size_t gridSize, const Func& weight) {
		Eigen::DiagonalMatrix<T, Eigen::Dynamic> weightMatrix;
		weightMatrix.resize(gridSize);
		for (size_t i = 0; i < gridSize; ++i) {
			weightMatrix.diagonal()(i) = weight(T(i) / T(gridSize - 1));
		}
		return weightMatrix;
	}

	template <class T, class Func>
	auto ResponseVector(size_t gridSize, const Func& response) {
		Eigen::Matrix<T, Eigen::Dynamic, 1> responseVector;
		responseVector.resize(gridSize);
		for (size_t i = 0; i < gridSize; ++i) {
			responseVector(i) = T(i);
		}
		responseVector *= T(1) / T(gridSize - 1);
		for (size_t i = 0; i < gridSize; ++i) {
			responseVector(i) = response(responseVector(i));
		}
		return responseVector;
	}

} // namespace impl

template <class SignalR, class ResponseFunc, class WeightFunc, std::enable_if_t<is_mutable_signal_v<SignalR>, int> = 0>
void KernelLeastSquares(SignalR&& coefficients, ResponseFunc responseFunc, WeightFunc weightFunc, size_t gridSize = 0) {
	using R = typename std::decay_t<SignalR>::value_type;
	using T = remove_complex_t<R>;

	const size_t filterLength = (coefficients.Size() + 1) / 2;
	gridSize = gridSize == 0 ? 4 * filterLength : std::min(filterLength, gridSize);

	const auto coefficientMatrix = impl::CoefficientMatrix<T>(filterLength, gridSize);
	const auto weightMatrix = impl::WeightMatrix<T>(gridSize, weightFunc);
	const auto responseVector = impl::ResponseVector<T>(gridSize, responseFunc);

	Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixX<T>> decomp{ weightMatrix * coefficientMatrix };
	const Eigen::VectorX<T> halfFilter = decomp.solve(weightMatrix * responseVector);
	for (size_t i = 0; i < filterLength; ++i) {
		coefficients[i] = halfFilter(filterLength - i - 1);
		coefficients[i + filterLength - 1] = halfFilter[i];
	}
}

} // namespace dspbb::fir


#pragma once

/* Classical constant propagation domain */

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/support/stats.hpp>

#include <boost/optional.hpp>

namespace crab {
namespace domains {
/**
 *  Each variable is mapped to an element in this lattice:
 *
 *            top
 *             |
 * ...,-3,-2,-1,0,1,2,3,...
 *             |
 *           bottom
 *
 * top means that it might not be a constant value
 **/
template <typename Number> class constant {
  boost::optional<Number> m_constant;
  bool m_is_bottom;
  using constant_t = constant<Number>;
  constant(bool is_bottom) : m_constant(boost::none), m_is_bottom(is_bottom) {}
public:

  constant(Number c) : m_constant(c), m_is_bottom(false) {}

  static constant_t bottom() { return constant(true); }

  static constant_t top() { return constant(false); }

  static constant_t zero() { return constant(Number(0));}

  bool is_bottom() const { return m_is_bottom; }

  bool is_top() const { return (!is_bottom() && !m_constant); }

  bool is_constant() const { return m_constant != boost::none; }

  Number get_constant() const {
    assert(is_constant());
    return *m_constant;
  }

  /** Begin Lattice Operations **/
  bool operator<=(const constant_t &o) const {
    if (is_bottom() || o.is_top()) {
      return true;
    } else if (o.is_bottom() || is_top()) {
      return false;
    } else {
      assert(is_constant());
      assert(o.is_constant());
      return get_constant() == o.get_constant();
    }
  }

  bool operator==(const constant_t &o) const {
    return (m_is_bottom == o.m_is_bottom && m_constant == o.m_constant);
  }

  constant_t operator|(const constant_t &o) const {
    if (is_bottom() || o.is_top())
      return o;
    else if (is_top() || o.is_bottom())
      return *this;
    else {
      assert(is_constant());
      assert(o.is_constant());
      if (get_constant() == o.get_constant()) {
        return *this;
      } else {
        return constant_t::top();
      }
    }
  }

  constant_t operator||(const constant_t &o) const { return *this | o; }

  template <typename Thresholds>
  constant_t widening_thresholds(const constant_t &o,
                                 const Thresholds &ts /*unused*/) const {
    return *this | o;
  }

  constant_t operator&(const constant_t &o) const {
    if (is_bottom() || o.is_top())
      return *this;
    else if (is_top() || o.is_bottom()) {
      return o;
    } else {
      assert(is_constant());
      assert(o.is_constant());
      if (get_constant() == o.get_constant()) {
        return *this;
      } else {
        return constant_t::bottom();
      }
    }
  }

  constant_t operator&&(const constant_t &o) const { return *this & o; }
  /** End  Lattice Operations **/

  /** Begin arithmetic operations **/
  constant_t Add(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() + o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t Sub(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() - o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t Mul(const constant_t &o) const {
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() * o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t SDiv(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() / o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t SRem(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    if (is_constant() && o.is_constant()) {
      return constant_t(get_constant() % o.get_constant());
    } else {
      return constant_t::top();
    }
  }
  constant_t UDiv(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    return constant_t::top();
  }
  constant_t URem(const constant_t &o) const {
    if (o.is_constant() && o.get_constant() == Number(0)) {
      return constant_t::bottom();
    }
    return constant_t::top();
  }
  /** End arithmetic operations **/

  /** Begin bitwise operations **/
  // These operations depend on the type of Number
  constant_t BitwiseAnd(const constant_t &o) const;
  constant_t BitwiseOr(const constant_t &o) const;
  constant_t BitwiseXor(const constant_t &o) const;
  constant_t BitwiseShl(const constant_t &o) const;
  constant_t BitwiseLShr(const constant_t &o) const;
  constant_t BitwiseAShr(const constant_t &o) const;
  /** End bitwise operations **/

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
    } else if (is_top()) {
      o << "top";
    } else {
      assert(is_constant());
      o << get_constant();
    }
  }

  friend inline crab_os &operator<<(crab_os &o, const constant_t &c) {
    c.write(o);
    return o;
  }

};

namespace constant_details {
using z_constant_t = constant<ikos::z_number>;
using q_constant_t = constant<ikos::q_number>;
} // end namespace constant_details

template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseAnd(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() & o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseOr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() | o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseXor(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    return constant_details::z_constant_t(get_constant() ^ o.get_constant());
  } else {
    return constant_details::z_constant_t::top();
  }
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseShl(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    if (o.get_constant() >= ikos::z_number(0)) {
      return constant_details::z_constant_t(get_constant() << o.get_constant());
    }
  }
  return constant_details::z_constant_t::top();
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseLShr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    // if get_contant() is non-negative then LShr = AShr.
    if (get_constant() >= ikos::z_number(0)) {
      if (o.get_constant() >= ikos::z_number(0)) {
	return constant_details::z_constant_t(get_constant() >> o.get_constant());
      }
    }
  }
  return constant_details::z_constant_t::top();
}
template<> constant_details::z_constant_t
inline constant_details::z_constant_t::BitwiseAShr(const constant_details::z_constant_t &o) const {
  if (is_constant() && o.is_constant()) {
    if (o.get_constant() >= ikos::z_number(0)) {
      return constant_details::z_constant_t(get_constant() >> o.get_constant());
    }
  }
  return constant_details::z_constant_t::top();
}

template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseAnd(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseOr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseXor(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseShl(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseLShr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}
template<> constant_details::q_constant_t
inline constant_details::q_constant_t::BitwiseAShr(const constant_details::q_constant_t &o) const {
  return constant_details::q_constant_t::top();
}

template <typename Number, typename VariableName>
class constant_domain final : public crab::domains::abstract_domain_api<
                                  constant_domain<Number, VariableName>> {
public:
  using constant_domain_t = constant_domain<Number, VariableName>;
  using abstract_domain_t =
      crab::domains::abstract_domain_api<constant_domain_t>;
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::interval_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::reference_constraint_t;
  using typename abstract_domain_t::variable_or_constant_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  using typename abstract_domain_t::variable_or_constant_vector_t;
  using constant_t = constant<Number>;
  using number_t = Number;
  using varname_t = VariableName;

private:
  using interval_domain_t = ikos::interval_domain<number_t, varname_t>;
  using separate_domain_t = ikos::separate_domain<variable_t, constant_t>;

private:
  separate_domain_t m_env;

  constant_domain(separate_domain_t &&env) : m_env(std::move(env)) {}

  constant_t eval(const linear_expression_t &expr) const {
    assert(!is_bottom());
    constant_t r(expr.constant());
    for (auto const&kv : expr) {
      constant_t c(kv.first);
      r  = r.Add(c.Mul(m_env[kv.second]));
      if (r.is_top()) {
	break;
      }
    }
    return r;
  }

  constant_t compute_residual(const linear_constraint_t &cst, const variable_t &pivot) const {
    constant_t residual(cst.constant());
    for (auto const&kv: cst) {
      constant_t c(kv.first);
      const variable_t &v = kv.second;
      if (!(v == pivot)) {
	residual = residual.Sub(c.Mul(m_env[v]));
	if (residual.is_top()) {
	  break;
	}
      }
    }
    return residual;
  }

  void propagate(const linear_constraint_t &cst){
    if (is_bottom()) {
      return;
    }

    if (cst.is_inequality() || cst.is_strict_inequality() || cst.is_disequation()) {
      constant_t e = eval(cst.expression());
      if (e.is_constant()) {
	if (cst.is_inequality()) {
	  if (!(e.get_constant() <= number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	} else if (cst.is_disequation()) {
	  if (!(e.get_constant() != number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	} else if (cst.is_strict_inequality()) {
	  if (!(e.get_constant() < number_t(0))) {
	    set_to_bottom();
	    return;
	  }
	}
      }
    } else if (cst.is_equality()) {
      for (auto kv : cst) {
	number_t c = kv.first;
	const variable_t &pivot = kv.second;
	constant_t new_c = compute_residual(cst, pivot).SDiv(c);
	if (!new_c.is_top()) {
	  m_env.set(pivot, m_env[pivot] & new_c);
	}
      }
    }
  }

  void solve_constraints(const linear_constraint_system_t &csts) {
    for (auto const &c : csts) {
      if (is_bottom()) {
	return;
      }
      if (c.is_inequality() && c.is_unsigned()) {
	// we don't handle unsigned constraints
	continue;
      }
      if (c.is_tautology()) {
	continue;
      }
      if (c.is_contradiction()) {
	set_to_bottom();
	return;
      }
      propagate(c);
    }
  }

public:
  constant_domain_t make_top() const override {
    return constant_domain_t(separate_domain_t::top());
  }

  constant_domain_t make_bottom() const override {
    return constant_domain_t(separate_domain_t::bottom());
  }

  void set_to_top() override {
    constant_domain abs(separate_domain_t::top());
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    constant_domain abs(separate_domain_t::bottom());
    std::swap(*this, abs);
  }

  constant_domain() : m_env(separate_domain_t::top()) {}

  constant_domain(const constant_domain_t &e) : m_env(e.m_env) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
  }

  constant_domain(constant_domain_t &&e) : m_env(std::move(e.m_env)) {}

  constant_domain_t &operator=(const constant_domain_t &o) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
    if (this != &o) {
      m_env = o.m_env;
    }
    return *this;
  }

  constant_domain_t &operator=(constant_domain_t &&o) {
    if (this != &o) {
      m_env = std::move(o.m_env);
    }
    return *this;
  }

  constant_t get_constant(const variable_t &v) const {
    return m_env[v];
  }

  void set_constant(const variable_t &v, constant_t c) {
    m_env.set(v, c);
  }

  bool is_bottom() const override { return m_env.is_bottom(); }

  bool is_top() const override { return m_env.is_top(); }

  bool operator<=(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");
    return (m_env <= o.m_env);
  }

  void operator|=(const constant_domain_t &o) override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    CRAB_LOG("constant-domain",
             crab::outs() << "Join " << m_env << " and " << o.m_env << "\n";);
    m_env = m_env | o.m_env;
    CRAB_LOG("constant-domain", crab::outs() << "Res=" << m_env << "\n";);
  }

  constant_domain_t operator|(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    return (m_env | o.m_env);
  }

  constant_domain_t operator&(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");
    return (m_env & o.m_env);
  }

  constant_domain_t operator||(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return (m_env || o.m_env);
  }

  constant_domain_t widening_thresholds(
      const constant_domain_t &o,
      const crab::iterators::thresholds<number_t> &ts) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return m_env.widening_thresholds(o.m_env, ts);
  }

  constant_domain_t operator&&(const constant_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.narrowing");
    crab::ScopedCrabStats __st__(domain_name() + ".narrowing");
    return (m_env && o.m_env);
  }

  void operator-=(const variable_t &v) override {
    crab::CrabStats::count(domain_name() + ".count.forget");
    crab::ScopedCrabStats __st__(domain_name() + ".forget");
    m_env -= v;
  }

  interval_t operator[](const variable_t &v) override {
    constant_t c = m_env[v];
    if (c.is_bottom()) {
      return interval_t::bottom();
    } else if (c.is_top()) {
      return interval_t::top();
    } else {
      assert(c.is_constant());
      return interval_t(c.get_constant());
    }
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    crab::CrabStats::count(domain_name() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");
    solve_constraints(csts);
  }

  void assign(const variable_t &x, const linear_expression_t &e) override {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");
    if (boost::optional<variable_t> v = e.get_variable()) {
      m_env.set(x, m_env[(*v)]);
    } else {
      m_env.set(x, eval(e));
    }
  }

  void apply(crab::domains::arith_operation_t op, const variable_t &x,
             const variable_t &y, const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc = m_env[z];
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_ADDITION:
	xc = yc.Add(zc);
	break;
      case crab::domains::OP_SUBTRACTION:
	xc = yc.Sub(zc);
	break;
      case crab::domains::OP_MULTIPLICATION:
	xc = yc.Mul(zc);
	break;
      case crab::domains::OP_SDIV:
	xc = yc.SDiv(zc);
	break;
      case crab::domains::OP_SREM:
	xc = yc.SRem(zc);
	break;
      case crab::domains::OP_UDIV:
	xc = yc.UDiv(zc);
	break;
      case crab::domains::OP_UREM:
	xc = yc.URem(zc);
	break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }
  }

  void apply(crab::domains::arith_operation_t op, const variable_t &x,
             const variable_t &y, number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc(k);
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_ADDITION:
	xc = yc.Add(zc);
	break;
      case crab::domains::OP_SUBTRACTION:
	xc = yc.Sub(zc);
	break;
      case crab::domains::OP_MULTIPLICATION:
	xc = yc.Mul(zc);
	break;
      case crab::domains::OP_SDIV:
	xc = yc.SDiv(zc);
	break;
      case crab::domains::OP_SREM:
	xc = yc.SRem(zc);
	break;
      case crab::domains::OP_UDIV:
	xc = yc.UDiv(zc);
	break;
      case crab::domains::OP_UREM:
	xc = yc.URem(zc);
	break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }

  }

  // intrinsics operations
  void intrinsic(std::string name,
		 const variable_or_constant_vector_t &inputs,
                 const variable_vector_t &outputs) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }

  void backward_intrinsic(std::string name,
			  const variable_or_constant_vector_t &inputs,
                          const variable_vector_t &outputs,
                          const constant_domain_t &invariant) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }

  // backward arithmetic operations
  void backward_assign(const variable_t &x, const linear_expression_t &e,
                       const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_assign");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t z,
                      const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, const variable_t &z,
                      const constant_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  // cast operations
  void apply(crab::domains::int_conv_operation_t /*op*/, const variable_t &dst,
             const variable_t &src) override {
    // ignore the widths
    assign(dst, src);
  }

  // bitwise operations
  void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
             const variable_t &y, const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc = m_env[z];
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_AND:
	xc = yc.BitwiseAnd(zc);
	break;
      case crab::domains::OP_OR:
	xc = yc.BitwiseOr(zc);
	break;
      case crab::domains::OP_XOR:
	xc = yc.BitwiseXor(zc);
	break;
      case crab::domains::OP_SHL:
	xc = yc.BitwiseShl(zc);
	break;
      case crab::domains::OP_LSHR:
	xc = yc.BitwiseLShr(zc);
	break;
      case crab::domains::OP_ASHR: {
	xc = yc.BitwiseAShr(zc);
	break;
      }
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }
  }

  void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
             const variable_t &y, number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      constant_t yc = m_env[y];
      constant_t zc(k);
      constant_t xc = constant_t::top();
      switch (op) {
      case crab::domains::OP_AND:
	xc = yc.BitwiseAnd(zc);
	break;
      case crab::domains::OP_OR:
	xc = yc.BitwiseOr(zc);
	break;
      case crab::domains::OP_XOR:
	xc = yc.BitwiseXor(zc);
	break;
      case crab::domains::OP_SHL:
	xc = yc.BitwiseShl(zc);
	break;
      case crab::domains::OP_LSHR:
	xc = yc.BitwiseLShr(zc);
	break;
      case crab::domains::OP_ASHR: {
	xc = yc.BitwiseAShr(zc);
	break;
      }
      default:
	CRAB_ERROR("Operation ", op, " not supported");
      }
      m_env.set(x, xc);
    }

  }

  virtual void select(const variable_t &lhs, const linear_constraint_t &cond,
                      const linear_expression_t &e1,
                      const linear_expression_t &e2) override {
    crab::CrabStats::count(domain_name() + ".count.select");
    crab::ScopedCrabStats __st__(domain_name() + ".select");

    if (!is_bottom()) {
      constant_domain_t inv1(*this);
      inv1 += cond;
      if (inv1.is_bottom()) {
        assign(lhs, e2);
        return;
      }
      constant_domain_t inv2(*this);
      inv2 += cond.negate();
      if (inv2.is_bottom()) {
        assign(lhs, e1);
        return;
      }
      m_env.set(lhs, eval(e1) | eval(e2));
    }
  }

  /// constant_domain implements only standard abstract operations of
  /// a numerical domain so it is intended to be used as a leaf domain
  /// in the hierarchy of domains.
  BOOL_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)
  ARRAY_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)
  REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(constant_domain_t)

  void forget(const variable_vector_t &variables) override {
    if (is_bottom() || is_top()) {
      return;
    }
    for (auto const &var : variables) {
      this->operator-=(var);
    }
  }

  void project(const variable_vector_t &variables) override {
    crab::CrabStats::count(domain_name() + ".count.project");
    crab::ScopedCrabStats __st__(domain_name() + ".project");

    m_env.project(variables);
  }

  void rename(const variable_vector_t &from,
              const variable_vector_t &to) override {
    crab::CrabStats::count(domain_name() + ".count.rename");
    crab::ScopedCrabStats __st__(domain_name() + ".rename");

    m_env.rename(from, to);
  }

  void expand(const variable_t &x, const variable_t &new_x) override {
    crab::CrabStats::count(domain_name() + ".count.expand");
    crab::ScopedCrabStats __st__(domain_name() + ".expand");

    if (is_bottom() || is_top()) {
      return;
    }

    m_env.set(new_x, m_env[x]);
  }

  void normalize() override {}

  void minimize() override {}

  void write(crab::crab_os &o) const override {
    crab::CrabStats::count(domain_name() + ".count.write");
    crab::ScopedCrabStats __st__(domain_name() + ".write");

    m_env.write(o);
  }

  linear_constraint_system_t to_linear_constraint_system() const override {
    crab::CrabStats::count(domain_name() +
                           ".count.to_linear_constraint_system");
    crab::ScopedCrabStats __st__(domain_name() +
                                 ".to_linear_constraint_system");

    linear_constraint_system_t csts;

    if (this->is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (auto it = m_env.begin(); it != m_env.end(); ++it) {
      const variable_t &v = it->first;
      const constant_t &c = it->second;
      if (c.is_constant()) {
        csts += linear_constraint_t(v == c.get_constant());
      }
    }
    return csts;
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system() const override {
    auto lin_csts = to_linear_constraint_system();
    if (lin_csts.is_false()) {
      return disjunctive_linear_constraint_system_t(true /*is_false*/);
    } else if (lin_csts.is_true()) {
      return disjunctive_linear_constraint_system_t(false /*is_false*/);
    } else {
      return disjunctive_linear_constraint_system_t(lin_csts);
    }
  }

  std::string domain_name() const override { return "ConstantDomain"; }

}; // class constant_domain
} // namespace domains
} // namespace crab

namespace crab {
namespace domains {
template <typename Number, typename VariableName>
struct abstract_domain_traits<constant_domain<Number, VariableName>> {
  using number_t = Number;
  using varname_t = VariableName;
};

} // namespace domains
} // namespace crab


#pragma once

#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @return The distance between p0 and p1.
template <typename DerivedP0, typename DerivedP1>
inline auto point_point_distance(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1)
{
    return (p1 - p0).squaredNorm();
}

/// @brief Compute the gradient of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] grad The computed gradient.
template <typename DerivedP0, typename DerivedP1, typename DerivedGrad>
inline void point_point_distance_gradient(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p0.size() == p1.size());
    grad.resize(p0.size() + p1.size());
    grad.head(p0.size()) = 2.0 * (p0 - p1);
    grad.tail(p1.size()) = -grad.head(p0.size());
}

/// @brief Compute the hessian of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] hess The computed hessian.
template <typename DerivedP0, typename DerivedP1, typename DerivedHess>
inline void point_point_distance_hessian(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = p0.size();
    assert(p1.size() == dim);

    hess.resize(2 * dim, 2 * dim);

    hess.setZero();
    hess.diagonal().setConstant(2.0);
    for (int i = 0; i < dim; i++) {
        hess(i, i + dim) = hess(i + dim, i) = -2;
    }
}

} // namespace ipc


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

#pragma once

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

#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/bessel.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/sphere.hpp>

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

using namespace Eigen;
using std::cout;
using std::endl;
using std::min;

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

};

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

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

/* von-Mises-Fisher distribution in D=3 dimensions
 */
template<typename T>
class vMF : public Distribution<T>
{
public:
  uint32_t  D_;

  vMF(const Matrix<T,Dynamic,1>& mu, T tau, boost::mt19937 *pRndGen);
  vMF(const vMF<T>& vmf);
  ~vMF();

  T logPdf(const Matrix<T,Dynamic,1>& x) const;

  Matrix<T,Dynamic,1> sample();

  void print() const;

  const Matrix<T,Dynamic,1>& mu() const {return mu_;};
  void mu(const Matrix<T,Dynamic,1>& mu) const {mu_ = mu;};

  T tau() const {return tau_;};
  void tau(const T tau) {tau_ = tau;};

  Matrix<T,Dynamic,1> mu_;
  T tau_;
private:

// Gaussian as a proposal distribution
  boost::mt19937 *pRndGen_;
  boost::uniform_01<> unif_;
  boost::normal_distribution<> gauss_;
};

typedef vMF<double> vMFd;
typedef vMF<float> vMFf;

template<typename T>
vMF<T>::vMF(const Matrix<T,Dynamic,1>& mu, T tau, boost::mt19937 *pRndGen)
  : Distribution<T>(pRndGen), D_(mu.rows()), mu_(mu), tau_(tau),
  pRndGen_(pRndGen), gauss_(0,1)
{};

template<typename T>
vMF<T>::vMF(const vMF<T>& vmf)
  : Distribution<T>(vmf.pRndGen_), D_(vmf.D_), mu_(vmf.mu()), tau_(vmf.tau()),
    pRndGen_(vmf.pRndGen_), gauss_(0,1)
{};

template<typename T>
vMF<T>::~vMF()
{};

template<typename T>
T vMF<T>::logPdf(const Matrix<T,Dynamic,1>& x) const
{
  // modified bessel function of the first kind
  // http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
  //
  const T d = static_cast<T>(D_);
  if (tau_ < 1e-9) {
    // TODO insert general formula here (this currently works only
    // for D=3
    assert(D_==3);
    return -LOG_4PI;
  } else {
    if (D_ == 3) {
      return -LOG_2PI + log(tau_) + tau_*(mu_.dot(x)-1.) -
        log(1.-exp(-2.*tau_));
    }else {
      return (d/2. -1.)*log(tau_) - (d/2.)*LOG_2PI
        - logBesselI<T>(d/2. -1.,tau_) + tau_*mu_.dot(x);
    }
  }
};

template<typename T>
Matrix<T,Dynamic,1> vMF<T>::sample()
{
  assert(D_==3);
  if (tau_ < 1e-10) {
//    Eigen::VectorXf x;
//    x << gauss_(*pRndGen_), gauss_(*pRndGen_), gauss_(*pRndGen_);
//    return x.normalized();
    return Eigen::Matrix<T,3,1>(gauss_(*pRndGen_), gauss_(*pRndGen_), gauss_(*pRndGen_)).normalized();
  }
  // https://www.mitsuba-renderer.org/~wenzel/files/vmf.pdf
  // sample around (0,0,1)
  Eigen::Matrix<T,2,1> v(gauss_(*pRndGen_), gauss_(*pRndGen_));
  v.normalize();
  const T u = unif_(*pRndGen_);
  const T w = 1. + log(u+(1.-u)*exp(-2.*tau_))/tau_;
  const T a = sqrtf(1.-w*w);
  Eigen::Matrix<T,3,1> x(a*v(0), a*v(1), w);

  // rotate to mu
  Eigen::Matrix<T,3,1> axis = Eigen::Matrix<T,3,1>(0,0,1).cross(Eigen::Matrix<T,3,1>(mu_(0),mu_(1),mu_(2)));
  T angle = acos(mu_[2]);

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

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

template<typename T>
void vMF<T>::print() const
{
  cout<<"mu = "<<mu_.transpose()<<"\t tau = "<<tau_<<endl;
};


#include <stdlib.h>     // srand, rand
#include <unistd.h>	// sleep
#include <armadillo>
#include <boost/math/special_functions/digamma.hpp>

#include "Sto_IHT.h"
#include "Functions.h"

using namespace arma;
using namespace std;

vec expected_ln_beta_dist(const vec pos_count, const vec neg_count){
// The logarithm of the geometric mean GX of a distribution with random variable X is the arithmetic mean of ln(X)
//https://en.wikipedia.org/wiki/Beta_distribution#Geometric_mean
vec ln_GX(pos_count.n_elem,fill::zeros);
for (unsigned int n = 0; n < ln_GX.n_elem; n++){
	ln_GX(n) = boost::math::digamma(pos_count(n)) - boost::math::digamma(pos_count(n) + neg_count(n));

}
return ln_GX;
}

double expected_ln_beta_dist(const double pos_count, const double neg_count){
// The logarithm of the geometric mean GX of a distribution with random variable X is the arithmetic mean of ln(X)
//https://en.wikipedia.org/wiki/Beta_distribution#Geometric_mean
double ln_GX = boost::math::digamma(pos_count) - boost::math::digamma(pos_count + neg_count);

return ln_GX;
}

ivec generate_support(const uvec &curr_supp,const uvec &prev_supp,const int sig_dim){
	ivec supp_data(sig_dim,fill::ones);
	supp_data *= -1;

	uvec mask_old(sig_dim,fill::zeros);	// mask for the previous support
	mask_old(prev_supp) = ones<uvec>(prev_supp.n_elem);

	uvec mask_new(sig_dim,fill::zeros);	// mask for the current support
	mask_new(curr_supp) = ones<uvec>(curr_supp.n_elem);

	const uvec zero_indices = find(mask_old%(1 - mask_new));
	const uvec one_indices  = find(mask_new%(1 - mask_old)) ;
	supp_data.elem( zero_indices ).zeros();
	supp_data.elem( one_indices ).ones();

	return supp_data;
}

void update_tally_bayesian(vec &pos_count, vec &neg_count, double &reliability_pos,
	double &reliability_neg, vec &expected_u, const ivec &support_data,
	const ivec &prev_support_data, const double P_rand, const unsigned int global_iters,
	const unsigned int local_iters){
	// initialization
	const unsigned int sig_dim = pos_count.n_elem;

	uvec old_zero_indices = find(prev_support_data ==0);
	uvec old_one_indices = find(prev_support_data ==1);
	vec  old_expected_u  = expected_u;

	uvec zero_indices = find(support_data ==0);
	uvec one_indices = find(support_data ==1);
	const uvec obs_indices = find(support_data !=-1);

	// #### Update U  #####
	mat Ln_Q(sig_dim,2,fill::zeros);
	uvec U(1);
	// evaluate update rules for U = 0
	U.zeros();
	//prior term
	Ln_Q(obs_indices,U).fill(expected_ln_beta_dist(reliability_neg,reliability_pos)) ;

	//liklihood term
	Ln_Q(zero_indices,U) += expected_ln_beta_dist(pos_count(zero_indices),neg_count(zero_indices));
	Ln_Q(one_indices,U)  += expected_ln_beta_dist(neg_count(one_indices),pos_count(one_indices));

	// evaluate update rules for U = 1
	U.ones();
	// prior term
	Ln_Q(obs_indices,U).fill( expected_ln_beta_dist(reliability_pos,reliability_neg) );
	// liklihood term
	Ln_Q(zero_indices,U) += expected_ln_beta_dist(neg_count(zero_indices),pos_count(zero_indices));
	Ln_Q(one_indices,U) += expected_ln_beta_dist(pos_count(one_indices),neg_count(one_indices));

	mat Q = exp(Ln_Q);		// posterior mass function (not normalized)
	expected_u(obs_indices) = Q(obs_indices,U)/sum(Q.rows(obs_indices),1);

	// #### Update R  #####
	// using coefficient reliability
	// prior term (XXX: comment tu use the prevoius value as prior)
	reliability_pos = 1;
	reliability_neg = 1;
	// liklihood term (coefficient reliability)
	reliability_pos += sum(expected_u(obs_indices));
	reliability_neg += sum(1 - expected_u(obs_indices));
	// liklihood term (number of iterations)
	reliability_pos += local_iters;
	reliability_neg += global_iters - local_iters;

	// #### Update Phi  #####
	#pragma omp critical
	{
	pos_count(one_indices) += expected_u(one_indices);
	neg_count(zero_indices) += expected_u(zero_indices);
	if (any(prev_support_data!=-1)){
		pos_count(old_one_indices) -= old_expected_u(old_one_indices);
		neg_count(old_zero_indices) -= old_expected_u(old_zero_indices);
	}
	}

	return;
}

vec bayesian_Sto_IHT(const mat &A, const vec &y, const int sparsity, const vec prob_vec,
		const unsigned int max_iter, const double gamma,const double tol,
		unsigned int &num_iters, const simulation_parameters simulation_params){
	uvec slow_cores;
	set_slow_cores(slow_cores, simulation_params);

	const unsigned int sig_dim = A.n_cols;
	// initialization of variables that are SHARED among cores
	uvec updated_indices;
	vec pos_count(sig_dim,fill::ones);	// positive count for tally score
	vec neg_count(sig_dim,fill::ones);	// negative count for tally score
	vec x_hat_total(sig_dim,fill::zeros);	// estimation of the signal
	bool done = false;			// flag to check the convergence criteria
	unsigned int i = 0;			// total number of iterations
	const double P_rand = sparsity/sig_dim;	//  probability of unreliable '1' measurement
	// parallel section of the code starts here
	#pragma omp parallel num_threads(simulation_params.num_cores)
	{

	// initializaiotn of variables that are LOCAL to each core
	uvec updated_indices;
	vec x_hat_local(sig_dim,fill::zeros);  	// this is local to each core
	unsigned int iter_local = 0;		// number of iteration for this core
	ivec support_data(sig_dim,fill::zeros);	// data on estimated support
	ivec prev_support_data(sig_dim);	// previous estimated support
	prev_support_data.fill(-1);
	double reliability_pos = 1;		// positive count for core reliability
	double reliability_neg = 1;		// negative count for core reliability
	vec expected_u(sig_dim);	// expected coeffcient reliability (vote)
	expected_u.fill(1);

	// iterations to find the solutions
	while(!done){
		vec tally= pos_count/ (pos_count + neg_count);
		// master thread uses the tally vector to check the convergence criteria
		if (omp_get_thread_num() == 0){
			const uvec sorted_ind = sort_index(abs(tally),"descend");
			const uvec est_supp = sorted_ind(span(0,sparsity - 1));
			const mat A_supp = A.cols(est_supp);
			x_hat_local.zeros();
			x_hat_local(est_supp) = solve(A_supp,y);
			if (norm (y - A*x_hat_local) < tol || i >= max_iter){
				x_hat_total = x_hat_local;
				done = true;
			}
		}

		//slow cores sleep for  simulation_params.sleep_slow_cores microseconds
		if (any( slow_cores == omp_get_thread_num()) ){
			usleep(simulation_params.sleep_slow_cores);
		}

		i++;
		iter_local++;

		// update the local estimate of the support
		uvec est_supp_local;
        est_supp_local = Sto_IHT_async_iteration(x_hat_local, tally, A, y,
            sparsity, prob_vec, gamma);

		// generate support data
		support_data.fill(-1);support_data.elem(est_supp_local).ones();
		update_tally_bayesian(pos_count, neg_count, reliability_pos, reliability_neg, expected_u, support_data, prev_support_data, P_rand, i , iter_local );

		prev_support_data = support_data;
	}
	}
	// parallel section of the code ends here

	num_iters = i;
	return x_hat_total;
}


/**
 * @file Compute condition numbers of cell mass and stiffness matrices
 * for a bunch of finite element shape function sets.
 *
 * @note This file  is part of https://github.com/guidokanschat/benchmarks.git
 */

#include <iostream>
#include <fstream>

#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_bernstein.h>
#include <deal.II/lac/lapack_full_matrix.h>

using namespace dealii;

enum class MatrixTypes
{
    mass,
    stiffness
};

template <int dim>
void matrix (const FiniteElement<dim>& element,
	     const MatrixTypes type)
{
  Triangulation<dim> tr;
  GridGenerator::hyper_cube (tr, 0., 1.);
  DoFHandler<dim> dh (tr);
  dh.distribute_dofs (element);

  const unsigned int degree = element.tensor_degree();
  QGauss<dim> quadrature(degree+1);
  LAPACKFullMatrix<double> M(element.dofs_per_cell);

  FEValues<dim> fe(element, quadrature,
		   update_JxW_values | update_values | update_gradients);
  fe.reinit(dh.begin_active());

  for (unsigned int k=0;k<quadrature.size();++k)
    for (unsigned int i=0;i<M.m();++i)
      for (unsigned int j=0;j<M.n();++j)
	for (unsigned int d=0;d<element.n_components();++d)
	  {
	    switch (type)
	      {
	    	case MatrixTypes::mass:
		      M(i,j) += fe.JxW(k)
				* fe.shape_value_component(j,k,d)
				* fe.shape_value_component(i,k,d);
	      	      break;
	      	case MatrixTypes::stiffness:
	      	      M(i,j) += fe.JxW(k)
	      			* (fe.shape_grad_component(j,k,d)
	      			   * fe.shape_grad_component(i,k,d));
	      	      break;
	      }
	  }

  M.compute_eigenvalues();
  double lmin = 1.e30;
  double lmax = -1.;

  for (unsigned int i=0;i<M.m();++i)
    {
      const double lambda = M.eigenvalue(i).real();
if (std::fabs(lambda) >= 1.e-9)
	{
	  if (lambda < lmin)
	    lmin = lambda;
	  if (lambda > lmax)
	    lmax = lambda;
	}
    }
  deallog << "\t[" << lmin << "\t, " << lmax
	  << "]\tcond " << lmax/lmin;
  deallog << std::endl;
}

template<int dim>
void doit ()
{
  typedef std::shared_ptr<const FiniteElement<dim> > FEPtr;
  std::vector<FEPtr> elements;
  elements.push_back(FEPtr(new FE_Q<dim>(1)));
  elements.push_back(FEPtr(new FE_Q<dim>(2)));
  elements.push_back(FEPtr(new FE_Q<dim>(3)));
  elements.push_back(FEPtr(new FE_Q<dim>(4)));
  elements.push_back(FEPtr(new FE_Q<dim>(5)));
  elements.push_back(FEPtr(new FE_Q<dim>(6)));
  elements.push_back(FEPtr(new FE_Q<dim>(7)));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(1))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(2))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(3))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(4))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(5))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(6))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(7))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(8))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(2))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(3))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(4))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(5))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(6))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(7))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(8))));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(1)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(2)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(3)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(4)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(5)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(6)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(7)));

for (const FEPtr& fe : elements)
  {
deallog << fe->get_name() << "::mass ";
matrix(*fe, MatrixTypes::mass);
deallog << fe->get_name() << "::stiff";
matrix(*fe, MatrixTypes::stiffness);
}

}

int main ()
{
  doit<2> ();
deallog << std::endl;
  doit<3> ();
}


#include "em_gmm.h"

#include <cassert>
#include <iostream>
#include <vector>
#include <random>

//< Uncomment the line below to utilize Math Kernel Library
//#define EIGEN_USE_MKL_ALL
#include <Eigen/Eigen>

namespace {
    //< Constant for numerical stability
    const float eps_covariance = 1e-10;
    const float eps_zero = 1e-10;
    const float eps_log_negative_inf = -1e30;
    const float eps_convergence = 1e-4;
    const float eps_regularize = 1e-30;
}

using namespace std;
using namespace Eigen;

typedef Matrix<float, Dynamic, Dynamic, RowMajor> RowMatrixXf;

inline float log_sum(const float& log_a, const float& log_b) {
    return log_a < log_b ?
         (log_b + std::log (1.0 + std::exp (log_a - log_b)))
         : (log_a + std::log (1.0 + std::exp (log_b - log_a)));
}

void calculate_log_prob_spherical(
        const RowMatrixXf& mat_data,
        const VectorXf& vec_nrm2_pts,
        const RowVectorXf& vec_weights,
        const RowMatrixXf& mat_means,
        const RowMatrixXf& mat_diag_covs,
        RowMatrixXf& mat_log_probs) {

    const long num_modes = mat_means.rows();
    const long num_pts = mat_data.rows();
    const long dim = mat_data.cols();

    assert( vec_nrm2_pts.rows() == num_pts &&
            vec_weights.cols() == num_modes &&
            mat_means.cols() == dim &&
            mat_diag_covs.rows() == num_modes &&
            mat_diag_covs.cols() == dim &&
            mat_log_probs.rows() == num_pts &&
            mat_log_probs.cols() == num_modes );

    RowVectorXf vec_nrm2_centers(num_modes);
    for (long c = 0; c < num_modes; c++) {
        vec_nrm2_centers(c) = mat_means.row(c).squaredNorm();
    }

    mat_log_probs.noalias() = mat_data * mat_means.transpose();
    mat_log_probs *= -2;
    #pragma omp parallel for
    for (long c = 0; c < num_modes; c++) {
        mat_log_probs.col(c) += vec_nrm2_pts;
    }
    #pragma omp parallel for
    for (long n = 0; n < num_pts; n++) {
        mat_log_probs.row(n) += vec_nrm2_centers;
    }

    #pragma omp parallel for
    for (long c = 0; c < num_modes; c++) {
        const float cov = mat_diag_covs(c,0);
        const float c1 = log(vec_weights(c) + eps_regularize)
            - 0.5*dim*log(2*M_PI) - 0.5*dim*log(cov);
        const float c2 = -0.5f/cov;
        mat_log_probs.col(c) *= c2;
        mat_log_probs.col(c) = (mat_log_probs.col(c).array() + c1).matrix();
    }
}

void calculate_log_prob_diagonal(
        const RowMatrixXf& mat_data,
        const RowVectorXf& vec_weights,
        const RowMatrixXf& mat_means,
        const RowMatrixXf& mat_diag_covs,
        RowMatrixXf& mat_log_probs) {

    const long num_modes = mat_means.rows();
    const long num_pts = mat_data.rows();
    const long dim = mat_data.cols();

    assert( vec_weights.cols() == num_modes &&
            mat_means.cols() == dim &&
            mat_diag_covs.rows() == num_modes &&
            mat_diag_covs.cols() == dim &&
            mat_log_probs.rows() == num_pts &&
            mat_log_probs.cols() == num_modes);

    const float c0(-0.5f*dim*log(2*M_PI));
    RowVectorXf vec_c1_cov_prod(num_modes);
    for (long c = 0; c < num_modes; c++) {
        vec_c1_cov_prod(c) = -0.5f*(mat_diag_covs.row(c).array().log().sum());
    }

    #pragma omp parallel for
    for (long n = 0; n < num_pts; n++) {
        RowVectorXf vec_data = mat_data.row(n);
        for (long c = 0; c < num_modes; c++) {
            RowVectorXf delta = (vec_data - mat_means.row(c));
            mat_log_probs(n,c) = -0.5f*(delta.array()*mat_diag_covs.row(c).array().cwiseInverse()).matrix().dot(delta)
                + c0 + vec_c1_cov_prod(c);
        }
    }
}

void em_gmm(
        const float *data,
        const long num_pts,
        const long dim,
        const int num_modes,
        float *means,
        float *diag_covs,
        float *weights,
        bool should_fit_spherical_gaussian) {

    using namespace std;

    assert (num_modes < num_pts && "Not enough data for em");

    RowVectorXf vec_eps_regularize(num_modes);
    vec_eps_regularize.fill(eps_regularize);

    //< K-means to initialize the EM
    std::vector<int> labels(num_pts, -1);

    Map<const RowMatrixXf> mat_data(data, num_pts, dim);
    Map<RowMatrixXf> mat_means(means, num_modes, dim);
    Map<RowMatrixXf> mat_diag_covs(diag_covs, num_modes, dim);
    Map<RowVectorXf> vec_weights(weights, num_modes);

    //< Random init
    random_device rd;
    default_random_engine gen(rd());
    uniform_int_distribution<long> kmeans_seed_dist(0, num_pts-1);
    vector<long> center_indices(num_modes);
    generate(center_indices.begin(), center_indices.end(), [&]{
        return kmeans_seed_dist(gen);
    });
    #pragma omp parallel for
    for (int c = 0; c < num_modes; c++) {
        long n = center_indices[c];
        mat_means.row(c) = mat_data.row(n);
    }

    //< kmeans convergence
    const int max_kmeans_iterations = 20;
    bool is_converged = false;
    float eps(0.0f);

    RowMatrixXf mat_distance(num_pts, num_modes);

    VectorXf vec_nrm2_pts(num_pts);
    #pragma omp parallel for
    for (long r = 0; r < num_pts; r++) {
        vec_nrm2_pts(r) = mat_data.row(r).squaredNorm();
    }

    RowVectorXf vec_nrm2_centers(num_modes);

    //< K-means
    RowMatrixXf mat_saved_means(num_modes, dim);
    RowVectorXf assigned_counts(num_modes);

    int iterations = 0;
    while ((iterations++ < max_kmeans_iterations) && !is_converged) {

        //< save previous centers
        mat_saved_means = mat_means;

        //< calculate point to center L2 distance
        //< (X - C)^2 = X^2 + C^2 - 2*X*C
        #pragma omp parallel for
        for (int c = 0; c < num_modes; c++) {
            vec_nrm2_centers(c) = mat_means.row(c).squaredNorm();
        }
        mat_distance.noalias() = mat_data * mat_means.transpose();
        mat_distance *= -2;
        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            mat_distance.col(c) += vec_nrm2_pts;
        }

        //< nearest centers along one column
        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            mat_distance.row(n) += vec_nrm2_centers;
            mat_distance.row(n).minCoeff(&labels[n]);
        }

        assigned_counts.fill(0.0f);
        for (long n = 0; n < num_pts; n++) {
            long c = labels[n];
            if (assigned_counts(c) < 1e-3) {
                mat_means.row(c) = mat_data.row(n);
            } else {
                mat_means.row(c) += mat_data.row(n);
            }
            assigned_counts(c) += 1.0f;
        }

        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            if (assigned_counts(c) > 1e-3) {
                mat_means.row(c) /= assigned_counts(c);
            }
        }

        //< evaluation
        const float prev_eps = eps;
        eps = (mat_saved_means - mat_means).norm();
        is_converged = (eps < eps_convergence);
        cout << "kmeans " << "[" << iterations << "] " << prev_eps << " " << eps << endl;
    }

    //< covariances and weights
    vec_weights = assigned_counts / (float)num_pts;
    mat_diag_covs.fill(0);
    for (long n = 0; n < num_pts; n++) {
        long c = labels[n];
        mat_diag_covs.row(c) += mat_data.row(n).array().square().matrix();
    }
    for (long c = 0; c < num_modes; c++) {
        if (assigned_counts(c) > 1e-3) {
            mat_diag_covs.row(c) /= assigned_counts(c);
            mat_diag_covs.row(c) -= mat_means.row(c).array().square().matrix();
        }
    }

    if (should_fit_spherical_gaussian) {
        for (int c = 0; c < num_modes; c++) {
            const float spherical_val = std::max(mat_diag_covs.row(c).sum()/dim, eps_covariance);
            mat_diag_covs.row(c).fill(spherical_val);
        }
    }

    //< EM
    RowMatrixXf& mat_log_probs = mat_distance;

    const int max_em_iterations = 20;
    is_converged = false; //< use weights as approximated indicator
    float expectation(std::numeric_limits<float>::lowest());

    RowVectorXf vec_evals(num_pts);
    RowVectorXf vec_log_sum_probs(num_pts);
    RowVectorXf vec_occup_eN(num_modes);
    RowMatrixXf mat_occup_eX(num_modes, dim);
    RowMatrixXf mat_occup_eX2(num_modes, dim);

    iterations = 0;
    while ((iterations++ < max_em_iterations) && !is_converged) {

        //< calculate log probabilities
        if (should_fit_spherical_gaussian) {
            calculate_log_prob_spherical(mat_data, vec_nrm2_pts, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
        } else {
            calculate_log_prob_diagonal(mat_data, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
        }

        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            vec_log_sum_probs(n) = mat_log_probs(n,0);
            for (long c = 1; c < num_modes; c++) {
                if (mat_log_probs(n,c) > eps_log_negative_inf) {
                    vec_log_sum_probs(n) = log_sum(vec_log_sum_probs(n), mat_log_probs(n,c));
                }
            }
        }

        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            RowVectorXf soft_count = (mat_log_probs.row(n).array() - vec_log_sum_probs(n)).exp().matrix();
            vec_evals(n) = 0;
            for (long c = 0; c < num_modes; c++) {
                if (soft_count(c) > eps_zero) {
                    vec_evals(n) += mat_log_probs(n,c)*soft_count(c);
                }
            }
            mat_log_probs.row(n) = soft_count;
        }

        ////< Occupation counts
        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            vec_occup_eN(c) = mat_log_probs.col(c).sum();
        }

        mat_occup_eX = mat_log_probs.transpose() * mat_data;
        //< M-Step: update means/diag_covs/weights
        vec_weights = vec_occup_eN / vec_occup_eN.sum();
        vec_occup_eN += vec_eps_regularize;
        #pragma omp parallel for
        for (int c = 0; c < num_modes; c++) {
            if (vec_weights(c) > eps_zero) {
                mat_means.row(c) = mat_occup_eX.row(c)/vec_occup_eN(c);
            }
        }

        VectorXf vec_sum_occup_nrm2_pts = mat_log_probs.transpose() * vec_nrm2_pts;
        if (should_fit_spherical_gaussian) {
            for (int c = 0; c < num_modes; c++) {
                const float spherical_val
                    = (vec_sum_occup_nrm2_pts(c) - 2*mat_means.row(c).dot(mat_occup_eX.row(c)) + vec_nrm2_centers(c)*vec_occup_eN(c))
                    / (dim*vec_occup_eN(c));
                mat_diag_covs.row(c).fill(spherical_val);
            }
        } else {
            mat_occup_eX2 = mat_log_probs.transpose() * mat_data.array().square().matrix();
            for (long c = 0; c < num_modes; c++) {
                mat_diag_covs.row(c) = ((mat_occup_eX2.row(c)/vec_occup_eN(c)).array() - mat_means.row(c).array().square()).max(eps_covariance).matrix();
            }
        }

        const float prev_expectation = expectation;
        expectation = vec_evals.sum();
        const float scale = 1e5;
        const float delta = exp((expectation - prev_expectation)/scale) - 1;
        is_converged = (iterations > 0 && delta < eps_convergence);
        cout << "em " << "[" << iterations << "] " << delta << " " << expectation << endl;
    } //< em LOOP END

} //< function: fit_mixture_model

void likelihood_gmm(
        const float *data,
        const long num_pts,
        const long dim,
        const int num_modes,
        const float *means,
        const float *diag_covs,
        const float *weights,
        float *log_probs, //< num_pts x num_modes
        bool is_spherical_gaussian) {

    Map<const RowMatrixXf> mat_data(data, num_pts, dim);
    Map<const RowMatrixXf> mat_means(means, num_modes, dim);
    Map<const RowMatrixXf> mat_diag_covs(diag_covs, num_modes, dim);
    Map<const RowVectorXf> vec_weights(weights, num_modes);

    VectorXf vec_nrm2_pts(num_pts);
    #pragma omp parallel for
    for (long r = 0; r < num_pts; r++) {
        vec_nrm2_pts(r) = mat_data.row(r).squaredNorm();
    }

    RowVectorXf vec_nrm2_centers(num_modes);

    RowMatrixXf mat_log_probs(num_pts, num_modes);

    //< calculate log probabilities
    if (is_spherical_gaussian) {
        calculate_log_prob_spherical(mat_data, vec_nrm2_pts, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
    } else {
        calculate_log_prob_diagonal(mat_data, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
    }

    std::copy(mat_log_probs.data(), mat_log_probs.data() + num_pts*num_modes, log_probs);
}


#ifndef HAMILTONIANS_XXX_HPP
#define HAMILTONIANS_XXX_HPP
#include <Eigen/Eigen>

class XXX
{
private:
	int n_;

public:

	XXX(int n)
		: n_(n)
	{
	}

	template<class State>
	typename State::ValueType operator()(const State& smp) const
	{
		typename State::ValueType s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += yysign; //zz
			s += (1.0-yysign)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t,double> res;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			res[col ^ x] += 1-sgn;
			res[col] += sgn;
		}
		return res;
	}
};
#endif//HAMILTONIANS_XXX_HPP


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

using namespace Eigen;
using namespace std;

int main()
{
  ArrayXXf  m(2,2);

  //assign some values coefficient by coefficient
  m(0,0) = 1.0; m(0,1) = 2.0;
  m(1,0) = 3.0; m(1,1) = 4.0;

  //print values to standard output
  cout << m << endl << endl;

  // using the comma-initializer is also allowed
  m << 1.0,2.0,
       3.0,4.0;

  //print values to standard output
  cout << m << endl;
}