#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <EigenRand/EigenRand>
#include <cmath>

#ifdef EIGEN_VECTORIZE_NEON

TEST(MorePacketMath, log)
{
	Eigen::ArrayXf x(8), y(8);
	x << -1, 0, 0.1, 0.2, 0.5, 1.0, 2.0, 5.0;
	y.writePacket<0>(0, Eigen::internal::plog(x.packet<0>(0)));
	y.writePacket<0>(4, Eigen::internal::plog(x.packet<0>(4)));
	std::cout << y.transpose() << std::endl;
	for (int i = 0; i < x.size(); ++i)
	{
		if (std::isnan(std::log(x[i])))
		{
			EXPECT_TRUE(std::isnan(y[i]));
		}
		else
		{
			EXPECT_FLOAT_EQ(std::log(x[i]), y[i]);
		}
	}
}

TEST(MorePacketMath, sin)
{
	Eigen::ArrayXf x(8), y(8);
	x << -1, 0, 0.1, 0.2, 0.5, 1.0, 2.0, 5.0;
	y.writePacket<0>(0, Eigen::internal::psin(x.packet<0>(0)));
	y.writePacket<0>(4, Eigen::internal::psin(x.packet<0>(4)));
	std::cout << y.transpose() << std::endl;
	for (int i = 0; i < x.size(); ++i)
	{
		if (std::isnan(std::sin(x[i])))
		{
			EXPECT_TRUE(std::isnan(y[i]));
		}
		else
		{
			EXPECT_FLOAT_EQ(std::sin(x[i]), y[i]);
		}
	}
}

TEST(MorePacketMath, sincos)
{
	Eigen::ArrayXf x(8), y(8), z(8);
	x << -1, 0, 0.1, 0.2, 0.5, 1.0, 2.0, 5.0;
	Eigen::internal::Packet4f s, c;
	Eigen::internal::psincos(x.packet<0>(0), s, c);
	y.writePacket<0>(0, s);
	z.writePacket<0>(0, c);
	Eigen::internal::psincos(x.packet<0>(4), s, c);
	y.writePacket<0>(4, s);
	z.writePacket<0>(4, c);
	std::cout << y.transpose() << std::endl;
	std::cout << z.transpose() << std::endl;
	for (int i = 0; i < x.size(); ++i)
	{
		if (std::isnan(std::sin(x[i])))
		{
			EXPECT_TRUE(std::isnan(y[i]));
		}
		else
		{
			EXPECT_FLOAT_EQ(std::sin(x[i]), y[i]);
		}

		if (std::isnan(std::cos(x[i])))
		{
			EXPECT_TRUE(std::isnan(z[i]));
		}
		else
		{
			EXPECT_FLOAT_EQ(std::cos(x[i]), z[i]);
		}
	}
}

TEST(MorePacketMath, sqrt)
{
	Eigen::ArrayXf x(8), y(8);
	x << -1, 0, 0.1, 0.2, 0.5, 1.0, 2.0, 5.0;
	y.writePacket<0>(0, Eigen::internal::psqrt(x.packet<0>(0)));
	y.writePacket<0>(4, Eigen::internal::psqrt(x.packet<0>(4)));
	std::cout << y.transpose() << std::endl;
	for (int i = 0; i < x.size(); ++i)
	{
		if (std::isnan(std::sqrt(x[i])))
		{
			EXPECT_TRUE(std::isnan(y[i]));
		}
		else
		{
			EXPECT_FLOAT_EQ(std::sqrt(x[i]), y[i]);
		}
	}
}

#endif

template <class T>
class ContinuousDistTest : public testing::Test
{
};

using ETypes = testing::Types<float, double>;

TYPED_TEST_CASE(ContinuousDistTest, ETypes);

TYPED_TEST(ContinuousDistTest, balanced)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::balanced<Matrix>(8, 8, gen);
	mat = Eigen::Rand::balanced<Matrix>(3, 3, gen);
	mat = Eigen::Rand::balanced<Matrix>(5, 5, gen);
	mat = Eigen::Rand::balanced<Matrix>(5, 3, gen);
	mat = Eigen::Rand::balanced<Matrix>(1, 3, gen);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, balanced2)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::balanced<Matrix>(8, 8, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(3, 3, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(5, 5, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(5, 3, gen, 0.5, 2);
	mat = Eigen::Rand::balanced<Matrix>(1, 3, gen, 0.5, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, uniformReal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::uniformReal<Matrix>(8, 8, gen);
	mat = Eigen::Rand::uniformReal<Matrix>(3, 3, gen);
	mat = Eigen::Rand::uniformReal<Matrix>(5, 5, gen);
	mat = Eigen::Rand::uniformReal<Matrix>(5, 3, gen);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, stdNormal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::normal<Matrix>(8, 8, gen);
	mat = Eigen::Rand::normal<Matrix>(3, 3, gen);
	mat = Eigen::Rand::normal<Matrix>(5, 5, gen);
	mat = Eigen::Rand::normal<Matrix>(5, 3, gen);
	mat = Eigen::Rand::normal<Matrix>(1, 3, gen);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, normal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::normal<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::normal<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, exponential)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::exponential<Matrix>(8, 8, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(3, 3, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(5, 5, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(5, 3, gen, 2);
	mat = Eigen::Rand::exponential<Matrix>(1, 3, gen, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, lognormal)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::lognormal<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::lognormal<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::lognormal<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::lognormal<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::lognormal<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, beta)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::beta<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::beta<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::beta<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::beta<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::beta<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, cauchy)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::cauchy<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::cauchy<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::cauchy<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::cauchy<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::cauchy<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, studentT)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::studentT<Matrix>(8, 8, gen, 5);
	mat = Eigen::Rand::studentT<Matrix>(3, 3, gen, 5);
	mat = Eigen::Rand::studentT<Matrix>(5, 5, gen, 5);
	mat = Eigen::Rand::studentT<Matrix>(5, 3, gen, 5);
	mat = Eigen::Rand::studentT<Matrix>(1, 3, gen, 5);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, gamma)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::gamma<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::gamma<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::gamma<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::gamma<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::gamma<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, weibull)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::weibull<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::weibull<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::weibull<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::weibull<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::weibull<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, extremeValueLike)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::extremeValue<Matrix>(8, 8, gen, 1, 2);
	mat = Eigen::Rand::extremeValue<Matrix>(3, 3, gen, 1, 2);
	mat = Eigen::Rand::extremeValue<Matrix>(5, 5, gen, 1, 2);
	mat = Eigen::Rand::extremeValue<Matrix>(5, 3, gen, 1, 2);
	mat = Eigen::Rand::extremeValue<Matrix>(1, 3, gen, 1, 2);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, chiSquared)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::chiSquared<Matrix>(8, 8, gen, 7);
	mat = Eigen::Rand::chiSquared<Matrix>(3, 3, gen, 7);
	mat = Eigen::Rand::chiSquared<Matrix>(5, 5, gen, 7);
	mat = Eigen::Rand::chiSquared<Matrix>(5, 3, gen, 7);
	mat = Eigen::Rand::chiSquared<Matrix>(1, 3, gen, 7);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, fisherF1)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::fisherF<Matrix>(8, 8, gen, 1, 1);
	mat = Eigen::Rand::fisherF<Matrix>(3, 3, gen, 1, 1);
	mat = Eigen::Rand::fisherF<Matrix>(5, 5, gen, 1, 1);
	mat = Eigen::Rand::fisherF<Matrix>(5, 3, gen, 1, 1);
	mat = Eigen::Rand::fisherF<Matrix>(1, 3, gen, 1, 1);
	std::cout << mat << std::endl;
}

TYPED_TEST(ContinuousDistTest, fisherF2)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::fisherF<Matrix>(8, 8, gen, 5, 5);
	mat = Eigen::Rand::fisherF<Matrix>(3, 3, gen, 5, 5);
	mat = Eigen::Rand::fisherF<Matrix>(5, 5, gen, 5, 5);
	mat = Eigen::Rand::fisherF<Matrix>(5, 3, gen, 5, 5);
	mat = Eigen::Rand::fisherF<Matrix>(1, 3, gen, 5, 5);
	std::cout << mat << std::endl;
}

template <class T>
class DiscreteDistTest : public testing::Test
{
};

TYPED_TEST_CASE(DiscreteDistTest, testing::Types<int32_t>);

TYPED_TEST(DiscreteDistTest, uniformInt)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::uniformInt<Matrix>(8, 8, gen, 0, 9);
	mat = Eigen::Rand::uniformInt<Matrix>(3, 3, gen, 0, 9);
	mat = Eigen::Rand::uniformInt<Matrix>(5, 5, gen, 0, 9);
	mat = Eigen::Rand::uniformInt<Matrix>(5, 3, gen, 0, 9);
	mat = Eigen::Rand::uniformInt<Matrix>(1, 3, gen, 0, 9);
	std::cout << mat << std::endl;
}

TYPED_TEST(DiscreteDistTest, discrete)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::discrete<Matrix>(8, 8, gen, { 1, 2, 3, 4, 5, 6 });
	mat = Eigen::Rand::discrete<Matrix>(3, 3, gen, { 1, 2, 3, 4, 5, 6 });
	mat = Eigen::Rand::discrete<Matrix>(5, 5, gen, { 1, 2, 3, 4, 5, 6 });
	mat = Eigen::Rand::discrete<Matrix>(5, 3, gen, { 1, 2, 3, 4, 5, 6 });
	mat = Eigen::Rand::discrete<Matrix>(1, 3, gen, { 1, 2, 3, 4, 5, 6 });
	std::cout << mat << std::endl;
}

TYPED_TEST(DiscreteDistTest, poisson)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::poisson<Matrix>(8, 8, gen, 1);
	mat = Eigen::Rand::poisson<Matrix>(3, 3, gen, 1);
	mat = Eigen::Rand::poisson<Matrix>(5, 5, gen, 1);
	mat = Eigen::Rand::poisson<Matrix>(5, 3, gen, 1);
	mat = Eigen::Rand::poisson<Matrix>(1, 3, gen, 1);
	std::cout << mat << std::endl;
}

TYPED_TEST(DiscreteDistTest, binomial)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::binomial<Matrix>(8, 8, gen, 10, 0.5);
	mat = Eigen::Rand::binomial<Matrix>(3, 3, gen, 10, 0.5);
	mat = Eigen::Rand::binomial<Matrix>(5, 5, gen, 10, 0.5);
	mat = Eigen::Rand::binomial<Matrix>(5, 3, gen, 10, 0.5);
	mat = Eigen::Rand::binomial<Matrix>(1, 3, gen, 10, 0.5);
	std::cout << mat << std::endl;
}

TYPED_TEST(DiscreteDistTest, negativeBinomial)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::negativeBinomial<Matrix>(8, 8, gen, 10, 0.5);
	mat = Eigen::Rand::negativeBinomial<Matrix>(3, 3, gen, 10, 0.5);
	mat = Eigen::Rand::negativeBinomial<Matrix>(5, 5, gen, 10, 0.5);
	mat = Eigen::Rand::negativeBinomial<Matrix>(5, 3, gen, 10, 0.5);
	mat = Eigen::Rand::negativeBinomial<Matrix>(1, 3, gen, 10, 0.5);
	std::cout << mat << std::endl;
}

TYPED_TEST(DiscreteDistTest, geometric)
{
	using Matrix = Eigen::Matrix<TypeParam, -1, -1>;
	Eigen::Rand::P8_mt19937_64 gen{ 42 };
	Matrix mat;

	mat = Eigen::Rand::geometric<Matrix>(8, 8, gen, 0.25);
	mat = Eigen::Rand::geometric<Matrix>(3, 3, gen, 0.25);
	mat = Eigen::Rand::geometric<Matrix>(5, 5, gen, 0.25);
	mat = Eigen::Rand::geometric<Matrix>(5, 3, gen, 0.25);
	mat = Eigen::Rand::geometric<Matrix>(1, 3, gen, 0.25);
	std::cout << mat << std::endl;
}

TEST(Issue, 29)
{
	typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> CMatrix;
	using Eigen::Rand::Vmt19937_64;
	using Eigen::Rand::uniformReal;

	Vmt19937_64 gen_eigen;
	CMatrix X = uniformReal<CMatrix>(5, 3, gen_eigen);
}


// Copyright 2015-2020 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

/// @file
/// Test the functionality of the BasicProperties_calculator.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <bulk_hdf5.hpp>
#include <bulk_properties.hpp>
#include <analytic1d.hpp>

TEST(analytic1D_basicprop_case, bulk_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    // obtain phonon data from HDF5 file
    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto processes = std::move(std::get<4>(hdf5_data));

    double T = 300.0;

    Eigen::ArrayXXd w(alma::calc_w0_threeph(*grid, *processes, T, world));

    // create transport direction
    Eigen::Vector3d u100(1.0, 0.0, 0.0);

    if (my_id == 0) { // analytic1D code is not parallellised; avoid
                      // running duplicates.
        alma::analytic1D::BasicProperties_calculator propCalc(
            poscar.get(), grid.get(), &w, T);

        // test bulk conductivity
        propCalc.setDirection(u100);
        double kappa = propCalc.getConductivity();
        Eigen::MatrixXd kappamatrix = alma::calc_kappa(*poscar, *grid, w, T);
        Eigen::MatrixXd buffer =
            (u100.transpose()).matrix() * kappamatrix * u100.matrix();
        double kappa_ref = buffer(0);
        double ratio = kappa / kappa_ref;
        EXPECT_NEAR(ratio, 1.0, 5e-3);

        // test heat capacity
        double Cv = propCalc.getCapacity();
        double Cv_ref = 1e27 * alma::calc_cv(*poscar, *grid, T);

        ratio = Cv / Cv_ref;
        EXPECT_NEAR(ratio, 1.0, 5e-3);
    }
}

TEST(analytic1D_basicprop_case, kappacumul_MFP_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    // obtain phonon data from HDF5 file
    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto processes = std::move(std::get<4>(hdf5_data));

    double T = 300.0;

    Eigen::ArrayXXd w(alma::calc_w0_threeph(*grid, *processes, T, world));

    // create transport direction
    Eigen::Vector3d u100(1.0, 0.0, 0.0);

    if (my_id == 0) { // analytic1D code is not parallellised; avoid
                      // running duplicates.
        alma::analytic1D::BasicProperties_calculator propCalc(
            poscar.get(), grid.get(), &w, T);
        propCalc.setDirection(u100);

        // test cumulative conductivity curves
        int Nbins = 9;

        propCalc.setLogMFPbins(1e-9, 1e-5, Nbins);
        propCalc.setLogRTbins(1e-12, 1e-9, Nbins);

        propCalc.resolveByMFP();
        Eigen::VectorXd kappacumul_byMFP = propCalc.getCumulativeConductivity();
        Eigen::VectorXd kappacumul_ref1(Nbins);
        kappacumul_ref1 << 0.09085889702, 0.9403198652, 4.278638123,
            20.78872976, 63.55033952, 94.76717674, 98.89460337, 136.9234453,
            147.6715806;
        Eigen::VectorXd ratiovector =
            kappacumul_byMFP.array() / kappacumul_ref1.array();

        for (int nbin = 0; nbin < Nbins; nbin++) {
            EXPECT_NEAR(ratiovector(nbin), 1.0, 5e-3);
        }
    }
}

TEST(analytic1D_basicprop_case, kappacumul_RT_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    // obtain phonon data from HDF5 file
    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto processes = std::move(std::get<4>(hdf5_data));

    double T = 300.0;

    Eigen::ArrayXXd w(alma::calc_w0_threeph(*grid, *processes, T, world));

    // create transport direction
    Eigen::Vector3d u100(1.0, 0.0, 0.0);

    if (my_id == 0) { // analytic1D code is not parallellised; avoid
                      // running duplicates.
        alma::analytic1D::BasicProperties_calculator propCalc(
            poscar.get(), grid.get(), &w, T);
        propCalc.setDirection(u100);

        // test cumulative conductivity curves
        int Nbins = 9;

        propCalc.setLogMFPbins(1e-9, 1e-5, Nbins);
        propCalc.setLogRTbins(1e-12, 1e-9, Nbins);

        propCalc.resolveByRT();
        Eigen::VectorXd kappacumul_byRT = propCalc.getCumulativeConductivity();
        Eigen::VectorXd kappacumul_ref2(Nbins);
        kappacumul_ref2 << 0.4831467689, 3.143165535, 7.665954394, 32.35090534,
            75.82629437, 90.79953246, 98.89460337, 125.379803, 147.6715806;
        Eigen::VectorXd ratiovector =
            kappacumul_byRT.array() / kappacumul_ref2.array();

        for (int nbin = 0; nbin < Nbins; nbin++) {
            EXPECT_NEAR(ratiovector(nbin), 1.0, 5e-3);
        }
    }
}

TEST(analytic1D_basicprop_case, thinfilm_crossplane_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    // obtain phonon data from HDF5 file
    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto processes = std::move(std::get<4>(hdf5_data));

    double T = 300.0;

    Eigen::ArrayXXd w(alma::calc_w0_threeph(*grid, *processes, T, world));

    // create transport direction
    Eigen::Vector3d u100(1.0, 0.0, 0.0);

    if (my_id == 0) { // analytic1D code is not parallellised; avoid
                      // running duplicates.
        alma::analytic1D::BasicProperties_calculator propCalc(
            poscar.get(), grid.get(), &w, T);
        propCalc.setDirection(u100);

        // test thickness dependent conductivity

        Eigen::VectorXd kappa_crossplane(3);
        Eigen::VectorXd thicknesslist(3);
        thicknesslist << 5e-6, 500e-9, 50e-9;

        for (int nfilm = 0; nfilm < 3; nfilm++) {
            propCalc.setCrossPlaneFilm(thicknesslist(nfilm));
            kappa_crossplane(nfilm) = propCalc.getConductivity();
        }

        Eigen::VectorXd kappa_crossplane_ref(3);
        kappa_crossplane_ref << 121.9813832, 76.32486756, 26.74947278;

        Eigen::VectorXd ratiovector =
            kappa_crossplane.array() / kappa_crossplane_ref.array();

        for (int nfilm = 0; nfilm < 3; nfilm++) {
            EXPECT_NEAR(ratiovector(nfilm), 1.0, 5e-3);
        }
    }
}

int main(int argc, char** argv) {
    boost::mpi::environment env;

    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman
    Copyright (c) 2001-2010 Hartmut Kaiser

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Calculator example demonstrating generation of AST from which we generate
//  a simple byte code representation being interpreted by a similar virtual
//  machine.
//
//  [ JDG April 28, 2008 ]
//  [ HK May 05, 2008 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/config/warning_disable.hpp>

#include <iostream>
#include <vector>
#include <string>

#include "calc2_ast_vm.hpp"

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/fusion/include/adapt_struct.hpp>

using namespace boost::spirit;
using namespace boost::spirit::ascii;

///////////////////////////////////////////////////////////////////////////////
//  Our calculator parser grammar
///////////////////////////////////////////////////////////////////////////////
template <typename Iterator>
struct calculator
  : qi::grammar<Iterator, expression_ast(), space_type>
{
    calculator() : calculator::base_type(expression)
    {
        expression =
            term                            [_val = _1]
            >> *(   ('+' >> term            [_val += _1])
                |   ('-' >> term            [_val -= _1])
                )
            ;

        term =
            factor                          [_val = _1]
            >> *(   ('*' >> factor          [_val *= _1])
                |   ('/' >> factor          [_val /= _1])
                )
            ;

        factor =
            uint_                           [_val = _1]
            |   '(' >> expression           [_val = _1] >> ')'
            |   ('-' >> factor              [_val = neg(_1)])
            |   ('+' >> factor              [_val = pos(_1)])
            ;
    }

    qi::rule<Iterator, expression_ast(), space_type> expression, term, factor;
};

///////////////////////////////////////////////////////////////////////////////
//  The Virtual Machine
///////////////////////////////////////////////////////////////////////////////
class vmachine
{
public:
    union element {
        int code;
        char bytes[sizeof(int)];
    };

    vmachine(unsigned stackSize = 4096)
      : stack(stackSize)
      , stack_ptr(stack.begin())
    {
    }

    int top() const { return stack_ptr[-1]; };
    void execute(std::vector<element> const& code);

private:
    std::vector<int> stack;
    std::vector<int>::iterator stack_ptr;
};

void vmachine::execute(std::vector<element> const& code)
{
    std::vector<element>::const_iterator pc = code.begin();
    stack_ptr = stack.begin();

    while ((*pc).code && pc != code.end())
    {
        switch ((*pc++).code)
        {
            case op_neg:
                stack_ptr[-1] = -stack_ptr[-1];
                break;

            case op_add:
                --stack_ptr;
                stack_ptr[-1] += stack_ptr[0];
                break;

            case op_sub:
                --stack_ptr;
                stack_ptr[-1] -= stack_ptr[0];
                break;

            case op_mul:
                --stack_ptr;
                stack_ptr[-1] *= stack_ptr[0];
                break;

            case op_div:
                --stack_ptr;
                stack_ptr[-1] /= stack_ptr[0];
                break;

            case op_int:
                *stack_ptr++ = (*pc++).code;
                break;
        }
    }
}

// We need to tell fusion about our binary_op and unary_op structs
// to make them a first-class fusion citizen
//
// Note: we register the members exactly in the same sequence as we need them
//       in the grammar
BOOST_FUSION_ADAPT_STRUCT(
    binary_op,
    (expression_ast, left)
    (expression_ast, right)
    (int, op)
)

BOOST_FUSION_ADAPT_STRUCT(
    unary_op,
    (expression_ast, right)
    (int, op)
)

///////////////////////////////////////////////////////////////////////////////
//  Our AST grammar for the generator, this just dumps the AST as a expression
///////////////////////////////////////////////////////////////////////////////
template <typename OuputIterator, typename Delimiter>
struct generate_byte_code
  : karma::grammar<OuputIterator, expression_ast(), Delimiter>
{
    generate_byte_code() : generate_byte_code::base_type(ast_node)
    {
        ast_node %= int_node | binary_node | unary_node;
        int_node %= dword(op_int) << dword;
        binary_node %= ast_node << ast_node << byte_;
        unary_node %= ast_node << byte_;
    }

    karma::rule<OuputIterator, expression_ast(), Delimiter> ast_node;
    karma::rule<OuputIterator, int(), Delimiter> int_node;
    karma::rule<OuputIterator, binary_op(), Delimiter> binary_node;
    karma::rule<OuputIterator, unary_op(), Delimiter> unary_node;
};

///////////////////////////////////////////////////////////////////////////////
// helper function helping to deduce the delimiter type
template <typename Delimiter>
bool generate_vm_code(expression_ast const& ast,
    std::vector<vmachine::element>& code, Delimiter const& d)
{
    // Our generator grammar definitions
    typedef char* output_iterator_type;
    typedef generate_byte_code<output_iterator_type, Delimiter> generate_byte_code;

    char* outbuffer = (*code.begin()).bytes;
    generate_byte_code gen_vm;
    return karma::generate_delimited(outbuffer, gen_vm, d, ast);
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Compile simple expressions to bytecode...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type an expression...or [q or Q] to quit\n\n";

    //  Our parser grammar definitions
    typedef std::string::const_iterator iterator_type;
    typedef calculator<iterator_type> calculator;

    calculator calc;

    std::string str;
    while (std::getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        expression_ast ast;
        std::string::const_iterator iter = str.begin();
        std::string::const_iterator end = str.end();
        bool r = qi::phrase_parse(iter, end, calc, space, ast);

        if (r && iter == end)
        {
            // we assume a vm code size of 4096 is sufficient
            std::vector<vmachine::element> code (4096);
            r = generate_vm_code(ast, code, pad(4));

            if (r)
            {
                vmachine vm;
                vm.execute(code);
                std::cout << "\nresult = " << vm.top() << std::endl;
                std::cout << "-------------------------\n";
            }
            else
            {
                std::cout << "-------------------------\n";
                std::cout << "Generating failed\n";
                std::cout << "-------------------------\n";
            }
        }
        else
        {
            std::string rest(iter, end);
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "stopped at: \": " << rest << "\"\n";
            std::cout << "-------------------------\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


#include <elemental.hpp>
#include <skylark.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <cstdlib>
#include <string>
#include <boost/mpi.hpp>
#include <boost/program_options.hpp>
#include <boost/any.hpp>
#include "kernels.hpp"
#include "hilbert.hpp"
#include <omp.h>
#include "../base/context.hpp"

namespace bmpi =  boost::mpi;
namespace po = boost::program_options;

typedef skylark::base::sparse_matrix_t<double> sparse_matrix_t;

int main (int argc, char** argv) {
    /* Various MPI/Skylark/Elemental/OpenMP initializations */

	std::cout << "Running skylark_ml" << std::endl;

    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    bmpi::environment env (argc, argv);
    boost::mpi::communicator comm;

    hilbert_options_t options (argc, argv, comm.size());
    skylark::base::context_t context (options.seed);

    elem::Initialize (argc, argv);

    if (options.exit_on_return) { return -1; }
    if (comm.rank() == 0)
        std::cout << options.print();

    bool sparse = (options.fileformat == LIBSVM_SPARSE) || (options.fileformat == HDF5_SPARSE);
    int flag = 0;

    if (sparse)
        flag = run<sparse_matrix_t, elem::Matrix<double> >(comm, context, options);
    else
        flag = run<LocalMatrixType, LocalMatrixType>(comm, context, options);

    comm.barrier();
    elem::Finalize();
    return flag;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_BERNOULLI_DETAIL_HPP_
#define BOOST_MATH_BERNOULLI_DETAIL_HPP_

#include "../math_fwd.hpp"
#include <boost/config.hpp>
#include <boost/detail/lightweight_mutex.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/math/tools/toms748_solve.hpp>

#ifdef BOOST_HAS_THREADS

#ifndef BOOST_NO_CXX11_HDR_ATOMIC
#  include <atomic>
#  define BOOST_MATH_ATOMIC_NS std
#if ATOMIC_INT_LOCK_FREE == 2
typedef std::atomic<int> atomic_counter_type;
typedef int atomic_integer_type;
#elif ATOMIC_SHORT_LOCK_FREE == 2
typedef std::atomic<short> atomic_counter_type;
typedef short atomic_integer_type;
#elif ATOMIC_LONG_LOCK_FREE == 2
typedef std::atomic<long> atomic_counter_type;
typedef long atomic_integer_type;
#elif ATOMIC_LLONG_LOCK_FREE == 2
typedef std::atomic<long long> atomic_counter_type;
typedef long long atomic_integer_type;
#else
#  define BOOST_MATH_NO_ATOMIC_INT
#endif

#else // BOOST_NO_CXX11_HDR_ATOMIC
//
// We need Boost.Atomic, but on any platform that supports auto-linking we do
// not need to link against a separate library:
//
#define BOOST_ATOMIC_NO_LIB
#include <boost/atomic.hpp>
#  define BOOST_MATH_ATOMIC_NS boost

namespace boost{ namespace math{ namespace detail{

//
// We need a type to use as an atomic counter:
//
#if BOOST_ATOMIC_INT_LOCK_FREE == 2
typedef boost::atomic<int> atomic_counter_type;
typedef int atomic_integer_type;
#elif BOOST_ATOMIC_SHORT_LOCK_FREE == 2
typedef boost::atomic<short> atomic_counter_type;
typedef short atomic_integer_type;
#elif BOOST_ATOMIC_LONG_LOCK_FREE == 2
typedef boost::atomic<long> atomic_counter_type;
typedef long atomic_integer_type;
#elif BOOST_ATOMIC_LLONG_LOCK_FREE == 2
typedef boost::atomic<long long> atomic_counter_type;
typedef long long atomic_integer_type;
#else
#  define BOOST_MATH_NO_ATOMIC_INT
#endif

}}} // namespaces

#endif  // BOOST_NO_CXX11_HDR_ATOMIC

#endif // BOOST_HAS_THREADS

namespace boost{ namespace math{ namespace detail{
//
// Asymptotic expansion for B2n due to
// Luschny LogB3 formula (http://www.luschny.de/math/primes/bernincl.html)
//
template <class T, class Policy>
T b2n_asymptotic(int n)
{
   BOOST_MATH_STD_USING
   const T nx = static_cast<T>(n);
   const T nx2(nx * nx);

   const T approximate_log_of_bernoulli_bn =
        ((boost::math::constants::half<T>() + nx) * log(nx))
        + ((boost::math::constants::half<T>() - nx) * log(boost::math::constants::pi<T>()))
        + (((T(3) / 2) - nx) * boost::math::constants::ln_two<T>())
        + ((nx * (T(2) - (nx2 * 7) * (1 + ((nx2 * 30) * ((nx2 * 12) - 1))))) / (((nx2 * nx2) * nx2) * 2520));
   return ((n / 2) & 1 ? 1 : -1) * (approximate_log_of_bernoulli_bn > tools::log_max_value<T>()
      ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, nx, Policy())
      : static_cast<T>(exp(approximate_log_of_bernoulli_bn)));
}

template <class T, class Policy>
T t2n_asymptotic(int n)
{
   BOOST_MATH_STD_USING
   // Just get B2n and convert to a Tangent number:
   T t2n = fabs(b2n_asymptotic<T, Policy>(2 * n)) / (2 * n);
   T p2 = ldexp(T(1), n);
   if(tools::max_value<T>() / p2 < t2n)
      return policies::raise_overflow_error<T>("boost::math::tangent_t2n<%1%>(std::size_t)", 0, T(n), Policy());
   t2n *= p2;
   p2 -= 1;
   if(tools::max_value<T>() / p2 < t2n)
      return policies::raise_overflow_error<T>("boost::math::tangent_t2n<%1%>(std::size_t)", 0, Policy());
   t2n *= p2;
   return t2n;
}
//
// We need to know the approximate value of /n/ which will
// cause bernoulli_b2n<T>(n) to return infinity - this allows
// us to elude a great deal of runtime checking for values below
// n, and only perform the full overflow checks when we know that we're
// getting close to the point where our calculations will overflow.
// We use Luschny's LogB3 formula (http://www.luschny.de/math/primes/bernincl.html)
// to find the limit, and since we're dealing with the log of the Bernoulli numbers
// we need only perform the calculation at double precision and not with T
// (which may be a multiprecision type).  The limit returned is within 1 of the true
// limit for all the types tested.  Note that although the code below is basically
// the same as b2n_asymptotic above, it has been recast as a continuous real-valued
// function as this makes the root finding go smoother/faster.  It also omits the
// sign of the Bernoulli number.
//
struct max_bernoulli_root_functor
{
   max_bernoulli_root_functor(long long t) : target(static_cast<double>(t)) {}
   double operator()(double n)
   {
      BOOST_MATH_STD_USING

      // Luschny LogB3(n) formula.

      const double nx2(n * n);

      const double approximate_log_of_bernoulli_bn
         =   ((boost::math::constants::half<double>() + n) * log(n))
           + ((boost::math::constants::half<double>() - n) * log(boost::math::constants::pi<double>()))
           + (((double(3) / 2) - n) * boost::math::constants::ln_two<double>())
           + ((n * (2 - (nx2 * 7) * (1 + ((nx2 * 30) * ((nx2 * 12) - 1))))) / (((nx2 * nx2) * nx2) * 2520));

      return approximate_log_of_bernoulli_bn - target;
   }
private:
   double target;
};

template <class T, class Policy>
inline std::size_t find_bernoulli_overflow_limit(const mpl::false_&)
{
   long long t = lltrunc(boost::math::tools::log_max_value<T>());
   max_bernoulli_root_functor fun(t);
   boost::math::tools::equal_floor tol;
   boost::uintmax_t max_iter = boost::math::policies::get_max_root_iterations<Policy>();
   return static_cast<std::size_t>(boost::math::tools::toms748_solve(fun, sqrt(double(t)), double(t), tol, max_iter).first) / 2;
}

template <class T, class Policy>
inline std::size_t find_bernoulli_overflow_limit(const mpl::true_&)
{
   return max_bernoulli_index<bernoulli_imp_variant<T>::value>::value;
}

template <class T, class Policy>
std::size_t b2n_overflow_limit()
{
   // This routine is called at program startup if it's called at all:
   // that guarantees safe initialization of the static variable.
   typedef mpl::bool_<(bernoulli_imp_variant<T>::value >= 1) && (bernoulli_imp_variant<T>::value <= 3)> tag_type;
   static const std::size_t lim = find_bernoulli_overflow_limit<T, Policy>(tag_type());
   return lim;
}

//
// The tangent numbers grow larger much more rapidly than the Bernoulli numbers do....
// so to compute the Bernoulli numbers from the tangent numbers, we need to avoid spurious
// overflow in the calculation, we can do this by scaling all the tangent number by some scale factor:
//
template <class T>
inline typename enable_if_c<std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::radix == 2), T>::type tangent_scale_factor()
{
   BOOST_MATH_STD_USING
   return ldexp(T(1), std::numeric_limits<T>::min_exponent + 5);
}
template <class T>
inline typename disable_if_c<std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::radix == 2), T>::type tangent_scale_factor()
{
   return tools::min_value<T>() * 16;
}
//
// Initializer: ensure all our constants are initialized prior to the first call of main:
//
template <class T, class Policy>
struct bernoulli_initializer
{
   struct init
   {
      init()
      {
         //
         // We call twice, once to initialize our static table, and once to
         // initialize our dymanic table:
         //
         boost::math::bernoulli_b2n<T>(2, Policy());
#ifndef BOOST_NO_EXCEPTIONS
         try{
#endif
            boost::math::bernoulli_b2n<T>(max_bernoulli_b2n<T>::value + 1, Policy());
#ifndef BOOST_NO_EXCEPTIONS
         } catch(const std::overflow_error&){}
#endif
         boost::math::tangent_t2n<T>(2, Policy());
      }
      void force_instantiate()const{}
   };
   static const init initializer;
   static void force_instantiate()
   {
      initializer.force_instantiate();
   }
};

template <class T, class Policy>
const typename bernoulli_initializer<T, Policy>::init bernoulli_initializer<T, Policy>::initializer;

//
// We need something to act as a cache for our calculated Bernoulli numbers.  In order to
// ensure both fast access and thread safety, we need a stable table which may be extended
// in size, but which never reallocates: that way values already calculated may be accessed
// concurrently with another thread extending the table with new values.
//
// Very very simple vector class that will never allocate more than once, we could use
// boost::container::static_vector here, but that allocates on the stack, which may well
// cause issues for the amount of memory we want in the extreme case...
//
template <class T>
struct fixed_vector : private std::allocator<T>
{
   typedef unsigned size_type;
   typedef T* iterator;
   typedef const T* const_iterator;
   fixed_vector() : m_used(0)
   {
      std::size_t overflow_limit = 5 + b2n_overflow_limit<T, policies::policy<> >();
      m_capacity = static_cast<unsigned>((std::min)(overflow_limit, static_cast<std::size_t>(100000u)));
      m_data = this->allocate(m_capacity);
   }
   ~fixed_vector()
   {
      for(unsigned i = 0; i < m_used; ++i)
         this->destroy(&m_data[i]);
      this->deallocate(m_data, m_capacity);
   }
   T& operator[](unsigned n) { BOOST_ASSERT(n < m_used); return m_data[n]; }
   const T& operator[](unsigned n)const { BOOST_ASSERT(n < m_used); return m_data[n]; }
   unsigned size()const { return m_used; }
   unsigned size() { return m_used; }
   void resize(unsigned n, const T& val)
   {
      if(n > m_capacity)
      {
         BOOST_THROW_EXCEPTION(std::runtime_error("Exhausted storage for Bernoulli numbers."));
      }
      for(unsigned i = m_used; i < n; ++i)
         new (m_data + i) T(val);
      m_used = n;
   }
   void resize(unsigned n) { resize(n, T()); }
   T* begin() { return m_data; }
   T* end() { return m_data + m_used; }
   T* begin()const { return m_data; }
   T* end()const { return m_data + m_used; }
   unsigned capacity()const { return m_capacity; }
private:
   T* m_data;
   unsigned m_used, m_capacity;
};

template <class T, class Policy>
class bernoulli_numbers_cache
{
public:
   bernoulli_numbers_cache() : m_overflow_limit((std::numeric_limits<std::size_t>::max)())
#if defined(BOOST_HAS_THREADS) && !defined(BOOST_MATH_NO_ATOMIC_INT)
      , m_counter(0)
#endif
   {}

   typedef fixed_vector<T> container_type;

   void tangent(std::size_t m)
   {
      static const std::size_t min_overflow_index = b2n_overflow_limit<T, Policy>() - 1;
      tn.resize(static_cast<typename container_type::size_type>(m), T(0U));

      BOOST_MATH_INSTRUMENT_VARIABLE(min_overflow_index);

      std::size_t prev_size = m_intermediates.size();
      m_intermediates.resize(m, T(0U));

      if(prev_size == 0)
      {
         m_intermediates[1] = tangent_scale_factor<T>() /*T(1U)*/;
         tn[0U] = T(0U);
         tn[1U] = tangent_scale_factor<T>()/* T(1U)*/;
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[0]);
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[1]);
      }

      for(std::size_t i = std::max<size_t>(2, prev_size); i < m; ++i)
      {
         bool overflow_check = false;
         if(i >= min_overflow_index && (boost::math::tools::max_value<T>() / (i-1) < m_intermediates[1]) )
         {
            std::fill(tn.begin() + i, tn.end(), boost::math::tools::max_value<T>());
            break;
         }
         m_intermediates[1] = m_intermediates[1] * (i-1);
         for(std::size_t j = 2; j <= i; ++j)
         {
            overflow_check =
                  (i >= min_overflow_index) && (
                  (boost::math::tools::max_value<T>() / (i - j) < m_intermediates[j])
                  || (boost::math::tools::max_value<T>() / (i - j + 2) < m_intermediates[j-1])
                  || (boost::math::tools::max_value<T>() - m_intermediates[j] * (i - j) < m_intermediates[j-1] * (i - j + 2))
                  || ((boost::math::isinf)(m_intermediates[j]))
                );

            if(overflow_check)
            {
               std::fill(tn.begin() + i, tn.end(), boost::math::tools::max_value<T>());
               break;
            }
            m_intermediates[j] = m_intermediates[j] * (i - j) + m_intermediates[j-1] * (i - j + 2);
         }
         if(overflow_check)
            break; // already filled the tn...
         tn[static_cast<typename container_type::size_type>(i)] = m_intermediates[i];
         BOOST_MATH_INSTRUMENT_VARIABLE(i);
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[static_cast<typename container_type::size_type>(i)]);
      }
   }

   void tangent_numbers_series(const std::size_t m)
   {
      BOOST_MATH_STD_USING
      static const std::size_t min_overflow_index = b2n_overflow_limit<T, Policy>() - 1;

      typename container_type::size_type old_size = bn.size();

      tangent(m);
      bn.resize(static_cast<typename container_type::size_type>(m));

      if(!old_size)
      {
         bn[0] = 1;
         old_size = 1;
      }

      T power_two(ldexp(T(1), static_cast<int>(2 * old_size)));

      for(std::size_t i = old_size; i < m; ++i)
      {
         T b(static_cast<T>(i * 2));
         //
         // Not only do we need to take care to avoid spurious over/under flow in
         // the calculation, but we also need to avoid overflow altogether in case
         // we're calculating with a type where "bad things" happen in that case:
         //
         b  = b / (power_two * tangent_scale_factor<T>());
         b /= (power_two - 1);
         bool overflow_check = (i >= min_overflow_index) && (tools::max_value<T>() / tn[static_cast<typename container_type::size_type>(i)] < b);
         if(overflow_check)
         {
            m_overflow_limit = i;
            while(i < m)
            {
               b = std::numeric_limits<T>::has_infinity ? std::numeric_limits<T>::infinity() : tools::max_value<T>();
               bn[static_cast<typename container_type::size_type>(i)] = ((i % 2U) ? b : T(-b));
               ++i;
            }
            break;
         }
         else
         {
            b *= tn[static_cast<typename container_type::size_type>(i)];
         }

         power_two = ldexp(power_two, 2);

         const bool b_neg = i % 2 == 0;

         bn[static_cast<typename container_type::size_type>(i)] = ((!b_neg) ? b : T(-b));
      }
   }

   template <class OutputIterator>
   OutputIterator copy_bernoulli_numbers(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol)
   {
      //
      // There are basically 3 thread safety options:
      //
      // 1) There are no threads (BOOST_HAS_THREADS is not defined).
      // 2) There are threads, but we do not have a true atomic integer type,
      //    in this case we just use a mutex to guard against race conditions.
      // 3) There are threads, and we have an atomic integer: in this case we can
      //    use the double-checked locking pattern to avoid thread synchronisation
      //    when accessing values already in the cache.
      //
      // First off handle the common case for overflow and/or asymptotic expansion:
      //
      if(start + n > bn.capacity())
      {
         if(start < bn.capacity())
         {
            out = copy_bernoulli_numbers(out, start, bn.capacity() - start, pol);
            n -= bn.capacity() - start;
            start = static_cast<std::size_t>(bn.capacity());
         }
         if(start < b2n_overflow_limit<T, Policy>() + 2u)
         {
            for(; n; ++start, --n)
            {
               *out = b2n_asymptotic<T, Policy>(static_cast<typename container_type::size_type>(start * 2U));
               ++out;
            }
         }
         for(; n; ++start, --n)
         {
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(start), pol);
            ++out;
         }
         return out;
      }
   #if !defined(BOOST_HAS_THREADS)
      //
      // Single threaded code, very simple:
      //
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = (std::max)(std::size_t(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[i];
         ++out;
      }
   #elif defined(BOOST_MATH_NO_ATOMIC_INT)
      //
      // We need to grab a mutex every time we get here, for both readers and writers:
      //
      boost::detail::lightweight_mutex::scoped_lock l(m_mutex);
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = (std::max)(std::size_t(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[i];
         ++out;
      }

   #else
      //
      // Double-checked locking pattern, lets us access cached already cached values
      // without locking:
      //
      // Get the counter and see if we need to calculate more constants:
      //
      if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
      {
         boost::detail::lightweight_mutex::scoped_lock l(m_mutex);

         if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
         {
            if(start + n >= bn.size())
            {
               std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
               tangent_numbers_series(new_size);
            }
            m_counter.store(static_cast<atomic_integer_type>(bn.size()), BOOST_MATH_ATOMIC_NS::memory_order_release);
         }
      }

      for(std::size_t i = (std::max)(static_cast<std::size_t>(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[static_cast<typename container_type::size_type>(i)];
         ++out;
      }

   #endif
      return out;
   }

   template <class OutputIterator>
   OutputIterator copy_tangent_numbers(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol)
   {
      //
      // There are basically 3 thread safety options:
      //
      // 1) There are no threads (BOOST_HAS_THREADS is not defined).
      // 2) There are threads, but we do not have a true atomic integer type,
      //    in this case we just use a mutex to guard against race conditions.
      // 3) There are threads, and we have an atomic integer: in this case we can
      //    use the double-checked locking pattern to avoid thread synchronisation
      //    when accessing values already in the cache.
      //
      //
      // First off handle the common case for overflow and/or asymptotic expansion:
      //
      if(start + n > bn.capacity())
      {
         if(start < bn.capacity())
         {
            out = copy_tangent_numbers(out, start, bn.capacity() - start, pol);
            n -= bn.capacity() - start;
            start = static_cast<std::size_t>(bn.capacity());
         }
         if(start < b2n_overflow_limit<T, Policy>() + 2u)
         {
            for(; n; ++start, --n)
            {
               *out = t2n_asymptotic<T, Policy>(static_cast<typename container_type::size_type>(start));
               ++out;
            }
         }
         for(; n; ++start, --n)
         {
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(start), pol);
            ++out;
         }
         return out;
      }
   #if !defined(BOOST_HAS_THREADS)
      //
      // Single threaded code, very simple:
      //
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }
   #elif defined(BOOST_MATH_NO_ATOMIC_INT)
      //
      // We need to grab a mutex every time we get here, for both readers and writers:
      //
      boost::detail::lightweight_mutex::scoped_lock l(m_mutex);
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }

   #else
      //
      // Double-checked locking pattern, lets us access cached already cached values
      // without locking:
      //
      // Get the counter and see if we need to calculate more constants:
      //
      if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
      {
         boost::detail::lightweight_mutex::scoped_lock l(m_mutex);

         if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
         {
            if(start + n >= bn.size())
            {
               std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
               tangent_numbers_series(new_size);
            }
            m_counter.store(static_cast<atomic_integer_type>(bn.size()), BOOST_MATH_ATOMIC_NS::memory_order_release);
         }
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }

   #endif
      return out;
   }

private:
   //
   // The caches for Bernoulli and tangent numbers, once allocated,
   // these must NEVER EVER reallocate as it breaks our thread
   // safety guarentees:
   //
   fixed_vector<T> bn, tn;
   std::vector<T> m_intermediates;
   // The value at which we know overflow has already occurred for the Bn:
   std::size_t m_overflow_limit;
#if !defined(BOOST_HAS_THREADS)
#elif defined(BOOST_MATH_NO_ATOMIC_INT)
   boost::detail::lightweight_mutex m_mutex;
#else
   boost::detail::lightweight_mutex m_mutex;
   atomic_counter_type m_counter;
#endif
};

template <class T, class Policy>
inline bernoulli_numbers_cache<T, Policy>& get_bernoulli_numbers_cache()
{
   //
   // Force this function to be called at program startup so all the static variables
   // get initailzed then (thread safety).
   //
   bernoulli_initializer<T, Policy>::force_instantiate();
   static bernoulli_numbers_cache<T, Policy> data;
   return data;
}

}}}

#endif // BOOST_MATH_BERNOULLI_DETAIL_HPP


#pragma once

#include <route_solver/structs/data_structs.hpp>
#include <route_solver/solution/fail_statistics.hpp>
#include <Eigen/Core>

namespace rs {

struct PointStat
{
	PointStat(
		std::uint32_t nodeId,
		std::size_t time,
		double workload,
		double volume,
		double distance,
		int tasksCount
	);

	std::uint32_t nodeId;
	std::size_t time;
	double workload;
	double volume;
	double distance;
	int tasksCount;
};

class CurrentRouteState {
public:

	CurrentRouteState(
		const Vehicle& vehicle,
		const Eigen::MatrixXd& destTimeTable,
		const Eigen::MatrixXd& destTable,
		FailedStatisticsCollector& failsCollector,
		std::vector<PointStat>* stats = 0);

	void moveNextStart(std::int32_t nextNode, const Task& task);

	bool processLoad(const Task& task);

	bool processUnload(const Task& task);

	bool moveNextEnd(std::int32_t nextNode, const Task& task);

	std::size_t totalTimeSpend() const;

	double totalDistance() const;

	double totalCostOfRoute() const;

	void pushStatsIfNeeded(double nodeDistance);

private:
	const Vehicle& m_vehicle;
	const Eigen::MatrixXd& m_destTimeTable;
	const Eigen::MatrixXd& m_destTable;

	std::size_t m_currentTime = 0;
	double m_currentDistanceMeters = 0;
	std::size_t m_routeStartTime = 0;
	double m_currentLoad = 0;
	double m_currentVolume = 0;

	std::uint32_t m_taskTaken = 0;
	std::int32_t m_currentNode = -1;
	std::vector<PointStat>* m_stats;
	FailedStatisticsCollector& m_failsCollector;
};

}


//
// Created by sean on 10/07/16.
//

#ifndef PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP
#define PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP

#include <Eigen/Core>

#include "pcltools/common.hpp"

using namespace pcltools::literals;

namespace pcltools {

namespace linalg {

/**
 * Converts a 4x4 rigid transformation matrix to a quaternion and translation vector pair
 * @param transformation  A 4x4 matrix representing a rigid transformation.
 * @return                A quaternion and vector pair representing the passed transformation.
 */
template <typename Scalar>
auto getQuaternionAndTranslation (Eigen::Matrix <Scalar, 4, 4> const & transformation)
-> std::pair <Eigen::Quaternion <Scalar>, Eigen::Matrix <Scalar, 3, 1>> {
  auto rotation_matrix =
  Eigen::Matrix <Scalar, 3, 3> {transformation.block (0, 0, 3, 3)};
  auto translation_vector =
      static_cast <Eigen::Matrix <Scalar, 3, 1>> (transformation.block (0, 3, 3, 1));
  return std::make_pair (Eigen::Quaternion <Scalar> (rotation_matrix), translation_vector);
}

/**
 * Converts a quaternion and translation vector representing a rigid transformation to 4x4 matrix
 * @param quaternion  The quaternion representing the rigid rotation.
 * @param translation The translation represented as a vector (a 3x1 matrix).
 * @return            The 4x4 matrix that represents a rigid body transformation.
 */
template <typename Scalar>
auto getTransformationMatrix (Eigen::Quaternion <Scalar> const & quaternion,
                              Eigen::Matrix <Scalar, 3, 1> const & translation)
-> Eigen::Matrix <Scalar, 4, 4> {
  auto rotation = Eigen::Matrix <Scalar, 3, 3> {quaternion.toRotationMatrix ()};
  auto transformation = Eigen::Matrix <Scalar, 4, 4> {Eigen::Matrix <Scalar, 4, 4>::Identity ()};

  for (auto col = 0_sz; col < rotation.cols (); ++col)
    for (auto row = 0_sz; row < rotation.rows (); ++row)
      transformation (row, col) = rotation (row, col);

  for (auto index = 0_sz; index < translation.size (); ++index)
    transformation (index, transformation.cols () - 1) = translation (index);

  return transformation;
}

}
}

#endif //PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP


// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/mp11/function.hpp>
#include <boost/mp11/utility.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>

int main()
{
    using boost::mp11::mp_nth_element_q;
    using boost::mp11::mp_list_c;
    using boost::mp11::mp_sort_q;
    using boost::mp11::mp_less;
    using boost::mp11::mp_at_c;
    using boost::mp11::mp_size_t;
    using boost::mp11::mp_rename;
    using boost::mp11::mp_quote;

    using Q_less = mp_quote<mp_less>;

    {
        using L1 = mp_list_c<int, 7, 1, 11, 3, 2, 2, 4>;
        using L2 = mp_sort_q<L1, Q_less>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<0>, Q_less>, mp_at_c<L2, 0>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<1>, Q_less>, mp_at_c<L2, 1>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<2>, Q_less>, mp_at_c<L2, 2>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<3>, Q_less>, mp_at_c<L2, 3>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<4>, Q_less>, mp_at_c<L2, 4>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<5>, Q_less>, mp_at_c<L2, 5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<6>, Q_less>, mp_at_c<L2, 6>>));
    }

    {
        using L1 = mp_rename<mp_list_c<int, 7, 1, 11, 3, 2, 2, 4>, std::tuple>;
        using L2 = mp_sort_q<L1, Q_less>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<0>, Q_less>, mp_at_c<L2, 0>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<1>, Q_less>, mp_at_c<L2, 1>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<2>, Q_less>, mp_at_c<L2, 2>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<3>, Q_less>, mp_at_c<L2, 3>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<4>, Q_less>, mp_at_c<L2, 4>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<5>, Q_less>, mp_at_c<L2, 5>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_nth_element_q<L1, mp_size_t<6>, Q_less>, mp_at_c<L2, 6>>));
    }

    return boost::report_errors();
}


/*

Project: basic icp
Date: 20/08/04
@Author: Yang, Wang.
Detail: align numberous pointclouds
Scenario: VO

*/

#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/features/normal_3d.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/transformation_estimation_lm.h>

#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sys/types.h>
#include <string.h>
#include <dirent.h>
#include <stdio.h>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormal;
//typedef Eigen::Matrix<double, 4, 4> Matrix4f;

//#pragma pack(1)

struct PCDMap
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    int index =0;
    PointCloud::Ptr cloud;
    Eigen::Matrix4f global_transform;

    //Eigen::Vector3d move;
    //Eigen::Quaterniond q;

    //Eigen::Vector3d move_after_g2o;
    //Eigen::Quaterniond q_after_g2o;
    bool doing_g2o = false;
    PCDMap():cloud(new PointCloud){};
};

//uniform sampling func
//data acquired: Cloud's ptr for processing.
void uniform_sampling (PointCloud::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	//cout << "原始点云个数：" << cloud->points.size() << endl;
	pcl::UniformSampling<pcl::PointXYZ> US;
   	US.setInputCloud(cloud);
    	US.setRadiusSearch(0.2f);
    	US.filter(*cloud_filtered);
	*cloud = *cloud_filtered;
    	//cout << "均匀采样之后点云的个数：" << cloud->points.size() << endl;
}

//remove singel func
//data acquired: Cloud's ptr for processing.
void remove_single (PointCloud::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        //cout << "原始点云个数：" << cloud->points.size() << endl;
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> RS;
	RS.setInputCloud(cloud);
	RS.setRadiusSearch(0.5);
	RS.setMinNeighborsInRadius(4);
	RS.filter(*cloud_filtered);
        *cloud = *cloud_filtered;
	//cout << "除去离群点后点云的个数：" << cloud->points.size() << endl;
}

//another new filter to remove the point faraway from the origin
//added on 7/30 by Wang
//data acquired: pcl::PointCloud<PointNormal> PointCloudN
void remove_distant (PointCloud::Ptr cloud)
{
    PointCloud::Ptr cloud_filtered(new PointCloud);
    //cout << "Cloud points number is:" << cloud->points.size() << endl;
    pcl::StatisticalOutlierRemoval<PointT> RD;
    RD.setInputCloud(cloud);
    RD.setMeanK(50);
    RD.setStddevMulThresh(1.0);
    RD.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
    //cout << "Cloud points number after remove distant:" << cloud->points.size() << endl;
}

//icp point align algorithm
//data acquired: 1 & 2 is the Cloud's ptr for aligning, 3 is the Cloud's ptr for output, 4 is transformation matrix, 5 is unclear.
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f&final_transform, int n)
{
	int iterations = 100;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_src);
        icp.setInputTarget(cloud_tgt);
        icp.setTransformationEpsilon(1e-15);
	icp.setMaxCorrespondenceDistance(2);
        icp.setEuclideanFitnessEpsilon(0.001);
        icp.setMaximumIterations(iterations);

	icp.align(*output);
	//cout<<"\nThe "<<n<< " frame and the "<<n+1<<" frame have converged and score is "<<icp.getFitnessScore()<<endl;
	//cout<<"Transformation is\n"<<icp.getFinalTransformation()<<endl;
	final_transform = icp.getFinalTransformation();
}

//.pcd viewer related
//data acquired: Cloud's ptr for display
void showPCD (const PointCloud::Ptr clouddata)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer_final (new pcl::visualization::PCLVisualizer("result1"));
        viewer_final->setBackgroundColor(0, 0, 0);

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>clouddatacolor (clouddata, 0, 255, 0);
        viewer_final->addPointCloud<pcl::PointXYZ> (clouddata, clouddatacolor, "cloud data");
        viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud data");
	while (!viewer_final->wasStopped())
        {
                viewer_final->spinOnce(100);
	}

}

int main(int argc, char** argv)
{
    vector<PCDMap> data;
    vector<PCDMap> secdata;
    vector<PCDMap> thidata;
    vector<PCDMap> findata;

    string data_c1 = argv[1];
    string data_c2 = argv[2];
    //string data_c3 = argv[3];
    int data_s = stoi(data_c1);
    int data_e = stoi(data_c2);
    //int data_num = stoi(data_c3);
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int i = data_s; i < data_e; i=i+2)
    {
        char namebuf[20];
        sprintf(namebuf, "../%d.pcd", i);
        cout<<namebuf<<endl;
        PCDMap m;
        pcl::io::loadPCDFile (namebuf, *m.cloud); // set the struct.cloud
        m.index = i;                              // set the struct.index
        //m.afterg2o = Eigen::Matrix4d::Identity();
        data.push_back(m);
    }

    PointCloud::Ptr source, target;
    int j = 0;
    int l = 0;
    int n = 0;

    //refinied 20/08/07
    //the first loop of align, for pre-process
    cout<<data.size()<<endl;
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
    for(size_t i = 0; i<data.size()-6; i=i+5)
    {
        target = data[i].cloud;
        uniform_sampling(target);
        remove_single(target);

        PointCloud::Ptr mid (new PointCloud);
        for(int fif = 1; fif < 6; fif++)
        {
            source = data[i+fif].cloud;
            uniform_sampling(source);
            remove_single(source);

            PointCloud::Ptr temp (new PointCloud);
            pairAlign(source, target, temp, pairTransform, i);
            pcl::transformPointCloud(*source, *mid, pairTransform);
            uniform_sampling(target);
            remove_single(target);
            remove_distant(target);
            *target = *target + *mid;
        }
        cout<<"1st: "<< j <<endl;
        GlobalTransform = pairTransform*GlobalTransform;
        PointCloud::Ptr target_1st (new PointCloud);
        pcl::transformPointCloud(*target, *target_1st, GlobalTransform);
        j++;
        PCDMap m_1st;
        *m_1st.cloud = *target_1st;
        m_1st.index = j;
        m_1st.global_transform = GlobalTransform;
        secdata.push_back(m_1st);
    }

    //the second loop of align, for the keyframe
    cout<<secdata.size()<<endl;
    PointCloud::Ptr source2, target2;
    Eigen::Matrix4f GlobalTransform2 = Eigen::Matrix4f::Identity (), pairTransform2;
    for(size_t k = 0; k< secdata.size()-4; k=k+3)
    {
        target2 = secdata[k].cloud;
        PointCloud::Ptr mid2 (new PointCloud);
        for(int fif = 1; fif < 4; fif++)
        {
            source2 = secdata[k+fif].cloud;
            PointCloud::Ptr temp (new PointCloud);
            pairAlign(source2, target2, temp, pairTransform2, k);
            pcl::transformPointCloud(*source2, *mid2, pairTransform2);
            *target2 = *target2 + *mid2;
            uniform_sampling(target2);
            remove_single(target2);

        }
        cout<<"2nd: "<< l <<endl;
        GlobalTransform2 = pairTransform2*GlobalTransform2;
        PointCloud::Ptr target_2nd (new PointCloud);
        pcl::transformPointCloud(*target2, *target_2nd, GlobalTransform2);
        l++;
        PCDMap m_2nd;
        *m_2nd.cloud = *target_2nd;
        m_2nd.index = l;
        m_2nd.global_transform = GlobalTransform2;
        thidata.push_back(m_2nd);
    }

    //the third loop of align, for the result
    cout<<thidata.size()<<endl;
    PointCloud::Ptr source3, target3;
    Eigen::Matrix4f GlobalTransform3 = Eigen::Matrix4f::Identity (), pairTransform3;
    PointCloud::Ptr result (new PointCloud);
    result = thidata[0].cloud;
    for(size_t m = 1; m < thidata.size();m++)
    {
        target3 = thidata[m-1].cloud;
        source3 = thidata[m].cloud;
        PointCloud::Ptr temp (new PointCloud);
        PointCloud::Ptr mid3 (new PointCloud);
        pairAlign(source3, target3, temp, pairTransform3, m);
        GlobalTransform3 = pairTransform3*GlobalTransform3;
        pcl::transformPointCloud(*source3, *mid3, GlobalTransform3);
        *result = *result + *mid3;

        PCDMap m_fin;
        m_fin.index = n;
        m_fin.global_transform = GlobalTransform3;
        findata.push_back(m_fin);

    }

    //get the estimated global transform of the No. num keyframe
    //int num = data_num;
    for(size_t o = 0; o < secdata.size()-5;o++)
    {
        Eigen::Matrix4f GlobalTransform_real = Eigen::Matrix4f::Identity ();
        cout << "We would like to have the global transform of the No. " << o << " frame" << endl;
        GlobalTransform_real = secdata[o].global_transform * thidata[o/3].global_transform * findata[o/3].global_transform;
        cout << GlobalTransform_real << endl;
    }

    uniform_sampling(result);
    remove_single(result);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
    cout << "time cost is " << time_used.count() << " seconds." << endl;
    pcl::io::savePCDFileASCII ("../test.pcd", *result);
    showPCD(result);

}