#define _CRT_SECURE_NO_WARNINGS
#include <boost/algorithm/string.hpp>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <vector>
#include <array>
#include <cmath>
using namespace std;

bool MergeFV(const vector<string>& inFiles, const vector<double>& inWeights_, const string& outFile) {
	vector<double> inWeights = inWeights_;
	if (inWeights.empty())
		for (size_t i = 0; i < inFiles.size(); i++)
			inWeights.push_back(1.0 / inFiles.size());

	vector<unique_ptr<ifstream>> ifsList;
	for (const auto& path : inFiles)
		ifsList.emplace_back(new ifstream(path, ios_base::binary));
	if (!all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::good, placeholders::_1))) {
		cerr << "エラー: 入力ファイルの読み込みに失敗: " << strerror(errno) << endl;
		return false;
	}
	ofstream ofs(outFile, ios_base::binary);
	if (!ofs) {
		cerr << "エラー: 出力ファイルの読み込みに失敗: " << strerror(errno) << endl;
		return false;
	}

	cout << "マージ中 . . ." << endl;

	struct Stat
	{
		double sumDiff = 0, sumAbsDiff = 0, sumSqDiff = 0;
		int diffMin = +65536, diffMax = -65536;
	};
	vector<Stat> stats(inFiles.size());
	vector<int16_t> values(inFiles.size());

	size_t elementCount = 0;
	for (; ; elementCount++) {
		for (size_t i = 0; i < inFiles.size(); i++)
			ifsList[i]->read((char*)&values[i], sizeof values[i]);
		if (all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::eof, placeholders::_1)))
			break;
		if (any_of(ifsList.begin(), ifsList.end(), bind(&ifstream::eof, placeholders::_1))) {
			cerr << "エラー: 入力ファイルのサイズ不一致" << endl;
			return false;
		}
		if (!all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::good, placeholders::_1))) {
			cerr << "エラー: 読み込み失敗: " << strerror(errno) << endl;
			return false;
		}

		double result = 0;
		for (size_t i = 0; i < values.size(); i++)
			result += values[i] * inWeights[i];
		int16_t result16 =
			INT16_MAX <= result ? INT16_MAX :
			result <= -INT16_MAX ? INT16_MAX :
			(int16_t)round(result);

		// 入力と結果の差異について色々統計情報を算出する
		for (size_t i = 0; i < values.size(); i++) {
			auto d = (int)result16 - (int)values[i];
			stats[i].sumDiff += d;
			stats[i].sumAbsDiff += abs(d);
			stats[i].sumSqDiff += d * d;
			if (d < stats[i].diffMin) stats[i].diffMin = d;
			if (stats[i].diffMax < d) stats[i].diffMax = d;
		}

		// 出力
		ofs.write((const char*)&result16, sizeof result16);
	}

	cout << fixed;
	cout << "入力ファイル:" << endl;
	for (auto& path : inFiles)
		cout << "    " << path << endl;
	cout << "出力ファイル:  " << outFile << endl;
	cout << "要素数:        " << elementCount << endl;
	cout << "差の平均:              ";
	for (auto& stat : stats)
		cout << " " << setw(10) << setprecision(3) << stat.sumDiff / elementCount;
	cout << endl;
	cout << "差の絶対値の平均:      ";
	for (auto& stat : stats)
		cout << " " << setw(10) << setprecision(1) << stat.sumAbsDiff / elementCount;
	cout << endl;
	cout << "差の二乗平均平方根:    ";
	for (auto& stat : stats)
		cout << " " << setw(10) << setprecision(1) << sqrt(stat.sumSqDiff / elementCount);
	cout << endl;
	cout << "差の最大値:            ";
	for (auto& stat : stats)
		cout << " " << setw(10) << stat.diffMin;
	cout << endl;
	cout << "差の最小値:            ";
	for (auto& stat : stats)
		cout << " " << setw(10) << stat.diffMax;
	cout << endl;
	cout << "マージ完了" << endl;
	return true;
}

void Usage() {
	cerr << "usage: MergeFV [options] InFile1 [InFile2 ...] OutFile" << endl;
	cerr << endl;
	cerr << "複数の重みファイル(Bonanzaのfv.binのような2バイト符号付整数値が" << endl;
	cerr << "連続で記録されたバイナリファイル)の重み付き加算を行います。" << endl;
	cerr << endl;
	cerr << "    InFile     入力元のファイルパス" << endl;
	cerr << "    OutFile    出力先のファイルパス" << endl;
	cerr << endl;
	cerr << "    -w 重み     各入力の重みを実数のカンマ区切りで指定する。省略時は1.0/入力ファイル数になる。(通常の算術平均)" << endl;
	cerr << endl;
	cerr << "  例：MergeFV -w 0.3,0.7 FV1.bin FV2.bin result.bin" << endl;
	cerr << "    ⇒ FV1.binの各要素の値に0.3を掛けたものと" << endl;
	cerr << "       FV2.binの各要素の値に0.7を掛けたものの和(端数は四捨五入)を、" << endl;
	cerr << "       result.binに出力。" << endl;
}

int main(int argc, char* argv[]) {
	vector<string> files;
	vector<double> inWeights;
	bool hasInWeights = false;
	{
		vector<string> args(argv + 1, argv + argc);
		for (size_t i = 0; i < args.size(); i++) {
			if (args[i] == "-w") {
				i++;
				if (args.size() <= i) {
					cerr << "不正なオプション: " << args[i - 1] << endl;
					Usage();
					return 1;
				}
				hasInWeights = true;
				vector<string> sp;
				boost::split(sp, args[i], boost::is_any_of(","));
				for (auto s : sp)
					inWeights.push_back(stod(s));
			} else {
				files.push_back(args[i]);
			}
		}
		if (files.size() < 2) {
			cerr << "不正なオプション: 入力ファイルと出力ファイルが未指定" << endl;
			Usage();
			return 1;
		}
		if (hasInWeights && inWeights.size() != files.size() - 1) {
			cerr << "不正なオプション: 入力ファイル数と重み(-w)の数が不一致" << endl;
			Usage();
			return 1;
		}
	}
	string outFile = files.back();
	files.pop_back();

	// 処理
	return MergeFV(files, inWeights, outFile) ? 0 : 2;
}


#define SUCCESS 1
#define ERROR_CLI -1
#define ERROR_PTCLOUD_LOAD -2

#define PT_SIZE 2

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

using namespace std;

void printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options]\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-h             this help\n"
            << "-p filename    Point Cloud P [*.pcd]\n"
            << "-q filename    Point Cloud Q [*.pcd]\n"
            << "-t filename    Transformation Matrix (optional)\n"
            << "-i             Invert transormation matrix before applying it\n"
            << "\n\n";
}

int main (int argc, char** argv){
  bool transform(false);
  bool inv_T(false);
  std::string ptCloudP_filename, ptCloudQ_filename, trans_filename;
  Eigen::Matrix3f R;
  Eigen::Vector3f t;

  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (argc < 3 || pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return ERROR_CLI;
  }
  if (pcl::console::find_argument (argc, argv, "-p") >= 0){
    pcl::console::parse_argument (argc, argv, "-p", ptCloudP_filename);
  } else {
    printUsage(argv[0]);
    return ERROR_CLI;
  }
  if (pcl::console::find_argument (argc, argv, "-q") >= 0){
    pcl::console::parse_argument (argc, argv, "-q", ptCloudQ_filename);
  } else {
    printUsage(argv[0]);
    return ERROR_CLI;
  }
  if (pcl::console::find_argument (argc, argv, "-t") >= 0){
    transform = true;
    pcl::console::parse_argument (argc, argv, "-t", trans_filename);
  }
  if (pcl::console::find_argument (argc, argv, "-i") >= 0){
    inv_T = true;
  }

  // -------------------------------------
  // ----------Load point clouds----------
  // -------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr ptCloud_P (new pcl::PointCloud<pcl::PointXYZ> ());
  if (pcl::io::loadPCDFile (ptCloudP_filename, *ptCloud_P) < 0)  {
    cerr << "Error loading point cloud " << ptCloudP_filename << endl << endl;
    return ERROR_PTCLOUD_LOAD;
  } else {
    cout << "Point cloud " << ptCloudP_filename << " successfully loaded, "<< ptCloud_P->width << " pts.";
    cout << "\tColor: Green" << endl;
  }
  pcl::PointCloud<pcl::PointXYZ>::Ptr ptCloud_Q (new pcl::PointCloud<pcl::PointXYZ> ());
  if (pcl::io::loadPCDFile (ptCloudQ_filename, *ptCloud_Q) < 0)  {
    cerr << "Error loading point cloud " << ptCloudQ_filename << endl << endl;
    return ERROR_PTCLOUD_LOAD;
  } else {
    cout << "Point cloud " << ptCloudQ_filename << " successfully loaded, "<< ptCloud_P->width << " pts.";
    cout << "\tColor: Red" << endl;
  }

  // -------------------------------------
  // --------Transform pt. cloud Q--------
  // -------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr Q_transformed (new pcl::PointCloud<pcl::PointXYZ> ());
  if(transform){
    cout<< "Transformation matrix "<<trans_filename<<endl;
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
    ifstream f(trans_filename);
    for (int i = 0; i < 4; i++){
      cout<< "\t";
      for (int j = 0; j < 4; j++){
        f >> T(i, j);
        cout << T(i, j)<<" ";
      }
      cout<<endl;
    }
    f.close();

    if(inv_T){ //if transformation inversion has been requested
      R = T.block<3, 3>(0, 0);
      t = T.block<3, 1>(0, 3);
      T.block<3, 3>(0, 0) = R.transpose();
      T.block<3, 1>(0, 3) = -R.transpose()*t;
      cout << "Inverse matrix: " << endl << T << endl;
    }

    pcl::transformPointCloud (*ptCloud_Q, *Q_transformed, T);
  }

  // -------------------------------------
  // --------------Visualize--------------
  // -------------------------------------
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_P(ptCloud_P, 0, 255, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_Q(ptCloud_Q, 255, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (ptCloud_P, color_P, "PtCloud_P");
  if(transform)
    viewer->addPointCloud<pcl::PointXYZ> (Q_transformed, color_Q, "PtCloud_Q");
  else
    viewer->addPointCloud<pcl::PointXYZ> (ptCloud_Q, color_Q, "PtCloud_Q");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, PT_SIZE, "PtCloud_P");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, PT_SIZE, "PtCloud_Q");
  //viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();

  while (!viewer->wasStopped())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return SUCCESS;
}


#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <cassert>
#include <unordered_map>
#include <boost/algorithm/string.hpp>

using namespace std;

namespace kissToBlif
{

typedef unordered_map<string, unsigned> StateMap;

void tokenize(string x, vector<string>& toks, const unsigned& size)
{
    toks.clear();
    boost::erase_all(x, "\n");  boost::erase_all(x, "\r");
    boost::split(toks, x, boost::is_any_of(" \t"));
    toks.erase(remove_if(toks.begin(), toks.end(), [](const string& s){ return s.empty(); }), toks.end());
    assert(toks.size() == size);
}

unsigned getStateIdx(const string &name, StateMap &stateMap)
{
    if(stateMap.count(name) > 0)
        return stateMap[name];
    unsigned ret = stateMap.size();
    stateMap[name] = ret;
    return ret;
}

/* string toBinStr(const unsigned& x, const unsigned& n)
{
    string ret = "";
    unsigned tmp = x;
    for(unsigned i=0; i<n; ++i) {
        ret += to_string(tmp & 1);
        tmp >>= 1;
    }
    assert(!tmp && ret.size() == n);
    return ret;
} */

// usage: ./kiss1hot <input_kiss> <output_blif>
void oneHotEnc(const char *inKiss, const char *outBlif, const bool dcVal)
{
    ifstream ifs(inKiss);
    if(!ifs.is_open()) {
        cerr << "File \"" << inKiss << "\" not found.\n";
        return;
    }

    string buf;
    vector<string> toks;

    unsigned ni, no, np, ns, rst;
    ni = no = np = ns = rst = 0;
    StateMap stateMap;

    while(1) {
        getline(ifs, buf);
        if(buf[0] != '#') break;
    }

    tokenize(buf, toks, 2);
    assert(toks[0] == ".i");
    ni = stoi(toks[1]);

    getline(ifs, buf);  tokenize(buf, toks, 2);
    assert(toks[0] == ".o");
    no = stoi(toks[1]);

    getline(ifs, buf);  tokenize(buf, toks, 2);
    assert(toks[0] == ".p");
    np = stoi(toks[1]);

    getline(ifs, buf);  tokenize(buf, toks, 2);
    assert(toks[0] == ".s");
    ns = stoi(toks[1]);

    getline(ifs, buf);  tokenize(buf, toks, 2);
    assert(toks[0] == ".r");
    rst = getStateIdx(toks[1], stateMap);  //stoi(toks[1]);

    vector<vector<unsigned> > ocVec, ncVec;
    vector<vector<string> > oiVec, niVec;
    ocVec.reserve(no);  ncVec.reserve(ns);
    oiVec.reserve(no);  niVec.reserve(ns);
    for(unsigned i=0; i<no; ++i) {
        ocVec.push_back(vector<unsigned>());
        oiVec.push_back(vector<string>());
    }
    for(unsigned i=0; i<ns; ++i) {
        ncVec.push_back(vector<unsigned>());
        niVec.push_back(vector<string>());
    }

    for(unsigned i=0; i<np; ++i) {
        getline(ifs, buf);  tokenize(buf, toks, 4);
        assert(toks[0].size() == ni && toks[3].size() == no);
        //toks[1].erase(0, 1);  toks[2].erase(0, 1);
        //unsigned x = stoi(toks[1]), y = stoi(toks[2]);
        unsigned x = getStateIdx(toks[1], stateMap);
        unsigned y = getStateIdx(toks[2], stateMap);
        ncVec[y].push_back(x);
        niVec[y].push_back(toks[0]);
        for(unsigned j=0; j<no; ++j)
            if((toks[3][j] == '1') || (dcVal && (toks[3][j] == '-'))) {
                ocVec[j].push_back(x);
                oiVec[j].push_back(toks[0]);
            }
    }

    getline(ifs, buf);  assert(buf[0] == '.');
    ifs.close();

    vector<string> sMap;  sMap.reserve(ns);
    for(unsigned i=0; i<ns; ++i) {
        sMap.push_back(string(ns, '0'));
        sMap[i][i] = '1';
    }

    ofstream ofs(outBlif);
    if (!ofs.is_open()) {
        cerr << "cannot open file \"" << outBlif << "\"!!" << endl;
        return;
    }
    ofs << ".model " << inKiss << ".blif\n";
    ofs << ".inputs ";
    for(unsigned i=0; i<ni; ++i)
        ofs << "i" << i << ((i==ni-1) ? '\n' : ' ');
    ofs << ".outputs ";
    for(unsigned i=0; i<no; ++i)
        ofs << "o" << i << ((i==no-1) ? '\n' : ' ');
    for(unsigned i=0; i<ns; ++i)
        ofs << ".latch n" << i << " c" << i << " " << sMap[rst][i] << "\n";

    string head = "";
    head += ".names ";
    for(unsigned i=0; i<ni; ++i)
        head += "i" + to_string(i) + " ";
    for(unsigned i=0; i<ns; ++i)
        head += "c" + to_string(i) + " ";

    for(unsigned i=0; i<no; ++i) {
        unsigned n = oiVec[i].size();
        assert(n == ocVec[i].size());
        if(!n) continue;
        ofs << head << "o" << i << "\n";
        for(unsigned j=0; j<n; ++j)
            ofs << oiVec[i][j] << sMap[ocVec[i][j]] << " 1\n";
    }

    for(unsigned i=0; i<ns; ++i) {
        unsigned n = niVec[i].size();
        assert(n == ncVec[i].size());
        if(!n) continue;
        ofs << head << "n" << i << "\n";
        for(unsigned j=0; j<n; ++j)
            ofs << niVec[i][j] << sMap[ncVec[i][j]] << " 1\n";
    }

    ofs << ".end\n";
    ofs.close();

}

} // end namespace kissToBlif


///////////////////////////////////////////////////////////////
//  Copyright 2012-2021 John Maddock.
//  Copyright 2021 Matt Borland. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#define BOOST_MP_STANDALONE

#include <boost/config.hpp>

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

#include "test_arithmetic.hpp"

int main()
{
#ifdef BOOST_HAS_FLOAT128
   test<boost::multiprecision::complex128>();
#endif
   return boost::report_errors();
}


#define BOOST_TEST_MODULE Test_VectorCursor

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/vector/all.h++>

typedef boost::mpl::list<
	mla::vector::Dense<float>,
	mla::vector::Dense<double>,
	mla::vector::SparseCS<float>,
	mla::vector::SparseCS<double>
> vector_type_list;

BOOST_AUTO_TEST_CASE_TEMPLATE( VectorCursor_iterate_through_dense, VectorType, vector_type_list )
{
	using namespace mla::vector;

	size_t size = 3;
	VectorType m(size);

	m.setZero();
	for(size_t i = 0; i < size; i++)
	{
		m.setValue(i, 1.0f*i);
	}

	typename VectorType::Cursor cursor(m);

	// checks if the cursor is able to return the right values
	for(size_t i = 0; i < size; i++)
	{
		BOOST_CHECK_CLOSE( cursor.element(), 1.0f*i, 1.0e-5 );
		cursor.next();
	}
}


//  Copyright (c) 2018-2019 Cem Bassoy
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <boost/numeric/ublas/tensor/extents.hpp>
#include <boost/numeric/ublas/tensor/expression.hpp>
#include <boost/numeric/ublas/tensor/tensor.hpp>
#include <boost/test/unit_test.hpp>
#include "utility.hpp"

#include <functional>
#include <complex>

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture
{
    using extents_type = boost::numeric::ublas::extents<>;

    const std::vector<extents_type> extents
    {
//      extents_type{ },            // 0

      extents_type{1,1},         // 1
      extents_type{1,2},         // 2
      extents_type{2,1},         // 3

      extents_type{2,3},         // 4
      extents_type{2,3,1},       // 5
      extents_type{1,2,3},       // 6
      extents_type{1,1,2,3},     // 7
      extents_type{1,2,3,1,1},   // 8

      extents_type{4,2,3},       // 9
      extents_type{4,2,1,3},     // 10
      extents_type{4,2,1,3,1},   // 11
      extents_type{1,4,2,1,3,1}   // 12
      };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_expression_access, value,  test_types, fixture)
{
    namespace ublas     = boost::numeric::ublas;
    using value_t       = typename value::first_type;
    using layout_t      = typename value::second_type;
    using tensor_t      = ublas::tensor_dynamic<value_t, layout_t>;
    using expression_t  = typename tensor_t::super_type;

    for(auto const& e : extents) {

      if(!ublas::is_valid(e)){
        continue;
      }

        auto v = value_t{};
        auto t = tensor_t(e);

        for(auto& tt: t){ tt = v; v+=value_t{1}; }
        const auto& tensor_expression_const = static_cast<expression_t const&>( t );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( tensor_expression_const()(i), t(i)  );
        }

    }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_unary_expression, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_t  = typename value::first_type;
    using layout_t = typename value::second_type;
    using tensor_t = ublas::tensor_dynamic<value_t, layout_t>;

    auto uplus1 = [](auto const& a){ return a+value_t{1}; };
    //auto uplus1 = std::bind(  std::plus<value_t>{}, std::placeholders::_1, value_t(1) );

    for(auto const& e : extents) {

        auto t = tensor_t(e);
        auto v = value_t{};
        for(auto& tt: t) { tt = v; v+=value_t{1}; }

        const auto uexpr = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus1 );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( uexpr(i), uplus1(t(i))  );
        }

        auto uexpr_uexpr = ublas::detail::make_unary_tensor_expression<tensor_t>( uexpr, uplus1 );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( uexpr_uexpr(i), uplus1(uplus1(t(i)))  );
        }

        const auto & uexpr_e = uexpr.e;

        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(uexpr_e) >, tensor_t > )   );

        const auto & uexpr_uexpr_e_e = uexpr_uexpr.e.e;

        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(uexpr_uexpr_e_e) >, tensor_t > )   );

    }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_binary_expression, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_t  = typename value::first_type;
    using layout_t = typename value::second_type;
    using tensor_t = ublas::tensor_dynamic<value_t, layout_t>;

    auto uplus1 = [](auto const& a){ return a+value_t{1}; };
    auto uplus2 = [](auto const& a){ return a+value_t{2}; };
    //auto uplus1 = std::bind(  std::plus<value_t>{}, std::placeholders::_1, value_t(1) );
    //auto uplus2 = std::bind(  std::plus<value_t>{}, std::placeholders::_1, value_t(2) );
    auto bplus  = std::plus <value_t>{};
    auto bminus = std::minus<value_t>{};

    for(auto const& e : extents) {

        auto t = tensor_t(e);
        auto v = value_t{};
        for(auto& tt: t){ tt = v; v+=value_t{1}; }

        auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus1 );
        auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus2 );

        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(uexpr1.e) >, tensor_t > )   );
        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(uexpr2.e) >, tensor_t > )   );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( uexpr1(i), uplus1(t(i))  );
        }

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( uexpr2(i), uplus2(t(i))  );
        }

        auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( uexpr1, uexpr2, bplus );

        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_uexpr.el.e) >, tensor_t > )   );
        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_uexpr.er.e) >, tensor_t > )   );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( bexpr_uexpr(i), bplus(uexpr1(i),uexpr2(i))  );
        }

        auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( bexpr_uexpr, t, bminus );

        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_bexpr_uexpr.el.el.e) >, tensor_t > )   );
        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_bexpr_uexpr.el.er.e) >, tensor_t > )   );
        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_bexpr_uexpr.er) >, tensor_t > )   );
        BOOST_CHECK( ( std::is_same_v< std::decay_t< decltype(bexpr_bexpr_uexpr.er) >, tensor_t > )   );

        for(auto i = 0ul; i < t.size(); ++i){
            BOOST_CHECK_EQUAL( bexpr_bexpr_uexpr(i), bminus(bexpr_uexpr(i),t(i))  );
        }

    }

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_VECTOR_RVEC_MAT_MULT_INCLUDE
#define MTL_VECTOR_RVEC_MAT_MULT_INCLUDE

#include <cassert>
#include <boost/mpl/bool.hpp>

#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/is_static.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/update.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/meta_math/loop.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

namespace impl {

    template <std::size_t Index0, std::size_t Max0, std::size_t Index1, std::size_t Max1, typename Assign>
    struct fully_unroll_rvec_mat_mult
      : public meta_math::loop2<Index0, Max0, Index1, Max1>
    {
	typedef meta_math::loop2<Index0, Max0, Index1, Max1>                              base;
	typedef fully_unroll_rvec_mat_mult<base::next_index0, Max0, base::next_index1, Max1, Assign>  next_t;

	template <typename VectorIn, typename Matrix, typename VectorOut>
	static inline void apply(const VectorIn& v, const Matrix& A, VectorOut& w)
	{
	    Assign::update(w[base::index0], v[base::index1] * A[base::index1][base::index0]);
	    next_t::apply(v, A, w);
	}
    };

    template <std::size_t Max0, std::size_t Max1, typename Assign>
    struct fully_unroll_rvec_mat_mult<Max0, Max0, Max1, Max1, Assign>
      : public meta_math::loop2<Max0, Max0, Max1, Max1>
    {
	typedef meta_math::loop2<Max0, Max0, Max1, Max1>                              base;

	template <typename VectorIn, typename Matrix, typename VectorOut>
	static inline void apply(const VectorIn& v, const Matrix& A, VectorOut& w)
	{
	    Assign::update(w[base::index0], v[base::index1] * A[base::index1][base::index0]);
	}
    };

    struct rvec_mat_noop
    {
	template <typename VectorIn, typename Matrix, typename VectorOut>
	static inline void apply(const VectorIn&, const Matrix&, VectorOut&) {}
    };
} // impl

// Dense matrix vector multiplication with run-time matrix size
template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void dense_rvec_mat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, boost::mpl::true_)
{
    vampir_trace<1008> tracer;
    typedef typename static_num_rows<Matrix>::type size_type;
    static const size_type rows_a= static_num_rows<Matrix>::value, cols_a= static_num_cols<Matrix>::value;

    assert(rows_a > 0 && cols_a > 0);
    // w= A[all][0] * v[0];  N.B.: 1D is unrolled by the compiler faster (at least on gcc)
    for (size_type i= 0; i < cols_a; i++)
	Assign::first_update(w[i], v[0] * A[0][i]);

    // corresponds to w+= v[1:] * A[1:][all]; if necessary
    typedef impl::fully_unroll_rvec_mat_mult<1, cols_a, 2, rows_a, Assign>  f2;
    typedef typename boost::mpl::if_c<(rows_a > 1), f2, impl::rvec_mat_noop>::type   f3;
    f3::apply(v, A, w);
}

// Dense matrix vector multiplication with run-time matrix size
template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void dense_rvec_mat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, boost::mpl::false_)
{
    // Naive implementation, will be moved to a functor and complemented with more efficient ones
	vampir_trace<3027> tracer;
    using math::zero; using mtl::vector::set_to_zero;
    if (size(w) == 0) return;

    if (Assign::init_to_zero) set_to_zero(w);

    typedef typename Collection<VectorOut>::value_type value_type;
    typedef typename Collection<Matrix>::size_type     size_type;

    for (size_type i= 0; i < num_cols(A); i++) {
	value_type tmp= zero(w[i]);
	for (size_type j= 0; j < num_rows(A); j++)
	    tmp+= v[j] * A[j][i];
	Assign::update(w[i], tmp);
    }
}

// Dense vector matrix multiplication
template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void rvec_mat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, tag::flat<tag::dense>)
{
# ifdef MTL_NOT_UNROLL_FSIZE_MAT_VEC_MULT
    boost::mpl::false_        selector;
# else
    mtl::traits::is_static<Matrix> selector;
# endif
    dense_rvec_mat_mult(v, A, w, Assign(), selector);
}

// Multi-vector vector multiplication (tag::multi_vector is derived from dense)
template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void rvec_mat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, tag::flat<tag::multi_vector>)
{
	vampir_trace<2025> tracer;
    if (Assign::init_to_zero) set_to_zero(w);
    for (unsigned i= 0; i < num_cols(A); i++)
	Assign::update(w[i], dot_real(v, A.vector(i)));
}

// Transposed multi-vector vector multiplication (tag::transposed_multi_vector is derived from dense)
template <typename VectorIn, typename TransposedMatrix, typename VectorOut, typename Assign>
inline void rvec_mat_mult(const VectorIn& v, const TransposedMatrix& A, VectorOut& w, Assign, tag::flat<tag::transposed_multi_vector>)
{
	vampir_trace<2026> tracer;
    typename TransposedMatrix::const_ref_type B= A.ref; // Referred matrix

    if (Assign::init_to_zero) set_to_zero(w);
    for (unsigned i= 0; i < num_cols(B); i++)
	Assign::update(w, v[i] * trans(B.vector(i)));
}

// Hermitian multi-vector vector multiplication (tag::hermitian_multi_vector is derived from dense)
template <typename VectorIn, typename HermitianMatrix, typename VectorOut, typename Assign>
inline void rvec_mat_mult(const VectorIn& v, const HermitianMatrix& A, VectorOut& w, Assign, tag::flat<tag::hermitian_multi_vector>)
{
	vampir_trace<2027> tracer;
    typename HermitianMatrix::const_ref_type B= A.const_ref(); // Referred matrix

    if (Assign::init_to_zero) set_to_zero(w);
    for (unsigned i= 0; i < num_cols(B); i++)
	Assign::update(w, v[i] * mtl::vector::conj(trans(B.vector(i))));
}

// Vector sparse matrix multiplication
template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void rvec_mat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, tag::flat<tag::sparse>)
{
	vampir_trace<3028> tracer;
    rvec_smat_mult(v, A, w, Assign(), typename OrientedCollection<Matrix>::orientation());
}

template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void rvec_smat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, tag::row_major)
{
	using namespace tag; namespace traits = mtl::traits;
	using traits::range_generator;
	using mtl::vector::set_to_zero;
        typedef typename range_generator<row, Matrix>::type       a_cur_type;
        typedef typename range_generator<nz, a_cur_type>::type    a_icur_type;

	typename traits::col<Matrix>::type                        col_a(A);
	typename traits::const_value<Matrix>::type                value_a(A);

	if (Assign::init_to_zero) set_to_zero(w);

	unsigned cv= 0; // traverse all columns of v
	for (a_cur_type ac= begin<row>(A), aend= end<row>(A); ac != aend; ++ac, ++cv) {
	    typename Collection<VectorIn>::value_type    vv= v[cv];
	    for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic)
		Assign::update(w[col_a(*aic)], vv * value_a(*aic));
	}
}

template <typename VectorIn, typename Matrix, typename VectorOut, typename Assign>
inline void rvec_smat_mult(const VectorIn& v, const Matrix& A, VectorOut& w, Assign, tag::col_major)
{
    using namespace tag;
    using mtl::traits::range_generator;
    using math::zero;
    using mtl::vector::set_to_zero;

    typedef typename range_generator<col, Matrix>::type       a_cur_type;
    typedef typename range_generator<nz, a_cur_type>::type    a_icur_type;
    typename mtl::traits::row<Matrix>::type                   row_a(A);
    typename mtl::traits::const_value<Matrix>::type           value_a(A);

    if (Assign::init_to_zero) set_to_zero(w);

    typedef typename Collection<VectorOut>::value_type        value_type;

    a_cur_type ac= begin<col>(A), aend= end<col>(A);
    for (unsigned i= 0; ac != aend; ++ac, ++i) {
	value_type tmp= zero(w[i]);
	for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic)
	    tmp+= v[row_a(*aic)] * value_a(*aic);
	Assign::update(w[i], tmp);
    }
}

}} // namespace mtl::vector

#endif // MTL_VECTOR_RVEC_MAT_MULT_INCLUDE


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/meta/is_bitwise_logical.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

STF_CASE( "Check bitwise_cast between integer types" )
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<uint16_t>::static_size;

  {
    using ps_t = bs::pack<std::uint16_t,N>;
    using pc_t = bs::pack<std::uint8_t,2*N>;

    STF_EQUAL ( bs::bitwise_cast<ps_t>( pc_t(0x01) )  , ps_t(0x0101)  );
    STF_EQUAL ( bs::bitwise_cast<pc_t>( ps_t(0x0101) ), pc_t(0x01)     );
  }

  {
    using ps_t = bs::pack<std::uint16_t,N/2>;
    using pc_t = bs::pack<std::uint8_t,N>;

    STF_EQUAL ( bs::bitwise_cast<ps_t>( pc_t(0x01) )  , ps_t(0x0101)  );
    STF_EQUAL ( bs::bitwise_cast<pc_t>( ps_t(0x0101) ), pc_t(0x01)     );
  }

  {
    using ps_t = bs::pack<std::uint16_t,2*N>;
    using pc_t = bs::pack<std::uint8_t,4*N>;

    STF_EQUAL ( bs::bitwise_cast<ps_t>( pc_t(0x01) )  , ps_t(0x0101)  );
    STF_EQUAL ( bs::bitwise_cast<pc_t>( ps_t(0x0101) ), pc_t(0x01)     );
  }
}

STF_CASE( "Check bitwise_cast between integer & real types" )
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<float>::static_size;

  {
    using pf_t = bs::pack<float,N>;
    using pi_t = bs::pack<std::uint32_t,N>;
    STF_EQUAL ( bs::bitwise_cast<pi_t>( pf_t(1.f) )         , pi_t(0x3F800000UL)  );
    STF_EQUAL ( bs::bitwise_cast<pf_t>( pi_t(0x3F800000UL) ), pf_t(1.f)           );
  }

  {
    using pf_t = bs::pack<float,N/2>;
    using pi_t = bs::pack<std::uint32_t,N/2>;
    STF_EQUAL ( bs::bitwise_cast<pi_t>( pf_t(1.f) )         , pi_t(0x3F800000UL)  );
    STF_EQUAL ( bs::bitwise_cast<pf_t>( pi_t(0x3F800000UL) ), pf_t(1.f)           );
  }

  {
    using pf_t = bs::pack<float,N*2>;
    using pi_t = bs::pack<std::uint32_t,N*2>;
    STF_EQUAL ( bs::bitwise_cast<pi_t>( pf_t(1.f) )         , pi_t(0x3F800000UL)  );
    STF_EQUAL ( bs::bitwise_cast<pf_t>( pi_t(0x3F800000UL) ), pf_t(1.f)           );
  }
}

template<typename T, typename Env> void logical_test(Env& $, std::true_type const&)
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<T>::static_size;

  {
    using pa_t = bs::pack<T,N>;
    using pl_t = bs::pack<bs::logical<T>,N>;
    STF_IEEE_EQUAL ( bs::bitwise_cast<pa_t>( pl_t(true) )         , bs::Allbits<pa_t>() );
    STF_IEEE_EQUAL ( bs::bitwise_cast<pl_t>( bs::Allbits<pa_t>() ), pl_t(true)          );
  }

  {
    using pa_t = bs::pack<T,N/2>;
    using pl_t = bs::pack<bs::logical<T>,N/2>;
    STF_IEEE_EQUAL ( bs::bitwise_cast<pa_t>( pl_t(true) )         , bs::Allbits<pa_t>() );
    STF_IEEE_EQUAL ( bs::bitwise_cast<pl_t>( bs::Allbits<pa_t>() ), pl_t(true)          );
  }

  {
    using pa_t = bs::pack<T,N*2>;
    using pl_t = bs::pack<bs::logical<T>,N*2>;
    STF_IEEE_EQUAL ( bs::bitwise_cast<pa_t>( pl_t(true) )         , bs::Allbits<pa_t>() );
    STF_IEEE_EQUAL ( bs::bitwise_cast<pl_t>( bs::Allbits<pa_t>() ), pl_t(true)          );
  }
}

template<typename T, typename Env> void logical_test(Env& $, std::false_type const&)
{
  STF_PASS("Logical type are not bitwise compatible on this architecture.");
}

STF_CASE_TPL( "Check bitwise_cast between arithmetic & logical types", STF_NUMERIC_TYPES )
{
  logical_test<T>($, typename boost::simd::is_bitwise_logical<T>::type{});
}

template<typename Env> void logical_diff_test(Env& $, std::true_type const&)
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<float>::static_size;

  {
    using pf_t  = bs::pack<float,N>;
    using plf_t = bs::pack<bs::logical<float>,N>;
    using pli_t = bs::pack<bs::logical<std::uint32_t>,N>;

    STF_EQUAL     ( bs::bitwise_cast<pli_t>( bs::Allbits<pf_t>() ), pli_t(true)         );
    STF_IEEE_EQUAL( bs::bitwise_cast<pf_t>( pli_t(true) )         , bs::Allbits<pf_t>() );
    STF_IEEE_EQUAL( bs::bitwise_cast<plf_t>( pli_t(true) )        , plf_t(true)         );
  }

  {
    using pf_t  = bs::pack<float,N/2>;
    using plf_t = bs::pack<bs::logical<float>,N/2>;
    using pli_t = bs::pack<bs::logical<std::uint32_t>,N/2>;

    STF_EQUAL     ( bs::bitwise_cast<pli_t>( bs::Allbits<pf_t>() ), pli_t(true)         );
    STF_IEEE_EQUAL( bs::bitwise_cast<pf_t>( pli_t(true) )         , bs::Allbits<pf_t>() );
    STF_IEEE_EQUAL( bs::bitwise_cast<plf_t>( pli_t(true) )        , plf_t(true)         );
  }

  {
    using pf_t  = bs::pack<float,N*2>;
    using plf_t = bs::pack<bs::logical<float>,N*2>;
    using pli_t = bs::pack<bs::logical<std::uint32_t>,N*2>;

    STF_EQUAL     ( bs::bitwise_cast<pli_t>( bs::Allbits<pf_t>() ), pli_t(true)         );
    STF_IEEE_EQUAL( bs::bitwise_cast<pf_t>( pli_t(true) )         , bs::Allbits<pf_t>() );
    STF_IEEE_EQUAL( bs::bitwise_cast<plf_t>( pli_t(true) )        , plf_t(true)         );
  }
}

template<typename Env> void logical_diff_test(Env& $, std::false_type const&)
{
  STF_PASS("Logical types are not bitwise compatible on this architecture.");
}

STF_CASE( "Check bitwise_cast between logical types of different base types" )
{
  logical_diff_test($, typename boost::simd::is_bitwise_logical<float>::type{});
}


/*
 * open_ggcm_test.cpp
 *
 *  Created on: Sep 23, 2009
 *      Author: dberrios
 */

#include <boost/lexical_cast.hpp>
#include <iostream>
#include <ccmc/MAS.h>
#include <ccmc/Interpolator.h>
#include <string>

using namespace std;

/**
 * enlil <inputfile> variable r theta(longitude) phi(latitude)
 */
int main (int argc, char * argv[])
{
	if (argc != 6)
	{
		cout << "mas <inputfile> variable r theta(latitude) phi(longitude)" << endl;
		exit(1);
	}

	string filename = argv[1];
	string variable = argv[2];
	float r = boost::lexical_cast<float>(argv[3]);
	float lat = boost::lexical_cast<float>(argv[4]);
	float lon = boost::lexical_cast<float>(argv[5]);
	ccmc::MAS mas;
	mas.open(filename);
	std::cerr << "opened file" << std::endl;
	mas.loadVariable(variable);

	ccmc::Interpolator * interpolator = mas.createNewInterpolator();
	float value = interpolator->interpolate(variable, r, lat, lon);
	delete interpolator;

	string units = mas.getNativeUnit(variable);

	cout << "interpolated value: " << value << " " << units << endl;
	mas.close();

}


//=====================================================
// File   :  ublas_interface.hh
// Author :  L. Plagne <laurent.plagne@edf.fr)>
// Copyright (C) EDF R&D,  lun sep 30 14:23:27 CEST 2002
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef UBLAS_INTERFACE_HH
#define UBLAS_INTERFACE_HH

#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/vector_sp.hpp>
#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/matrix_sp.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/config.hpp>
#include <boost/numeric/ublas/concepts.hpp>
#include <boost/numeric/ublas/storage.hpp>

template <class real>
class ublas_interface{

public :

  typedef real real_type ;

  typedef std::vector<real>  stl_vector;
  typedef std::vector<stl_vector > stl_matrix;

  //typedef  numerics::matrix<real,numerics::row_major,numerics::unbounded_array<real> >  gene_matrix;
  //typedef  numerics::vector<real,numerics::unbounded_array<real> > gene_vector;
  typedef  typename boost::numeric::ublas::matrix<real>  gene_matrix;
  typedef  typename boost::numeric::ublas::vector<real> gene_vector;

  static inline std::string name( void )
  {
    return "ublas";
  }

  static void free_matrix(gene_matrix & A, int N){

    return ;
  }

  static void free_vector(gene_vector & B){

    return ;

  }

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){

    A.resize(A_stl.size(),A_stl[0].size());

    for (int i=0; i<A_stl.size() ; i++){
      for (int j=0; j<A_stl[i].size() ; j++){
	A(i,j)=A_stl[i][j];
      }

    }
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){

    B.resize(B_stl.size());

    for (int i=0; i<B_stl.size() ; i++){
      B(i)=B_stl[i];
    }
  }

  static inline void vector_to_stl(gene_vector & B, stl_vector & B_stl){

    for (int i=0; i<B_stl.size() ; i++){
      B_stl[i]=B(i);
    }
  }

  static inline void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){

    int N=A_stl.size();

    for (int i=0;i<N;i++){
      A_stl[i].resize(N);
      for (int j=0;j<N;j++){
	A_stl[i][j]=A(i,j);
      }
    }

  }

  static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){
    for (int i=0;i<N;i++){
      cible(i)=source(i);
    }
  }

  static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){
    for (int i=0;i<N;i++){
      for (int j=0;j<N;j++){
	cible(i,j)=source(i,j);
      }
    }
  }

   static inline void matrix_vector_product_slow(gene_matrix & A, gene_vector & B, gene_vector & X, int N)
  {
    X =  prod(A,B);
  }

  static inline void matrix_matrix_product_slow(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N)
  {
    X =  prod(A,B);
  }

  static inline void axpy_slow(const real coef, const gene_vector & X, gene_vector & Y, int N)
  {
    Y+=coef*X;
  }

  // alias free assignements

  static inline void matrix_vector_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N)
  {
    X.assign(prod(A,B));
  }

  static inline void matrix_matrix_product(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N)
  {
    X.assign(prod(A,B));
  }

  static inline void axpy(const real coef, const gene_vector & X, gene_vector & Y, int N)
  {
    Y.plus_assign(coef*X);
  }

  static inline void ata_product(gene_matrix & A, gene_matrix & X, int N)
  {
    // X =  prod(trans(A),A);
    X.assign(prod(trans(A),A));
  }

  static inline void aat_product(gene_matrix & A, gene_matrix & X, int N)
  {
    // X =  prod(A,trans(A));
    X.assign(prod(A,trans(A)));
  }

};

#endif


/*

    This program uses dlib to make an object detector
    for 5 concentric circles. In
    particular, we go though the steps to train the kind of sliding window
    object detector first published by Dalal and Triggs in 2005 in the paper
    Histograms of Oriented Gradients for Human Detection.

    Note that this program executes fastest when compiled with at least SSE2
    instructions enabled.  So if you are using a PC with an Intel or AMD chip
    then you should enable at least SSE2 instructions.  If you are using cmake
    to compile this program you can enable them by using one of the following
    commands when you create the build project:
        cmake path_to_dlib_root/examples -DUSE_SSE2_INSTRUCTIONS=ON
        cmake path_to_dlib_root/examples -DUSE_SSE4_INSTRUCTIONS=ON
        cmake path_to_dlib_root/examples -DUSE_AVX_INSTRUCTIONS=ON
    This will set the appropriate compiler options for GCC, clang, Visual
    Studio, or the Intel compiler.  If you are using another compiler then you
    need to consult your compiler's manual to determine how to enable these
    instructions.  Note that AVX is the fastest but requires a CPU from at least
    2011.  SSE4 is the next fastest and is supported by most current machines.

    Follow along with the example file fhog_object_detector_ex.cpp to understand how this
    file works.

*/

#include <dlib/svm_threaded.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>
#include <dlib/data_io.h>

#include <iostream>
#include <fstream>

using namespace std;
using namespace dlib;
int main(int argc, char** argv)
{

    try
    {
        // In this example we are going to train a face detector based on the
        // concentric circles in the test_images directory.  So the first
        // thing we do is load that dataset.  This means you need to supply the
        // path to this faces folder as a command line argument so we will know
        // where it is.
        if (argc != 2)
        {
            cout << "Give the path to the test_images directory as the argument to this" << endl;
            cout << "program. " << endl;
            cout << endl;
            return 0;
        }

        const std::string faces_directory = argv[1];

        dlib::array<array2d<unsigned char> > images_train, images_test;
        std::vector<std::vector<rectangle> > face_boxes_train, face_boxes_test;

        load_image_dataset(images_train, face_boxes_train, faces_directory+"/training.xml");
        load_image_dataset(images_test, face_boxes_test, faces_directory+"/testing.xml");

        upsample_image_dataset<pyramid_down<2> >(images_train, face_boxes_train);
        upsample_image_dataset<pyramid_down<2> >(images_test,  face_boxes_test);

        typedef scan_fhog_pyramid<pyramid_down<6> > image_scanner_type;
        image_scanner_type scanner;
        scanner.set_detection_window_size(140, 40);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);

        // The trainer is a kind of support vector machine and therefore has the usual SVM
        // C parameter.  In general, a bigger C encourages it to fit the training data
        // better but might lead to overfitting.  You must find the best C value
        // empirically by checking how well the trained detector works on a test set of
        // images you haven't trained on.  Don't just leave the value set at 1.  Try a few
        // different C values and see what works best for your data.
        trainer.set_c(1);
        // We can tell the trainer to print it's progress to the console if we want.
        trainer.be_verbose();
        // The trainer will run until the "risk gap" is less than 0.01.  Smaller values
        // make the trainer solve the SVM optimization problem more accurately but will
        // take longer to train.  For most problems a value in the range of 0.1 to 0.01 is
        // plenty accurate.  Also, when in verbose mode the risk gap is printed on each
        // iteration so you can see how close it is to finishing the training.
        trainer.set_epsilon(0.01);

        object_detector<image_scanner_type> detector = trainer.train(images_train, face_boxes_train);

        // Now that we have a face detector we can test it.  The first statement tests it
        // on the training data.  It will print the precision, recall, and then average precision.
        cout << "training results: " << test_object_detection_function(detector, images_train, face_boxes_train) << endl;
        // However, to get an idea if it really worked without overfitting we need to run
        // it on images it wasn't trained on.  The next line does this.  Happily, we see
        // that the object detector works perfectly on the testing images.
        cout << "testing results:  " << test_object_detection_function(detector, images_test, face_boxes_test) << endl;

        // If you have read any papers that use HOG you have probably seen the nice looking
        // "sticks" visualization of a learned HOG detector.  This next line creates a
        // window with such a visualization of our detector.  It should look somewhat like
        // 5 concentric circles.
        image_window hogwin(draw_fhog(detector), "Learned fHOG detector");

        // Now for the really fun part.  Let's display the testing images on the screen and
        // show the output of the circle detector overlaid on each image.  You will see that
        // it finds all the circles without false alarming on any non-faces.
        image_window win;
        for (unsigned long i = 0; i < images_test.size(); ++i)
        {
            // Run the detector and get the face detections.
            std::vector<rectangle> dets = detector(images_test[i]);
            win.clear_overlay();
            win.set_image(images_test[i]);
            win.add_overlay(dets, rgb_pixel(255,0,0));
            cout << "Hit enter to process the next image..." << endl;
            cin.get();
        }

        // Like everything in dlib, you can save your detector to disk using the
        // serialize() function.
        serialize("detector.svm") << detector;
    }
    catch (exception& e)
    {
        cout << "\nexception thrown!" << endl;
        cout << e.what() << endl;
    }
}


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/image_processing/threshold.hpp>

using namespace boost::gil;

int main() {
  rgb8_image_t img;
  read_image("test.jpg", img, jpeg_tag{});
  rgb8_image_t img_out(img.dimensions());

  //    performing binary threshold on each channel of the image
  //    if the pixel value is more than 150 than it will be set to 255 else to 0
  boost::gil::threshold_binary(const_view(img), view(img_out), 150, 255);
  write_view("out-threshold-binary.jpg", view(img_out), jpeg_tag{});

  //    if the pixel value is more than 150 than it will be set to 150 else no
  //    change
  boost::gil::threshold_truncate(const_view(img), view(img_out), 150,
                                 threshold_truncate_mode::threshold);
  write_view("out-threshold-binary_inv.jpg", view(img_out), jpeg_tag{});

  return 0;
}


#include "array3d.h"
#include <iostream>
#include <random>
#include <functional>
#include <chrono>
#include <boost/format.hpp>
#include <vector>
#include <cmath>
#include "mpi.h"
#include <algorithm>

using namespace std;

uniform_real_distribution<double> generator { -10, 10 };
default_random_engine re;

int mpi_rank;
int mpi_size;

const double GB = 1024 * 1024 * 1024;

double gen() {
    return generator(re);
}

template <typename T>
void randomize(T & a) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++){
                a(i, j, k) = gen();
            }
        }
    }
}

template <typename T>
void run_test(std::function<void(T&, T&)> func, const std::string & testname, int size, int iterations=10) {
    T a(size);
    T b(size);
    randomize(b);
    vector<double> times(iterations);
    for (int i = 0; i < iterations; i++) {
        randomize(a);
        MPI_Barrier(MPI_COMM_WORLD);
        chrono::steady_clock::time_point begin = chrono::steady_clock::now();
        func(a, b);
        MPI_Barrier(MPI_COMM_WORLD);
        chrono::steady_clock::time_point end = chrono::steady_clock::now();
        times[i] = chrono::duration_cast<chrono::nanoseconds>(end - begin).count() / 1e6;
    }
    if (mpi_rank == 0) {
        sort(times.begin(), times.end());
        times.pop_back();
        double avg = accumulate(times.begin(), times.end(), 0.0) / times.size();
        double avg_sq = inner_product(times.begin(), times.end(), times.begin(), 0.0) / times.size();
        double standard_deviation = sqrt(avg_sq - avg * avg);
        double throughput = (static_cast<double>(mpi_size) * size * size * size * sizeof(double)) / GB * 1e3 / avg; // Gb / s
        cout << boost::format("%15s ") % testname << boost::format("%5d: ") % size << boost::format("%9.3f ms") % avg
                << boost::format(" (+-%7.3f ms)") % standard_deviation << boost::format(" %5.2f Gb/s") % throughput
                << endl;
    }
}

template <typename T>
void run_test(std::function<void(T&, T&)> func, const std::string & testname, const vector<int> & sizes, int iterations=10) {
    for (int size : sizes) {
        run_test(func, testname, size, iterations);
    }
}

template <typename T>
void copy_test(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                b(i, j, k) = a(i, j, k);
            }
        }
    }
}

template <typename T>
void copy_wo_test(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int k = 0; k < n; k++) {
        for (int j = 0; j < n; j++) {
            for (int i = 0; i < n; i++) {
                b(i, j, k) = a(i, j, k);
            }
        }
    }
}

template <typename T>
void copy_true_test(T & a, T & b) {
    b = a;
}

void copy_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        b[i] = a[i];
    }
}

template <typename T>
void sum_triplets_bad(T & a, T & b) {
    int n = a.get_n();
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(0, j, k) = a(0, j, k) + a(1, j, k);
        }
    }
    #pragma omp parallel for
    for (int i = 1; i < n - 1; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                b(i, j, k) = a(i-1, j, k) + a(i, j, k) + a(i+1, j, k);
            }
        }
    }
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(n-1, j, k) = a(n-2, j, k) + a(n-1, j, k);
        }
    }
}

void sum_triplets_bad_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        if (a.is_xmin(i)) {
            b[i] = a[i] + a[a.get_x_next(i)];
        } else if (a.is_xmax(i)) {
            b[i] = a[i] + a[a.get_x_prev(i)];
        } else {
            b[i] = a[i] + a[a.get_x_next(i)] + a[a.get_x_prev(i)];
        }
    }
}

template <typename T>
void sum_triplets_good(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            b(i, j, 0) = a(i, j, 0) + a(i, j, 1);
            for (int k = 1; k < n-1; k++) {
                b(i, j, k) = a(i, j, k-1) + a(i, j, k) + a(i, j, k+1);
            }
            b(i, j, n-1) = a(i, j, n-1) + a(i, j, n-2);
        }
    }
}

void sum_triplets_good_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        if (a.is_zmin(i)) {
            b[i] = a[i] + a[a.get_z_next(i)];
        } else if (a.is_zmax(i)) {
            b[i] = a[i] + a[a.get_z_prev(i)];
        } else {
            b[i] = a[i] + a[a.get_z_next(i)] + a[a.get_z_prev(i)];
        }
    }
}

template <typename T>
void sum_pentlets_bad(T & a, T & b) {
    int n = a.get_n();
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(0, j, k) = a(0, j, k) + a(1, j, k) + a(2, j, k);
        }
    }
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(1, j, k) = a(0, j, k) + a(1, j, k) + a(2, j, k) + a(3, j, k);
        }
    }
    #pragma omp parallel for
    for (int i = 2; i < n - 2; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                b(i, j, k) = a(i-2, j, k) + a(i-1, j, k) + a(i, j, k) + a(i+1, j, k) + a(i+2, j, k);
            }
        }
    }
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(n-2, j, k) = a(n-4, j, k) + a(n-3, j, k) + a(n-2, j, k) + a(n-1, j, k);
        }
    }
    for (int j = 0; j < n; j++) {
        for (int k = 0; k < n; k++) {
            b(n-1, j, k) = a(n-3, j, k) + a(n-2, j, k) + a(n-1, j, k);
        }
    }
}

void sum_pentlets_bad_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        if (a.is_xmin(i)) {
            int i1 = a.get_x_next(i);
            int i2 = a.get_x_next(i1);
            b[i] = a[i] + a[i1] + a[i2];
        } else if (a.is_xmax(i)) {
            int i1 = a.get_x_prev(i);
            int i2 = a.get_x_prev(i1);
            b[i] = a[i] + a[i1] + a[i2];
        } else {
            int i1_prev = a.get_x_prev(i);
            int i1_next = a.get_x_next(i);
            if (a.is_xmin(i1_prev)) {
                int i2_next = a.get_x_next(i1_next);
                b[i] = a[i1_prev] + a[i] + a[i1_next] + a[i2_next];
            } else if (a.is_xmax(i1_next)) {
                int i2_prev = a.get_x_prev(i1_prev);
                b[i] = a[i2_prev] + a[i1_prev] + a[i] + a[i1_next];
            } else {
                int i2_next = a.get_x_next(i1_next);
                int i2_prev = a.get_x_prev(i1_prev);
                b[i] = a[i2_prev] + a[i1_prev] + a[i] + a[i1_next] + a[i2_next];
            }
        }
    }
}

template <typename T>
void sum_pentlets_good(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            b(i, j, 0) = a(i, j, 0) + a(i, j, 1) + a(i, j, 2);
            b(i, j, 1) = a(i, j, 0) + a(i, j, 1) + a(i, j, 2) + a(i, j, 3);
            for (int k = 2; k < n-2; k++) {
                b(i, j, k) = a(i, j, k-2) + a(i, j, k-1) + a(i, j, k) + a(i, j, k+1) + a(i, j, k+2);
            }
            b(i, j, n-2) = a(i, j, n-4) + a(i, j, n-3) + a(i, j, n-2) + a(i, j, n-1);
            b(i, j, n-1) = a(i, j, n-3) + a(i, j, n-2) + a(i, j, n-1);
        }
    }
}

void sum_pentlets_good_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        if (a.is_zmin(i)) {
            int i1 = a.get_z_next(i);
            int i2 = a.get_z_next(i1);
            b[i] = a[i] + a[i1] + a[i2];
        } else if (a.is_zmax(i)) {
            int i1 = a.get_z_prev(i);
            int i2 = a.get_z_prev(i1);
            b[i] = a[i] + a[i1] + a[i2];
        } else {
            int i1_prev = a.get_z_prev(i);
            int i1_next = a.get_z_next(i);
            if (a.is_zmin(i1_prev)) {
                int i2_next = a.get_z_next(i1_next);
                b[i] = a[i1_prev] + a[i] + a[i1_next] + a[i2_next];
            } else if (a.is_zmax(i1_next)) {
                int i2_prev = a.get_z_prev(i1_prev);
                b[i] = a[i2_prev] + a[i1_prev] + a[i] + a[i1_next];
            } else {
                int i2_next = a.get_z_next(i1_next);
                int i2_prev = a.get_z_prev(i1_prev);
                b[i] = a[i2_prev] + a[i1_prev] + a[i] + a[i1_next] + a[i2_next];
            }
        }
    }
}

template <typename T>
void sum_neighbors(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int i = 0; i < n-1; i++) {
        for (int j = 0; j < n-1; j++) {
            for (int k = 0; k < n-1; k++) {
                b(i, j, k) = a(i, j, k) + a(i+1, j, k) + a(i, j+1, k) + a(i+1, j+1, k) + a(i, j, k+1) + a(i+1, j, k+1)
                           + a(i, j+1, k+1) + a(i+1, j+1, k+1);
            }
            b(i, j, n-1) = a(i, j, n-1) + a(i+1, j, n-1) + a(i, j+1, n-1) + a(i+1, j+1, n-1);
        }
        for (int k = 0; k < n-1; k++) {
            b(i, n-1, k) = a(i, n-1, k) + a(i+1, n-1, k) + a(i, n-1, k+1) + a(i+1, n-1, k+1);
        }
        b(i, n-1, n-1) = a(i, n-1, n-1) + a(i+1, n-1, n-1);
    }
    for (int j = 0; j < n-1; j++) {
        for (int k = 0; k < n-1; k++) {
            b(n-1, j, k) = a(n-1, j, k) + a(n-1, j+1, k) + a(n-1, j, k+1) + a(n-1, j+1, k+1);
        }
        b(n-1, j, n-1) = a(n-1, j, n-1) + a(n-1, j+1, n-1);
    }
    for (int k = 0; k < n-1; k++) {
        b(n-1, n-1, k) = a(n-1, n-1, k) + a(n-1, n-1, k+1);
    }
    b(n-1, n-1, n-1) = a(n-1, n-1, n-1);
}

void sum_neighbors_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        if (a.is_xmax(i)) {
            if (a.is_ymax(i)) {
                if (a.is_zmax(i)) {
                    b[i] = a[i];
                } else {
                    b[i] = a[i] + a[a.get_z_next(i)];
                }
            } else {
                if (a.is_zmax(i)) {
                    b[i] = a[i] + a[a.get_y_next(i)];
                } else {
                    int i_y_next = a.get_y_next(i);
                    int i_z_next = a.get_z_next(i);
                    int i_yz_next = a.get_y_next(i_z_next);
                    b[i] = a[i] + a[i_y_next] + a[i_z_next] + a[i_yz_next];
                }
            }
        } else if (a.is_ymax(i)) {
            if (a.is_zmax(i)) {
                b[i] = a[i] + a[a.get_x_next(i)];
            } else {
                int i_x_next = a.get_x_next(i);
                int i_z_next = a.get_z_next(i);
                int i_xz_next = a.get_x_next(i_z_next);
                b[i] = a[i] + a[i_x_next] + a[i_z_next] + a[i_xz_next];
            }
        } else if (a.is_zmax(i)) {
            int i_x_next = a.get_x_next(i);
            int i_y_next = a.get_y_next(i);
            int i_xy_next = a.get_x_next(i_y_next);
            b[i] = a[i] + a[i_x_next] + a[i_y_next] + a[i_xy_next];
        } else {
            int x_next = a.get_x_next(i);
            int y_next = a.get_y_next(i);
            int z_next = a.get_z_next(i);
            int xy_next = a.get_x_next(y_next);
            int xz_next = a.get_x_next(z_next);
            int yz_next = a.get_y_next(z_next);
            int xyz_next = a.get_z_next(xy_next);
            b[i] = a[i] + a[x_next] + a[y_next] + a[z_next] + a[xy_next] + a[xz_next] + a[yz_next] + a[xyz_next];
        }
    }
}

template <typename T>
void sum_neighbors_periodic(T & a, T & b) {
    int n = a.get_n();
    #pragma omp parallel for
    for (int i = 0; i < n-1; i++) {
        for (int j = 0; j < n-1; j++) {
            for (int k = 0; k < n-1; k++) {
                b(i, j, k) = a(i, j, k) + a(i+1, j, k) + a(i, j+1, k) + a(i+1, j+1, k) + a(i, j, k+1) + a(i+1, j, k+1)
                           + a(i, j+1, k+1) + a(i+1, j+1, k+1);
            }
            b(i, j, n-1) = a(i, j, n-1) + a(i+1, j, n-1) + a(i, j+1, n - 1) + a(i+1, j+1, n-1) + a(i, j, 0)
                    + a(i+1, j, 0) + a(i, j+1, 0) + a(i+1, j+1, 0);
        }
        for (int k = 0; k < n-1; k++) {
            b(i, n-1, k) = a(i, n-1, k) + a(i+1, n-1, k) + a(i, n-1, k+1) + a(i+1, n-1, k+1) + a(i, 0, k)
                    + a(i+1, 0, k) + a(i, 0, k+1) + a(i+1, 0, k+1);
        }
        b(i, n-1, n-1) = a(i, n-1, n-1) + a(i+1, n-1, n-1) + a(i, 0, n-1) + a(i+1, 0, n-1) + a(i, n-1, 0)
                + a(i+1, n-1, 0) + a(i, 0, 0) + a(i+1, 0, 0);
    }
    for (int j = 0; j < n-1; j++) {
        for (int k = 0; k < n-1; k++) {
            b(n-1, j, k) = a(n-1, j, k) + a(n-1, j+1, k) + a(n-1, j, k+1) + a(n-1, j+1, k+1) + a(0, j, k)
                    + a(0, j+1, k) + a(0, j, k+1) + a(0, j+1, k+1);;
        }
        b(n-1, j, n-1) = a(n-1, j, n-1) + a(n-1, j+1, n-1) + a(0, j, n-1) + a(0, j+1, n-1) + a(n-1, j, 0)
                + a(n-1, j+1, 0) + a(0, j, 0) + a(0, j+1, 0);
    }
    for (int k = 0; k < n-1; k++) {
        b(n-1, n-1, k) = a(n-1, n-1, k) + a(n-1, n-1, k+1) + a(0, n-1, k) + a(0, n-1, k+1) + a(n-1, 0, k)
                + a(n-1, 0, k+1) + a(0, 0, k) + a(0, 0, k+1);
    }
    b(n-1, n-1, n-1) = a(n-1, n-1, n-1) + a(n-1, n-1, 0) + a(n-1, 0, n-1) + a(n-1, 0, 0) + a(0, n-1, n-1)
            + a(0, n-1, 0) + a(0, 0, n-1) + a(0, 0, 0);
}

void sum_neighbors_periodic_z(morton_array<double> & a, morton_array<double> & b) {
    int n = a.get_size();
    for (int i = 0; i < n; i++) {
        unsigned int x_next = a.get_x_next(i);
        unsigned int y_next = a.get_y_next(i);
        unsigned int z_next = a.get_z_next(i);
        unsigned int xy_next = a.get_x_next(y_next);
        unsigned int xz_next = a.get_x_next(z_next);
        unsigned int yz_next = a.get_y_next(z_next);
        unsigned int xyz_next = a.get_z_next(xy_next);
        b[i] = a[i] + a[x_next] + a[y_next] + a[z_next] + a[xy_next] + a[xz_next] + a[yz_next] + a[xyz_next];
    }
}

template<typename T>
void print_array(T& a) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                cout << boost::format("%6.2f") % a(i, j, k);
            }
            cout << endl;
        }
        cout << endl;
    }
}

template <typename T, typename V>
void copy(T & a, V & b) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                b(i, j, k) = a(i, j, k);
            }
        }
    }
}

template <typename T, typename V>
bool is_equal(T & a, V & b) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                if (std::fabs(b(i, j, k) - a(i, j, k)) > 1e-6) return false;
            }
        }
    }
    return true;
}

void run_checks(int size) {
    cout << "Checking size " << size << "..." << endl;
    morton_array<double> a(size);

    {
        cached_array<double> b(size);
        simple_array<double> c(size);

        randomize(a);
        copy(a, b);
        copy(a, c);
        if (!is_equal(a, b)) cout << "a and b are different" << endl;
        if (!is_equal(a, c)) cout << "a and c are different" << endl;
        if (!is_equal(b, c)) cout << "b and c are different" << endl;
    }

    morton_array<double> b(size);
    morton_array<double> c(size);

    copy(a, b);
    copy_test(a, c);
    if (!is_equal(b, c)) cout << "copy_test is different" << endl;

    copy(a, b);
    copy_wo_test(a, c);
    if (!is_equal(b, c)) cout << "copy_wo_test is different" << endl;

    copy(a, b);
    copy_true_test(a, c);
    if (!is_equal(b, c)) cout << "copy_true_test is different" << endl;

    copy(a, b);
    copy_z(a, c);
    if (!is_equal(b, c)) cout << "copy_z is different" << endl;

    sum_triplets_bad(a, b);
    sum_triplets_bad_z(a, c);
    if (!is_equal(b, c)) cout << "sum_triplets_bad_z is different" << endl;

    sum_triplets_good(a, b);
    sum_triplets_good_z(a, c);
    if (!is_equal(b, c)) cout << "sum_triplets_good_z is different" << endl;

    sum_pentlets_bad(a, b);
    sum_pentlets_bad_z(a, c);
    if (!is_equal(b, c)) cout << "sum_pentlets_bad_z is different" << endl;

    sum_pentlets_good(a, b);
    sum_pentlets_good_z(a, c);
    if (!is_equal(b, c)) cout << "sum_pentlets_good_z is different" << endl;

    sum_neighbors(a, b);
    sum_neighbors_z(a, c);
    if (!is_equal(b, c)) cout << "sum_neighbors_z is different" << endl;

    sum_neighbors_periodic(a, b);
    sum_neighbors_periodic_z(a, c);
    if (!is_equal(b, c)) cout << "sum_neighbors_periodic_z is different" << endl;
}

int main(int argc, char **argv) {
    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);

    re.seed(chrono::system_clock::now().time_since_epoch().count());

    const int iterations = 6;

    vector<int> sizes = {32, 64, 128, 256, 512};

    if (argc > 1) {
        sizes.erase(sizes.begin(), sizes.end());
        for (int i = 1; i < argc; i++) {
            sizes.push_back(atoi(argv[i]));
        }
    }

    if (mpi_rank == 0) {
        cout << "Sizes:";
        for (auto size : sizes) {
            cout << " " << size;
        }
        cout << endl;
    }

    if (mpi_rank == 0) {
        for (auto size : sizes) {
            run_checks(size);
        }
    }

    if (mpi_rank == 0) {
        cout << "Morton array:" << endl;
    }

    run_test<morton_array<double>>(copy_test<morton_array<double>>, "copy", sizes, iterations);
    run_test<morton_array<double>>(copy_wo_test<morton_array<double>>, "copy_wo", sizes, iterations);
    run_test<morton_array<double>>(copy_z, "copy_z", sizes, iterations);
    run_test<morton_array<double>>(copy_true_test<morton_array<double>>, "copy_true", sizes, iterations);
    run_test<morton_array<double>>(sum_triplets_bad<morton_array<double>>, "triplets_bad", sizes, iterations);
    run_test<morton_array<double>>(sum_triplets_bad_z, "triplets_bad_z", sizes, iterations);
    run_test<morton_array<double>>(sum_triplets_good<morton_array<double>>, "triplets_good", sizes, iterations);
    run_test<morton_array<double>>(sum_triplets_good_z, "triplets_good_z", sizes, iterations);
    run_test<morton_array<double>>(sum_pentlets_bad<morton_array<double>>, "pentlets_bad", sizes, iterations);
    run_test<morton_array<double>>(sum_pentlets_bad_z, "pentlets_bad_z", sizes, iterations);
    run_test<morton_array<double>>(sum_pentlets_good<morton_array<double>>, "pentlets_good", sizes, iterations);
    run_test<morton_array<double>>(sum_pentlets_good_z, "pentlets_good_z", sizes, iterations);
    run_test<morton_array<double>>(sum_neighbors<morton_array<double>>, "sum_neighbors", sizes, iterations);
    run_test<morton_array<double>>(sum_neighbors_z, "sum_neighbors_z", sizes, iterations);
    run_test<morton_array<double>>(sum_neighbors_periodic<morton_array<double>>, "sum_neighbors_p", sizes, iterations);
    run_test<morton_array<double>>(sum_neighbors_periodic_z, "sum_neighbor_pz", sizes, iterations);

    if (mpi_rank == 0) {
        cout << "Simple array:" << endl;
    }

    run_test<simple_array<double>>(copy_test<simple_array<double>>, "copy", sizes, iterations);
    run_test<simple_array<double>>(copy_wo_test<simple_array<double>>, "copy_wo", sizes, iterations);
    run_test<simple_array<double>>(copy_true_test<simple_array<double>>, "copy_true", sizes, iterations);
    run_test<simple_array<double>>(sum_triplets_bad<simple_array<double>>, "triplets_bad", sizes, iterations);
    run_test<simple_array<double>>(sum_triplets_good<simple_array<double>>, "triplets_good", sizes, iterations);
    run_test<simple_array<double>>(sum_pentlets_bad<simple_array<double>>, "pentlets_bad", sizes, iterations);
    run_test<simple_array<double>>(sum_pentlets_good<simple_array<double>>, "pentlets_good", sizes, iterations);
    run_test<simple_array<double>>(sum_neighbors<simple_array<double>>, "sum_neighbors", sizes, iterations);
    run_test<simple_array<double>>(sum_neighbors_periodic<simple_array<double>>, "sum_neighbors_p", sizes, iterations);

    if (mpi_rank == 0) {
        cout << "Cached array:" << endl;
    }

    run_test<cached_array<double>>(copy_test<cached_array<double>>, "copy", sizes, iterations);
    run_test<cached_array<double>>(copy_wo_test<cached_array<double>>, "copy_wo", sizes, iterations);
    run_test<cached_array<double>>(copy_true_test<cached_array<double>>, "copy_true", sizes, iterations);
    run_test<cached_array<double>>(sum_triplets_bad<cached_array<double>>, "triplets_bad", sizes, iterations);
    run_test<cached_array<double>>(sum_triplets_good<cached_array<double>>, "triplets_good", sizes, iterations);
    run_test<cached_array<double>>(sum_pentlets_bad<cached_array<double>>, "pentlets_bad", sizes, iterations);
    run_test<cached_array<double>>(sum_pentlets_good<cached_array<double>>, "pentlets_good", sizes, iterations);
    run_test<cached_array<double>>(sum_neighbors<cached_array<double>>, "sum_neighbors", sizes, iterations);
    run_test<cached_array<double>>(sum_neighbors_periodic<cached_array<double>>, "sum_neighbors_p", sizes, iterations);

    MPI_Finalize();
    return 0;
}


// Component
#include "RosConversionHelper.hpp"

// Ros
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/shared_ptr.hpp>

// Standard
#include <utility>

namespace cm
{

pcl::PointCloud<pcl::PointXYZ>::Ptr RosConversionHelper::pointCloud2toPCL(const sensor_msgs::PointCloud2::ConstPtr& pc2)
{
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*pc2, pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ> return_cloud;
    pcl::fromPCLPointCloud2(pcl_pc2, return_cloud);

    return boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(std::move(return_cloud));
}

Eigen::Matrix3d RosConversionHelper::quaternionMsgToRotationMatrix(const geometry_msgs::Quaternion& q)
{
    tf::Quaternion new_q;
    tf::quaternionMsgToTF(q, new_q);
    tf::Matrix3x3 tf_mat(new_q);

    Eigen::Matrix3d mat;
    tf::matrixTFToEigen(tf_mat, mat);

    return mat;
}

Eigen::Matrix3d RosConversionHelper::quaternionMsgToRotationMatrixIgnoreYaw(const geometry_msgs::Quaternion& q)
{
    tf::Quaternion new_q;
    tf::quaternionMsgToTF(q, new_q);
    tf::Matrix3x3 tf_mat(new_q);

    float64_t roll_r;
    float64_t pitch_r;
    float64_t yaw_r;

    tf_mat.getRPY(roll_r, pitch_r, yaw_r);
    tf_mat.setRPY(roll_r, pitch_r, 0.0);

    Eigen::Matrix3d mat;
    tf::matrixTFToEigen(tf_mat, mat);

    return mat;
}

} // namespace cm


// SEM.cpp : Defines the exported functions for the DLL.
#include "pch.h"
#include <iostream>
#include <boost/math/special_functions/polygamma.hpp>
#include <math.h>
#include <nlopt.hpp>
#include <iomanip>
#include <vector>

#include<gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "SEM.h"

double one_get_cpp(double teth, void* others)
{
	double k;
	double ki_sum;
	double ki_sumln;
	double poly;
	double funa;

	// repacking

	double* dated = static_cast<double*>(others);
	int n = static_cast<int>(*dated);

	// computations

	ki_sum = 0;
	ki_sumln = 0;
	for (int i = 1; i <= n; i++)
	{
		ki_sum += *(dated + i);
		ki_sumln += log(*(dated + i));
	}

	k = ki_sum / (n * teth);
	poly = boost::math::polygamma(0, k);
	funa = -(n * poly) - (n * log(teth)) + ki_sumln;

	return abs(funa);
}

double targetta(const std::vector<double>& x, std::vector<double>& grad, void* my_func_data)
{
	double resa;
	resa = one_get_cpp(x[0], my_func_data);
	return resa;
}

double* one_optima(int data_len, double* data, double low_prec)
{
	low_prec = 1e-10;

	double data_sum;
	double teth_gained;
	double ka_gained;

	int over = data_len + 1;

	double* dat = new double[over];

	*dat = (double)data_len;
	for (int i = 0; i < data_len; i++)
	{
		*(dat + i + 1) = *(data + i);
	}

	std::vector<double> x(1);
	x[0] = 1.0;

	nlopt::opt opt(nlopt::LN_NELDERMEAD, 1);
	std::vector<double> lb(1);
	lb[0] = low_prec;
	opt.set_lower_bounds(lb);
	opt.set_min_objective(targetta, dat);

	double minf;

	try
	{
		nlopt::result result = opt.optimize(x, minf);
	}
	catch (std::exception & e)
	{
		x[0] = -999999;
	}

	teth_gained = x[0];

	data_sum = 0.0;
	for (int i = 0; i < data_len; i++)
	{
		data_sum += *(data + i);
	}

	ka_gained = data_sum / ((double)data_len * teth_gained);
	double rei = 0;

	double* res = new double[3]{ teth_gained, ka_gained, rei };

	return res;
}

double* givemedata(double* donor, int len)
{
	double* result = new double[len];
	for (int i = 0; i < len; i++)
	{
		result[i] = donor[i];
	}
	return result;
}

double* sem(int niter, int k, double* data, int n)
{

	// Initialisation

	double** G = new double* [n];
	double** y = new double* [n];
	double* P = new double[k];
	int* V = new int[k];
	double* tethas_gained = new double[k];
	double* ks_gained = new double[k];
	double** clusters = new double* [k];
	int* clusters_n = new int[k];
	double* res;
	int recounter;
	double numerator;
	double denominator;

	//		here we initialise a random number generator
	const gsl_rng* r = gsl_rng_alloc(gsl_rng_taus);

	//		sampling size
	size_t K = k;

	//		number of items
	unsigned int N = 1;

	//		where to save samples
	//unsigned int* neine;

	for (int j = 0; j < n; j++)
	{
		G[j] = new double[k];
		y[j] = new double[k];

		for (int i = 0; i < k; i++)
		{
			G[j][i] = 1.0 / (double)k;
		}

	}

	//iterations
	for (int t = 0; t < niter; t++)
	{
		//std::cout << std::endl;
		//std::cout << "Iteration: " << t << std::endl;
		//std::cout << std::endl;

		// S-step

		for (int j = 0; j < n; j++)
		{

			const double* p = givemedata(G[j], k);

			/*
			if (j == 0)
			{
				std::cout << std::endl;
				for (int dd = 0; dd < k; dd++)
				{
					std::cout << p[dd] << "\t";
				}
				std::cout << std::endl;
				char a;
				std::cin >> a;
			}
			*/
			// temp array to save sample
			unsigned int* neine = new unsigned int[k];

			// generate
			gsl_ran_multinomial(r, K, N, p, neine);

			// copy data
			//std::cout << std::endl;
			for (int i = 0; i < k; i++)
			{
				y[j][i] = neine[i];
				//std::cout << neine[i] << "\t";
			}
			//std::cout << std::endl;

			delete[] neine;
			delete[] p;
		}

		// clusters
		for (int i = 0; i < k; i++)
		{
			V[i] = 0;
		}
		for (int j = 0; j < n; j++)
		{
			for (int i = 0; i < k; i++)
			{
				if (y[j][i] == 1)
				{
					V[i]++;
				}
			}
		}

		// clusters data
		for (int i = 0; i < k; i++)
		{
			clusters[i] = new double[V[i]];
		}

		for (int i = 0; i < k; i++)
		{
			recounter = 0;
			for (int j = 0; j < n; j++)
			{
				if (y[j][i] == 1)
				{
					clusters[i][recounter] = data[j];
					recounter++;
				}
			}
		}

		// M-step

		// gaining
		for (int i = 0; i < k; i++)
		{
			res = one_optima(V[i], clusters[i], 0);
			tethas_gained[i] = res[0];
			ks_gained[i] = res[1];
			// outer niggers from alien gay space
			//std::cout << tethas_gained[i] << "\t";
		}
		//std::cout << std::endl;
		//char pudge;
		//std::cin >> pudge;

		// del clusters
		for (int i = 0; i < k; i++)
		{
			delete[] clusters[i];
		}

		// WUT?
		for (int i = 0; i < k; i++)
		{
			P[i] = (double)V[i] / (double)n;
			//std::cout << P[i] << "\t";
		}
		//std::cout << std::endl;

		// E-step
		for (int j = 0; j < n; j++)
		{
			for (int i = 0; i < k; i++)
			{
				numerator = P[i] * gsl_ran_gamma_pdf(data[j], tethas_gained[i], ks_gained[i]);
				denominator = 0;
				for (int l = 0; l < k; l++)
				{
					denominator = denominator + (P[l] * gsl_ran_gamma_pdf(data[j], tethas_gained[i], ks_gained[i]));
				}
				G[j][i] = numerator / denominator;
			}
		}
	}

	// return

	double* rs = new double[((k * 2) + 2)];

	for (int i = 0; i < k; i++)
	{
		rs[i * 2] = tethas_gained[i];
		rs[i * 2 + 1] = ks_gained[i];
	}

	for (int r = 0; r < k; r++)
	{
		rs[(r + k * 2)] = P[r];
	}

	return rs;
	//return data;

}


#include "knapsacksolver/generator.hpp"

#include "knapsacksolver/algorithms/algorithms.hpp"

#include <iomanip>
#include <sstream>

#include <boost/program_options.hpp>

using namespace knapsacksolver;

std::string path(std::string d, ItemPos n)
{
    if (d == "u3") {
        return "data/smallcoeff/knapPI_1_" + std::to_string(n) + "_1000/"
                             + "knapPI_1_" + std::to_string(n) + "_1000_";
    } else if (d == "u4") {
        return "data/smallcoeff/knapPI_1_" + std::to_string(n) + "_10000/"
                             + "knapPI_1_" + std::to_string(n) + "_10000_";
    } else if (d == "u5") {
        return "data/largecoeff/knapPI_1_" + std::to_string(n) + "_100000/"
                             + "knapPI_1_" + std::to_string(n) + "_100000_";
    } else if (d == "u6") {
        return "data/largecoeff/knapPI_1_" + std::to_string(n) + "_1000000/"
                             + "knapPI_1_" + std::to_string(n) + "_1000000_";
    } else if (d == "u7") {
        return "data/largecoeff/knapPI_1_" + std::to_string(n) + "_10000000/"
                             + "knapPI_1_" + std::to_string(n) + "_10000000_";

    } else if (d == "wc3") {
        return "data/smallcoeff/knapPI_2_" + std::to_string(n) + "_1000/"
                             + "knapPI_2_" + std::to_string(n) + "_1000_";
    } else if (d == "wc4") {
        return "data/smallcoeff/knapPI_2_" + std::to_string(n) + "_10000/"
                             + "knapPI_2_" + std::to_string(n) + "_10000_";
    } else if (d == "wc5") {
        return "data/largecoeff/knapPI_2_" + std::to_string(n) + "_100000/"
                             + "knapPI_2_" + std::to_string(n) + "_100000_";
    } else if (d == "wc6") {
        return "data/largecoeff/knapPI_2_" + std::to_string(n) + "_1000000/"
                             + "knapPI_2_" + std::to_string(n) + "_1000000_";
    } else if (d == "wc7") {
        return "data/largecoeff/knapPI_2_" + std::to_string(n) + "_10000000/"
                             + "knapPI_2_" + std::to_string(n) + "_10000000_";

    } else if (d == "sc3") {
        return "data/smallcoeff/knapPI_3_" + std::to_string(n) + "_1000/"
                             + "knapPI_3_" + std::to_string(n) + "_1000_";
    } else if (d == "sc4") {
        return "data/smallcoeff/knapPI_3_" + std::to_string(n) + "_10000/"
                             + "knapPI_3_" + std::to_string(n) + "_10000_";
    } else if (d == "sc5") {
        return "data/largecoeff/knapPI_3_" + std::to_string(n) + "_100000/"
                             + "knapPI_3_" + std::to_string(n) + "_100000_";
    } else if (d == "sc6") {
        return "data/largecoeff/knapPI_3_" + std::to_string(n) + "_1000000/"
                             + "knapPI_3_" + std::to_string(n) + "_1000000_";
    } else if (d == "sc7") {
        return "data/largecoeff/knapPI_3_" + std::to_string(n) + "_10000000/"
                             + "knapPI_3_" + std::to_string(n) + "_10000000_";

    } else if (d == "isc3") {
        return "data/smallcoeff/knapPI_4_" + std::to_string(n) + "_1000/"
                             + "knapPI_4_" + std::to_string(n) + "_1000_";
    } else if (d == "isc4") {
        return "data/smallcoeff/knapPI_4_" + std::to_string(n) + "_10000/"
                             + "knapPI_4_" + std::to_string(n) + "_10000_";
    } else if (d == "isc5") {
        return "data/largecoeff/knapPI_4_" + std::to_string(n) + "_100000/"
                             + "knapPI_4_" + std::to_string(n) + "_100000_";
    } else if (d == "isc6") {
        return "data/largecoeff/knapPI_4_" + std::to_string(n) + "_1000000/"
                             + "knapPI_4_" + std::to_string(n) + "_1000000_";
    } else if (d == "isc7") {
        return "data/largecoeff/knapPI_4_" + std::to_string(n) + "_10000000/"
                             + "knapPI_4_" + std::to_string(n) + "_10000000_";

    } else if (d == "asc3") {
        return "data/smallcoeff/knapPI_5_" + std::to_string(n) + "_1000/"
                             + "knapPI_5_" + std::to_string(n) + "_1000_";
    } else if (d == "asc4") {
        return "data/smallcoeff/knapPI_5_" + std::to_string(n) + "_10000/"
                             + "knapPI_5_" + std::to_string(n) + "_10000_";
    } else if (d == "asc5") {
        return "data/largecoeff/knapPI_5_" + std::to_string(n) + "_100000/"
                             + "knapPI_5_" + std::to_string(n) + "_100000_";
    } else if (d == "asc6") {
        return "data/largecoeff/knapPI_5_" + std::to_string(n) + "_1000000/"
                             + "knapPI_5_" + std::to_string(n) + "_1000000_";
    } else if (d == "asc7") {
        return "data/largecoeff/knapPI_5_" + std::to_string(n) + "_10000000/"
                             + "knapPI_5_" + std::to_string(n) + "_10000000_";

    } else if (d == "ss3") {
        return "data/smallcoeff/knapPI_6_" + std::to_string(n) + "_1000/"
                             + "knapPI_6_" + std::to_string(n) + "_1000_";
    } else if (d == "ss4") {
        return "data/smallcoeff/knapPI_6_" + std::to_string(n) + "_10000/"
                             + "knapPI_6_" + std::to_string(n) + "_10000_";
    } else if (d == "ss5") {
        return "data/largecoeff/knapPI_6_" + std::to_string(n) + "_100000/"
                             + "knapPI_6_" + std::to_string(n) + "_100000_";
    } else if (d == "ss6") {
        return "data/largecoeff/knapPI_6_" + std::to_string(n) + "_1000000/"
                             + "knapPI_6_" + std::to_string(n) + "_1000000_";
    } else if (d == "ss7") {
        return "data/largecoeff/knapPI_6_" + std::to_string(n) + "_10000000/"
                             + "knapPI_6_" + std::to_string(n) + "_10000000_";

    } else if (d == "sw5") {
        return "data/smallcoeff/knapPI_9_" + std::to_string(n) + "_1000/"
                             + "knapPI_9_" + std::to_string(n) + "_1000_";
    } else if (d == "sw7") {
        return "data/largecoeff/knapPI_9_" + std::to_string(n) + "_1000000/"
                             + "knapPI_9_" + std::to_string(n) + "_1000000_";
    } else if (d == "sw8") {
        return "data/largecoeff/knapPI_9_" + std::to_string(n) + "_1000000/"
                             + "knapPI_9_" + std::to_string(n) + "_1000000_";

    } else if (d == "sp/u3") {
        return "data/hardinstances/knapPI_11_" + std::to_string(n) + "_1000/"
                                + "knapPI_11_" + std::to_string(n) + "_1000_";
    } else if (d == "sp/wc3") {
        return "data/hardinstances/knapPI_12_" + std::to_string(n) + "_1000/"
                                + "knapPI_12_" + std::to_string(n) + "_1000_";
    } else if (d == "sp/sc3") {
        return "data/hardinstances/knapPI_13_" + std::to_string(n) + "_1000/"
                                + "knapPI_13_" + std::to_string(n) + "_1000_";
    } else if (d == "mstr3") {
        return "data/hardinstances/knapPI_14_" + std::to_string(n) + "_1000/"
                                + "knapPI_14_" + std::to_string(n) + "_1000_";
    } else if (d == "pceil3") {
        return "data/hardinstances/knapPI_15_" + std::to_string(n) + "_1000/"
                                + "knapPI_15_" + std::to_string(n) + "_1000_";
    } else if (d == "circle3") {
        return "data/hardinstances/knapPI_16_" + std::to_string(n) + "_1000/"
                                + "knapPI_16_" + std::to_string(n) + "_1000_";
    }
    return "";
}

void bench_literature(
        std::string algorithm,
        std::string dataset_name,
        std::vector<std::string>& dataset,
        std::mt19937_64& gen)
{
    std::ofstream file(algorithm + "_" + dataset_name + ".csv");

    // CSV
    file << "n \\ ds";
    for (auto& d: dataset)
        file << "," << d;
    file << std::endl;

    for (ItemIdx n: {50, 100, 200, 500, 1000, 2000, 5000, 10000}) {

        file << n << std::flush; // CSV

        for (Counter k = 0; k < (Counter)dataset.size(); ++k) {

            std::cout << "n " << n << ", " << dataset[k] << "..." << std::flush; // Standard output

            double t_max = 300;
            double t_total = 0.0;
            double mean = -1;
            for (Counter h = 1; h <= 100 && t_total < t_max; ++h) {
                Instance instance(path(dataset[k], n) + std::to_string(h) + ".csv", "pisinger");
                //std::cout << path(dataset[k], n) + std::to_string(h) + ".csv" << std::endl;
                try {
                    Info info = Info()
                        .set_time_limit(t_max - t_total)
                        //.set_verbose(true)
                        ;
                    run(algorithm, instance, gen, info);
                    double t = info.elapsed_time();
                    t_total += t;
                } catch (...) {
                    t_total = t_max + 1;
                }
            }

            if (t_total <= t_max) {
                mean = round(t_total * 100) / 10;
                std::cout << " mean " << mean << std::endl; // Standard output
                file << "," << mean << std::flush; // CSV
            } else {
                std::cout << " x" << std::endl; // Standard output
                file << ",x" << std::flush; // CSV
            }
        }

        file << std::endl; // CSV
    }

    std::cout << std::endl; // Standard output
    file << std::endl; // CSV
}

void bench_normal(std::string algorithm, double time_limit, std::mt19937_64& gen)
{
    std::vector<ItemIdx> ns {100, 1000, 10000, 100000};
    std::vector<Weight> rs {1000, 10000, 100000, 1000000, 10000000, 100000000};
    std::vector<double> xs {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9};

    nlohmann::json json;
    json["lab"][0] = {"n", ns};
    json["lab"][1] = {"r", rs};
    json["lab"][2] = {"x", xs};

    int val_max_r = 255 - 52;
    int val_max_g = 255 - 101;
    int val_max_b = 255 - 164;
    int col_r = 0;
    int col_g = 0;
    int col_b = 0;

    for (Counter in = 0; in < (Counter)ns.size(); ++in) {
        ItemIdx n = ns[in];
        for (Counter ir = 0; ir < (Counter)rs.size(); ++ir) {
            Weight r = rs[ir];

            // Standard output
            std::string s = "--- n " + std::to_string(n) + " r " + std::to_string(r) + " --- ";
            int pos = (int)((80 - s.length())/2);
            for(int i = 0; i < pos; i++)
                std::cout << " ";
            std::cout << s << std::endl;

            for (Counter ix = 0; ix < (Counter)xs.size(); ++ix) {
                double x = xs[ix];

                // Standard output
                std::cout
                    << "x " << std::right << std::setw(5) << x
                    << std::flush;

                Instance instance("data/normal/knap_n" + std::to_string(n)
                        + "_r" + std::to_string(r)
                        + "_x0." + std::to_string((int)(10 * x)), "standard");
                double t = time_limit + 1;
                Info info = Info()
                    .set_time_limit(time_limit)
                    //.set_verbose(true)
                    ;
                Output output(instance, info);
                try {
                    info.reset_time();
                    output = run(algorithm, instance, gen, info);
                    t = info.elapsed_time();
                } catch (...) {
                }

                std::stringstream t_str;
                if (t <= time_limit && output.lower_bound == output.upper_bound) {
                    t_str << (double)std::round(t * 10000) / 10;
                    std::cout << "\033[32m";
                    col_r = 255 - (int)(val_max_r * cbrt(t / time_limit));
                    col_g = 255 - (int)(val_max_g * cbrt(t / time_limit));
                    col_b = 255 - (int)(val_max_b * cbrt(t / time_limit));
                } else {
                    t_str << "> " << time_limit * 1000;
                    col_r = 0;
                    col_g = 0;
                    col_b = 0;
                }

                // Json
                std::string rgb_str = "rgb("
                    + std::to_string(col_r) + ","
                    + std::to_string(col_g) + ","
                    + std::to_string(col_b) + ")";
                json["tab"][in][ir][ix][0]["c"] = rgb_str;
                json["tab"][in][ir][ix][0]["t"] = t_str.str();

                // Standard output
                std::cout << " | LB" << std::right << std::setw(20) << output.solution.profit();
                std::cout << " | UB" << std::right << std::setw(20) << output.upper_bound;
                std::cout << " | T (s)" << std::right << std::setw(8) << t_str.str();
                std::cout << "\033[0m" << std::endl;
            }
        }
    }

    std::ofstream o(algorithm + ".json");
    o << std::setw(4) << json << std::endl;
}

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

    // Parse program options
    std::vector<std::string> algorithms;
    std::vector<std::string> datasets;
    double time_limit = 1;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("algorithms,a", po::value<std::vector<std::string>>(&algorithms)->multitoken(), "set algorithms")
        ("datasets,d", po::value<std::vector<std::string>>(&datasets)->multitoken(), "datasets (easy, difficultlarge, difficultsmall)")
        ("time-limit,t", po::value<double>(&time_limit), "time limit")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
        std::cout << desc << std::endl;;
        return 1;
    }
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        return 1;
    }

    Seed seed = 0;
    std::mt19937_64 gen(seed);

    for (std::string algorithm: algorithms) {
        for (std::string dataset: datasets) {
            std::cout << "*** " << algorithm << " / " << dataset << " ***" << std::endl;

            if (dataset == "easy" || dataset == "literature") {
                std::vector<std::string> dataset_easy {
                        "u3", "u4",
                        "wc3", "wc4",
                        "sc3", "sc4",
                        "isc3", "isc4",
                        "asc3", "asc4",
                        "ss3", "ss4",
                        "sw5",
                };
                bench_literature(algorithm, "easy", dataset_easy, gen);
            }

            if (dataset == "difficultsmall" || dataset == "literature") {
                std::vector<std::string> dataset_difficultsmall {
                        "sp/u3", "sp/wc3", "sp/sc3", "mstr3", "pceil3", "circle3",
                };
                bench_literature(algorithm, "difficultsmall", dataset_difficultsmall, gen);
            }

            if (dataset == "difficultlarge" || dataset == "literature") {
                std::vector<std::string> dataset_difficultlarge {
                        "u5", "u6", "u7",
                        "wc5", "wc6", "wc7",
                        "sc5", "sc6", "sc7",
                        "isc5", "isc6", "isc7",
                        "asc5", "asc6", "asc7",
                        "ss5", "ss6", "ss7",
                        "sw7", "sw8",
                };
                bench_literature(algorithm, "difficultlarge", dataset_difficultlarge, gen);
            }

            if (dataset == "normal")
                bench_normal(algorithm, time_limit, gen);

        }
    }

    return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Hello
#include <boost/test/unit_test.hpp>
#include <bits/stdc++.h>
using namespace std;
vector<int> nSort(vector<int> arr){
    // sort(arr.begin(),arr.end());
    for(long i=0;i<10;i++){
        int index=rand()%arr.size();
        int index2=(index+1)%arr.size();
        if(index>index2 && arr[index]<arr[index2]) swap(arr[index2],arr[index]);
        else if(index<index2 && arr[index]>arr[index2]) swap(arr[index2],arr[index]);
    }
    for(auto itr:arr){
        cout<<itr<<" ";
    }
    cout<<endl;
    vector<int> arr1={3,2,5,2,5,1,5,7,29,99};
    return arr;
    // return arr1;
}
BOOST_AUTO_TEST_SUITE(SUitname)
BOOST_AUTO_TEST_CASE(subtest1){
    vector<int> arr={3,2,5,2,5,1,5,7,29,0};
    vector<int> rarr=nSort(arr);
    {
        BOOST_CHECK_EQUAL(arr.size(),rarr.size());
    }
    {
        unordered_map<int, int> mp;
        for(auto itr:rarr){
            mp[itr]++;
        }
        for(auto itr:arr){
            BOOST_CHECK(mp[itr]!=0);
            mp[itr]--;
        }
    }
    {
        for(long i=0;i<rarr.size()-1;i++){
            BOOST_CHECK(rarr[i]<=rarr[i+1]);
        }
    }
}
BOOST_AUTO_TEST_SUITE_END()

Code/Testing Tools/Boost and Google test/boost/for.cpp