// ------------------------------------------------------------------------
//  Copyright (C)
//  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
//  University of California Berkeley (UCB) - USA
//
//  Jordi Pont-Tuset <jordi.pont@upc.edu>
//  Pablo Arbelaez <arbelaez@berkeley.edu>
//  June 2014
// ------------------------------------------------------------------------
// This file is part of the MCG package presented in:
//    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
//    "Multiscale Combinatorial Grouping,"
//    Computer Vision and Pattern Recognition (CVPR) 2014.
// Please consider citing the paper if you use this code.
// ------------------------------------------------------------------------
#ifndef UCM2HIER_HPP
#define UCM2HIER_HPP

#include <iostream>
#include "containers.hpp"
#include <Eigen/Dense>

void insert_labels_to_merge(label_type lab1,label_type lab2,std::list<set_type>& to_merge)
{
    std::list<set_type>::iterator l1 = to_merge.begin();
    for( ; l1!=to_merge.end(); ++l1)
        if (l1->find(lab1) != l1->end())
            break;
    std::list<set_type>::iterator l2 = to_merge.begin();
    for( ; l2!=to_merge.end(); ++l2)
        if (l2->find(lab2) != l2->end())
            break;

    if (l1==to_merge.end() && l2==to_merge.end())  // Neither label found --> New set
    {
        set_type to_put;
         to_put.insert(lab1);
         to_put.insert(lab2);
        to_merge.push_back(to_put);
    }
    else if (l1==to_merge.end() && l2!=to_merge.end())  // Found one of the two in one set --> Add the other
        l2->insert(lab1);
    else if (l1!=to_merge.end() && l2==to_merge.end())  // Found one of the two in one set --> Add the other
        l1->insert(lab2);
    else if (l1!=l2)  // Both found in different sets --> Merge the two sets
    {
        l1->insert(l2->begin(), l2->end());
        to_merge.erase(l2);
    }
}

merging_sequence ucm2hier(const Eigen::Array<double,Eigen::Dynamic,Eigen::Dynamic>& ucm2, const Eigen::Array<label_type,Eigen::Dynamic,Eigen::Dynamic>& lp)
{
    /* Sizes */
    std::size_t sx = lp.rows();
    std::size_t sy = lp.cols();

    /*-------------------------------------------------------------*/
    /* Create LUT of contour positions and sets of labels to merge */
    /*-------------------------------------------------------------*/
    map_type ucm_th_leaves_pairs;

    /* Scan horizontal contours*/
    for (std::size_t xx=2; xx<2*sx; xx+=2)
    {
        for (std::size_t yy=1; yy<2*sy; yy+=2)
        {
            if (ucm2(xx,yy)>0)
            {
                std::size_t n1_x = (xx/2)-1;
                std::size_t n1_y = (yy-1)/2;
                std::size_t n2_x = xx/2;
                std::size_t n2_y = (yy-1)/2;
                label_type lab1 = std::min(lp(n1_x,n1_y),lp(n2_x,n2_y));
                label_type lab2 = std::max(lp(n1_x,n1_y),lp(n2_x,n2_y));

                /* Get the sets of regions to be merged                       */
                /* In one threshold there can be more than one region forming */
                map_type::iterator map_it = ucm_th_leaves_pairs.find(ucm2(xx,yy));
                if (map_it==ucm_th_leaves_pairs.end()) // New ucm threshold
                {
                    set_type set_to_put;
                     set_to_put.insert(lab1);
                     set_to_put.insert(lab2);
                    std::list<set_type> to_put;
                    to_put.push_back(set_to_put); // Add up and down neighbors
                    ucm_th_leaves_pairs.insert(map_type::value_type(ucm2(xx,yy), to_put));
                }
                else
                    insert_labels_to_merge(lab1,lab2,map_it->second);
            }
        }
    }

    /* Scan vertical contours*/
    for (std::size_t xx=1; xx<2*sx; xx+=2)
    {
        for (std::size_t yy=2; yy<2*sy; yy+=2)
        {
            if (ucm2(xx,yy)>0)
            {
                std::size_t n1_x = (xx-1)/2;
                std::size_t n1_y = (yy/2)-1;
                std::size_t n2_x = (xx-1)/2;
                std::size_t n2_y = yy/2;
                label_type lab1 = std::min(lp(n1_x,n1_y),lp(n2_x,n2_y));
                label_type lab2 = std::max(lp(n1_x,n1_y),lp(n2_x,n2_y));

                /* Get the sets of regions to be merged                       */
                /* In one threshold there can be more than one region forming */
                map_type::iterator map_it = ucm_th_leaves_pairs.find(ucm2(xx,yy));
                if (map_it==ucm_th_leaves_pairs.end()) // New ucm threshold
                {
                    set_type set_to_put;
                     set_to_put.insert(lab1);
                     set_to_put.insert(lab2);
                    std::list<set_type> to_put;
                    to_put.push_back(set_to_put); // Add left and right neighbors
                    ucm_th_leaves_pairs.insert(map_type::value_type(ucm2(xx,yy), to_put));
                }
                else
                    insert_labels_to_merge(lab1,lab2,map_it->second);
            }
        }
    }

    /* Number of thresholds is number of mergings */
    int n_merges = ucm_th_leaves_pairs.size();
    /*-------------------------------------------------------------*/

    /*-----------------------------------------------------------*/
    /*  Scan all thresholds of the UCM in increasing order and   */
    /*   do the mergings of the N regions that 'disappear'       */
    /*-----------------------------------------------------------*/

    /* Get number of leaves */
    label_type curr_max = 0;
    for (std::size_t xx=0; xx<sx; ++xx)
        for (std::size_t yy=0; yy<sy; ++yy)
            curr_max = std::max(curr_max, lp(xx,yy));
    label_type n_leaves = curr_max;

    /* Start LUT with leaves */
    std::vector<label_type> lut(n_leaves);
    std::vector<set_type> ilut(n_leaves);
    for (std::size_t ll=0; ll<n_leaves; ++ll)
    {
        lut[ll] = ll+1;
        ilut[ll].insert(ll+1);
    }

    /* Scan */
    merging_sequence ms;
    ms.n_max_children = 1;
    ms.start_ths = std::vector<double>(n_leaves,0);

    map_type::iterator it = ucm_th_leaves_pairs.begin();
    for ( ; it!=ucm_th_leaves_pairs.end(); ++it)
    {
        std::list<set_type>& leaves_to_merge = it->second;
        std::list<set_type>    regs_to_merge;

        std::list<set_type>::iterator it3 = leaves_to_merge.begin();
        for ( ; it3!=leaves_to_merge.end(); ++it3)
        {
            /* Get the labels of the children regions from the leave labels */
            set_type::iterator it4  = it3->begin();
            set_type::iterator it4b = it3->begin();
            ++it4b;

            for( ; it4b!=it3->end(); ++it4b)
                insert_labels_to_merge(lut[*it4-1],lut[*it4b-1],regs_to_merge);
        }

        it3 = regs_to_merge.begin();
        for ( ; it3!=regs_to_merge.end(); ++it3)
        {
            curr_max += 1;
            ilut.push_back(set_type());

            /* Update LUT */
            set_type to_update(*it3);
            while(!to_update.empty())
            {
                /* Get the first to update */
                label_type curr = *(to_update.begin());
                to_update.erase(to_update.begin());
                if (lut[curr-1]!=curr)
                    to_update.insert(lut[curr-1]);

                set_type::iterator it5 = ilut[curr-1].begin();
                for( ; it5!=ilut[curr-1].end(); ++it5)
                {
                    lut[*it5-1] = curr_max;
                    ilut[curr_max-1].insert(*it5);
                }
                ilut[curr-1].clear();
            }

            /* Copy the labels to the merging sequence */
            ms.children_labels.push_back(std::list<label_type>(it3->begin(),it3->end()));
            ms.n_max_children = std::max(ms.n_max_children,it3->size());

            /* Add parent to the LUT and to out*/
            lut.push_back(curr_max);
            ms.parent_labels.push_back(curr_max);

            /* Store the start contour threshold */
            ms.start_ths.push_back(it->first);
        }
    }
    ms.n_regs = curr_max;
    ms.n_leaves = n_leaves;
    /*-----------------------------------------------------------*/

    if(ms.n_regs!=ms.start_ths.size())
        mexErrMsgTxt("Oh oh");

    return ms;
}

#endif


#include <boost/test/unit_test.hpp>
#include <rushhour_schedule_fixed.h>

namespace rushhour {
namespace schedule {

BOOST_AUTO_TEST_CASE(Fixed_SteadyPeriod) {
  Fixed schedule(170.0, 100);
  BOOST_REQUIRE_EQUAL(100, schedule.size());

  for (auto ns : schedule) {
    BOOST_CHECK_CLOSE(1.0e9 / 170.0, static_cast<double>(ns.count()), 1.0);
  }
}

BOOST_AUTO_TEST_CASE(Fixed_SizeZero) {
  Fixed schedule(75.0, 0);
  BOOST_REQUIRE_EQUAL(0, schedule.size());
  BOOST_CHECK_THROW(schedule[0], std::out_of_range);
}

BOOST_AUTO_TEST_CASE(Fixed_OutOfBounds) {
  Fixed schedule(75.0, 5);
  BOOST_REQUIRE_EQUAL(5, schedule.size());
  BOOST_CHECK_THROW(schedule[5], std::out_of_range);
}

}  // namespace schedule
}  // namespace rushhour


// Copyright 2019 Fred Hutchinson Cancer Research Center
// See the included LICENSE file for details on the licence that is granted to the user of this software.
#include <cytolib/POPINDICES.hpp>
#include <boost/foreach.hpp>

namespace cytolib
{
void packToBytes(const vector <bool> & x, vector<unsigned char> & bytes){
	/*
	 * pack bits into bytes
	 */

	for(unsigned i =0 ; i < x.size(); i++) {
		unsigned byteIndex = i / 8;
		unsigned bitIndex = i % 8;
		if(x[i]) {
			// set bit
			unsigned char mask  = 1 << bitIndex;
			bytes[byteIndex] = bytes[byteIndex] | mask;
		}
	}

}
void unpackFromBytes(vector <bool> & x, const vector<unsigned char>& x_bytes){

	/*
	 * unpack bytes into bits
	 */
	for(unsigned i =0 ; i < x.size(); i++) {
		unsigned byteIndex = i / 8;
		unsigned bitIndex = i % 8;

		x[i] = x_bytes[byteIndex] & (1 << bitIndex);
	}

}

	BOOLINDICES::BOOLINDICES(vector <unsigned> _ind, unsigned _nEvent){
		nEvents = _nEvent;
		x.resize(nEvents);
		for(auto i : _ind)
			x[i] = true;
	}
	BOOLINDICES::BOOLINDICES(vector <bool> _ind){
		x=_ind;
		nEvents=_ind.size();

	}
	vector<unsigned> BOOLINDICES::getIndices_u(){
		vector<unsigned> res;

		for(unsigned i = 0; i < x.size(); i++){
			if(x[i])
				res.push_back(i);
		}
		return res;
	}

	void BOOLINDICES::convertToPb(pb::POPINDICES & ind_pb){
		ind_pb.set_indtype(pb::BOOL);
		unsigned nBits=x.size();
		unsigned nBytes=ceil(float(nBits)/8);
		vector<unsigned char> bytes(nBytes,0);
		packToBytes(x, bytes);
		string * byte_pb = ind_pb.mutable_bind();
		for(unsigned i = 0; i < bytes.size(); i++){
			unsigned char byte = bytes[i];
			byte_pb->append(string(1, byte));
		}
		ind_pb.set_nevents(nEvents);
	}
	BOOLINDICES::BOOLINDICES(const pb::POPINDICES & ind_pb){
		nEvents = ind_pb.nevents();
		//fetch byte stream from pb
		vector<unsigned char> bytes(ind_pb.bind().begin(),ind_pb.bind().end());
		//convert it to bit vector
		x.resize(nEvents,false);
		unpackFromBytes(x, bytes);
	}

	INTINDICES::INTINDICES(vector <bool> _ind){

		for(vector<bool>::iterator it=_ind.begin();it!=_ind.end();it++)
		{
			unsigned i=it-_ind.begin();
			if(*it)
				x.push_back(i);
		}
		nEvents=_ind.size();
	}

	vector<bool> INTINDICES::getIndices(){

		vector<bool> res(nEvents,false);

		for(vector<unsigned>::iterator it=x.begin();it!=x.end();it++){
			unsigned i=*it;
			res[i]=true;
		}
		return res;
	}

	void INTINDICES::convertToPb(pb::POPINDICES & ind_pb){
		ind_pb.set_indtype(pb::INT);
		BOOST_FOREACH(vector<unsigned>::value_type & it, x){
			ind_pb.add_iind(it);
		}
		ind_pb.set_nevents(nEvents);
	}

	INTINDICES::INTINDICES(const pb::POPINDICES & ind_pb){
		nEvents = ind_pb.nevents();
		unsigned nSize = ind_pb.iind_size();
		x = vector<unsigned>(nSize);
		for(unsigned i = 0; i < nSize; i++)
			x[i] = ind_pb.iind(i);
	}

	vector<unsigned> ROOTINDICES::getIndices_u(){

		vector<unsigned> res(nEvents);
		for(unsigned i = 0; i < nEvents; i++)
			res[i]=i;
		return res;
	}

};


//-----------------------------------------------------------------------------
// boost-libs futures/test/or_test.cpp source file
// See http://www.boost.org for updates, documentation, and revision history.
//-----------------------------------------------------------------------------
//
// Copyright (c) 2005
// Thorsten Schuett
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/test/minimal.hpp>

#if BOOST_MSVC != 0
#pragma warning (disable: 4355)   // this is used in member initialiser list
#endif

#include <boost/bind.hpp>
#include <boost/futures.hpp>

int fac(int n)
{
    if(n == 0)
        return 0;
    if(n == 1)
        return 1;
    return n * fac(n - 1);
}

double fac8()
{
    return (double)fac(8);
}

struct fac_functor
{
    fac_functor(int n) : n(n) {}

    int operator()()
    {
        return fac(n);
    }

    int n;
};

int test_main(int , char* [])
{
    using namespace boost::futures;
    using namespace boost;

    future<int> f1 = bind(fac, 4);
    future<double> f2 = fac8;
    future<int> f3 = fac_functor(6);

    variant<int, double> v = futurize(f1 || f2 || f3);

    std::cout << v.which() << ": " << v << std::endl;
    BOOST_CHECK((0 == v.which() && boost::get<int>(v) == 24) ||
                (0 == v.which() && boost::get<int>(v) == 720) ||
                (1 == v.which() && boost::get<double>(v) == 40320.0));

    BOOST_CHECK(f1() == 24);
    BOOST_CHECK(f2() == 40320);
    BOOST_CHECK(f3() == 720);

    return boost::exit_success;
}


//
// boost-geometry-render
//
// Copyright (c) 2011
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <cstdlib>
#include <gl/glut.h>
#include <boost/geometry/geometries/linestring.hpp>
#include <gl/graphics.hpp>
#include <gl/geometry_render.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/thread.hpp>

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

	typedef bg::model::d2::point_xy<float> point_type;
	typedef bg::model::multi_point<point_type> multi_point_type;
	typedef bg::model::polygon<point_type> polygon_type;
	multi_point_type multi_point;

	// 適当な描画管理クラス
	// arc, argv, width, height
	gl::graphics g(argc, argv, 500, 500);

	// glutDisplayFunc にコールバック関数を登録
	// convex_hull の描画
	gl::displayfunc(g, [&](){
		namespace bg = boost::geometry;
		if(multi_point.empty())	return;

		// convex_hull の取得
		// multi_point からでは直接取得出来ないので、
		// 一旦 Polygon に変換してから取得を行う
		polygon_type tmp;
		bg::assign_points(tmp, multi_point);
		bg::model::polygon<point_type> hull;
		bg::convex_hull(tmp, hull);
		glColor3f(0.0f, 1.0f, 0.0f);
		// 取得した Polygon の描画
		gl::render(hull);
	});

	// multi_point の描画
	// 登録順に描画が行われる
	gl::displayfunc(g, [&](){
		// gl の描画設定
		glColor3f(1.0f, 0.0f, 0.0f);
		glPointSize(5.0f);
		// Geometry の描画
		gl::render(multi_point);
	});

	// マウスイベント
	gl::mousefunc(g, [&](int button, int state, int x, int y){
		// 左クリックが押された時のみ頂点を追加する
		if(button != GLUT_LEFT_BUTTON || state != GLUT_DOWN)	return;

		// クリックされたウィンドウ座標をスクリーン座標に変換して Geometry に追加
		multi_point.push_back(point_type(x / 250.0f - 1.0f, y / -250.0f + 1.0f));
	});

	// キー入力イベント
	gl::keyboardfunc(g, [](unsigned char key, int x, int y){
		if(key == 'q') return exit(0);
	});

	// idle
	gl::idlefunc(g, [&](){
		boost::this_thread::sleep(boost::posix_time::milliseconds(16));
		glutPostRedisplay();
	});

	// glut のメインループ
	g.run();

	return 0;
}


#include <boost/numeric/interval/utility.hpp>


// Copyright (c) 1997-2001  ETH Zurich (Switzerland).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Kaspar Fischer <fischerk@inf.ethz.ch>

#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cstdlib>

#include <CGAL/boost/iterator/transform_iterator.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/shared_ptr.hpp>

#include <CGAL/Exact_rational.h>
#include <CGAL/MP_Float.h>
#include <CGAL/QP_solver/QP_solver.h>
#include <CGAL/QP_solver/QP_full_exact_pricing.h>
#include <CGAL/QP_solver/QP_exact_bland_pricing.h>
#include <CGAL/QP_solver/QP_partial_exact_pricing.h>
#include <CGAL/QP_solver/QP_full_filtered_pricing.h>
#include <CGAL/QP_solver/QP_partial_filtered_pricing.h>

#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>

#include <CGAL/boost_mp.h>
//Currently already included in boost_mp.h
//#ifdef CGAL_USE_BOOST_MP
//# include <boost/multiprecision/cpp_int.hpp>
//// After some CGAL includes so we get a chance to define CGAL_USE_GMP.
//# ifdef CGAL_USE_GMP
//#  include <boost/multiprecision/gmp.hpp>
//# endif
//#endif

// Routines to output to MPS format:
namespace QP_from_mps_detail {

  template<typename T>
  struct MPS_type_name {
    static const char *name() { return nullptr; }
  };

  template<>
  struct MPS_type_name<double> {
    static const char *name() { return "floating-point"; }
  };

  template<>
  struct MPS_type_name<int> {
    static const char *name() { return "integer"; }
  };
#ifdef CGAL_USE_BOOST_MP
  template <class Backend, boost::multiprecision::expression_template_option Eto>
  struct MPS_type_name<boost::multiprecision::number<Backend, Eto> > {
    typedef boost::multiprecision::number<Backend, Eto> NT;
    static const char *name() {
      if (boost::multiprecision::number_category<NT>::value == boost::multiprecision::number_kind_integer)
        return "integer";
      else if (boost::multiprecision::number_category<NT>::value == boost::multiprecision::number_kind_rational)
        return "rational";
      else
        return nullptr;
    }
  };
#endif
#ifdef CGAL_USE_GMPXX
  template<>
  struct MPS_type_name<mpq_class> {
    static const char *name() { return "rational"; }
  };
#endif
#ifdef CGAL_USE_GMP
  template<>
  struct MPS_type_name<CGAL::Gmpq> {
    static const char *name() { return "rational"; }
  };
#endif

#ifdef CGAL_USE_LEDA
  template<>
  struct MPS_type_name<leda::rational> {
    static const char *name() { return "rational"; }
  };
#endif
  template<>
  struct MPS_type_name<CGAL::Quotient<CGAL::MP_Float> > {
    static const char *name() { return "rational"; }
  };

  template<typename IT>
  struct IT_to_ET {
  };

  template<>
  struct IT_to_ET<double> {
    typedef CGAL::MP_Float ET;
  };
#ifdef CGAL_USE_BOOST_MP
  template<>
  struct IT_to_ET<boost::multiprecision::cpp_rational> {
    typedef boost::multiprecision::cpp_rational ET;
  };
#endif

#ifdef CGAL_USE_GMP
#ifdef CGAL_USE_GMPXX
  template<>
  struct IT_to_ET<int> {
    typedef mpz_class ET;
  };

  template<>
  struct IT_to_ET<mpq_class> {
    typedef mpq_class ET;
  };
#elif defined CGAL_USE_BOOST_MP
  template<>
  struct IT_to_ET<int> {
    typedef boost::multiprecision::mpz_int ET;
  };

  template<>
  struct IT_to_ET<boost::multiprecision::mpq_rational> {
    typedef boost::multiprecision::mpq_rational ET;
  };
#else
  template<>
  struct IT_to_ET<int> {
    typedef CGAL::Gmpz ET;
  };
#endif

  template<>
  struct IT_to_ET<CGAL::Gmpq> {
    typedef CGAL::Gmpq ET;
  };
#endif

#ifdef CGAL_USE_LEDA
// Pick one arbitrarily if we have both LEDA and GMP
#ifndef CGAL_USE_GMP
  template<>
  struct IT_to_ET<int> {
    typedef leda::integer ET;
  };
#endif

  template<>
  struct IT_to_ET<leda::rational> {
    typedef leda::rational ET;
  };
#endif
  template<>
  struct IT_to_ET<CGAL::Quotient<CGAL::MP_Float> > {
    typedef CGAL::Quotient<CGAL::MP_Float> ET;
  };

#if defined CGAL_USE_BOOST_MP && !defined CGAL_USE_GMP && !defined CGAL_USE_LEDA
  // Last chance for int
  template<>
  struct IT_to_ET<int> {
    typedef boost::multiprecision::cpp_int ET;
  };
#endif
} // QP_from_mps_detail

template<typename QP>
void write_MPS(std::ostream& out,
               const std::string& number_type, // pass "" to deduce
                                               // the number-type from
                                               // U_iterator::value_type
               const std::string& description,
               const std::string& generator_name,
               const std::string& problem_name,
               const QP& qp)
{
  // output header:
  if (number_type.length() == 0) {
    const char *tn = QP_from_mps_detail::MPS_type_name
      <typename
      std::iterator_traits<typename QP::U_iterator>::value_type>::name();
    if (tn != nullptr)
      out << "* Number-type: " << tn << "\n";
  } else
      out << "* Number-type: " << number_type << "\n";
  out << "* Description: " << description << "\n"
      << "* Generated-by: " << generator_name << "\n";

  // print in qmatrix format
  CGAL::print_quadratic_program(out, qp, problem_name);
}

boost::shared_ptr<std::ofstream>
create_output_file(const char *filename, // Note: "Bernd3" and not
                                         // "Bernd3.mps".
                   const char *directory,
                   const char *suffix)   // Note: "shifted"
                                         // and not "_shifted".
{
  std::string new_name = std::string(directory) +
    std::string("/") + std::string(filename) + std::string("_") +
    std::string(suffix) + std::string(".mps");
  return boost::shared_ptr<std::ofstream>(new std::ofstream(new_name.c_str(),
                                                            std::ios_base::trunc |
                                                            std::ios_base::out));
}

template<typename NT>
struct tuple_add :
  public CGAL::cpp98::unary_function<const boost::tuple<NT, NT>&, NT>
{
  NT operator()(const boost::tuple<NT, NT>& t) const
  {
    return boost::tuples::get<0>(t) + boost::tuples::get<1>(t);
  }
};

template<typename IT,   // input number type
         typename ET>   // exact number type compatible with IT (ET is used,
                        // for instance, by QP in the query methods
                        // has_equalities_only_and_full_rank())
void create_shifted_instance(const CGAL::Quadratic_program_from_mps <IT>& qp,
                             const char *file,   // Note: "Bernd3" and
                                                 // not "Bernd3.mps".
                             const char *dir)
{
  // This routine implements the following transformation:
  //
  //   A  -> A
  //   b  -> b + A v
  //   c  -> c - 2 v^T
  //   D  -> D
  //   row_types -> row_types
  //   l  -> l + v
  //   u  -> u + v
  //   fl -> fl
  //   fu -> fu
  //
  // where v = [1,...,n]^T.

  // extract data from qp:
  const int n = qp.get_n();
  const int m = qp.get_m();

  // offset vector:
  std::vector<IT> v(n);
  for (int i=0; i<n; ++i)
    v[i] = i+1;

  // compute A v into Av;
  std::vector<IT> Av(m, IT(0));
  for (int i=0; i<m; ++i)
    for (int j=0; j<n; ++j)
      Av[i] += (const IT&)(*(qp.get_a()+j))[i] * v[j];

  // compute - 2 v^T D into mvTD:
  std::vector<IT> mvTD(n, IT(0));  // -2D^Tv
  for (int i=0; i<n; ++i) {
    for (int j=0; j<n; ++j)
      mvTD[i]
        += ( j <= i ? (const IT&)(*(qp.get_d()+i))[j] : (const IT&)(*(qp.get_d()+j))[i]) * v[j];
    mvTD[i] *= -1;
  }

  // output:
  using boost::make_transform_iterator;
  using boost::make_zip_iterator;
  boost::shared_ptr<std::ofstream> out = create_output_file(file, dir, "shifted");

  write_MPS(*out,
                  "", // deduce number-type
                  "Shifted instance of original file",
                  "master_mps_to_derivatives-create_shifted_instance",
                  qp.get_problem_name(),
                  CGAL::make_quadratic_program_from_iterators(
     n,
     m,
     qp.get_a(),
     make_transform_iterator(
                             make_zip_iterator(boost::make_tuple(qp.get_b(),Av.begin())),
                             tuple_add<IT>()),
     qp.get_r(),
     qp.get_fl(),
     make_transform_iterator(
                              make_zip_iterator(boost::make_tuple(qp.get_l(),v.begin())),
                              tuple_add<IT>()),
     qp.get_fu(),
     make_transform_iterator(
                             make_zip_iterator(boost::make_tuple(qp.get_u(),v.begin())),
                             tuple_add<IT>()),
     qp.get_d(),
     make_transform_iterator(
                           make_zip_iterator(boost::make_tuple(qp.get_c(),mvTD.begin())),
                           tuple_add<IT>()),
     qp.get_c0()
     )
);
  out->close();
}

template<typename IT,   // input number type
         typename ET>   // exact number type compatible with IT (ET is used,
                        // for instance, by QP in the query methods
                        // has_equalities_only_and_full_rank())
void create_free_instance(CGAL::Quadratic_program_from_mps<IT>& qp,
                          const char *file,   // Note: "Bernd3" and
                                              // not "Bernd3.mps".
                          const char *dir)
{
  // This routine converts the given instance into an equivalent
  // problem where all bounds are modelled by additional rows of A and
  // where all variables are free.
  //
  // That is, the quantities c and D do not change, but A, b, and
  // row_types are augmented by at most 2n additional rows/entries
  // (and fl and fu are adjusted as well).

  // extract data from qp:
  const unsigned int n = qp.get_n();
  unsigned int m = qp.get_m();

  // add rows to A and corresponding entries to b:
  for (unsigned int i=0; i<n; ++i) {
    if (*(qp.get_fl()+i)) {                        // x >= l
      // add a row to A:
      for (unsigned int j=0; j<n; ++j)
        qp.set_a (j, m, (i==j? 1 : 0));

      // add corresponding entry to b:
      qp.set_b(m, qp.get_l()[i]);

      // add corresponding row type:
      qp.set_r(m, CGAL::LARGER);
      ++m;
    }
    qp.set_l(i, false);                           // variable becomes free
    if (*(qp.get_fu()+i)) {                        // x <= u
      // add a row to A:
      for (unsigned int j=0; j<n; ++j)
        qp.set_a (j, m ,(i==j? 1 : 0));

      // add corresponding entry to b:
      qp.set_b(m, qp.get_u()[i]);

      // add corresponding row type:
      qp.set_r(m, CGAL::SMALLER);
      ++m;
    }
    qp.set_u(i, false);                         // variable becomes free
  }
  // output:
  boost::shared_ptr<std::ofstream> out = create_output_file(file, dir, "free");
  write_MPS(*out,
                  "", // deduce number-type
                  "Freed instance of original file",
                  "master_mps_to_derivatives-create_free_instance",
                  qp.get_problem_name(),
            qp);
  out->close();
}

template<typename IT>
bool create_derivatives(const char *path,
                        const char *file,
                        const char *dir,
                        std::string& msg)
{
  using std::cerr;
  using CGAL::Tag_true;
  using CGAL::Tag_false;

  // diagnostics:
  cerr << "  Trying to load input MPS using "
       << QP_from_mps_detail::MPS_type_name<IT>::name()
       << " number-type...\n";

  // open input file:
  std::ifstream f(path);
  if (!f) {
    cerr << "    Could not open file '" << path << "'.\n";
    return false;
  }

  // load QP instance:
  typedef typename QP_from_mps_detail::IT_to_ET<IT>::ET ET;
  typedef CGAL::Quadratic_program_from_mps<IT> QP;
  QP qp(f);

  // check for format errors in MPS file:
  if (!qp.is_valid()) {
    msg = "Input is not a valid MPS file: " + qp.get_error();
    return false;
  }
  cerr << "    MPS-file successfully input.\n";

  // no derivatives if comment says so
  if (qp.get_comment().find(std::string("Derivatives: none"))!=std::string::npos)
    cerr << "    No derivatives made.\n";
  else {
    // derivates:
    create_shifted_instance<IT, ET>(qp, file, dir);
    create_free_instance<IT, ET>(qp, file, dir);
    // Note: insert additional derivative routines here! Your routine may use
    // create_output_file() to create the output file.
  }
  // cleanup:
  f.close();
  return true;
}

int main(const int argnr, const char **argv) {
  typedef CGAL::Exact_rational Rational;

  // output usage information:
  if (argnr != 4) {
    std::cerr << "Usage: " << argv[0] << " path-to-master.mps name "
              << "dest-directory\n\n"
              << "Given a master MPS-file, this program constructs from it "
              << "several other, derived\nMPS-files and saves them in the "
              << "destination directory.\n\n"
              << "The argument 'name' should be the basename (without "
              << "extension) of the\nargument 'path-to-master.mps'.\n\n"
              << "Usually, you do not have to call this program directly; "
              << "./create_testsuite does\nit for you automagically.\n";
    return 0; // Note: 0 because otherwise the testsuite will fail...
  }

  // extract arguments:
  const char *path = argv[1];
  const char *file = argv[2];
  const char *dir  = argv[3];

  // As we do not know the number-type used in the MPS-file, we simply try
  // to load the MPS-file once with a double type, once with a Rational
  // type, and once with an int type.
  // we try the most special one first, so the order is
  // int -> double -> rational
  std::string message;
  if (!create_derivatives<int>(path, file, dir, message))
    if (!create_derivatives<double>(path, file, dir, message))
      if (!create_derivatives<Rational>(path, file, dir, message)) {
        // Here, the MPS-file must be ill-formatted.
        std::cerr << "  " << message << "\n";
        return 2;
      }

  return 0;
}


#include "IntervalMap.hpp"
#include <iostream>
#include <string>
#include <sstream>
#include <boost/icl/interval_map.hpp>
#include <random>
#include <fstream>

//////////////////////////////////////////////////////////////////////////

#define CHECK(cond) (void)( (!!(cond)) || (std::terminate(), 0) )

template<class CharT, class TraitsT, class K, class V>
std::basic_ostream<CharT, TraitsT> &operator<<( std::basic_ostream<CharT, TraitsT> &os, const interval_map<K, V> &map )
{
  map.print( os );
  return os;
}
//////////////////////////////////////////////////////////////////////////
template<class K, class V>
void assign_check( interval_map<K, V> &map,
  const typename interval_map<K, V>::key_type &keyFirst,
  const typename interval_map<K, V>::key_type &keyLast,
  const typename interval_map<K, V>::mapped_type &val,
  const char *szContent
)
{
  std::cout << '[' << keyFirst << ", " << keyLast << ") = " << val << std::endl;
  map.set( keyFirst, keyLast, val );
  map.print_dbg( std::cout );
  std::cout << "     " << map << std::endl << std::endl;

  std::stringstream content;
  content << map;

  CHECK( content.str() == szContent );
}
//////////////////////////////////////////////////////////////////////////

void BasicOperations()
{
  interval_map<unsigned short, std::string> map("Z");

  assign_check(map, 0, 0, "Y","[0,65535)=Z");
  assign_check(map, 3, 7, "B","[0,3)=Z [3,7)=B [7,65535)=Z");
  assign_check(map, 5, 9, "C","[0,3)=Z [3,5)=B [5,9)=C [9,65535)=Z");
  assign_check(map, 6, 7, "D","[0,3)=Z [3,5)=B [5,6)=C [6,7)=D [7,9)=C [9,65535)=Z");
  assign_check(map, 3, 5, "F","[0,3)=Z [3,5)=F [5,6)=C [6,7)=D [7,9)=C [9,65535)=Z");
  assign_check(map, 0, 5, "E","[0,5)=E [5,6)=C [6,7)=D [7,9)=C [9,65535)=Z");
  assign_check(map, 3, 3, "G","[0,5)=E [5,6)=C [6,7)=D [7,9)=C [9,65535)=Z");
  assign_check(map, 0, 10, "A","[0,10)=A [10,65535)=Z");
}
//////////////////////////////////////////////////////////////////////////

void ElementAccess()
{
  interval_map<unsigned short, char> map('Z');
  assign_check(map, 3, 7, 'B',"[0,3)=Z [3,7)=B [7,65535)=Z");
  assign_check(map, 5, 9, 'C',"[0,3)=Z [3,5)=B [5,9)=C [9,65535)=Z");

  const char rgTst[] = "ZZZBBCCCCZZZZZZZZZZZZZ";

  for( unsigned short i = 0; i < std::size(rgTst) - 1; ++i )
    CHECK( rgTst[i] == map[i] );
}
//////////////////////////////////////////////////////////////////////////

void IntervalMerge()
{
  interval_map<unsigned short, std::string> map("Z");

  assign_check(map, 0, 0, "Y", "[0,65535)=Z");
  assign_check(map, 3, 7, "Z", "[0,65535)=Z");
  assign_check(map, 3, 7, "B", "[0,3)=Z [3,7)=B [7,65535)=Z");
  assign_check(map, 5, 9, "C", "[0,3)=Z [3,5)=B [5,9)=C [9,65535)=Z");
  assign_check(map, 3, 4, "Z", "[0,4)=Z [4,5)=B [5,9)=C [9,65535)=Z");
  assign_check(map, 0, 5, "Z", "[0,5)=Z [5,9)=C [9,65535)=Z");
  assign_check(map, 10, 20, "Z", "[0,5)=Z [5,9)=C [9,65535)=Z");
  assign_check(map, 7, 20, "Z", "[0,5)=Z [5,7)=C [7,65535)=Z");

  auto map2{ map }, map3{ map };

  assign_check(map, 2, 5, "Z", "[0,5)=Z [5,7)=C [7,65535)=Z");
  assign_check(map2, 2, 8, "Z", "[0,65535)=Z");
  assign_check(map, 2, 7, "Z", "[0,65535)=Z");
  assign_check(map3, 0, 7, "Z", "[0,65535)=Z");
}
//////////////////////////////////////////////////////////////////////////

template<class CharT, class TraitsT, class K, class V>
void print( std::basic_ostream<CharT, TraitsT> &os, const boost::icl::interval_map<K, V> &map )
{
  for( auto it = map.begin(); it != map.end(); ++it )
  {

    if( it != map.begin() )
      os << " ";

    os << it->first << "=" << it->second;
  }
}
//////////////////////////////////////////////////////////////////////////

template<class T = std::mt19937, std::size_t N = T::state_size * sizeof( typename T::result_type )>
T ProperlySeededRandomEngine()
{
    std::random_device source;
    std::random_device::result_type random_data[(N - 1) / sizeof( source() ) + 1];
    std::generate( std::begin( random_data ), std::end( random_data ), std::ref( source ) );
    std::seed_seq seeds( std::begin( random_data ), std::end( random_data ) );
    return T{ seeds };
}
//////////////////////////////////////////////////////////////////////////
void RandomCompareWithBoostIcl()
{
  namespace icl = boost::icl;
  typedef std::uniform_int_distribution<> TRngGen;
  typedef unsigned short TKey;
  typedef char TVal;

  interval_map<TKey, TVal> map('A');
  icl::interval_map<TKey, TVal> bstMap;
  bstMap.set( std::make_pair( icl::interval<TKey>::right_open(0, std::numeric_limits<TKey>::max()), 'A') );

  auto rng { ProperlySeededRandomEngine() };
  //TRngGen keysRnd(0, std::numeric_limits<TKey>::max() - 1 );
  TRngGen keysRnd(0, 20 );
  TRngGen valsRnd('A', 'A' + 10 );

  std::ofstream flOut("RandomCompareWithBoostIclOutput.txt");

  for( int i = 0; i < 100000; ++i )
  {
    TKey left( keysRnd(rng) ), right( keysRnd(rng) );
    TVal val( valsRnd(rng) );

    if( left > right )
      std::swap(left, right);

    map.set( left, right, val );
    bstMap.set( std::make_pair( icl::interval<TKey>::right_open(left, right), val) );

    std::stringstream myContent;
    myContent << map;

    std::stringstream bstContent;
    print( bstContent, bstMap );

    flOut << '[' << left << ", " << right << ") = " << val << " | " << myContent.str() << std::endl;

    if( bstMap.iterative_size() == 1 )
      flOut << "<<EMPTY>>" << std::endl;

    CHECK( myContent.str() == bstContent.str() );
  }
}
//////////////////////////////////////////////////////////////////////////

int main()
{
  BasicOperations();
  ElementAccess();
  IntervalMerge();
  RandomCompareWithBoostIcl();

  std::cout << "Done" << std::endl;

  return 0;
}


/*
 symmetry.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <cmath>
#include <iostream>
#include <iomanip>
#include "mathfunctions.h"
#include "symmetry.h"
#include "system.h"
#include "memory.h"
#include "constants.h"
#include "timer.h"
#include "error.h"
#include "interaction.h"
#include "files.h"
#include <cmath>
#include <vector>
#include <set>
#include <algorithm>

#ifdef _USE_EIGEN
#include <Eigen/Core>
#endif

using namespace ALM_NS;

Symmetry::Symmetry(ALM *alm) : Pointers(alm)
{
    file_sym = "SYMM_INFO";
}

Symmetry::~Symmetry()
{
    memory->deallocate(symrel);
    memory->deallocate(symrel_int);
    memory->deallocate(tnons);
    memory->deallocate(map_sym);
    memory->deallocate(map_p2s);
    memory->deallocate(map_s2p);
    memory->deallocate(symnum_tran);
    memory->deallocate(sym_available);
}

void Symmetry::init()
{
    int i, j;
    int nat = system->nat;

    std::cout << " SYMMETRY" << std::endl;
    std::cout << " ========" << std::endl << std::endl;

    setup_symmetry_operation(nat, nsym, system->lavec, system->rlavec,
        system->xcoord, system->kd);

    memory->allocate(tnons, nsym, 3);
    memory->allocate(symrel_int, nsym, 3, 3);

    int isym = 0;
    for (std::vector<SymmetryOperation>::iterator iter = SymmList.begin(); iter != SymmList.end(); ++iter) {
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel_int[isym][i][j] = (*iter).rot[i][j];
            }
        }
        for (i = 0; i < 3; ++i) {
            tnons[isym][i] = (*iter).tran[i];
        }
        ++isym;
    }

    std::cout << "  Number of symmetry operations = " << nsym << std::endl;
    memory->allocate(symrel, nsym, 3, 3);
    symop_in_cart(system->lavec, system->rlavec);

    memory->allocate(sym_available, nsym);
    int nsym_fc;
    symop_availability_check(symrel, sym_available, nsym, nsym_fc);

    if (nsym_fc == nsym) {
        std::cout << "  All symmetry operations will be used to reduce the number of force constants." << std::endl;
    } else {
        std::cout << "  " << nsym_fc << " symmetry operations out of "
            << nsym << " will be used to reduce the number of parameters." << std::endl;
        std::cout << "  Other " << nsym - nsym_fc
            << " symmetry operations will be imposed as constraints." << std::endl;
    }
    std::cout << std::endl;

    pure_translations();

    memory->allocate(map_sym, nat, nsym);
    memory->allocate(map_p2s, natmin, ntran);
    memory->allocate(map_s2p, nat);

    genmaps(nat, system->xcoord, map_sym, map_p2s, map_s2p);

    std::cout << std::endl;
    std::cout << "  **Cell-Atom Correspondens Below**" << std::endl;
    std::cout << std::setw(6) << " CELL" << " | " << std::setw(5) << "ATOM" << std::endl;

    for (int i = 0; i < ntran; ++i) {
        std::cout << std::setw(6) << i + 1 << " | ";
        for (int j = 0; j < natmin; ++j)  {
            std::cout << std::setw(5) << map_p2s[j][i] + 1;
            if((j + 1)%5 == 0) {
                std::cout << std::endl << "       | ";
            }
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;

    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;
}

void Symmetry::setup_symmetry_operation(int nat, unsigned int &nsym,
                                        double aa[3][3], double bb[3][3], double **x, int *kd)
{
    int i, j;

    SymmList.clear();

    if (nsym == 0) {

        // Automatically find symmetries.

        std::cout << "  NSYM = 0 : Trying to find symmetry operations." << std::endl;
        std::cout << "             Please be patient. " << std::endl;
        std::cout << "             This can take a while for a large supercell." << std::endl << std::endl;

        findsym(nat, aa, x, SymmList);
        // The order in SymmList changes for each run because it was generated
        // with OpenMP. Therefore, we sort the list here to have the same result.
        std::sort(SymmList.begin()+1,SymmList.end());
        nsym = SymmList.size();

        if (is_printsymmetry) {
            std::ofstream ofs_sym;
            std::cout << "  PRINTSYM = 1: Symmetry information will be stored in SYMM_INFO file."
                << std::endl << std::endl;
            ofs_sym.open(file_sym.c_str(), std::ios::out);
            ofs_sym << nsym << std::endl;

            for (std::vector<SymmetryOperation>::iterator p = SymmList.begin(); p != SymmList.end(); ++p) {
                for (i = 0; i < 3; ++i) {
                    for (j = 0; j < 3; ++j) {
                        ofs_sym << std::setw(4) << (*p).rot[i][j];
                    }
                }
                ofs_sym << "  ";
                for (i = 0; i < 3; ++i) {
                    ofs_sym << std::setprecision(15) << std::setw(20) << (*p).tran[i];
                }
                ofs_sym << std::endl;
            }

            ofs_sym.close();
        }

    } else if (nsym == 1) {

        // Identity operation only !

        std::cout << "  NSYM = 1 : Only the identity matrix will be considered." << std::endl << std::endl;

        int rot_tmp[3][3];
        double tran_tmp[3];

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                if (i == j) {
                    rot_tmp[i][j] = 1;
                } else {
                    rot_tmp[i][j] = 0;
                }
            }
            tran_tmp[i] = 0.0;
        }

        SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));

    } else {

        std::cout << "  NSYM > 1 : Symmetry operations will be read from SYMM_INFO file" << std::endl << std::endl;

        int nsym2;
        int rot_tmp[3][3];
        double tran_tmp[3];
        std::ifstream ifs_sym;

        ifs_sym.open(file_sym.c_str(), std::ios::in);
        ifs_sym >> nsym2;

        if (nsym != nsym2) error->exit("setup_symmetry_operations",
            "nsym in the given file and the input file are not consistent.");

        for (i = 0; i < nsym; ++i) {
            ifs_sym >> rot_tmp[0][0] >> rot_tmp[0][1] >> rot_tmp[0][2]
            >> rot_tmp[1][0] >> rot_tmp[1][1] >> rot_tmp[1][2]
            >> rot_tmp[2][0] >> rot_tmp[2][1] >> rot_tmp[2][2]
            >> tran_tmp[0] >> tran_tmp[1] >> tran_tmp[2];

            SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));
        }
        ifs_sym.close();
    }

#ifdef _DEBUG
    print_symmetrized_coordinate(x);
#endif
}

void Symmetry::findsym(int nat, double aa[3][3], double **x, std::vector<SymmetryOperation> &symop_all) {

    std::vector<RotationMatrix> LatticeSymmList;

    // Generate rotational matrices that don't change the metric tensor
    LatticeSymmList.clear();
    find_lattice_symmetry(aa, LatticeSymmList);

    // Generate all the space group operations with translational vectors
    symop_all.clear();
    find_crystal_symmetry(nat, system->nclassatom, system->atomlist_class, x,
        LatticeSymmList, symop_all);

    LatticeSymmList.clear();
}

void Symmetry::find_lattice_symmetry(double aa[3][3], std::vector<RotationMatrix> &LatticeSymmList) {

    /*
    Find the rotational matrices that leave the metric tensor invariant.

    Metric tensor G = (g)_{ij} = a_{i} * a_{j} is invariant under crystal symmetry operations T,
    i.e. T^{t}GT = G. Since G can be written as G = A^{t}A, the invariance condition is given by
    (AT)^{t}(AT) = G0 (original).
    */

    int i, j, k;
    int m11, m12, m13, m21, m22, m23, m31, m32, m33;

    int nsym_tmp = 0;
    int mat_tmp[3][3];
    double det, res;
    double rot_tmp[3][3];
    double aa_rot[3][3];

    double metric_tensor[3][3];
    double metric_tensor_rot[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            metric_tensor[i][j] = 0.0;
            for (k = 0; k < 3; ++k) {
                metric_tensor[i][j] += aa[k][i] * aa[k][j];
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                mat_tmp[i][i] = 1;
            } else {
                mat_tmp[i][j] = 0;
            }
        }
    }

    // Identity matrix should be the first entry.
    LatticeSymmList.push_back(mat_tmp);

    for (m11 = -1; m11 <= 1; ++m11){
        for (m12 = -1; m12 <= 1; ++m12) {
            for (m13 = -1; m13 <= 1; ++m13){
                for (m21 = -1; m21 <= 1; ++m21){
                    for (m22 = -1; m22 <= 1; ++m22){
                        for (m23 = -1; m23 <= 1; ++m23){
                            for (m31 = -1; m31 <= 1; ++m31){
                                for (m32 = -1; m32 <= 1; ++m32){
                                    for (m33 = -1; m33 <= 1; ++m33){

                                        if (m11 == 1 && m12 == 0 && m13 == 0 &&
                                            m21 == 0 && m22 == 1 && m23 == 0 &&
                                            m31 == 0 && m32 == 0 && m33 == 1) continue;

                                        det = m11 * (m22 * m33 - m32 * m23)
                                            - m21 * (m12 * m33 - m32 * m13)
                                            + m31 * (m12 * m23 - m22 * m13);

                                        if (det != 1 && det != -1) continue;

                                        rot_tmp[0][0] = m11;
                                        rot_tmp[0][1] = m12;
                                        rot_tmp[0][2] = m13;
                                        rot_tmp[1][0] = m21;
                                        rot_tmp[1][1] = m22;
                                        rot_tmp[1][2] = m23;
                                        rot_tmp[2][0] = m31;
                                        rot_tmp[2][1] = m32;
                                        rot_tmp[2][2] = m33;

                                        // Here, aa_rot = aa * rot_tmp is correct.
                                        matmul3(aa_rot, aa, rot_tmp);

                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                metric_tensor_rot[i][j] = 0.0;
                                                for (k = 0; k < 3; ++k) {
                                                    metric_tensor_rot[i][j] += aa_rot[k][i] * aa_rot[k][j];
                                                }
                                            }
                                        }

                                        res = 0.0;
                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                res += std::pow(metric_tensor[i][j] - metric_tensor_rot[i][j], 2.0);
                                            }
                                        }

                                        // Metric tensor is invariant under symmetry operations.

                                        if (res < tolerance * tolerance) {
                                            ++nsym_tmp;
                                            for (i = 0; i < 3; ++i) {
                                                for (j = 0; j < 3; ++j) {
                                                    mat_tmp[i][j] = static_cast<int>(rot_tmp[i][j]);
                                                }
                                            }
                                            LatticeSymmList.push_back(mat_tmp);
                                        }

                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    if (LatticeSymmList.size() > 48) {
        error->exit("find_lattice_symmetry", "Number of lattice symmetry is larger than 48.");
    }
}

void Symmetry::find_crystal_symmetry(int nat, int nclass, std::vector<unsigned int> *atomclass, double **x,
                                     std::vector<RotationMatrix> LatticeSymmList,
                                     std::vector<SymmetryOperation> &CrystalSymmList)
{
    unsigned int i, j;
    unsigned int iat, jat, kat, lat;
    double x_rot[3];
    double rot[3][3], rot_tmp[3][3], rot_cart[3][3];
    double mag[3], mag_rot[3];
    double tran[3];
    double x_rot_tmp[3];
    double tmp[3];
    double diff;

    int rot_int[3][3];

    int ii, jj, kk;
    unsigned int itype;

    bool is_found;
    bool isok;
    bool mag_sym1, mag_sym2;

    bool is_identity_matrix;

    // Add identity matrix first.
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                rot_int[i][j] = 1;
            } else {
                rot_int[i][j] = 0;
            }
        }
        tran[i] = 0.0;
    }

    CrystalSymmList.push_back(SymmetryOperation(rot_int, tran));

    for (std::vector<RotationMatrix>::iterator it_latsym = LatticeSymmList.begin();
        it_latsym != LatticeSymmList.end(); ++it_latsym) {

            iat = atomclass[0][0];

            for (i = 0; i < 3; ++i) {
                for (j = 0; j < 3; ++j) {
                    rot[i][j] = static_cast<double>((*it_latsym).mat[i][j]);
                }
            }

            rotvec(x_rot, x[iat], rot);

#ifdef _OPENMP
#pragma omp parallel for private(jat, tran, isok, kat, x_rot_tmp, is_found, lat, tmp, diff, \
    i, j, itype, jj, kk, is_identity_matrix, mag, mag_rot, rot_tmp, rot_cart, mag_sym1, mag_sym2)
#endif
            for (ii = 0; ii < atomclass[0].size(); ++ii) {
                jat = atomclass[0][ii];

                for (i = 0; i < 3; ++i) {
                    tran[i] = x[jat][i] - x_rot[i];
                    tran[i] = tran[i] - nint(tran[i]);
                }

                if ((std::abs(tran[0]) > eps12 && !interaction->is_periodic[0]) ||
                    (std::abs(tran[1]) > eps12 && !interaction->is_periodic[1]) ||
                    (std::abs(tran[2]) > eps12 && !interaction->is_periodic[2])) continue;

                is_identity_matrix =
                    ( std::pow(rot[0][0] - 1.0, 2) + std::pow(rot[0][1], 2) + std::pow(rot[0][2], 2)
                    + std::pow(rot[1][0], 2) + std::pow(rot[1][1] - 1.0, 2) + std::pow(rot[1][2], 2)
                    + std::pow(rot[2][0], 2) + std::pow(rot[2][1], 2) + std::pow(rot[2][2] - 1.0, 2)
                    + std::pow(tran[0], 2) + std::pow(tran[1], 2) + std::pow(tran[2], 2) ) < eps12;
                if (is_identity_matrix) continue;

                isok = true;

                for (itype = 0; itype < nclass; ++itype) {

                    for (jj = 0; jj < atomclass[itype].size(); ++jj) {

                        kat = atomclass[itype][jj];

                        rotvec(x_rot_tmp, x[kat], rot);

                        for (i = 0; i < 3; ++i) {
                            x_rot_tmp[i] += tran[i];
                        }

                        is_found = false;

                        for (kk = 0; kk < atomclass[itype].size(); ++kk) {

                            lat = atomclass[itype][kk];

                            for (i = 0; i < 3; ++i) {
                                tmp[i] = std::fmod(std::abs(x[lat][i] - x_rot_tmp[i]), 1.0);
                                tmp[i] = std::min<double>(tmp[i], 1.0 - tmp[i]);
                            }
                            diff = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2];
                            if (diff < tolerance * tolerance) {
                                is_found = true;
                                break;
                            }
                        }

                        if (!is_found) isok = false;
                    }
                }

                if (isok && system->lspin && system->noncollinear) {
                    for (i = 0; i < 3; ++i) {
                        mag[i] = system->magmom[jat][i];
                        mag_rot[i] = system->magmom[iat][i];
                    }

                    matmul3(rot_tmp, rot, system->rlavec);
                    matmul3(rot_cart, system->lavec, rot_tmp);

                    for (i = 0; i < 3; ++i) {
                        for (j = 0; j < 3; ++j) {
                            rot_cart[i][j] /= (2.0 * pi);
                        }
                    }
                    rotvec(mag_rot, mag_rot, rot_cart);

                    // In the case of improper rotation, the factor -1 should be multiplied
                    // because the inversion operation doesn't flip the spin.
                    if (!is_proper(rot_cart)) {
                        for (i = 0; i < 3; ++i) {
                            mag_rot[i] = -mag_rot[i];
                        }
                    }

                    mag_sym1 = (std::pow(mag[0] - mag_rot[0], 2.0)
                        + std::pow(mag[1] - mag_rot[1], 2.0)
                        + std::pow(mag[2] - mag_rot[2], 2.0) ) < eps6;

                    mag_sym2 = (std::pow(mag[0] + mag_rot[0], 2.0)
                        + std::pow(mag[1] + mag_rot[1], 2.0)
                        + std::pow(mag[2] + mag_rot[2], 2.0) ) < eps6;

                    if (!mag_sym1 && !mag_sym2) {
                        isok = false;
                    } else if (!mag_sym1 && mag_sym2 && !trev_sym_mag) {
                        isok = false;
                    }
                }

                if (isok) {
#ifdef _OPENMP
#pragma omp critical
#endif
                    CrystalSymmList.push_back(SymmetryOperation((*it_latsym).mat, tran));
                }
            }

    }
}

/*
void Symmetry::find_nnp_for_translation(unsigned int &ret, std::vector<SymmetryOperationTransFloat> symminfo)
{
int i;

ret = 1;

std::set<double> translation_set;
double tran_tmp;
double tran_numerator;
bool is_integer;
bool is_found;

translation_set.clear();

for (std::vector<SymmetryOperationTransFloat>::iterator it = symminfo.begin(); it != symminfo.end(); ++it) {

for (i = 0; i < 3; ++i) {
tran_tmp = std::abs((*it).tran[i]);

if (translation_set.find(tran_tmp) == translation_set.end()) {
translation_set.insert(tran_tmp);
}
}
}

is_found = false;

while(1) {

is_integer = true;

for (std::set<double>::iterator it = translation_set.begin(); it != translation_set.end(); ++it) {

tran_numerator = (*it) * static_cast<double>(ret);
if (std::abs(tran_numerator - static_cast<double>(nint(tran_numerator))) > tolerance) {
is_integer = false;
break;
}
}

if (is_integer) {
is_found = true;
break;
}

if (ret > 1000) break;
++ret;
}

if (!is_found) {
// This should not happen.
error->exit("find_nnp_for_translation", "Cannot find nnp.");
}
}
*/

void Symmetry::symop_in_cart(double lavec[3][3], double rlavec[3][3])
{
    int i, j;

#ifdef _USE_EIGEN
    Eigen::Matrix3d aa, bb, sym_tmp;
    Eigen::Matrix3d sym_crt;

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            aa(i,j) = lavec[i][j];
            bb(i,j) = rlavec[i][j];
        }
    }

#else
    double sym_tmp[3][3], sym_crt[3][3];
    double tmp[3][3];
#endif

    for (int isym = 0; isym < nsym; ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                sym_tmp(i,j) = static_cast<double>(symrel_int[isym][i][j]);
#else
                sym_tmp[i][j] = static_cast<double>(symrel_int[isym][i][j]);
#endif
            }
        }
#ifdef _USE_EIGEN
        sym_crt = (aa * (sym_tmp * bb)) / (2.0 * pi);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel[isym][i][j] = sym_crt(i,j);
            }
        }
#else
        matmul3(tmp, sym_tmp, rlavec);
        matmul3(sym_crt, lavec, tmp);

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel[isym][i][j] = sym_crt[i][j] / (2.0 * pi);
            }
        }
#endif
    }

#ifdef _DEBUG

    std::cout << "Symmetry Operations in Cartesian Coordinate" << std::endl;
    for (int isym = 0; isym < nsym; ++isym) {
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(8) << symrel[isym][i][j];
            }
        }
        std::cout << std::endl;
    }
#endif
}

void Symmetry::pure_translations()
{
    int i;

    ntran = 0;
    for (i = 0; i < nsym; ++i) {
        if (is_translation(symrel_int[i])) ++ntran;
    }

    natmin = system->nat / ntran;

    if (ntran > 1) {
        std::cout << "  Given system is not primitive cell." << std::endl;
        std::cout << "  There are " << std::setw(5) <<  ntran << " translation operations." << std::endl;
    } else {
        std::cout << "  Given system is a primitive cell." << std::endl;
    }
    std::cout << "  Primitive cell contains " << natmin << " atoms" << std::endl;

    if (system->nat % ntran) {
        error->exit("pure_translations", "nat != natmin * ntran. Something is wrong in the structure.");
    }

    memory->allocate(symnum_tran, ntran);

    int isym = 0;

    for (i = 0; i < nsym; ++i) {
        if (is_translation(symrel_int[i])) symnum_tran[isym++] = i;
    }
}

void Symmetry::genmaps(int nat, double **x, int **map_sym, int **map_p2s, Maps *map_s2p)
{
    int isym, iat, jat;
    int i, j;
    int itype;
    int ii, jj;
    double xnew[3];
    double tmp[3], diff;
    double rot_double[3][3];

    for (iat = 0; iat < nat; ++iat) {
        for (isym = 0; isym < nsym; ++isym) {
            map_sym[iat][isym] = -1;
        }
    }

#ifdef  _OPENMP
#pragma omp parallel for private(i, j, rot_double, itype, ii, iat, xnew, jj, jat, tmp, diff, isym)
#endif
    for (isym = 0; isym < nsym; ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                rot_double[i][j] = static_cast<double>(symrel_int[isym][i][j]);
            }
        }

        for (itype = 0; itype < system->nclassatom; ++itype) {

            for (ii = 0; ii < system->atomlist_class[itype].size(); ++ii) {

                iat = system->atomlist_class[itype][ii];

                rotvec(xnew, x[iat], rot_double);

                for (i = 0; i < 3; ++i) xnew[i] += tnons[isym][i];

                for (jj = 0; jj < system->atomlist_class[itype].size(); ++jj) {

                    jat = system->atomlist_class[itype][jj];

                    for (i = 0; i < 3; ++i) {
                        tmp[i] = std::fmod(std::abs(x[jat][i] - xnew[i]), 1.0);
                        tmp[i] = std::min<double>(tmp[i], 1.0 - tmp[i]);
                    }
                    diff = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2];
                    if (diff < tolerance * tolerance) {
                        map_sym[iat][isym] = jat;
                        break;
                    }
                }
                if (map_sym[iat][isym] == -1) error->exit("genmaps", "cannot find symmetry for operation # ", isym + 1);
            }
        }
    }

    bool *is_checked;
    memory->allocate(is_checked, nat);

    for (i = 0; i < nat; ++i) is_checked[i] = false;

    jat = 0;
    int atomnum_translated;
    for (iat = 0; iat < nat; ++iat) {

        if (is_checked[iat]) continue;
        for (i = 0; i < ntran; ++i) {
            atomnum_translated = map_sym[iat][symnum_tran[i]];
            map_p2s[jat][i] = atomnum_translated;
            is_checked[atomnum_translated] = true;
        }
        ++jat;
    }

    memory->deallocate(is_checked);

    for (iat = 0; iat < natmin; ++iat) {
        for (i  = 0; i < ntran; ++i) {
            atomnum_translated = map_p2s[iat][i];
            map_s2p[atomnum_translated].atom_num = iat;
            map_s2p[atomnum_translated].tran_num = i;
        }
    }
}

bool Symmetry::is_translation(int **rot)
{
    bool ret;

    ret =
        rot[0][0] == 1 && rot[0][1] == 0 && rot[0][2] == 0 &&
        rot[1][0] == 0 && rot[1][1] == 1 && rot[1][2] == 0 &&
        rot[2][0] == 0 && rot[2][1] == 0 && rot[2][2] == 1;

    return ret;
}

void Symmetry::symop_availability_check(double ***rot, bool *flag, const int n, int &nsym_fc)
{
    int i, j, k;
    int nfinite;

    nsym_fc = 0;

    for (i = 0; i < nsym; ++i) {

        nfinite = 0;
        for (j = 0; j < 3; ++j) {
            for (k = 0; k < 3; ++k) {
                if(std::abs(rot[i][j][k]) > eps) ++nfinite;
            }
        }

        if (nfinite == 3) {
            ++nsym_fc;
            flag[i] = true;
        } else {
            flag[i] = false;
        }
    }
}

void Symmetry::print_symmetrized_coordinate(double **x)
{
    int i, j, k, l;
    int isym = 0;
    int nat = system->nat;
    int m11, m12, m13, m21, m22, m23, m31, m32, m33;
    int det;
    double tran[3];
    double **x_symm, **x_avg;
#ifdef _USE_EIGEN
    Eigen::Matrix3d rot;
    Eigen::Vector3d wsi, usi, vsi, tmp;
#else
    double rot[3][3];
    double wsi[3], usi[3], vsi[3], tmp[3];
#endif

    memory->allocate(x_symm, nat, 3);
    memory->allocate(x_avg, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_avg[i][j] = 0.0;
        }
    }

    for (std::vector<SymmetryOperation>::iterator it = SymmList.begin(); it != SymmList.end(); ++it) {

        ++isym;
        std::cout << "Symmetry No. : " << std::setw(5) << isym << std::endl;

        m11 = (*it).rot[0][0];
        m12 = (*it).rot[0][1];
        m13 = (*it).rot[0][2];
        m21 = (*it).rot[1][0];
        m22 = (*it).rot[1][1];
        m23 = (*it).rot[1][2];
        m31 = (*it).rot[2][0];
        m32 = (*it).rot[2][1];
        m33 = (*it).rot[2][2];

        for (i = 0; i < 3; ++i) tran[i] = (*it).tran[i];

        det = m11 * (m22 * m33 - m32 * m23)
            - m21 * (m12 * m33 - m32 * m13)
            + m31 * (m12 * m23 - m22 * m13);

#ifdef _USE_EIGEN
        rot(0,0) = static_cast<double>((m22 * m33 - m23 * m32) * det);
        rot(0,1) = static_cast<double>((m23 * m31 - m21 * m33) * det);
        rot(0,2) = static_cast<double>((m21 * m32 - m22 * m31) * det);
        rot(1,0) = static_cast<double>((m32 * m13 - m33 * m12) * det);
        rot(1,1) = static_cast<double>((m33 * m11 - m31 * m13) * det);
        rot(1,2) = static_cast<double>((m31 * m12 - m32 * m11) * det);
        rot(2,0) = static_cast<double>((m12 * m23 - m13 * m22) * det);
        rot(2,1) = static_cast<double>((m13 * m21 - m11 * m23) * det);
        rot(2,2) = static_cast<double>((m11 * m22 - m12 * m21) * det);
#else
        rot[0][0] = static_cast<double>((m22 * m33 - m23 * m32) * det);
        rot[0][1] = static_cast<double>((m23 * m31 - m21 * m33) * det);
        rot[0][2] = static_cast<double>((m21 * m32 - m22 * m31) * det);
        rot[1][0] = static_cast<double>((m32 * m13 - m33 * m12) * det);
        rot[1][1] = static_cast<double>((m33 * m11 - m31 * m13) * det);
        rot[1][2] = static_cast<double>((m31 * m12 - m32 * m11) * det);
        rot[2][0] = static_cast<double>((m12 * m23 - m13 * m22) * det);
        rot[2][1] = static_cast<double>((m13 * m21 - m11 * m23) * det);
        rot[2][2] = static_cast<double>((m11 * m22 - m12 * m21) * det);
#endif

        for (i = 0; i < nat; ++i) {
            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                wsi(j) = x[i][j] - tran[j];
#else
                wsi[j] = x[i][j] - tran[j];
#endif
            }

#ifdef _USE_EIGEN
            usi = rot * wsi;
#else
            rotvec(usi, wsi, rot);
#endif

            l = -1;

            for (j = 0; j < nat; ++j) {
                for (k = 0; k < 3; ++k) {
#ifdef _USE_EIGEN
                    vsi(k) = x[j][k];
                    tmp(k) = std::fmod(std::abs(usi(k) - vsi(k)), 1.0);
                    // need "std" to specify floating point operation
                    // especially for Intel compiler (there was no problem in MSVC)
                    tmp(k) = std::min<double>(tmp(k), 1.0 - tmp(k)) ;
#else
                    vsi[k] = x[j][k];
                    tmp[k] = std::fmod(std::abs(usi[k] - vsi[k]), 1.0);
                    tmp[k] = std::min<double>(tmp[k], 1.0 - tmp[k]);
#endif
                }
#ifdef _USE_EIGEN
                double diff = tmp.dot(tmp);
#else
                double diff = tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2];
#endif
                if (diff < tolerance * tolerance) {
                    l = j;
                    break;
                }
            }
            if (l == -1) error->exit("print_symmetrized_coordinate", "This cannot happen.");

            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                x_symm[l][j] = usi(j);
#else
                x_symm[l][j] = usi[j];
#endif
                /*
                do {
                if (x_symm[l][j] < 0.0) {
                x_symm[l][j] += 1.0;
                } else if (x_symm[l][j] >= 1.0) {
                x_symm[l][j] -= 1.0;
                }
                } while(x_symm[l][j] < 0.0 || x_symm[l][j] >= 1.0);
                */
            }

        }

        for (i = 0; i < nat; ++i) {
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(20) << std::scientific << x_symm[i][j];
            }
            std::cout << " ( ";
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(20) << std::scientific << x_symm[i][j]-x[i][j];
            }
            std::cout << " )" << std::endl;

            for (j = 0; j < 3; ++j) {
                x_avg[i][j] += x_symm[i][j];
            }
        }

    }

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_avg[i][j] /= static_cast<double>(SymmList.size());
        }
    }

    std::cout << "Symmetry Averaged Coordinate" << std::endl;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            std::cout << std::setw(20) << std::scientific << std::setprecision(9) << x_avg[i][j];
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;

    std::cout.setf(std::ios::floatfield);

    memory->deallocate(x_symm);
    memory->deallocate(x_avg);
}

bool Symmetry::is_proper(double rot[3][3])
{
    double det;
    bool ret;

    det = rot[0][0] * (rot[1][1] * rot[2][2] - rot[2][1] * rot[1][2])
        - rot[1][0] * (rot[0][1] * rot[2][2] - rot[2][1] * rot[0][2])
        + rot[2][0] * (rot[0][1] * rot[1][2] - rot[1][1] * rot[0][2]);

    if (std::abs(det - 1.0) < eps12) {
        ret = true;
    } else if (std::abs(det + 1.0) < eps12) {
        ret = false;
    } else {
        error->exit("is_proper", "This cannot happen.");
    }

    return ret;
}


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Implementation of Kalman filtering accounting for unknown inputs.
// Please refer to: https://hal.archives-ouvertes.fr/hal-00143941/document
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/scalar_output_smoother.h>
#include <quads/types.h>

#include <Eigen/QR>
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>

namespace quads {

void ScalarOutputSmoother::Update(double y, double dt) {
  // Discretize time.
  const Matrix4x4d A_dt = (A_ * dt).exp();

  // Predict step.
  const Vector4d x_predict = A_dt * x_;
  const Matrix4x4d Px_predict = A_dt * Px_ * A_dt.transpose() + W_ * dt;

  // Update step.
  const double innovation = y - H_ * x_predict;
  const double S = V_ + H_ * Px_predict * H_.transpose();
  const Vector4d K = Px_predict * H_.transpose() / S;

  x_ = x_predict + K * innovation;
  Px_ = (Matrix4d::Identity() - K * H_) * Px_predict;
}

}  // namespace quads


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

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

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

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

#include <boost/format.hpp>

using namespace std;
#include <sys/time.h>

// the filters are all built for a 48KHz sample rate.
// now let's see what happens..

int main(int argc, char * argv[])
{
  (void) argc; (void) argv;

  int i, j;
  const int samp_len = 65536;
  int inbuflen = 2304;

  double sample_rate = 48000.0;

  SoDa::OSFilter SSBFilter(80.0, 150.0, 2300.0, 2400.0, 512, 1.0, sample_rate, inbuflen);
  SoDa::OSFilter CWFilter(300.0, 400.0, 900.0, 1000.0, 512, 1.0, sample_rate, inbuflen);

  complex<float> in[samp_len];
  complex<float> ssb_out[samp_len];
  complex<float> cw_out[samp_len];

  // sweep the frequency from -22kHz to 22kHz
  double freq;
  for(freq = -22e3; freq < 22e3; freq += 5.0) {
    double phase_step = 2.0 * M_PI * freq / ((double) sample_rate);
    double ang = 0.0;
    for(i = 0; i < samp_len; i++) {
      in[i] = complex<float>(cos(ang), sin(ang));
      ang += phase_step;
      if(ang > M_PI) ang = ang - 2.0 * M_PI;
      if(ang < -M_PI) ang = ang + 2.0 * M_PI;
    }
    for(i = 0; i < (samp_len - inbuflen); i += inbuflen) {
      SSBFilter.apply(&(in[i]), &(ssb_out[i]));
      CWFilter.apply(&(in[i]), &(cw_out[i]));
    }

    // now measure the phase and amplitude
    // do this somewhere near the middle...

    double ssb_gain, ssb_phase, cw_gain, cw_phase;
    for(i = 4000, j = 0; j < 5; i += 1000, j++) {
      ssb_gain = std::abs(ssb_out[i]) / std::abs(in[i]);
      ssb_phase = std::arg(ssb_out[i]) - std::arg(in[i]);
      cw_gain = std::abs(cw_out[i]) / std::abs(in[i]);
      cw_phase = std::arg(cw_out[i]) - std::arg(in[i]);
      if(ssb_phase > M_PI) ssb_phase -= 2.0*M_PI;
      if(ssb_phase < -M_PI) ssb_phase += 2.0*M_PI;
      if(cw_phase > M_PI) cw_phase -= 2.0*M_PI;
      if(cw_phase < -M_PI) cw_phase += 2.0*M_PI;
      std::cout << boost::format("%12.8f %6g %6g %6g %6g\n") % freq % ssb_gain % ssb_phase % cw_gain % cw_phase;
    }

  }

  return 0;
}


/**
 * \ file AttackReleaseHysteresisFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseHysteresisFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1024*64;

TEST(AttackReleaseHysteresis, attack_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_attack(0.5);
  ASSERT_EQ(filter.get_attack(), 0.5);
}

TEST(AttackReleaseHysteresis, attack_range_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_attack(-0.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, attack_range2_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_attack(1.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, release_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release(0.5);
  ASSERT_EQ(filter.get_release(), 0.5);
}

TEST(AttackReleaseHysteresis, release_range_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_release(-0.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, release_range2_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_release(1.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, attack_hysteresis_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.1);
  filter.set_attack_hysteresis(0.5);
  ASSERT_EQ(filter.get_attack_hysteresis(), 0.5);
}

TEST(AttackReleaseHysteresis, attack_hysteresis_db_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.001);
  filter.set_attack_hysteresis_db(-20);
  ASSERT_NEAR(filter.get_attack_hysteresis(), 0.1, 0.1);
}

TEST(AttackReleaseHysteresis, attack_hysteresis_range_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(.5);
  ASSERT_THROW(filter.set_attack_hysteresis(.5 - 0.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, attack_hysteresis_range2_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_attack_hysteresis(1.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, hysteresis_release_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.5);
  ASSERT_EQ(filter.get_release_hysteresis(), 0.5);
}

TEST(AttackReleaseHysteresis, release_hysteresis_db_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis_db(-20);
  ASSERT_NEAR(filter.get_release_hysteresis(), 0.1, 0.1);
}

TEST(AttackReleaseHysteresis, release_hysteresis_range_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_release_hysteresis(-0.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresis, release_hysteresis_range2_test)
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  ASSERT_THROW(filter.set_release_hysteresis(1.000001), ATK::RuntimeError);
}

TEST(AttackReleaseHysteresisFilter, triangle_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AttackReleaseHysteresisFilter<double> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    ASSERT_GE(data[i], outdata[i]);
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    ASSERT_GE(outdata[PROCESSSIZE / 2 + i], outdata[PROCESSSIZE / 2 + i - 1]);
  }
}

#define CUSTOMPROCESSSIZE 7

TEST(AttackReleaseHysteresisFilter, release_custom_test)
{
  double data[] = {0., 1., .5, .4, .3, .2, .1};
  double target[] = {0., 1., 1., .46, .46, .226, .1126};

  ATK::InPointerFilter<double> generator(data, 1, CUSTOMPROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, CUSTOMPROCESSSIZE> outdata;

  ATK::AttackReleaseHysteresisFilter<double> filter(1);
  filter.set_attack(0);
  filter.set_release(.1);
  filter.set_release_hysteresis(.5);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, CUSTOMPROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(CUSTOMPROCESSSIZE);

  for(gsl::index i = 0; i < CUSTOMPROCESSSIZE; ++i)
  {
    ASSERT_NEAR(target[i], outdata[i], .001);
  }
}


#include <iostream>
#include <numeric>
#include <algorithm>
#include <vector>
#include <map>
#include <chrono>
#include <memory>
#include <typeinfo>

#include <hpx/hpx_init.hpp>
#include <hpx/include/iostreams.hpp>

#include <boost/serialization/access.hpp>

#include "DimacsParser.hpp"
#include "BitGraph.hpp"
#include "BitSet.hpp"

#include "YewPar.hpp"

#include "skeletons/Seq.hpp"
#include "skeletons/DepthBounded.hpp"
#include "skeletons/StackStealing.hpp"
#include "skeletons/Ordered.hpp"
#include "skeletons/Budget.hpp"

#include "util/func.hpp"
#include "util/NodeGenerator.hpp"

// Number of Words to use in our bitset representation
// Possible to specify at compile to to handler bigger graphs if required
#ifndef NWORDS
#define NWORDS 8
#endif

// Order a graphFromFile and return an ordered graph alongside a map to invert
// the vertex numbering at the end.
template<unsigned n_words_>
auto orderGraphFromFile(const dimacs::GraphFromFile & g, std::map<int,int> & inv) -> BitGraph<n_words_> {
  std::vector<int> order(g.first);
  std::iota(order.begin(), order.end(), 0);

  // Order by degree, tie break on number
  std::vector<int> degrees;
  std::transform(order.begin(), order.end(), std::back_inserter(degrees),
                 [&] (int v) { return g.second.find(v)->second.size(); });

  std::sort(order.begin(), order.end(),
            [&] (int a, int b) { return ! (degrees[a] < degrees[b] || (degrees[a] == degrees[b] && a > b)); });

  // Construct a new graph with this new ordering
  BitGraph<n_words_> graph;
  graph.resize(g.first);

  for (unsigned i = 0 ; i < g.first ; ++i)
    for (unsigned j = 0 ; j < g.first ; ++j)
      if (g.second.find(order[i])->second.count(order[j]))
        graph.add_edge(i, j);

  // Create inv map (maybe just return order?)
  for (int i = 0; i < order.size(); i++) {
    inv[i] = order[i];
  }

  return graph;
}

template<unsigned n_words_>
auto colour_class_order(const BitGraph<n_words_> & graph,
                        const BitSet<n_words_> & p,
                        std::array<unsigned, n_words_ * bits_per_word> & p_order,
                        std::array<unsigned, n_words_ * bits_per_word> & p_bounds) -> void {
  BitSet<n_words_> p_left = p; // not coloured yet
  unsigned colour = 0;         // current colour
  unsigned i = 0;              // position in p_bounds

  // while we've things left to colour
  while (! p_left.empty()) {
    // next colour
    ++colour;
    // things that can still be given this colour
    BitSet<n_words_> q = p_left;

    // while we can still give something this colour
    while (! q.empty()) {
      // first thing we can colour
      int v = q.first_set_bit();
      p_left.unset(v);
      q.unset(v);

      // can't give anything adjacent to this the same colour
      graph.intersect_with_row_complement(v, q);

      // record in result
      p_bounds[i] = colour;
      p_order[i] = v;
      ++i;
    }
  }
}

// Main Maxclique B&B Functions
// Probably needs a copy constructor
struct MCSol {
  std::vector<int> members;
  int colours;

  template <class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & members;
    ar & colours;
  }
};

struct MCNode {
  friend class boost::serialization::access;

  MCSol sol;
  int size;
  BitSet<NWORDS> remaining;

  int getObj() const {
    return size;
  }

  template <class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & sol;
    ar & size;
    ar & remaining;
  }

};

struct GenNode : YewPar::NodeGenerator<MCNode, BitGraph<NWORDS> > {
  std::array<unsigned, NWORDS * bits_per_word> p_order;
  std::array<unsigned, NWORDS * bits_per_word> colourClass;

  std::reference_wrapper<const BitGraph<NWORDS> > graph;

  MCSol childSol;
  int childBnd;
  BitSet<NWORDS> p;

  int v;

  GenNode(const BitGraph<NWORDS> & graph, const MCNode & n) : graph(std::cref(graph)) {
    colour_class_order(graph, n.remaining, p_order, colourClass);
    childSol = n.sol;
    childBnd = n.size + 1;
    p = n.remaining;
    numChildren = p.popcount();
    v = numChildren - 1;
  }

  // Get the next value
  MCNode next() override {
    auto sol = childSol;
    sol.members.push_back(p_order[v]);
    sol.colours = colourClass[v] - 1;

    auto cands = p;
    graph.get().intersect_with_row(p_order[v], cands);

    // Side effectful function update
    p.unset(p_order[v]);
    v--;

    return {sol, childBnd, cands};
  }

  MCNode nth(unsigned n) {
    auto pos = v - n;

    auto sol = childSol;
    sol.members.push_back(p_order[pos]);
    sol.colours = colourClass[pos] - 1;

    auto cands = p;
    // Remove all choices from the left "left" of the one we care about
    for (auto i = v; i > pos; --i) {
      cands.unset(p_order[i]);
    }

    graph.get().intersect_with_row(p_order[pos], cands);

    return {sol, childBnd, cands};
  }
};

int upperBound(const BitGraph<NWORDS> & space, const MCNode & n) {
  return n.size + n.sol.colours;
}

typedef func<decltype(&upperBound), &upperBound> upperBound_func;

int hpx_main(boost::program_options::variables_map & opts) {
  /*
  if (!opts.count("input-file")) {
    std::cerr << "You must provide an DIMACS input file with \n";
    hpx::finalize();
    return EXIT_FAILURE;
  }
  */

  //boost::program_options::notify(opts);

  auto inputFile = opts["input-file"].as<std::string>();

  auto gFile = dimacs::read_dimacs(inputFile);

  // Order the graph (keep a hold of the map)
  std::map<int, int> invMap;
  auto graph = orderGraphFromFile<NWORDS>(gFile, invMap);

  auto spawnDepth = opts["spawn-depth"].as<std::uint64_t>();
  auto decisionBound = opts["decisionBound"].as<int>();

  auto start_time = std::chrono::steady_clock::now();

  // Initialise Root Node
  MCSol mcsol;
  mcsol.members.reserve(graph.size());
  mcsol.colours = 0;

  BitSet<NWORDS> cands;
  cands.resize(graph.size());
  cands.set_all();
  MCNode root = { mcsol, 0, cands };

  auto sol = root;
  auto skeletonType = opts["skeleton"].as<std::string>();

  if (skeletonType == "depthbounded") {
    if (decisionBound != 0) {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.expectedObjective = decisionBound;
      searchParameters.spawnDepth = spawnDepth;
      sol = YewPar::Skeletons::DepthBounded<GenNode,
                                           YewPar::Skeletons::API::Decision,
                                           YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                           YewPar::Skeletons::API::PruneLevel>
            ::search(graph, root, searchParameters);
    } else {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.spawnDepth = spawnDepth;
      auto poolType = opts["poolType"].as<std::string>();
      // EXTENSION
      if (opts.count("nodes")) {
          sol = YewPar::Skeletons::DepthBounded<GenNode,
                                               YewPar::Skeletons::API::Optimisation,
                                               YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                               YewPar::Skeletons::API::PruneLevel,
                                               YewPar::Skeletons::API::NodeCounts,
                                               YewPar::Skeletons::API::DepthBoundedPoolPolicy<
                                                 Workstealing::Policies::DepthPoolPolicy> >
              ::search(graph, root, searchParameters);
      } else if (opts.count("backtracks")) {
          sol = YewPar::Skeletons::DepthBounded<GenNode,
                                               YewPar::Skeletons::API::Optimisation,
                                               YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                               YewPar::Skeletons::API::PruneLevel,
                                               YewPar::Skeletons::API::Backtracks,
                                               YewPar::Skeletons::API::DepthBoundedPoolPolicy<
                                                 Workstealing::Policies::DepthPoolPolicy> >
              ::search(graph, root, searchParameters);
      } else if (opts.count("regularity")) {
          sol = YewPar::Skeletons::DepthBounded<GenNode,
                                               YewPar::Skeletons::API::Optimisation,
                                               YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                               YewPar::Skeletons::API::PruneLevel,
                                               YewPar::Skeletons::API::Regularity,
                                               YewPar::Skeletons::API::DepthBoundedPoolPolicy<
                                                 Workstealing::Policies::DepthPoolPolicy> >
              ::search(graph, root, searchParameters);
      } else {
        sol = YewPar::Skeletons::DepthBounded<GenNode,
                                               YewPar::Skeletons::API::Optimisation,
                                               YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                               YewPar::Skeletons::API::PruneLevel,
                                               YewPar::Skeletons::API::DepthBoundedPoolPolicy<
                                               Workstealing::Policies::DepthPoolPolicy> >
              ::search(graph, root, searchParameters);
      }
    }
  } else if (skeletonType == "stacksteal") {
    if (decisionBound != 0) {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.expectedObjective = decisionBound;
      searchParameters.stealAll = static_cast<bool>(opts.count("chunked"));
      sol = YewPar::Skeletons::StackStealing<GenNode,
                                             YewPar::Skeletons::API::Decision,
                                             YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                             YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
    } else {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.stealAll = static_cast<bool>(opts.count("chunked"));
      if (opts.count("nodes")) {
        sol = YewPar::Skeletons::StackStealing<GenNode,
                                              YewPar::Skeletons::API::Optimisation,
                                              YewPar::Skeletons::API::NodeCounts,
                                              YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                              YewPar::Skeletons::API::PruneLevel>
            ::search(graph, root, searchParameters);
      } else if (opts.count("backtracks"))  {
        sol = YewPar::Skeletons::StackStealing<GenNode,
                                              YewPar::Skeletons::API::Optimisation,
                                              YewPar::Skeletons::API::Backtracks,
                                              YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                              YewPar::Skeletons::API::PruneLevel>
            ::search(graph, root, searchParameters);
      } else if (opts.count("regularity")) {
        sol = YewPar::Skeletons::StackStealing<GenNode,
                                              YewPar::Skeletons::API::Optimisation,
                                              YewPar::Skeletons::API::Regularity,
                                              YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                              YewPar::Skeletons::API::PruneLevel>
            ::search(graph, root, searchParameters);
      } else {
        sol = YewPar::Skeletons::StackStealing<GenNode,
                                              YewPar::Skeletons::API::Optimisation,
                                              YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                              YewPar::Skeletons::API::PruneLevel>
            ::search(graph, root, searchParameters);
      }
    }
  } else if (skeletonType == "budget") {
    if (decisionBound != 0) {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.backtrackBudget = opts["backtrack-budget"].as<unsigned>();
      searchParameters.expectedObjective = decisionBound;
      sol = YewPar::Skeletons::Budget<GenNode,
                                      YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                      YewPar::Skeletons::API::Decision,
                                      YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
    } else {
      YewPar::Skeletons::API::Params<int> searchParameters;
      searchParameters.backtrackBudget = opts["backtrack-budget"].as<unsigned>();
      if (opts.count("nodes")) {
        sol = YewPar::Skeletons::Budget<GenNode,
                                        YewPar::Skeletons::API::Optimisation,
                                        YewPar::Skeletons::API::NodeCounts,
                                        YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                        YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
      } else if(opts.count("backtracks")) {
        sol = YewPar::Skeletons::Budget<GenNode,
                                        YewPar::Skeletons::API::Optimisation,
                                        YewPar::Skeletons::API::Backtracks,
                                        YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                        YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
      } else if (opts.count("regularity")) {
        sol = YewPar::Skeletons::Budget<GenNode,
                                        YewPar::Skeletons::API::Optimisation,
                                        YewPar::Skeletons::API::Regularity,
                                        YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                        YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
      } else {
        sol = YewPar::Skeletons::Budget<GenNode,
                                        YewPar::Skeletons::API::Optimisation,
                                        YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                        YewPar::Skeletons::API::PruneLevel>
          ::search(graph, root, searchParameters);
      }
    }
  } else {
    hpx::cout << "Invalid skeleton type option. Should be: seq, depthbound, stacksteal or ordered" << hpx::endl;
    hpx::finalize();
    return EXIT_FAILURE;
  }

  auto overall_time = std::chrono::duration_cast<std::chrono::milliseconds>
    (std::chrono::steady_clock::now() - start_time);

  hpx::cout << "MaxClique Size = " << sol.size << hpx::endl;
  hpx::cout << "cpu = " << overall_time.count() << hpx::endl;

  return hpx::finalize();
}

int main (int argc, char* argv[]) {
  boost::program_options::options_description
    desc_commandline("Usage: " HPX_APPLICATION_STRING " [options]");

  desc_commandline.add_options()
    ( "skeleton",
      boost::program_options::value<std::string>()->default_value("seq"),
      "Which skeleton to use: seq, depthbound, stacksteal, budget, or ordered"
      )
    ( "spawn-depth,d",
      boost::program_options::value<std::uint64_t>()->default_value(0),
      "Depth in the tree to spawn at"
      )
    ( "backtrack-budget,b",
      boost::program_options::value<unsigned>()->default_value(50),
      "Number of backtracks before spawning work"
      )
    ( "input-file,f",
      boost::program_options::value<std::string>()->required(),
      "DIMACS formatted input graph"
      )
    ("discrepancyOrder", "Use discrepancy order for the ordered skeleton")
    ("chunked", "Use chunking with stack stealing")
    ("poolType",
     boost::program_options::value<std::string>()->default_value("depthpool"),
     "Pool type for depthbounded skeleton")
    ( "decisionBound",
    boost::program_options::value<int>()->default_value(0),
    "For Decision Skeletons. Size of the clique to search for"
    )
    ("backtracks", "Collect the backtracks metric")
    ("nodes", "Collect the backtracks metric")
    ("regularity", "Collect the backtracks metric");

  YewPar::registerPerformanceCounters();

  return hpx::init(desc_commandline, argc, argv);
}