// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <sstream>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/iterators/ever_circling_iterator.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

template <typename G>
void test_geometry(std::string const& wkt)
{
    G geo;
    bg::read_wkt(wkt, geo);
    typedef typename boost::range_iterator<G const>::type iterator_type;

    // Run 3 times through the geometry
    int n = boost::size(geo) * 3;

    {
        std::ostringstream out;
        bg::ever_circling_iterator<iterator_type> it(boost::begin(geo), boost::end(geo));
        for (int i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "123451234512345");
    }

    {
        std::ostringstream out;
        // Start somewhere
        bg::ever_circling_iterator<iterator_type> it(
            boost::begin(geo), boost::end(geo), boost::begin(geo) + 1);
        for (int i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "234512345123451");
    }

    {
        std::ostringstream out;

        // Navigate to somewhere
        bg::ever_circling_iterator<iterator_type> it(boost::begin(geo), boost::end(geo));
        for (int i = 0; i < n; ++i, ++it)
        {
            const int m = boost::size(geo);
            it.moveto(boost::begin(geo) + m - (i % m) - 1);
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "543215432154321");
    }

    // Check the range_iterator-one
    {
        std::ostringstream out;
        bg::ever_circling_range_iterator<G> it(geo);
        for (int i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "123451234512345");
    }
}

template <typename P>
void test_all()
{
    test_geometry<bg::model::linestring<P> >("linestring(1 1,2 2,3 3,4 4,5 5)");
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


/**
 * @file imufilter.hpp
 * @brief Filter for obtaining roll/pitch/yaw data from the IMU on Skybotix VI-Sensor
 * @author Fernando Caballero, fcaballero@us.es
 * @author Francisco J Perez-Grau, fjperez@catec.aero
 * @date October 2016
 *
Copyright (c) 2016, fcaballero, fjperezgrau
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.

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

#ifndef __IMUFILTER_H__
#define __IMUFILTER_H__

#include <Eigen/Core>
#include <Eigen/Dense>
#include <sensor_msgs/Imu.h>
#include <vector>

// Convenient constants
#define RAD2DEG(X) ( (X) * 57.2957795131)
#define DEG2RAD(X) ( (X) * 0.01745329251)
#define LIN2GRAV(X) ( (X) * 0.10197162)

/**
 * @class ImuFilter
 * @brief Estimates IMU angles from acceleration and gyro data
 */
class ImuFilter
{
public:

    /** @brief Constructor
     * @param T Prediction period (seconds)
     * @param imuTopic Name of the IMU data topic
     * @param calibTime Initial calibration time (seconds)
     */
    ImuFilter(double T, std::string imuTopic, double calibTime = 5.0)
	{
        init_ = false;
        T_ = T;
        T2_ = T*T;
        calibTime_ = calibTime;

		// IMU EKF parameters
        accDev_ = 0.006;//0.001
        gyrDev_ = 0.005;//0.01;
        magDev_ = 0;
        magXs_ = 1.0;
        magYs_ = 1.0;
        magZs_ = 1.0;
        magXo_ = 0.0;
        magYo_ = 0.0;
        magZo_ = 0.0;
        biaDev_ = 0.00001;//0.000001;
        biaTh_ = 0.005; //0.001;

        // Setup IMU data subscriber
        sub_ = nh_.subscribe(imuTopic, 10, &ImuFilter::imuDataCallback, this);
	}

    /** @brief Initialize EKF
     * Input: vector of sensor_msgs::Imu
     * The user must continue calling this function with new gyro data until it returns true
     */
	bool initialize(void)
	{
		double gx_m, gy_m, gz_m, gx_m2, gy_m2, gz_m2, gx_d, gy_d, gz_d;

		// Compute mean value and mean square
		gx_m = gy_m = gz_m = gx_m2 = gy_m2 = gz_m2 = 0.0;
        for(int i = 0; i < (int)calibData_.size(); i++)
		{
            gx_m += calibData_[i].angular_velocity.x;
            gy_m += calibData_[i].angular_velocity.y;
            gz_m += calibData_[i].angular_velocity.z;
            gx_m2 += calibData_[i].angular_velocity.x*calibData_[i].angular_velocity.x;
            gy_m2 += calibData_[i].angular_velocity.y*calibData_[i].angular_velocity.y;
            gz_m2 += calibData_[i].angular_velocity.z*calibData_[i].angular_velocity.z;
		}
        gx_m = gx_m/(double)calibData_.size();
        gy_m = gy_m/(double)calibData_.size();
        gz_m = gz_m/(double)calibData_.size();
        gx_m2 = gx_m2/(double)calibData_.size();
        gy_m2 = gy_m2/(double)calibData_.size();
        gz_m2 = gz_m2/(double)calibData_.size();
		//std::cout << "gxM: " << gx_m << ", gyM: " << gy_m << ", gzM: " << gz_m << std::endl;

		// Compute standar deviation of gyros
		gx_d = sqrt(gx_m2-gx_m*gx_m);
		gy_d = sqrt(gy_m2-gy_m*gy_m);
		gz_d = sqrt(gz_m2-gz_m*gz_m);
		//std::cout << "gxDev: " << gx_d << ", gyDev: " << gy_d << ", gzDev: " << gz_d << std::endl;

		// Initalize compass calibration
        magCal_[0] = magXs_; magCal_[1] = magYs_; magCal_[2] = magZs_;
        magCal_[3] = magXo_; magCal_[4] = magYo_; magCal_[5] = magZo_;

		// Initialize sensor variances
        accVar_[0] = accDev_*accDev_; 	accVar_[1] = accDev_*accDev_; 	accVar_[2] = accDev_*accDev_;		// Variance in g
        gyrVar_[0] = gyrDev_*gyrDev_; gyrVar_[1] = gyrDev_*gyrDev_; gyrVar_[2] = gyrDev_*gyrDev_;	// Variance in rad/s
        magVar_[0] = magDev_*magDev_; 	magVar_[1] = magDev_*magDev_; 	magVar_[2] = magDev_*magDev_;	// Variance in mGaus wth data normalized to 1
        biaVar_[0] = biaDev_*biaDev_; biaVar_[1] = biaDev_*biaDev_; biaVar_[2] = biaDev_*biaDev_;	// Variance in rad/s

		// Initialize accelerometer threshold
        accTh_ = sqrt(accVar_[0]+accVar_[1]+accVar_[2]);

		// Initialize state vector x = [rx, ry, rz, gbx, gby, gbz]
        rx_ = ry_ = rz_ = 0.0;
        gbx_ = gx_m;
        gby_ = gy_m;
        gbz_ = gz_m;

		// Initialize covariance matrix
        P_.setIdentity(6, 6);
        P_(0,0) = M_PI_2;
        P_(1,1) = M_PI_2;
        P_(2,2) = M_PI_2;
        P_(3,3) = 0.01*0.01;
        P_(4,4) = 0.01*0.01;
        P_(5,5) = 0.01*0.01;
        /*P_(3,3) = biaVar_[0];
        P_(4,4) = biaVar_[1];
        P_(5,5) = biaVar_[2];*/

        if(gx_d < biaTh_ && gy_d < biaTh_ && gz_d < biaTh_)
		{
            init_ = true;
            ROS_INFO("IMU filter initialized");

			return true;
		}
		else
			return false;
	}

    /** EKF prediction stage based on gyro information
     * @param[in] gx Raw X gyro data (rad/s)
     * @param[in] gy Raw Y gyro data (rad/s)
     * @param[in] gz Raw Z gyro data (rad/s)
     */
	bool predict(double gx, double gy, double gz)
	{
		// Check initialization
        if(!init_)
			return false;

		// Compute matrix F
		Eigen::Matrix<double, 6, 6> F;
        F(0,0) = 1, F(0,1) = 0, F(0,2) = 0, F(0,3) = -T_, F(0,4) = 0,   F(0,5) = 0;
        F(1,0) = 0, F(1,1) = 1, F(1,2) = 0, F(1,3) = 0,  F(1,4) = -T_,  F(1,5) = 0;
        F(2,0) = 0, F(2,1) = 0, F(2,2) = 1, F(2,3) = 0,  F(2,4) = 0,   F(2,5) = -T_;
		F(3,0) = 0, F(3,1) = 0, F(3,2) = 0, F(3,3) = 1,  F(3,4) = 0,   F(3,5) = 0;
		F(4,0) = 0, F(4,1) = 0, F(4,2) = 0, F(4,3) = 0,  F(4,4) = 1,   F(4,5) = 0;
		F(5,0) = 0, F(5,1) = 0, F(5,2) = 0, F(5,3) = 0,  F(5,4) = 0,   F(5,5) = 1;

		// Update covariance matrix
        P_ = F*P_*F.transpose();
        P_(0,0) += gyrVar_[0]*T2_;
        P_(1,1) += gyrVar_[1]*T2_;
        P_(2,2) += gyrVar_[2]*T2_;
        P_(3,3) += biaVar_[0]*T_;
        P_(4,4) += biaVar_[1]*T_;
        P_(5,5) += biaVar_[2]*T_;

		// Update state vector
        rx_ += T_*(gx - gbx_);
        ry_ += T_*(gy - gby_);
        rz_ += T_*(gz - gbz_);

		return true;
	}

    /** EKF update stage based on accelerometer information
     * @param[in] ax Raw X accelerometer data (g)
     * @param[in] ay Raw Y accelerometer data (g)
     * @param[in] az Raw Z accelerometer data (g)
     */
	bool update(double ax, double ay, double az)
	{
		double crx, srx, cry, sry, mod, y[3];

		// Check initialization
        if(!init_)
			return false;

		// Pre-compute constants
        crx = cos(rx_);
        cry = cos(ry_);
        srx = sin(rx_);
        sry = sin(ry_);

		// Create measurement jacobian H
		Eigen::Matrix<double, 3, 6> H;
		H(0,0) = 0;			H(0,1) = cry;		H(0,2) = 0; H(0,3) = 0; H(0,4) = 0; H(0,5) = 0;
		H(1,0) = -crx*cry; 	H(1,1) = srx*sry; 	H(1,2) = 0; H(1,3) = 0; H(1,4) = 0; H(1,5) = 0;
		H(2,0) = cry*srx;	H(2,1) = crx*sry;	H(2,2) = 0; H(2,3) = 0; H(2,4) = 0; H(2,5) = 0;

		// Compute measurement noise jacoban R
		Eigen::Matrix<double, 3, 3> R;
		mod = fabs(sqrt(ax*ax + ay*ay + az*az)-1);
		R.setZero(3, 3);
        R(0,0) = accVar_[0];
        R(1,1) = accVar_[1];
        R(2,2) = accVar_[2];
		/*if(mod > accTh)
		{
			R(0,0) += 1.5*mod*mod;
			R(1,1) += 1.5*mod*mod;
			R(2,2) += 1.5*mod*mod;
		}*/

		// Compute innovation matrix
		Eigen::Matrix<double, 3, 3> S;
        S = H*P_*H.transpose()+R;

		// Compute kalman gain
		Eigen::Matrix<double, 6, 3> K;
        K = P_*H.transpose()*S.inverse();

		// Compute mean error
		y[0] = ax-sry;
		y[1] = ay+cry*srx;
		y[2] = az+crx*cry;

		// Compute new state vector
        rx_ += K(0,0)*y[0]+K(0,1)*y[1]+K(0,2)*y[2];
        ry_ += K(1,0)*y[0]+K(1,1)*y[1]+K(1,2)*y[2];
        rz_ += K(2,0)*y[0]+K(2,1)*y[1]+K(2,2)*y[2];
        gbx_ += K(3,0)*y[0]+K(3,1)*y[1]+K(3,2)*y[2];
        gby_ += K(4,0)*y[0]+K(4,1)*y[1]+K(4,2)*y[2];
        gbz_ += K(5,0)*y[0]+K(5,1)*y[1]+K(5,2)*y[2];

		// Compute new covariance matrix
		Eigen::Matrix<double, 6, 6> I;
		I.setIdentity(6, 6);
        P_ = (I-K*H)*P_;

		return true;
	}

    /** EKF update stage based on accelerometer information
     * @param[in] ax Raw X accelerometer data (g)
     * @param[in] ay Raw Y accelerometer data (g)
     * @param[in] az Raw Z accelerometer data (g)
     * @param[in] mx Raw X magnetometer data ()
     * @param[in] my Raw Y magnetometer data ()
     * @param[in] mz Raw Z magnetometer data ()
     */
	bool update(double ax, double ay, double az, double mx, double my, double mz)
	{
		double crx, srx, cry, sry, crz, srz, mod, y[5], hx, hy;

		// Check initialization
        if(!init_)
			return false;

		// Remove distortion and normalize magnetometer
        mx = (mx - magCal_[3])/magCal_[0];
        my = (my - magCal_[4])/magCal_[1];
        mz = (mz - magCal_[5])/magCal_[2];
		mod = sqrt(mx*mx + my*my + mz*mz);
		mx /= mod;
		my /= mod;
		mz /= mod;

		// Pre-compute constants
        crx = cos(rx_);
        cry = cos(ry_);
        crz = cos(rz_);
        srx = sin(rx_);
        sry = sin(ry_);
        srz = sin(rz_);

		// Create measurement jacobian H
		Eigen::Matrix<double, 5, 6> H;
		H(0,0) = 0;			H(0,1) = cry;		H(0,2) = 0; 	H(0,3) = 0; H(0,4) = 0; H(0,5) = 0;
		H(1,0) = -crx*cry; 	H(1,1) = srx*sry; 	H(1,2) = 0; 	H(1,3) = 0; H(1,4) = 0; H(1,5) = 0;
		H(2,0) = cry*srx;	H(2,1) = crx*sry;	H(2,2) = 0; 	H(2,3) = 0; H(2,4) = 0; H(2,5) = 0;
		H(3,0) = 0;			H(3,1) = 0;			H(3,2) = -srz; 	H(3,3) = 0; H(3,4) = 0; H(3,5) = 0;
		H(4,0) = 0;			H(4,1) = 0;			H(4,2) = -crz; 	H(4,3) = 0; H(4,4) = 0; H(4,5) = 0;

        // Compute measurement noise jacobian R
		Eigen::Matrix<double, 5, 5> R;
		mod = fabs(sqrt(ax*ax + ay*ay + az*az)-1);
		R.setZero(5, 5);
        R(0,0) = accVar_[0];
        R(1,1) = accVar_[1];
        R(2,2) = accVar_[2];
        R(3,3) = magVar_[0];
        R(4,4) = magVar_[1];
        if(mod > accTh_)
		{
			R(0,0) += 1.5*mod*mod;
			R(1,1) += 1.5*mod*mod;
			R(2,2) += 1.5*mod*mod;
		}

		// Compute innovation matrix
		Eigen::Matrix<double, 5, 5> S;
        S = H*P_*H.transpose()+R;

        // Compute Kalman gain
		Eigen::Matrix<double, 6, 5> K;
        K = P_*H.transpose()*S.inverse();

		// Compute mean error
		hx = mx*cry + mz*crx*sry + my*srx*sry;
		hy = my*crx - mz*srx;
		mod = sqrt(hx*hx+hy*hy);
		y[0] = ax-sry;
		y[1] = ay+cry*srx;
		y[2] = az+crx*cry;
		y[3] = hx/mod-crz;
		y[4] = hy/mod+srz;

		// Compute new state vector
        rx_ += K(0,0)*y[0]+K(0,1)*y[1]+K(0,2)*y[2]+K(0,3)*y[3]+K(0,4)*y[4];
        ry_ += K(1,0)*y[0]+K(1,1)*y[1]+K(1,2)*y[2]+K(1,3)*y[3]+K(1,4)*y[4];
        rz_ += K(2,0)*y[0]+K(2,1)*y[1]+K(2,2)*y[2]+K(2,3)*y[3]+K(2,4)*y[4];
        gbx_ += K(3,0)*y[0]+K(3,1)*y[1]+K(3,2)*y[2]+K(3,3)*y[3]+K(3,4)*y[4];
        gby_ += K(4,0)*y[0]+K(4,1)*y[1]+K(4,2)*y[2]+K(4,3)*y[3]+K(4,4)*y[4];
        gbz_ += K(5,0)*y[0]+K(5,1)*y[1]+K(5,2)*y[2]+K(5,3)*y[3]+K(5,4)*y[4];

		// Compute new covariance matrix
		Eigen::Matrix<double, 6, 6> I;
		I.setIdentity(6, 6);
        P_ = (I-K*H)*P_;

		return true;
	}

    /** @brief Get estimated Euler angles in radians interval [-PI,PI] rad
     * @param[out] rx Estimated roll (rad)
     * @param[out] ry Estimated pitch (rad)
     * @param[out] rz Estimated yaw (rad)
     */
    bool getAngles(double &rx, double &ry, double &rz)
	{
        rx = Pi2PiRange(rx_);
        ry = Pi2PiRange(ry_);
        rz = Pi2PiRange(rz_);
		return true;
	}

    /** @brief Get estimated integrated Euler angles since last reset
     * @param[out] rx Estimated integrated roll (rad)
     * @param[out] ry Estimated integrated pitch (rad)
     * @param[out] rz Estimated integrated yaw (rad)
     */
    bool getAngleIntegration(double &irx, double &iry, double &irz)
    {
        irx = irx_;
        iry = iry_;
        irz = irz_;
        return true;
    }

    /** @brief Reset integrated angles
     */
    bool resetAngleIntegration(void)
    {
        irx_ = iry_ = irz_ = 0.0;
        return true;
    }

    /** @brief Get estimated gyro biases in rad/s
     * @param[out] gbx Estimated X gyro bias (rad/s)
     * @param[out] gby Estimated Y gyro bias (rad/s)
     * @param[out] gbz Estimated Z gyro bias (rad/s)
     */
    bool getBIAS(double &gbx, double &gby, double &gbz)
	{
        gbx = gbx_;
        gby = gby_;
        gbz = gbz_;

		return true;
	}

    /** @brief IMU initialization return function
     * @return Returns true if IMU initialized
     */
	bool isInit(void)
	{
        return init_;
	}

    /** IMU sensor data callback
     * @param[in] msg IMU data message
     */
	void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
	{
		// Check for IMU initialization
        if(!init_)
		{
            calibData_.push_back(*msg);
            if(calibData_.size() > calibTime_/T_)
				initialize();

			return;
		}

		// Process sensor data
		predict(msg->angular_velocity.z, msg->angular_velocity.x, msg->angular_velocity.y);
		update(LIN2GRAV(msg->linear_acceleration.z), LIN2GRAV(msg->linear_acceleration.x), LIN2GRAV(msg->linear_acceleration.y));

        // Integrate angle rates
        irx_ += (msg->angular_velocity.z-gbx_)*T_;
        iry_ += (msg->angular_velocity.x-gby_)*T_;
        irz_ += (msg->angular_velocity.y-gbz_)*T_;
    }

protected:

    /** @brief Round down absolute function
     * @param[in] value Input value
     * @return Input value floored
     */
    double floorAbsolute( double value )
	{
	  if (value < 0.0)
		return ceil( value );
	  else
		return floor( value );
	}

    /** @brief Convert angles into interval [-PI,PI] rad
     * @param[in] contAngle Input angle (rad)
     * @return Input angle in interval [-PI,PI] rad
     */
    double Pi2PiRange(double contAngle)
	{
        double boundAngle = 0.0;
        if(fabs(contAngle)<=M_PI)
            boundAngle= contAngle;
		else
		{
            if(contAngle > M_PI)
                boundAngle = (contAngle-2*M_PI) - 2*M_PI*floorAbsolute((contAngle-M_PI)/(2*M_PI));

            if(contAngle < - M_PI)
                boundAngle = (contAngle+2*M_PI) - 2*M_PI*floorAbsolute((contAngle+M_PI)/(2*M_PI));
		}

        return boundAngle;
	}

    double calibTime_;      /**< IMU calibration time*/

    double T_;                                  /**< IMU KF prediction period*/
    double T2_;                                 /**< Squared IMU KF prediction period*/
    double rx_, ry_, rz_, gbx_, gby_, gbz_;     /**< IMU KF state vector x = [rx, ry, rz, gbx, gby, gbz]*/
    Eigen::MatrixXd P_;                         /**< IMU KF matrix*/

    double magCal_[6];      /**< Sensor calib info [gainX, gainY, gainZ, offsetX, offsetY, offsetZ]*/

    double accVar_[3];      /**< Accelerometers variances [varX, varY, varZ]*/
    double magVar_[3];      /**< Magnetometers variances [varX, varY, varZ]*/
    double gyrVar_[3];      /**< Gyroscopes variances [varX, varY, varZ]*/
    double biaVar_[3];      /**< Bias variances [varX, varY, varZ]*/

    double accTh_;          /**< Accelerometer threshold for filter updating*/

    bool init_;             /**< Flag indicating if IMU has been initialized*/

	// EKF Parameters
    double accDev_;
    double gyrDev_;
    double magDev_;
    double biaDev_;
    double biaTh_;
    double magXs_;
    double magYs_;
    double magZs_;
    double magXo_;
    double magYo_;
    double magZo_;

    std::vector<sensor_msgs::Imu> calibData_;   /**< IMU data for initialization*/

    ros::NodeHandle nh_;        /**< ROS node handler*/
    ros::Subscriber sub_;       /**< IMU data subscriber*/

    double irx_, iry_, irz_;    /**< Integrated angles since last reset*/
};

#endif


//####### Test module for Breit Wheeler engine (SI units) ######################

//Define Module name
 #define BOOST_TEST_MODULE "Nonliner Breit Wheeler engine (normalized units)"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 #include <utility>

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

//SI units are used for this test
#define PXRMP_USE_NORMALIZED_UNITS
#include "breit_wheeler_engine.hpp"

//Uses random numbers
#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

//________________________________

//Helper function
template<typename REAL, typename WHATEVER>
breit_wheeler_engine<REAL, stl_rng_wrapper<REAL>> get_bw_stl_set_lambda(uint64_t seed, WHATEVER lambda,
breit_wheeler_engine_ctrl<REAL> bw_ctrl = breit_wheeler_engine_ctrl<REAL>())
{
    stl_rng_wrapper<REAL> wrap{seed};
    auto bw_engine =  breit_wheeler_engine<REAL, stl_rng_wrapper<REAL>>{std::move(wrap), 1.0, bw_ctrl};
    bw_engine.set_lambda(static_cast<REAL>(lambda));
    return bw_engine;
}

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

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

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

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

//Special tolerance for a specific case (check carefully)

//Special Tolerance for double precision calculations
const double double_spec_tolerance = 2.0e-2;

//Special Tolerance for single precision calculations
const float float_spec_tolerance = 2.0e-2;

//Special Templated tolerance
template <typename T>
T spec_tolerance()
{
    if(std::is_same<T,float>::value)
        return float_spec_tolerance;
    else
        return double_spec_tolerance;
}

//Test get/set lambda for breit_wheeler_engine generic
template <typename T>
void breit_wheeler_engine_gs()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_CHECK_EQUAL( static_cast<T>(800.0*si_nanometer), bw_engine.get_lambda());
}

//Test get/set lambda for breit_wheeler_engine (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_gs_double_1 )
{
    breit_wheeler_engine_gs<double>();
}

//Test get/set lambda for breit_wheeler_engine (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_gs_single_1 )
{
    breit_wheeler_engine_gs<float>();
}

// ------------- optical depth --------------
//Test get/set lambda for breit_wheeler_engine generic
template <typename T>
void breit_wheeler_engine_opt()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_TEST ( bw_engine.get_optical_depth() >= static_cast<T>(0.0) );
}

//Test get new optical depth (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_opt_double_1 )
{
    breit_wheeler_engine_opt<double>();
}

//Test get new optical depth (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_opt_single_1 )
{
    breit_wheeler_engine_opt<float>();
}

// ------------- Pair production rates --------------

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_1()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(0.0);
    T py =  static_cast<T>(0.0);
    T pz =  static_cast<T>(0.0);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);
    T lambda = static_cast<T>(800. * si_nanometer);

    //T exp = static_cast<T>(0.0);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_1 )
{
    breit_wheeler_engine_prod_1<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_1 )
{
    breit_wheeler_engine_prod_1<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_2()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(0.0);
    T pz =  static_cast<T>(0.0);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(0.0);
    T ez =  static_cast<T>(0.0);
    T bx =  static_cast<T>(0.0);
    T by =  static_cast<T>(0.0);
    T bz =  static_cast<T>(0.0);
    T lambda = static_cast<T>(800. * si_nanometer);

    //T exp = static_cast<T>(0.0);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_2 )
{
    breit_wheeler_engine_prod_2<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_2 )
{
    breit_wheeler_engine_prod_2<float>();
}

template <typename T>
void breit_wheeler_engine_prod_3()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(6.81696);
    T py =  static_cast<T>(9.68933);
    T pz =  static_cast<T>(2.81229);
    T ex =  static_cast<T>(-4.89986);
    T ey =  static_cast<T>(-9.65535);
    T ez =  static_cast<T>(3.69471);
    T bx =  static_cast<T>(8.89549);
    T by =  static_cast<T>(-5.46574);
    T bz =  static_cast<T>(-6.75393);
    T lambda = static_cast<T>(800. * si_nanometer);

    //T exp = static_cast<T>(0.0);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_3 )
{
    breit_wheeler_engine_prod_3<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_3 )
{
    breit_wheeler_engine_prod_3<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_4()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(933.115);
    T pz =  static_cast<T>(-538.195);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);
    T lambda = static_cast<T>(800. * si_nanometer);

    T exp = static_cast<T>(1.50648551484);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    std::cerr << chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) <<" " << exp << " " << res << std::endl;

    BOOST_CHECK_SMALL((exp-res)/exp, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_4 )
{
    breit_wheeler_engine_prod_4<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_4 )
{
    breit_wheeler_engine_prod_4<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_5()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(-44.4546);
    T py =  static_cast<T>(-0.2033);
    T pz =  static_cast<T>(94.5843);
    T ex =  static_cast<T>(39.8996);
    T ey =  static_cast<T>(-29.2501);
    T ez =  static_cast<T>(58.7720);
    T bx =  static_cast<T>(44.3417);
    T by =  static_cast<T>(15.5024);
    T bz =  static_cast<T>(29.4024);
    T lambda = static_cast<T>(800. * si_nanometer);

    //T exp = static_cast<T>(4.69766211952e-73);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_5 )
{
    breit_wheeler_engine_prod_5<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_5 )
{
    breit_wheeler_engine_prod_5<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_6()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(-965.61);
    T py =  static_cast<T>(-3975.11);
    T pz =  static_cast<T>(6917.22);
    T ex =  static_cast<T>(11.17);
    T ey =  static_cast<T>(-2117.72);
    T ez =  static_cast<T>(-1407.19);
    T bx =  static_cast<T>( 6259.79);
    T by =  static_cast<T>(7557.54);
    T bz =  static_cast<T>(773.11);
    T lambda = static_cast<T>(800. * si_nanometer);

    T exp = static_cast<T>(3.51855878777);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL((exp-res)/exp, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_6 )
{
    breit_wheeler_engine_prod_6<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_6 )
{
    breit_wheeler_engine_prod_6<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_7()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    T px =  static_cast<T>(61019.1);
    T py =  static_cast<T>(-24359.3);
    T pz =  static_cast<T>(65116.2);
    T ex =  static_cast<T>(69942.0);
    T ey =  static_cast<T>(38024.7);
    T ez =  static_cast<T>(-43604.1);
    T bx =  static_cast<T>(-26990.0);
    T by =  static_cast<T>(58267.8);
    T bz =  static_cast<T>(-63485.8);
    T lambda = static_cast<T>(800. * si_nanometer);

    T exp = static_cast<T>(7.63488202211);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz}), chi_photon(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_SMALL((exp-res)/exp, spec_tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_7 )
{
    breit_wheeler_engine_prod_7<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_7 )
{
    breit_wheeler_engine_prod_7<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_1()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(933.115);
    T pz =  static_cast<T>(-538.195);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);

    T initial_optical_depth = static_cast<T>(1.0);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01);

    T exp_rate = static_cast<T>(1.50648551484);

    bool has_event_happend;
    T dt_prod;
    bw_engine.compute_dN_dt_lookup_table(nullptr);
    std::tie(has_event_happend, dt_prod) =
        bw_engine.evolve_opt_depth_and_determine_event
        (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;

    BOOST_CHECK_EQUAL(has_event_happend, false);
    BOOST_CHECK_EQUAL(dt_prod, static_cast<T>(0.0));
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
        tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_1 )
{
    breit_wheeler_engine_detopt_1<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_1 )
{
    breit_wheeler_engine_detopt_1<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_2()
{
    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(933.115);
    T pz =  static_cast<T>(-538.195);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);

    T initial_optical_depth = static_cast<T>(1.0e-3);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01);

    T exp_rate = static_cast<T>(1.50648551484);

    bool has_event_happend;
    T dt_prod;
    bw_engine.compute_dN_dt_lookup_table(nullptr);
    std::tie(has_event_happend, dt_prod) =
        bw_engine.evolve_opt_depth_and_determine_event
        (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;
    T exp_dt_prod = initial_optical_depth/exp_rate ;

    BOOST_CHECK_EQUAL(has_event_happend, true);
    BOOST_CHECK_SMALL(exp_dt_prod, dt_prod);
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
        tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_2 )
{
    breit_wheeler_engine_detopt_2<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_2 )
{
    breit_wheeler_engine_detopt_2<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_3()
{
    breit_wheeler_engine_ctrl<T> bw_ctrl;
    bw_ctrl.chi_phot_min =  static_cast<T>(0.0001);

    auto bw_engine = get_bw_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer), bw_ctrl);

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(933.115);
    T pz =  static_cast<T>(-538.195);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);

    T initial_optical_depth = static_cast<T>(1.0e-3);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01);

    T exp_rate = static_cast<T>(1.50648551484);

    bool has_event_happend;
    T dt_prod;
    bw_engine.compute_dN_dt_lookup_table(nullptr);
    std::tie(has_event_happend, dt_prod) =
        bw_engine.evolve_opt_depth_and_determine_event
        (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;
    T exp_dt_prod = initial_optical_depth/exp_rate ;

    BOOST_CHECK_EQUAL(has_event_happend, true);
    BOOST_CHECK_SMALL(exp_dt_prod, dt_prod);
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
        tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_3 )
{
    breit_wheeler_engine_detopt_3<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_3 )
{
    breit_wheeler_engine_detopt_3<float>();
}


/*
  Copyright (c) 2012,2013,2014 Matthew H. Reilly (kb1vc)
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
  Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in
  the documentation and/or other materials provided with the
  distribution.

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

#include "HilbertTransformer.hxx"

#include <iostream>
#include <string.h>
#include <fftw3.h>
#include <boost/format.hpp>

int dbgctr = 0;

static unsigned int ipow(unsigned int x, unsigned int y) __attribute__ ((unused));
static unsigned int ipow(unsigned int x, unsigned int y)
{
  unsigned int ret;
  ret = 1;
  unsigned int i;

  for(i = 0; i < y; i++) {
    ret *= x;
  }

  return ret;
}

SoDa::HilbertTransformer::HilbertTransformer(unsigned int inout_buffer_length,
					     unsigned int filter_length) :
  SoDa::Base("HilbertTransformer")
{
  // these are the salient dimensions for this Overlap/Save
  // widget (for terminology, see Lyons pages 719ff
  M = inout_buffer_length;

  // now find N.
  N = 4 * filter_length;
  while(N < (M + filter_length)) {
    N = N * 2;
  }
  // now that we have N, we can back-calculate Q.
  Q = (N - M) + 1;

  //  std::cerr << "\n\nHILBERT picked N = " << N << " Q = " << Q << " M = " << M << std::endl;

  std::complex<float> htu[N], htl[N];

  // create the impulse response images
  // There is probably a simpler way, but the obvious real/imag swap
  // scheme doesn't work at all well.
  HTu_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  HTl_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  Pass_U_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  Pass_L_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  fftwf_plan HTu_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htu, (fftwf_complex *) HTu_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  if(HTu_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating HT upper plan...\n");
  }
  fftwf_plan HTl_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htl, (fftwf_complex *) HTl_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  if(HTl_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating HT lower plan...\n");
  }

  // now build the time domain image of the filter.
  unsigned int i, j;
  for(i = 0; i < N; i++) {
    htu[i] = htl[i] = std::complex<float>(0.0, 0.0);
  }
  // this is actually a scaled ht -- removing the Fs * 2 / pi scaling factor.
  for(i = 0, j = (Q / 2); i < (Q / 2); i++) {
    if((i & 1) != 0) {
      htu[j + i] = std::complex<float>(1.0 / ((float) i), 0.0);
      htu[j - i] = std::complex<float>(-1.0 / ((float) i), 0.0);
      htl[j + i] = std::complex<float>(-1.0 / ((float) i), 0.0);
      htl[j - i] = std::complex<float>(1.0 / ((float) i), 0.0);
    }
  }
  fftwf_execute(HTu_plan);
  fftwf_execute(HTl_plan);
  // now we have the HT filter image
  fftwf_destroy_plan(HTu_plan);
  fftwf_destroy_plan(HTl_plan);

  // now do the delay filter
  fftwf_plan dly_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htu, (fftwf_complex *) Pass_U_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  // load up a new impulse response
  for(i = 0; i < N; i++) htu[i] = std::complex<float>(0.0, 0.0);
  htu[Q/2] = std::complex<float>(1.0, 0.0);
  // now create the passthrough filter
  fftwf_execute(dly_plan);
  fftwf_destroy_plan(dly_plan);

  // Do some equalization on the pass filter to fix the low frequency response
  // to match the response of the HT filter.
  for(i = 0; i < N; i++) {
    float tumag = abs(HTu_filter[i]);
    float tlmag = abs(HTl_filter[i]);
    float pmag = abs(Pass_U_filter[i]);
    float uadj = 1.0;
    float ladj = 1.0;
    if(pmag > 0.001) {
      uadj = tumag / pmag;
      ladj = tlmag / pmag;
      if (uadj > 2) uadj = 1.0;
      if (ladj > 2) ladj = 1.0;
    }
    Pass_L_filter[i] = ladj * Pass_U_filter[i];
    Pass_U_filter[i] = uadj * Pass_U_filter[i];
  }

  // calculate the magnitudes of the two filters.
  float hmag, pmag;
  hmag = 0.0;
  pmag = 0.0;
  for(i = 0; i < N; i++) {
    hmag += HTu_filter[i].real() * HTu_filter[i].real()
      + HTu_filter[i].imag() * HTu_filter[i].imag();
    pmag += Pass_U_filter[i].real() * Pass_U_filter[i].real()
      + Pass_U_filter[i].imag() * Pass_U_filter[i].imag();
  }

  // now allocate all the storage vectors
  fft_I_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_Q_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_I_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_Q_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_I_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_Q_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_I_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_Q_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));

  // and create the plans
  forward_I_plan = fftwf_plan_dft_1d(N,
				     (fftwf_complex *) fft_I_input, (fftwf_complex *) fft_I_output,
				     FFTW_FORWARD, FFTW_ESTIMATE);
  if(forward_I_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating forward I plan...\n");
  }

  forward_Q_plan = fftwf_plan_dft_1d(N,
				     (fftwf_complex *) fft_Q_input,
				     (fftwf_complex *) fft_Q_output,
				     FFTW_FORWARD, FFTW_ESTIMATE);
  if(forward_Q_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating forward Q plan...\n");
  }

  backward_I_plan = fftwf_plan_dft_1d(N, (fftwf_complex *) ifft_I_input,
				      (fftwf_complex *) ifft_I_output,
				      FFTW_BACKWARD, FFTW_ESTIMATE);
  if(backward_I_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating backward I plan...\n");
  }
  backward_Q_plan = fftwf_plan_dft_1d(N, (fftwf_complex *) ifft_Q_input,
				      (fftwf_complex *) ifft_Q_output,
				      FFTW_BACKWARD, FFTW_ESTIMATE);
  if(backward_Q_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating backward Q plan...\n");
  }
  // zero out the start of the fft_input buffer for the first iteration.
  for(i = 0; i <= Q-1; i++) {
    fft_I_input[i] = 0.0;
    fft_Q_input[i] = 0.0;
  }

  // finally, set the transform gain (1/N)
  //   passthrough_gain = 1.0 / ((float) N);
  //  H_transform_gain = 2.0 / (M_PI * 0.966 * ((float) N)); // 0.966 is a fudge factor
  H_transform_gain = 2.0 / (M_PI * ((float) N));
  passthrough_gain = H_transform_gain;
}

unsigned int SoDa::HilbertTransformer::apply(std::complex<float> * inbuf,
					     std::complex<float> * outbuf,
					     bool pos_sided, float gain)
{
  unsigned int i, j;

  std::complex<float> *HT_F;
  std::complex<float> *PA_F;

  if(pos_sided) {
    HT_F = HTl_filter;
    PA_F = Pass_L_filter;
  }
  else {
    HT_F = HTu_filter;
    PA_F = Pass_U_filter;
  }
  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff
  // copy the I channel to the tail of the input buffer.
  memcpy(&(fft_I_input[Q-1]), inbuf, sizeof(std::complex<float>) * M);

  // do a pass through
  fftwf_execute(forward_I_plan);

  // now save the tail of the input to the save buffer
  memcpy(fft_I_input, &(inbuf[1 + (M - Q)]), sizeof(std::complex<float>) * (Q - 1));

  // now apply the delay filter and the hilbert transform
  for(i = 0; i < N; i++) {
    ifft_Q_input[i] = fft_I_output[i] * HT_F[i];
    ifft_I_input[i] = fft_I_output[i] * PA_F[i];
  }

  // do the inverse fft for the I and Q channels
  fftwf_execute(backward_I_plan);
  fftwf_execute(backward_Q_plan);

  // now put the two channels together
  // Note that we're shifting the normal sampling window.  This is because the
  // quadrature sampler is just not quite right for
  for(i = 0, j = Q-1; i < M; i++, j++) {
    // seems like it worked once... but apparent shift is same for either sideband
    outbuf[i] = std::complex<float>(ifft_I_output[j].real() * passthrough_gain * gain,
				    ifft_Q_output[j].real() * H_transform_gain * gain);
  }

  dbgctr++;

  return M;
}

unsigned int SoDa::HilbertTransformer::apply(float * inbuf,
					     std::complex<float> * outbuf,
					     bool pos_sided, float gain)
{
  unsigned int i;
  // This creates an analytic signal from a single input buffer.

  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff
  std::complex<float> cinbuf[M];

  // copy the I channel to the tail of the input buffer.
  for(i = 0; i < M; i++) {
    cinbuf[i] = std::complex<float>(inbuf[i], 0.0);
  }

  // call the complex HT
  return apply(cinbuf, outbuf, pos_sided, gain);

  return M;
}

unsigned int SoDa::HilbertTransformer::applyIQ(std::complex<float> * inbuf,
					       std::complex<float> * outbuf,
					       float gain)
{
  unsigned int i, j;
  // This creates an analytic signal from a single input buffer.

  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff

  // copy the I channel to the tail of the I input buffer.
  // copy the I channel to the tail of the Q input buffer.
  for(i = 0; i < M; i++) {
    fft_I_input[i + (Q-1)] = std::complex<float>(inbuf[i].real(), 0.0);
    fft_Q_input[i + (Q-1)] = std::complex<float>(inbuf[i].imag(), 0.0);
  }

  // do a the I (passthrough) and Q channel FFTs
  fftwf_execute(forward_I_plan);
  fftwf_execute(forward_Q_plan);

  // now save the tail of the input to the save buffer
  for(i = 0; i < (Q - 1); i++) {
    fft_I_input[i] = std::complex<float>(inbuf[i + 1 + (M - Q)].real(), 0.0);
    fft_Q_input[i] = std::complex<float>(inbuf[i + 1 + (M - Q)].imag(), 0.0);
  }

  // now apply the delay filter (to I) and the hilbert transform (to Q)
  for(i = 0; i < N; i++) {
    ifft_Q_input[i] = fft_Q_output[i] * HTu_filter[i];
    ifft_I_input[i] = fft_I_output[i] * Pass_U_filter[i];
  }

  // do the inverse fft for the I and Q channels
  fftwf_execute(backward_I_plan);
  fftwf_execute(backward_Q_plan);

  // now put the two channels together
  // Note that we're shifting the normal sampling window.  This is because the
  // quadrature sampler is just not quite right for
  for(i = 0, j = Q-1; i < M; i++, j++) {
    outbuf[i] = std::complex<float>(ifft_I_output[j].real() * passthrough_gain * gain,
				    ifft_Q_output[j].real() * H_transform_gain * gain);
  }

  dbgctr++;

  return M;
}

std::ostream & SoDa::HilbertTransformer::dump(std::ostream & os)
{
  unsigned int i, j;
  for(i = 0; i < N; i++) {
    j = i;
    float mag = std::abs(HTl_filter[i]);
    float ang = std::arg(HTl_filter[i]);
    float pmag = std::abs(Pass_L_filter[i]);
    float pang = std::arg(Pass_L_filter[i]);
    os << boost::format("%d %f %f %f %f %f %f %f %f\n")
      % j % HTu_filter[i].real() % HTu_filter[i].imag() % mag % ang %
      Pass_U_filter[i].real() % Pass_U_filter[i].imag() % pmag % pang;
  }
  return os;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Example combining Boost.Geometry with Boost.Units

#include <iostream>

#include <boost/geometry/geometry.hpp>

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/io.hpp>

// TEMPORARY this will go to somewhere within Boost.Geometry
namespace boost { namespace geometry
{

namespace cs
{

template <typename Unit>
struct units_cartesian {};

}

namespace traits
{
template<typename U>
struct cs_tag<cs::units_cartesian<U> >
{
    typedef cartesian_tag type;
};

}

namespace model
{

// Define a point type to interoperate with Boost.Units, having
// 1. a constructor taking quantities
// 2. defining a quantified coordinate system
// Note that all values are still stored in "normal" types as double
template <typename U, std::size_t D = 2, typename T = double, typename CS = cs::units_cartesian<U> >
class quantity_point : public model::point<T, D, CS>
{
    typedef boost::units::quantity<U, T> qtype;

public :

    // Templated constructor to allow constructing with other units then qtype,
    // e.g. to convert from centimeters to meters
    template <typename Q>
    inline quantity_point(Q const& x, Q const& y)
        : model::point<T, D, CS>(
            qtype(x).value(),
            qtype(y).value())
    {}
};

}

// Adapt quantity_point to the Point Concept
#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct tag<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef point_tag type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_type<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateType type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_system<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateSystem type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct dimension<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
    : boost::mpl::int_<DimensionCount>
{};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem, std::size_t Dimension>
struct access<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>, Dimension >
{
    static inline CoordinateType get(
        model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> const& p)
    {
        return p.template get<Dimension>();
    }

    static inline void set(model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>& p,
        CoordinateType const& value)
    {
        p.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

// For extra support for functions as distance,area,get,set
namespace units
{
    namespace detail
    {
        // Define an extra meta-function to get the units of a coordinate system
        template <typename CS>
        struct unit_dimension
        {
            // define it as dimensionless
            // or MPL ASSERT
        };

        template <typename U>
        struct unit_dimension<cs::units_cartesian<U> >
        {
            typedef U type;
        };
    }

    // Define an extra metafunction to define the quantity of a Geometry type
    template <typename Geometry, typename CT = typename coordinate_type<Geometry>::type>
    struct quantity
    {
        typedef boost::units::quantity
            <
                typename detail::unit_dimension
                    <
                        typename coordinate_system<Geometry>::type
                    >::type,
                CT
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    inline typename quantity<Geometry1, typename default_distance_result<Geometry1, Geometry2>::type>::type
        distance(Geometry1 const& g1, Geometry2 const& g2)
    {
        typedef typename quantity<Geometry1, typename default_distance_result<Geometry1, Geometry2>::type>::type q;
        return q::from_value(geometry::distance(g1, g2));
    }

    template <std::size_t Index, typename Point>
    inline typename quantity<Point>::type get(Point const& p)
    {
        typedef typename quantity<Point>::type q;
        return q::from_value(geometry::get<Index>(p));
    }
}

}}
// END TEMPORARY

int main(void)
{
    using namespace boost::geometry;
    using namespace boost::units;

    // 1: using it directly
    {
        typedef model::quantity_point<si::length, 2> point;
        point p1(1 * si::meter, 2 * si::meter);
        point p2(3 * si::meter, 4 * si::meter);

        std::cout << get<0>(p2) << std::endl;

        // This is a little inconvenient:
        quantity<si::length> d = distance(p1, p2) * si::meter;

        std::cout << d << std::endl;
    }

    // 2: same but now using centimeters, and using boost::geometry::units::
    {
        typedef model::quantity_point<cgs::length, 2> point;
        point p1(1 * si::meter, 2 * si::meter);
        point p2(3 * si::meter, 4 * si::meter);

        std::cout << boost::geometry::units::get<0>(p2) << std::endl;
        quantity<cgs::length> d = boost::geometry::units::distance(p1, p2);
        std::cout << d << std::endl;
    }

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/tuple/tuple.hpp>

template <typename G>
void test_get_set()
{
    typedef typename bg::coordinate_type<G>::type coordinate_type;

    G g;
    bg::set<0>(g, coordinate_type(1));
    bg::set<1>(g, coordinate_type(2));

    coordinate_type x = bg::get<0>(g);
    coordinate_type y = bg::get<1>(g);

    BOOST_CHECK_CLOSE(double(x), 1.0, 0.0001);
    BOOST_CHECK_CLOSE(double(y), 2.0, 0.0001);
}

template <typename P>
void test_all()
{
    typedef typename bg::coordinate_type<P>::type coordinate_type;

    // N-SPHERE, setting sphere center
    test_get_set<bg::model::nsphere<P, coordinate_type> >();

}

int test_main(int, char* [])
{
    test_get_set<int[2]>();
    test_get_set<float[2]>();
    test_get_set<double[2]>();
    test_get_set<double[3]>();

    test_get_set<boost::tuple<double, double> >();

    test_all<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
//  Sample output
//
// DFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// back: 1 --> 1
// tree: 1 --> 3
// back: 3 --> 1
// tree: 3 --> 4
// back: 4 --> 0
// back: 4 --> 1
// forward or cross: 2 --> 3

// BFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// tree: 2 --> 3
// cycle: 1 --> 1
// cycle: 1 --> 3
// cycle: 3 --> 1
// tree: 3 --> 4
// cycle: 4 --> 0
// cycle: 4 --> 1

#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <map>
#include <algorithm>
#include <utility>
#include <string>
#include <range/v3/algorithm/transform.hpp>

#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/depth_first_search.hpp>

template<class Tag>
struct edge_printer : public boost::base_visitor<edge_printer<Tag>>
{
  int *n = 0;
  std::vector<int> *vertexs_ = 0;

  typedef Tag event_filter;
  edge_printer(std::string edge_t) : m_edge_type(edge_t) {}
  template<class Edge, class Graph>
  void operator()(Edge e, Graph &G)
  {
    //std::cout << m_edge_type << ": " << source(e, G) << " --> " << target(e, G) << std::endl;
    if (n) {
      *n += 1;
    }
    if (vertexs_) {
      int src = source(e, G);
      int dst = target(e, G);
      vertexs_->push_back(src);
      vertexs_->push_back(dst);
    }
  }
  int Count() const
  {
    return *n;
  }

  std::string m_edge_type;
};
template<class Tag>
edge_printer<Tag> print_edge(std::string type, Tag)
{
  return edge_printer<Tag>(type);
}

typedef boost::adjacency_list<> Graph;
typedef std::pair<int, int> E;
struct Bags
{
  int test(const Graph &G, int n)
  {

    using std::endl;
    using std::cout;
    using namespace boost;

    //cout << endl
    //     << "BFS categorized directed graph" << endl;
    //boost::breadth_first_search(G, vertex(0, G), visitor(make_bfs_visitor(std::make_pair(print_edge("tree", on_tree_edge()), print_edge("cycle", on_non_tree_edge())))));
    //cout << endl
    //     << "BFS jiayuehua categorized directed graph" << endl;
    auto ep = print_edge("tree", on_tree_edge());
    //auto epb = print_edge("tree", on_non_tree_edge());
    int num = 0;
    std::vector<int> vertexs;
    ep.vertexs_ = &vertexs;
    boost::breadth_first_search(G, vertex(n, G), visitor(make_bfs_visitor(std::make_pair(ep, print_edge("cycle", on_non_tree_edge())))));
    //cout << endl;
    for (auto v : vertexs) {
      bagid_.erase(v);
    }
    return num + 1;
  }
  std::vector<E> edges_;
  std::set<int> bagid_;
  int shinygoldid_ = 0;
  Graph G_;
  int groupCnt = 0;

public:
  int groupsCnt()
  {
    while (!bagid_.empty()) {
      int groupsz = bagid_.size();
      int id = *bagid_.begin();
      test(G_, id);
      if (groupsz == bagid_.size()) {
        bagid_.erase(id);
      }
      ++groupCnt;
    }
    return groupCnt;
  }

  friend std::istream &operator>>(std::istream &is, Bags &bags)
  {
    std::multimap<int, int> edges_;
    std::string line;
    int src;
    int dst;

    for (int i = 0, n = 0; std::getline(is, line); ++n) {
      std::istringstream iss(line);
      iss >> src;
      bags.bagid_.insert(src);

      for (; iss >> dst;) {
        bags.bagid_.insert(dst);
        edges_.insert(std::pair{ src, dst });
        edges_.insert(std::pair{ dst, src });
      }
    }

    Graph G(edges_.begin(), edges_.end(), bags.bagid_.size());
    bags.G_ = G;

    return is;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string color, attr;
    Bags bags;
    ifs >> bags;
    std::cout << bags.groupsCnt() << std::endl;
  }
}


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/coarsening/rigid_body_modes.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/deflated_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/reorder.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>

#include <amgcl/profiler.hpp>

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    namespace po = boost::program_options;
    namespace io = amgcl::io;

    using amgcl::prof;
    using std::vector;
    using std::string;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "Show this help.")
        ("prm-file,P",
         po::value<string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ("matrix,A",
         po::value<string>()->required(),
         "System matrix in the MatrixMarket format."
        )
        (
         "rhs,f",
         po::value<string>(),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "defvec,D",
         po::value<string>(),
         "The near null-space vectors in the MatrixMarket format. "
        )
        (
         "coords,C",
         po::value<string>(),
         "Coordinate matrix where number of rows corresponds to the number of grid nodes "
         "and the number of columns corresponds to the problem dimensionality (2 or 3). "
         "Will be used to construct near null-space vectors as rigid body modes. "
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "single-level,1",
         po::bool_switch()->default_value(false),
         "When specified, the AMG hierarchy is not constructed. "
         "Instead, the problem is solved using a single-level smoother as preconditioner. "
        )
        (
         "output,o",
         po::value<string>(),
         "Output file. Will be saved in the MatrixMarket format. "
         "When omitted, the solution is not saved. "
        )
        ;

    po::positional_options_description p;
    p.add("prm", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    for (int i = 0; i < argc; ++i) {
        if (i) std::cout << " ";
        std::cout << argv[i];
    }
    std::cout << std::endl;

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<string>(), prm);
    }

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    if (!vm.count("defvec") && !vm.count("coords")) {
        std::cerr << "Either defvec or coords should be given" << std::endl;
        return 1;
    }

    ptrdiff_t rows, nv;
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs, z;

    {
        auto t = prof.scoped_tic("reading");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            ptrdiff_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            ptrdiff_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }

        if (vm.count("defvec")) {
            string nfile = vm["defvec"].as<string>();
            std::vector<double> N;

            ptrdiff_t m;

            if (binary) {
                io::read_dense(nfile, m, nv, N);
            } else {
                std::tie(m, nv) = io::mm_reader(nfile)(N);
            }

            precondition(m == rows, "Deflation vectors have wrong size");

            z.resize(N.size());
            for(ptrdiff_t i = 0; i < rows; ++i)
                for(ptrdiff_t j = 0; j < nv; ++j)
                    z[i + j * rows] = N[i * nv + j];
        } else if (vm.count("coords")) {
            string cfile = vm["coords"].as<string>();
            std::vector<double> coo;

            ptrdiff_t m, ndim;

            if (binary) {
                io::read_dense(cfile, m, ndim, coo);
            } else {
                std::tie(m, ndim) = io::mm_reader(cfile)(coo);
            }

            precondition(m * ndim == rows && (ndim == 2 || ndim == 3), "Coordinate matrix has wrong size");

            nv = amgcl::coarsening::rigid_body_modes(ndim, coo, z, /*transpose = */true);
        }

        prm.put("nvec", nv);
        prm.put("vec",  z.data());
    }

    std::vector<double> x(rows, 0);

    int    iters;
    double error;

    if (vm["single-level"].as<bool>())
        prm.put("precond.class", "relaxation");

    typedef amgcl::backend::builtin<double> Backend;
    typedef amgcl::deflated_solver<
        amgcl::runtime::preconditioner<Backend>,
        amgcl::runtime::solver::wrapper<Backend>
        > Solver;

    auto A = std::tie(rows, ptr, col, val);

    prof.tic("setup");
    Solver solve(A, prm);
    prof.toc("setup");

    prof.tic("solve");
    std::tie(iters, error) = solve(rhs, x);
    prof.toc("solve");

    if (vm.count("output")) {
        auto t = prof.scoped_tic("write");
        amgcl::io::mm_write(vm["output"].as<string>(), x.data(), x.size());
    }

    std::vector<double> r(rows);
    amgcl::backend::residual(rhs, A, x, r);

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl
              << "True error: " << sqrt(amgcl::backend::inner_product(r, r)) / sqrt(amgcl::backend::inner_product(rhs, rhs))
              << prof << std::endl;
}


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "lipton-tarjan.h"
#include "typedefs.h"
#include "strutil.h"
#include "graphutil.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/bimap.hpp>
#include <iostream>
#include <fstream>
#include <csignal>
#include <utility>
using namespace std;
using namespace boost;

int main(int argc, char* argv[])
{
        vector<string> fname;
        if( argc < 2 ){
                cout << "Usage: lt [filename]\n";
                return 0;
        }
        for( uint i = 1; i < argc; ++i ) fname.push_back(argv[i]);

        for( string& f : fname ){
                cout << "loading graph\n";

                Graph g = load_graph(f);

#ifdef GRAPH_TYPE_LIST
                init_vert_propmap(g);
#endif

                uint n = num_vertices(g);

                cout << "n: " << n << '\n';
                uint e = num_edges(g);

                cout << "starting lipton tarjan...\n";
                print_graph(g);

		try {
			Partition p = lipton_tarjan_separator(g);
			uint num_verts_finished = p.total_num_verts();
			cout << "Finished!\n";

                        p.print(&g);

                        BOOST_ASSERT(p.verify_edges(g));
			BOOST_ASSERT(p.verify_sizes(g));
			//p.print(vmap2);

			cout << "finished num verts: " << num_verts_finished << '\n';
		} catch (NotPlanarException e) {
			cout << "cannot finish lipton-tarjan because graph is not planar\n";
		} catch (NoNontreeEdgeException e) {
			cout << "cannot finish lipton-tarjan because I could not find a nontree edge out of " << e.num_edges << " edges\n";
		}
        }
}