#ifndef BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Purpose:  Implementation of the aitoff (Aitoff) and wintri (Winkel Tripel)
// projections.
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aitoff
    {

            struct par_aitoff
            {
                double    cosphi1;
                int        mode;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aitoff_spheroid : public base_t_fi<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aitoff m_proj_parm;

                inline base_aitoff_spheroid(const Parameters& par)
                    : base_t_fi<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double c, d;

                    if((d = acos(cos(lp_lat) * cos(c = 0.5 * lp_lon)))) {/* basic Aitoff */
                        xy_x = 2. * d * cos(lp_lat) * sin(c) * (xy_y = 1. / sin(d));
                        xy_y *= d * sin(lp_lat);
                    } else
                        xy_x = xy_y = 0.;
                    if (this->m_proj_parm.mode) { /* Winkel Tripel */
                        xy_x = (xy_x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                        xy_y = (xy_y + lp_lat) * 0.5;
                    }
                }
                /***********************************************************************************
                *
                * Inverse functions added by Drazen Tutic and Lovro Gradiser based on paper:
                *
                * I.Özbug Biklirici and Cengizhan Ipbüker. A General Algorithm for the Inverse
                * Transformation of Map Projections Using Jacobian Matrices. In Proceedings of the
                * Third International Symposium Mathematical & Computational Applications,
                * pages 175{182, Turkey, September 2002.
                *
                * Expected accuracy is defined by EPSILON = 1e-12. Should be appropriate for
                * most applications of Aitoff and Winkel Tripel projections.
                *
                * Longitudes of 180W and 180E can be mixed in solution obtained.
                *
                * Inverse for Aitoff projection in poles is undefined, longitude value of 0 is assumed.
                *
                * Contact : dtutic@geof.hr
                * Date: 2015-02-16
                *
                ************************************************************************************/

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                        int iter, MAXITER = 10, round = 0, MAXROUND = 20;
                    double EPSILON = 1e-12, D, C, f1, f2, f1p, f1l, f2p, f2l, dp, dl, sl, sp, cp, cl, x, y;

                    if ((fabs(xy_x) < EPSILON) && (fabs(xy_y) < EPSILON )) { lp_lat = 0.; lp_lon = 0.; return; }

                    /* intial values for Newton-Raphson method */
                    lp_lat = xy_y; lp_lon = xy_x;
                    do {
                        iter = 0;
                        do {
                            sl = sin(lp_lon * 0.5); cl = cos(lp_lon * 0.5);
                            sp = sin(lp_lat); cp = cos(lp_lat);
                            D = cp * cl;
                                   C = 1. - D * D;
                            D = acos(D) / pow(C, 1.5);
                                   f1 = 2. * D * C * cp * sl;
                                   f2 = D * C * sp;
                                   f1p = 2.* (sl * cl * sp * cp / C - D * sp * sl);
                                   f1l = cp * cp * sl * sl / C + D * cp * cl * sp * sp;
                                f2p = sp * sp * cl / C + D * sl * sl * cp;
                                  f2l = 0.5 * (sp * cp * sl / C - D * sp * cp * cp * sl * cl);
                                  if (this->m_proj_parm.mode) { /* Winkel Tripel */
                                f1 = 0.5 * (f1 + lp_lon * this->m_proj_parm.cosphi1);
                                f2 = 0.5 * (f2 + lp_lat);
                                f1p *= 0.5;
                                f1l = 0.5 * (f1l + this->m_proj_parm.cosphi1);
                                f2p = 0.5 * (f2p + 1.);
                                f2l *= 0.5;
                            }
                            f1 -= xy_x; f2 -= xy_y;
                            dl = (f2 * f1p - f1 * f2p) / (dp = f1p * f2l - f2p * f1l);
                            dp = (f1 * f2l - f2 * f1l) / dp;
                            while (dl > geometry::math::pi<double>()) dl -= geometry::math::pi<double>(); /* set to interval [-geometry::math::pi<double>(), geometry::math::pi<double>()]  */
                            while (dl < -geometry::math::pi<double>()) dl += geometry::math::pi<double>(); /* set to interval [-geometry::math::pi<double>(), geometry::math::pi<double>()]  */
                            lp_lat -= dp;    lp_lon -= dl;
                        } while ((fabs(dp) > EPSILON || fabs(dl) > EPSILON) && (iter++ < MAXITER));
                        if (lp_lat > geometry::math::two_pi<double>()) lp_lat -= 2.*(lp_lat-geometry::math::two_pi<double>()); /* correct if symmetrical solution for Aitoff */
                        if (lp_lat < -geometry::math::two_pi<double>()) lp_lat -= 2.*(lp_lat+geometry::math::two_pi<double>()); /* correct if symmetrical solution for Aitoff */
                        if ((fabs(fabs(lp_lat) - geometry::math::two_pi<double>()) < EPSILON) && (!this->m_proj_parm.mode)) lp_lon = 0.; /* if pole in Aitoff, return longitude of 0 */

                        /* calculate x,y coordinates with solution obtained */
                        if((D = acos(cos(lp_lat) * cos(C = 0.5 * lp_lon)))) {/* Aitoff */
                            x = 2. * D * cos(lp_lat) * sin(C) * (y = 1. / sin(D));
                            y *= D * sin(lp_lat);
                        } else
                            x = y = 0.;
                        if (this->m_proj_parm.mode) { /* Winkel Tripel */
                            x = (x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                            y = (y + lp_lat) * 0.5;
                        }
                    /* if too far from given values of x,y, repeat with better approximation of phi,lam */
                    } while (((fabs(xy_x-x) > EPSILON) || (fabs(xy_y-y) > EPSILON)) && (round++ < MAXROUND));

                    if (iter == MAXITER && round == MAXROUND) fprintf(stderr, "Warning: Accuracy of 1e-12 not reached. Last increments: dlat=%e and dlon=%e\n", dp, dl);
                }

                static inline std::string get_name()
                {
                    return "aitoff_spheroid";
                }

            };

            template <typename Parameters>
            void setup(Parameters& par, par_aitoff& proj_parm)
            {
                boost::ignore_unused(proj_parm);
                par.es = 0.;
            }

            // Aitoff
            template <typename Parameters>
            void setup_aitoff(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 0;
                setup(par, proj_parm);
            }

            // Winkel Tripel
            template <typename Parameters>
            void setup_wintri(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 1;
                if (pj_param(par.params, "tlat_1").i)
                    {
                    if ((proj_parm.cosphi1 = cos(pj_param(par.params, "rlat_1").f)) == 0.)
                        throw proj_exception(-22);
                    }
                else /* 50d28' or acos(2/pi) */
                    proj_parm.cosphi1 = 0.636619772367581343;
                setup(par, proj_parm);
            }

        }} // namespace detail::aitoff
    #endif // doxygen

    /*!
        \brief Aitoff projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_aitoff.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aitoff_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline aitoff_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_aitoff(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Winkel Tripel projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_wintri.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct wintri_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline wintri_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_wintri(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class aitoff_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<aitoff_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class wintri_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<wintri_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void aitoff_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("aitoff", new aitoff_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("wintri", new wintri_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP


#include <iostream>
#include "main.h"
#include <Eigen/Dense>

using namespace Eigen;

int main() {

  MatrixXd m(2,2);
  std::cout << "Hello from main.cc " << swag::getFive() << " times." << std::endl;

  return 0;
}


/*CHOMP class definition
 *
 * Copyright (C) 2016 Rafael Valencia. All rights reserved.
 * License (3-Cluase BSD): https://github.com/rafaelvalencia
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file path_adaptor.hpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs) to end point (qe) while
 * avoiding obstacles (obs)
 *
 *
 * USAGE:
 * 			1. Declare a CHOMP object with your selected parameters (passed as arguments in contructor).
 * 			2. Initialize a path, starting pose and ending pose. E.g. use  'setPath', 'initStraightLinePath' or 'initStackedPath'.
 * 			3. Set obstacles with 'setObstacles' (add more with 'addObstacle').
 * 			4. Run CHOMP with 'generatePath' or run a single iteration with 'chompIteration' (you need an initialized path for this step).
 *
 */
#ifndef CHOMP_H
#define CHOMP_H

#include <Eigen/Dense>
typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;

/////////////////////////////////////////////////////////////////////////////////////////////////////
// Base CHOMP class
class CHOMP
{
	protected:
		// trajectory
		Vector xi_;			// the trajectory (q_1, q_2, ...q_n)
		Vector qs_;			// the start config a.k.a. q_0
		Vector qe_;			// the end config a.k.a. q_(n+1)
		size_t nq_;			// number of robot poses q in xi
		size_t cdim_;		// dimension of config space
		size_t xidim_;		// dimension of trajectory, xidim = nq * cdim
		double dt_;	       	// time step
		double eta_; 		// >= 1, regularization factor for gradient descent
		double lambda_; 	// weight of smoothness objective (usually 10)
		double gamma_;
		double ts_;
		double te_;
		Vector ti_;

		//obstacles
		Matrix OBS_;		// Obstacles, each column contains (x,y,radius) of the obstacle
		Matrix Dx_;         // Signed Distance Field

		// gradient descent
		Matrix AA_;			// metric
		Matrix BB_;
		Matrix grid_;        // For computing Euclidean Distance Transform
		Vector bb_;			// acceleration bias for start and end config
		Vector tt_;
		Matrix Ainv_;		// inverse of AA
		Matrix Binv_;
		size_t  numIt_;		// Number of iterations
		double costGain_;   // Gain inside cost function (usually 10)
		double res_;		// Residual
		size_t cter_;		// Count of optimization iterations
		bool PATH_INIT_; //Indicates whether a path has been initialized

	public:
		CHOMP(double dt_input, double eta_input, double lambda_input, size_t nq_input, size_t cdim_input, size_t numIt_, double gain, double gamma_input);
		void initCHOMP(void); 			//Initializes gradient descent vectors and matrices
		void generatePath(Vector  &xi, Vector &ti);	//optimize path using with numIt_ iterations
		void generatePathWithSearchReg(Vector  &xi, double orientation, double &U_cost, double &curv);	//optimize path using with numIt_ iterations with serch region.
		double chompIteration(Vector  &xi, Vector &ti);	//performs a single gradient descent iteration
		double chompUpdate(Vector  &xi, double &U_cost, double &curv); //performs a single gradient descent iteration and gives more data
		double chompUpdateWithSearchRegion(Vector  &xi, double orientation, double &U_cost, double &curv);	//performs a single gradient descent iteration withing predefined search region
		void setObstacles(Matrix obs);	//sets the obstacle matrix OBS_
		void addObstacle(double px, double py, double radius);		//Appends a column in matrix OBS_ with the (x,y,radius) of the new obstacle
		void initStraightLinePath(Vector  &qs, Vector &qe);//initializes path xi as a straight line, with qs and qe, starting and ending point
		void initStackedPath(Vector  &qs, Vector &qe, double ts, double te);//initializes path with qs and qe, starting and ending point, where xi_ is stacked to qs point
		void setPath(Vector  &qs, Vector &qe, double ts, double tq, Vector &xi); // sets an aribitrary value for xi_, qs_, qe_
		void getPath(Vector &xi, Vector &ti); // gets xi_
		void makeGrid(double xmin, double ymin, double xmax, double ymax);
};

/////////////////////////////////////////////////////////////////////////////////////////////////////
// Derived CHOMP SE2 class
/*
 * CHOMP class to work with planar differential-drive robot, i.e. it can translate and rotate
 * and its workspace is still 2-dimensional,
 * but Configuration Space is the special Euclidean group SE(2) = R2 × SO(2)
 * (where SO(2) is the special orthogonal group of 2D rotations).
 * Its configuration is represented using 3 parameters (x, y, orientation).
*/
class CHOMP_SE2: public CHOMP
{
   public:
   	CHOMP_SE2(double dt_input, double eta_input, double lambda_input, size_t nq_input, size_t numIt_, double gain, double gamma_input);
	double chompIteration(Vector  &xi);	//performs a single gradient descent iteration considering nonholonomic constraints for a diff. drive robot.
	void boundTrajectoryAngles (void); // keep angles in trajectory inside pi to pi range
	void boundVectorAngles (Vector& VV); // keep angles in any trajectory vector inside pi to pi range
};

#endif


//
// Created by Standard on 17/04/2021.
//
#include "MTTensor.h"
#include "gnuplot-iostream.h"
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/program_options.hpp>
#include "Parser.h"
#include <fstream>
#include <strstream>
#include <algorithm>
#include <cmath>
#include <boost/filesystem.hpp>

struct measure{
    double val{nan("")};
    double error{0};
};

constexpr double mu0 = M_PI*4e-7;

measure get_rho(double z_real, double z_imag, double z_var, double freq);
measure get_phi(double z_real, double z_imag, double z_var);

boost::program_options::options_description parse_cmdline(int argc, char *argv[], boost::program_options::variables_map& p_vm);
std::string getFileContents(std::ifstream&);

int main(int argc, char* argv[]){
    try {
        // program configuration
        boost::program_options::variables_map vm;
        auto desc = parse_cmdline(argc, argv, vm);
        if (vm.count("help")) {
            std::cout << desc << std::endl;
            return 0;
        }
        auto fileName = vm["in-path"].as<std::string>();
        auto pathObject = boost::filesystem::path(fileName);
        std::cout << "handling file: " << pathObject.filename().string() << "\n";
        std::ifstream is;
        is.open(fileName);
        if(!is.is_open())throw std::runtime_error("Cannot open file " + fileName+ "\n");
        std::string fileContents = getFileContents(is);
        is.close();
        MTparser::Parser p;
        p.parse(fileContents);
        auto skip_string = p.get_option_list_for(">HEAD")["EMPTY"];
        std::cout << "skip_string: " << skip_string << "\n";
        auto edi_contents = p.get();
        MTparser::Option_list ol;
        MTparser::Data_set ds;
//        for (auto it=edi_contents.begin();it!=edi_contents.end();it++) {
//            std::cout << it->first << ":\n\toption list:\n";
//            ol = p.get_option_list_for(it->first);
//            for (auto oit = ol.begin(); oit != ol.end(); oit++) {
//                std::cout << "\t\t" << oit->first << "==" << oit->second << std::endl;
//            }
//        }

        Gnuplot gp;
        auto freq = MTparser::dataset2double(p.get_data_set_for(">FREQ"), skip_string);
        // OFF-DIAG
        auto zxyr = MTparser::dataset2double(p.get_data_set_for(">ZXYR"), skip_string);
        auto zyxr = MTparser::dataset2double(p.get_data_set_for(">ZYXR"), skip_string);
        auto zxyi = MTparser::dataset2double(p.get_data_set_for(">ZXYI"), skip_string);
        auto zyxi = MTparser::dataset2double(p.get_data_set_for(">ZYXI"), skip_string);
        // MAIN-DIAG
        auto zxxr = MTparser::dataset2double(p.get_data_set_for(">ZXXR"), skip_string);
        auto zyyr = MTparser::dataset2double(p.get_data_set_for(">ZYYR"), skip_string);
        auto zxxi = MTparser::dataset2double(p.get_data_set_for(">ZXXI"), skip_string);
        auto zyyi = MTparser::dataset2double(p.get_data_set_for(">ZYYI"), skip_string);

        auto zxyv = MTparser::dataset2double(p.get_data_set_for(">ZXY.VAR"), skip_string);
        auto zyxv = MTparser::dataset2double(p.get_data_set_for(">ZYX.VAR"), skip_string);
        auto zxxv = MTparser::dataset2double(p.get_data_set_for(">ZXX.VAR"), skip_string);
        auto zyyv = MTparser::dataset2double(p.get_data_set_for(">ZYY.VAR"), skip_string);

        std::vector<double> zxys,zyxs,zxxs,zyys;
        for (auto v : zxyv) zxys.push_back(std::sqrt(v));
        for (auto v : zyxv) zyxs.push_back(std::sqrt(v));
        for (auto v : zxxv) zxxs.push_back(std::sqrt(v));
        for (auto v : zyyv) zyys.push_back(std::sqrt(v));

        auto title = pathObject.filename().string();

        auto pos = title.find('_');
        if(pos!=std::string::npos)title.insert(pos,"\\");
        gp << "set term qt enhanced 'Times-Roman, 9'\n";
        gp << "set title '" + title +"'\n";
        gp << "unset key\n";
        gp << "set multiplot layout 2,2\n";
        gp << "set logscale x\n";
//        gp << "set logscale y\n";
        if (false) {
            gp << "set xlabel 'Frequency (Hz)'\n";
            gp << "set ylabel 'Impedance (Ohm)'\n";

            // XX
            gp << "set title '" + title + "-XX'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{xx})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{xx})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zxxr, zxxs));
            gp.send1d(std::make_tuple(freq, zxxi, zxxs));
            // XY
            gp << "set title '" + title + "-XY'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{xy})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{xy})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zxyr, zxys));
            gp.send1d(std::make_tuple(freq, zxyi, zxys));
            // YX
            gp << "set title '" + title + "-YX'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{yx})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{yx})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zyxr, zyxs));
            gp.send1d(std::make_tuple(freq, zyxi, zyxs));
            // YY
            gp << "set title '" + title + "-YY'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{yy})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{yy})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zyyr, zyys));
            gp.send1d(std::make_tuple(freq, zyyi, zyys));
        } else {
            std::vector<double> rhoxx, srhoxx,phixx, sphixx;
            for(int i=0;i<zxxr.size();++i){
                auto this_rho = get_rho(zxxr[i],zxxi[i],zxxv[i],freq[i]);
                rhoxx.push_back(this_rho.val);
                srhoxx.push_back(this_rho.error);
                auto this_phi = get_phi(zxxr[i],zxxi[i],zxxv[i]);
                phixx.push_back(this_phi.val);
                sphixx.push_back(this_phi.error);
            }

            std::vector<double> rhoxy, srhoxy,phixy, sphixy;
            for(int i=0;i<zxyr.size();++i){
                auto this_rho = get_rho(zxyr[i],zxyi[i],zxyv[i],freq[i]);
                rhoxy.push_back(this_rho.val);
                srhoxy.push_back(this_rho.error);
                auto this_phi = get_phi(zxyr[i],zxyi[i],zxyv[i]);
                phixy.push_back(this_phi.val);
                sphixy.push_back(this_phi.error);
            }

            std::vector<double> rhoyx, srhoyx,phiyx, sphiyx;
            for(int i=0;i<zyxr.size();++i){
                auto this_rho = get_rho(zyxr[i],zyxi[i],zyxv[i],freq[i]);
                rhoyx.push_back(this_rho.val);
                srhoyx.push_back(this_rho.error);
                auto this_phi = get_phi(zyxr[i],zyxi[i],zyxv[i]);
                phiyx.push_back(this_phi.val);
                sphiyx.push_back(this_phi.error);
            }

            std::vector<double> rhoyy, srhoyy,phiyy, sphiyy;
            for(int i=0;i<zyyr.size();++i){
                auto this_rho = get_rho(zyyr[i],zyyi[i],zyyv[i],freq[i]);
                rhoyy.push_back(this_rho.val);
                srhoyy.push_back(this_rho.error);
                auto this_phi = get_phi(zyyr[i],zyyi[i],zyyv[i]);
                phiyy.push_back(this_phi.val);
                sphiyy.push_back(this_phi.error);
            }

            gp << "set xlabel 'Frequency (Hz)'\n";
            gp << "set y2tics -180, 60\n";
            gp << "set y2range [-180:180]\n";
            gp << "set ytics nomirror\n";
            gp << "set logscale y\n";
            gp << "set ylabel '{/Symbol r}_{app} ({/Symbol W}m)'\n";
            gp << "set y2label '{/Symbol F} ({^0})'\n";

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoxx,srhoxx));
            gp.send1d(std::make_tuple(freq,phixx,sphixx));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoxy,srhoxy));
            gp.send1d(std::make_tuple(freq,phixy,sphixy));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoyx,srhoyx));
            gp.send1d(std::make_tuple(freq,phiyx,sphiyx));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoyy,srhoyy));
            gp.send1d(std::make_tuple(freq,phiyy,sphiyy));
        }

        return 0;
    }
    catch (std::exception &e){
        std::cout << "Error: " << e.what() <<"\n";
        return 1;
    }
    catch(...){
        std::cerr << "Unhandled exception. Quit.\n";
        return 2;
    }
    return 0;
}

boost::program_options::options_description parse_cmdline(int argc, char *argv[], boost::program_options::variables_map& p_vm){
    namespace po = boost::program_options;
    po::options_description generic("Generic options");
    generic.add_options()
            ("help,h", "display this message and exit.")
            ("in-path,i", po::value<std::string>(), "PATH to the input .edi file.")
            ("out-path, o", po::value<std::string>()->default_value("./"), "Where I save plots.");
    po::store(po::parse_command_line(argc, argv, generic), p_vm);
    po::notify(p_vm);
    return generic;
}

std::string getFileContents(std::ifstream& input){
    std::ostrstream sstr;
    sstr << input.rdbuf();
    return sstr.str();
}

measure get_rho(double z_real, double z_imag, double z_var, double freq){
    auto omega = 2*M_PI*freq;
    double rho = (z_real*z_real + z_imag*z_imag)/(mu0*omega);
    double sigma_z = sqrt(z_var);
    double sigma_rho = 2*sigma_z*sqrt(pow(z_real+z_imag,2))/(mu0*omega);
//            double sigma_phi = sqrt(
//            pow((z_imag/(z_real*z_real + z_imag*z_imag))*sigma_z,2) +
//            pow((-z_real/(z_real*z_real + z_imag*z_imag))*sigma_z,2)
//    );
    return {rho,sigma_rho};
}
measure get_phi(double z_real, double z_imag, double z_var){
    double phi = atan2(z_imag,z_real);
    double sigma_z = sqrt(z_var);
    double sigma_phi = sqrt(
            pow((z_imag/(z_real*z_real + z_imag*z_imag))*sigma_z,2) +
            pow((-z_real/(z_real*z_real + z_imag*z_imag))*sigma_z,2)
    );
    return {phi*180/M_PI, sigma_phi*180/M_PI};
}


#include <Eigen/Dense>
#include "Model.hpp"
#include "Logger.hpp"
#include <algorithm>

Model::Model(const char *filename)
  : m_filename(filename)
{
  std::string err = tinyobj::LoadObj(m_shapes, filename);
  ASSERT_MSG(err.empty(), "%s", err.c_str());

  for ( size_t i=0; i < m_shapes.size(); i++ ) {
    if ( m_shapes[i].mesh.normals.empty() )
      calculate_normal(i);
  }
}

Model::~Model()
{
}

void Model::debug() const
{
  for ( size_t i=0; i < m_shapes.size(); i++ ) {
    const tinyobj::shape_t & shape = m_shapes[i];
    const std::vector<unsigned int> & indices = shape.mesh.indices;
    const std::vector<float> & positions = shape.mesh.positions;
    const std::vector<float> & normals = shape.mesh.normals;

    INFO("Shape %lu: %s", i, shape.name.c_str());
    ASSERT(indices.size() % 3 == 0);
#if 0
    for ( size_t j=0; j < indices.size(); j+=3 ) {
      printf("triangle: (%.2f, %.2f, %.2f) (%.2f, %.2f, %.2f)  (%.2f, %.2f, %.2f)\n",
          positions[3*indices[j  ]], positions[3*indices[j  ]+1], positions[3*indices[j  ]+2],
          positions[3*indices[j+1]], positions[3*indices[j+1]+1], positions[3*indices[j+1]+2],
          positions[3*indices[j+2]], positions[3*indices[j+2]+1], positions[3*indices[j+2]+2]);

      if ( !normals.empty() ) {
        printf("normals: (%.2f, %.2f, %.2f) (%.2f, %.2f, %.2f)  (%.2f, %.2f, %.2f)\n",
            normals[3*indices[j  ]], normals[3*indices[j  ]+1], normals[3*indices[j  ]+2],
            normals[3*indices[j+1]], normals[3*indices[j+1]+1], normals[3*indices[j+1]+2],
            normals[3*indices[j+2]], normals[3*indices[j+2]+1], normals[3*indices[j+2]+2]);
      }
    }
#endif
    INFO("number of triangles = %lu", indices.size() / 3);
    INFO("number of vertices = %lu", positions.size() / 3);
    INFO("indices.size() = %lu", indices.size());
    INFO("positions.size() = %lu", positions.size());
    INFO("normals.size() = %lu", normals.size());
    INFO("min of index = %u", *(std::min_element(indices.begin(), indices.end())));
    INFO("max of index = %u", *(std::max_element(indices.begin(), indices.end())));
  }
}

size_t Model::numShapes() const
{
  return m_shapes.size();
}

size_t Model::vertexSize(size_t i) const
{
  return m_shapes[i].mesh.positions.size();
}

size_t Model::normalSize(size_t i) const
{
  return m_shapes[i].mesh.normals.size();
}

size_t Model::indexSize(size_t i) const
{
  return m_shapes[i].mesh.indices.size();
}

void *Model::vertexData(size_t i)
{
  return (void*)&(m_shapes[i].mesh.positions[0]);
}

void *Model::normalData(size_t i)
{
  return (void*)&(m_shapes[i].mesh.normals[0]);
}

void *Model::indexData(size_t i)
{
  return (void*)&(m_shapes[i].mesh.indices[0]);
}

void Model::calculate_normal(size_t idx)
{
  // Index is assumed
  ASSERT(!m_shapes[idx].mesh.indices.empty());

  const std::vector<unsigned int> & indices = m_shapes[idx].mesh.indices;
  const std::vector<float> & positions = m_shapes[idx].mesh.positions;

  // To store normal for each vertex
  std::vector<Vector3> new_normals(positions.size()/3, Vector3::Zero());

  // For each face, calculate face normal, add it to each vertex of the face
  for ( size_t i=0; i < indices.size(); i += 3 )
  {
    Vector3 a(positions[3*indices[i+1]  ]-positions[3*indices[i]  ],
                      positions[3*indices[i+1]+1]-positions[3*indices[i]+1],
                      positions[3*indices[i+1]+2]-positions[3*indices[i]+2]);
    Vector3 b(positions[3*indices[i+2]  ]-positions[3*indices[i]  ],
                      positions[3*indices[i+2]+1]-positions[3*indices[i]+1],
                      positions[3*indices[i+2]+2]-positions[3*indices[i]+2]);
    Vector3 n = a.cross(b).normalized();
    new_normals[indices[i  ]] += n;
    new_normals[indices[i+1]] += n;
    new_normals[indices[i+2]] += n;
  }
  for ( size_t i=0; i < new_normals.size(); i++ )
  {
    new_normals[i].normalize();
  }

  std::vector<float> & normals = m_shapes[idx].mesh.normals;
  if ( !normals.empty() )
    WARN("Overwriting exisiting normals...");
  normals.resize(positions.size());

  for ( size_t i=0; i < indices.size(); i++ )
  {
    normals[3*indices[i]  ] = new_normals[indices[i]].x();
    normals[3*indices[i]+1] = new_normals[indices[i]].y();
    normals[3*indices[i]+2] = new_normals[indices[i]].z();
  }
}

void Model::getTriangles(std::vector<Triangle> &triangles, const Matrix4 &transform)
{
  //const Matrix4 normal_transform = (transform.transpose()*transform).inverse()*transform.transpose();
  const Matrix4 normal_transform = transform.adjoint().transpose();
  //const Matrix4 normal_transform = transform.inverse().transpose();
  //const Matrix4 &normal_transform = transform;

  float x[2] = {99999.f, -99999.f};
  float y[2] = {99999.f, -99999.f};
  float z[2] = {99999.f, -99999.f};

  size_t n_filtered = 0;
  size_t n_remained = 0;

  for ( size_t i=0; i < m_shapes.size(); i++ )
  {
    const std::vector<unsigned int> & indices = m_shapes[i].mesh.indices;
    const std::vector<float> & positions = m_shapes[i].mesh.positions;
    const std::vector<float> & normals = m_shapes[i].mesh.normals;

    for ( size_t j=0; j < indices.size(); j += 3 )
    {
      Triangle t;

      // do the transformation
      for ( size_t k=0; k < 3; k++ )
      {
        Vector4 v(positions[3*indices[j+k]], positions[3*indices[j+k]+1], positions[3*indices[j+k]+2], 1.0);
        v = transform * v;
        v /= v.w();
        t.vertices[k] = Vector3(v.x(), v.y(), v.z());
      }

      // do statistics about vertex info
      for ( size_t k=0; k < 3; k++ )
      {
        x[0] = std::min(x[0], (float)(t.vertices[k].x()));
        x[1] = std::max(x[1], (float)(t.vertices[k].x()));
        y[0] = std::min(y[0], (float)(t.vertices[k].y()));
        y[1] = std::max(y[1], (float)(t.vertices[k].y()));
        z[0] = std::min(z[0], (float)(t.vertices[k].z()));
        z[1] = std::max(z[1], (float)(t.vertices[k].z()));
      }

      // filter out this triangle if all three vertices are outside of the viewing volume
      if ( (t.vertices[0].x() < -1.0f || t.vertices[0].x() > 1.0f ||
            t.vertices[0].y() < -1.0f || t.vertices[0].y() > 1.0f ||
            t.vertices[0].z() < -1.0f || t.vertices[0].z() > 1.0f )
        && (t.vertices[1].x() < -1.0f || t.vertices[1].x() > 1.0f ||
            t.vertices[1].y() < -1.0f || t.vertices[1].y() > 1.0f ||
            t.vertices[1].z() < -1.0f || t.vertices[1].z() > 1.0f )
        && (t.vertices[2].x() < -1.0f || t.vertices[2].x() > 1.0f ||
            t.vertices[2].y() < -1.0f || t.vertices[2].y() > 1.0f ||
            t.vertices[2].z() < -1.0f || t.vertices[2].z() > 1.0f ) )
      {
        n_filtered++;
        continue;
      }

#if 1
      // transform the normals as well
      for ( size_t k=0; k < 3; k++ )
      {
        Vector4 n(normals[3*indices[j+k]], normals[3*indices[j+k]+1], normals[3*indices[j+k]+2], 1.0);
        n = normal_transform * n;
        t.normals[k] = Vector3(n.x()/n.w(), n.y()/n.w(), n.z()/n.w());
        t.normals[k].normalize();
      }
#else
      {
        Vector3 a = t.vertices[1] - t.vertices[0];
        Vector3 b = t.vertices[2] - t.vertices[0];
        t.normals[0] = t.normals[1] = t.normals[2]
          = a.cross(b).normalized();
      }
#endif

      // filter out this triangle if it's facing backward to viewer
      // TODO: find out better solution
      if (0)
      //if ( t.normals[0].z() < 0 || t.normals[1].z() < 0 || t.normals[2].z() < 0 )
      //if ( t.normals[0].z() + t.normals[1].z() + t.normals[2].z() < 0 )
      {
        n_filtered++;
        continue;
      }
      if (0)
      {
        Vector3 n = t.normals[0] + t.normals[1] + t.normals[2];
        n.normalize();

        INFO("normal = (%.2f, %.2f, %.2f)", n.x(), n.y(), n.z());
      }

      triangles.push_back(t);
      n_remained++;
    }
  }
  INFO("range of x (before clip): (%.2f, %.2f)", x[0], x[1]);
  INFO("range of y (before clip): (%.2f, %.2f)", y[0], y[1]);
  INFO("range of z (before clip): (%.2f, %.2f)", z[0], z[1]);
  INFO("filtered: %.2f%% (%lu/%lu)", 100.0f*n_filtered/(n_filtered+n_remained), n_filtered, n_filtered+n_remained);
}

void Triangle::raster(std::vector<Pixel> &pixels, int w, int h) const
{
  int x[2] = {99999, -99999};
  int y[2] = {99999, -99999};
  int xd[3];
  int yd[3];

  // find discrete coordinates of each vertex in 2D plane
  for ( size_t i=0; i < 3; i++ )
  {
    xd[i] = int((vertices[i].x() + 1.0f) / 2.0f * w);
    yd[i] = int((vertices[i].y() + 1.0f) / 2.0f * h);
  }
  for ( size_t i=0; i < 3; i++ )
  {
    x[0] = std::min(x[0], xd[i]);
    x[1] = std::max(x[1], xd[i]);
    y[0] = std::min(y[0], yd[i]);
    y[1] = std::max(y[1], yd[i]);
  }

  // construct maxtrix A
  Matrix3 A(Matrix3::Ones());
  for ( size_t i=0; i < 3; i++ )
  {
    A(0, i) = xd[i];
    A(1, i) = yd[i];
  }
  A = A.inverse().eval();

  // find each pixel
  for ( int i=x[0]; i <= x[1]; i++ )
    for ( int j=y[0]; j <= y[1]; j++ )
    {
      Vector3 b(i, j, 1.0);
      Vector3 t = A * b;

      if ( t.x() < 0 || t.y() < 0 || t.z() < 0 )
        continue;

      pixels.push_back(Pixel(i, j, t));
    }
}

float Triangle::getDepth(const Pixel &p) const
{
  const Vector3 &t = p.t;

  return t.x()*vertices[0].z()
       + t.y()*vertices[1].z()
       + t.z()*vertices[2].z();
}

uint32_t Triangle::getColor(const Pixel &p) const
{
#if 0
  int t = 255 * (0.5*getDepth(p)+0.5);
  return (0xff000000 | t << 16 | t << 8 | t);
#endif

  // define static material color
  const static float shininess = 15.0f;
  const static Vector3 diffuse(0.929524f, 0.796542f, 0.178823f);
  const static Vector3 specular(1.00000f, 0.980392f, 0.549020f);

  // calculate position and normal for p
  const Vector3 &t = p.t;
  Vector3 v = t.x()*vertices[0] + t.y()*vertices[1] + t.z()*vertices[2];
  Vector3 n = t.x()*normals[0] + t.y()*normals[1] + t.z()*normals[2];
  n.normalize();

  const Vector3 light_position = Vector3(0.0, 5.0, 0.0);

  // calculate light vector, view vector and half vector
  Vector3 li = (light_position-v).normalized();
  Vector3 vi = (-v).normalized();
  Vector3 h = (li+vi).normalized();

  // calculate color using phong model
  Vector3 color = 0.3*diffuse;
  if ( n.dot(li) > 0 )
    color += diffuse * (n.dot(li));
  if ( n.dot(h) >= 0 )
    color += specular * std::pow((double)n.dot(h), (double)shininess);
  color(0) = std::max(color(0), 0.0); color(0) = std::min(color(0), 1.0);
  color(1) = std::max(color(1), 0.0); color(1) = std::min(color(1), 1.0);
  color(2) = std::max(color(2), 0.0); color(2) = std::min(color(2), 1.0);

  int r = 255 * color.x();
  int g = 255 * color.y();
  int b = 255 * color.z();
  return (0xff000000 | r << 16 | g << 8 | b);
}


#ifndef CEGO_H
#define CEGO_H

#include <memory>
#include <random>
#include <algorithm>
#include <numeric>
#include <vector>
#include <cassert>
#include <iostream>
#include <iterator>
#include <functional>
#include <tuple>
#include <map>
#include <string>
#include <sstream>
#include <stdexcept>
#include <cassert>
#include <future>       // std::packaged_task, std::future
#include <thread>       // std::thread, std::this_thread::sleep_for

#include <Eigen/Dense>
#include "CEGO/datatypes.hpp"
#include "CEGO/utilities.hpp"
#include "CEGO/lhs.hpp"

#include "CEGO/evolvers/evolvers.hpp"

#include "CEGO/concurrentqueue.h"
#include "ThreadPool.h"

#include "nlohmann/json.hpp"

namespace CEGO{

    struct Result {
        Eigen::ArrayXd c;
        double ssq;
        Result() {};
        Result(Eigen::ArrayXd &&c, double &&ssq) : c(c), ssq(ssq) {};
    };

    /// Return Mersenne twister generator
    inline std::mt19937 get_gen(){
        std::random_device rd;
        return std::mt19937(rd());
    };

    template<class T>
    inline std::string vec2string(const std::vector<T> &v, const std::string &sep = ", ") {
        using std::to_string;
        std::stringstream ss;
        for (auto &&el : v)
            ss << to_string(el) << sep;
        return ss.str();
    }

    inline std::string vec2string(const std::vector<std::vector<double> > &v, const std::string &sep = ", ") {
        std::stringstream ss;
        for (auto &el : v)
            ss << to_string(el) << sep;
        return ss.str();
    }

    /// Generate a random population of individuals
    template<typename T>
    Population random_population(const std::vector<CEGO::Bound> bounds, std::size_t count, const CostFunction &cost_function)
    {
        auto length_ind = bounds.size();
        auto gen = get_gen();
        Population out; out.reserve(count);
        for (std::size_t i = 0; i < count; ++i) {
            std::vector<T> c; c.reserve(length_ind);
            for (std::size_t j = 0; j < length_ind; ++j) {
                auto &&bound = bounds[j];
                double d = 0; int integer = 0;
                bound.gen_uniform(gen, d, integer);

                switch (bound.m_lower.type) {
                case CEGO::numberish::types::DOUBLE:
                    c.emplace_back(d); break;
                case CEGO::numberish::types::INT:
                    c.emplace_back(integer); break;
                }
            }
            assert(c.size() == length_ind);
            out.emplace_back(pIndividual(new NumericalIndividual<T>(std::move(c), cost_function)));
        }
        return out;
    }

    /// Generate a population of individuals with the use of Latin-Hypercube sampling
    template<typename T>
    Population LHS_population(const std::vector<CEGO::Bound> bounds, std::size_t count, const CostFunction &cost_function)
    {
        // Generate the set of floating parameters in [0,1]
        Eigen::ArrayXXd population = LHS_samples(count, bounds.size());

        auto length_ind = bounds.size();
        auto gen = get_gen();
        Population out; out.reserve(count);
        for (std::size_t i = 0; i < count; ++i) {
            std::vector<T> c; c.reserve(length_ind);
            for (std::size_t j = 0; j < length_ind; ++j) {
                auto &&bound = bounds[j];

                switch (bound.m_lower.type) {
                case CEGO::numberish::types::DOUBLE:
                {
                    double w = population(i,j);
                    c.emplace_back(bound.m_lower.as_double()*w + bound.m_upper.as_double()*(1-w)); break;
                }
                case CEGO::numberish::types::INT:{
                    double w = population(i, j);
                    c.emplace_back(int(round(bound.m_lower.as_int()*w + bound.m_upper.as_int()*(1 - w)))); break;
                }
                }
            }
            assert(c.size() == length_ind);
            out.emplace_back(pIndividual(new NumericalIndividual<T>(std::move(c), cost_function)));
        }
        return out;
    }

    enum class LoggingScheme { none = 0, all, custom };
    enum class FilterOptions { accept, reject };
    enum class GenerationOptions { LHS, random };

    template<typename T>
    class Layers {

    private:
        std::vector<Population> m_layers;
        std::size_t m_generation = 0;
        moodycamel::ConcurrentQueue<Result> result_queue;

        void initialize_layers() {

            // Generate all the individuals serially(!)
            MutantVector mutants;
            for (auto i = 0; i < Nlayers; ++i) {
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                for (auto && ind : generator(m_bounds, Npop_size, m_cost_function)) {
                    mutants.emplace_back(std::make_pair(i, std::move(ind)));
                }
            }

            // Evaluate all of the layers and individuals (in parallel, if desired)
            evaluate_mutants(mutants);

            // Recollect the individuals into layers, again in serial
            m_layers.resize(Nlayers);
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                m_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }

            // Sort all of the layers
            sort_all_layers();
        };
        std::unique_ptr<ThreadPool> m_pool;
        LoggingScheme m_log_scheme = LoggingScheme::none;
        std::function<FilterOptions(const Result &)> m_filter_function;
        std::unique_ptr<AbstractEvolver<T> > m_evolver; ///< The functor that is to be used to evolve a layer
        std::vector<Bound> m_bounds;
        GenerationOptions m_generation_flag = GenerationOptions::random;
        CostFunction m_cost_function;
        std::size_t m_Nelite = 2;
    public:

        bool parallel = false;
        bool print_chunk_times = false;
        std::size_t parallel_threads = 6;
        std::size_t Nind_size, Npop_size, Nlayers, age_gap;

        Layers(const std::function<double(const std::vector<T>&)> &function, std::size_t Nind_size, std::size_t Npop_size, std::size_t Nlayers, std::size_t age_gap = 5)
            : Nind_size(Nind_size), Npop_size(Npop_size), Nlayers(Nlayers), age_gap(age_gap){

            m_cost_function = [function](const CEGO::AbstractIndividual *pind) {
                const std::vector<T> &c = static_cast<const CEGO::NumericalIndividual<T>*>(pind)->get_coefficients();
                return function(c);
            };
        };

        /// Constructor into which is passed a CostFunction and information about the layers
        Layers(CostFunction &function, std::size_t Nind_size, std::size_t Npop_size, std::size_t Nlayers, std::size_t age_gap = 5)
            : Nind_size(Nind_size), Npop_size(Npop_size), Nlayers(Nlayers), age_gap(age_gap), m_cost_function(function){ };

        /// Specify the logging scheme that is to be employed
        void set_logging_scheme(LoggingScheme scheme){ m_log_scheme = scheme; }

        /// Get the logging scheme in use
        LoggingScheme get_logging_scheme() { return m_log_scheme; }

        /// Set the filtering function that should be used
        void set_filtering_function(const std::function<FilterOptions(const Result &)> &f){ m_filter_function = f; }

        /// Set the bounds on each element in the individual.  If a one-element vector, the same bounds are used for each parameter
        void set_bounds(const std::vector<Bound> &bounds){ m_bounds = bounds; };

        /// Get the bounds applied to each element in the individual
        const std::vector<Bound > & get_bounds (){ return m_bounds; };

        /// Get the flags for the evolver in JSON format
        const nlohmann::json get_evolver_flags() const {
            if (m_evolver == nullptr) {
                throw std::invalid_argument("Evolver has not been selected yet!");
            }
            return m_evolver->get_flags();
        }

        /// Set the flags for the evolver in JSON format
        const void set_evolver_flags(const nlohmann::json &flags) const {
            m_evolver->set_flags(flags);
        }

        /// Pick one of the builtin evolvers
        void set_builtin_evolver(BuiltinEvolvers e) {
            switch (e) {
            case BuiltinEvolvers::differential_evolution:
                m_evolver.reset(new DE1BinEvolver<T>); break;
            default:
                throw std::invalid_argument("Invalid builtin evolver");
            }
        }

        /// Get the cost function that is being used currently
        const CostFunction &get_cost_function() {
            return m_cost_function;
        }

        /// Set the flag to determine whether LHS or random (or other) is to be used to generate the population
        void set_generation_mode(GenerationOptions flag) {
            m_generation_flag = flag;
        }

        /// Get the flag to determine whether LHS or random is to be used to generate the population
        GenerationOptions get_generation_mode() {
            return m_generation_flag;
        }

        /** Iterate over the layers and find individuals that are too old for the given layer
         *
         * First try to see if the elderly individual dominates any individual in a layer with a higher age limit
         * If it does, replace the individual in the higher age limit layer
         */
        void graduate_elderly_individuals() {

            // Iterate backwards through the layers, starting at the N-1 layer, since the
            // last layer has an infinite age limit, and you cannot age out of the
            // highest age limit layer
            for (int ilayer = static_cast<int>(m_layers.size())-2; ilayer >= 0; --ilayer) {

                double age_threshold = static_cast<double>(age_gap*pow(2, ilayer));
                auto &this_layer = m_layers[ilayer];

                // Iterate backwards through the individuals in the layer
                for (int iind = static_cast<int>(this_layer.size())-1; iind >= static_cast<int>(m_Nelite); --iind) {
                    auto &ind = this_layer[iind];
                    if (ind->age() > age_threshold) {
                        // If higher age limit layer has an empty slot, use it directly
                        if (m_layers[ilayer+1].size() < Npop_size) {
                            m_layers[ilayer+1].push_back(std::move(ind));
                        }
                        // Or if it dominates the worst individual in the higher age limit layer, replace that one
                        // and sort the higher age limit layer
                        else if (ind->get_cost() < m_layers[ilayer + 1].back()->get_cost()) {
                            std::swap(m_layers[ilayer +1].back(), ind);
                            sort_layer(m_layers[ilayer + 1]);
                        }
                        // Say goodbye to this individual
                        this_layer.erase(this_layer.begin() + iind);
                    }
                }
            }
        }
        /// Repopulate the layer to replace removed old individuals
        void repopulate_layers() {

            // Generate the new individuals serially
            MutantVector mutants;
            for (auto i = 0; i < m_layers.size(); ++i) {
                auto &layer = m_layers[i];
                if (layer.size() < Npop_size) {
                    // How many individuals are missing?
                    auto missing_individuals_count = Npop_size - layer.size();
                    // Get the generator function
                    auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                    // Then we pad out the population with new random individuals as needed (they start with an age of zero)
                    for (auto && ind : generator(m_bounds, missing_individuals_count, m_cost_function)) {
                        mutants.emplace_back(std::make_pair(i, std::move(ind)));
                    }
                }
            }

            // Evaluate the individuals we just generated
            evaluate_mutants(mutants);

            // Recollect the individuals into layers, again in serial
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                m_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }

            // Sort all of the layers
            sort_all_layers();
        }

        /// Evaluate a single individual, and store the values if needed
        void evaluate_ind(pIndividual &ind) {
            if (ind->needs_evaluation()){
                ind->evaluate();
                switch (m_log_scheme) {
                    case LoggingScheme::none:
                        break;
                    case LoggingScheme::custom:{
                        if (!m_filter_function) {
                            throw std::invalid_argument("filtering function has not been provided, logging options are inconsistent!");
                        }
                        Result r(ind->get_coeffs_ArrayXd(), ind->get_cost());
                        // If the filter function returns the flag "accept", then store the result
                        switch (m_filter_function(r)) {
                        case FilterOptions::accept:
                            result_queue.enqueue(std::move(r)); break;
                        case FilterOptions::reject: break;
                        }
                        break;
                    }
                    case LoggingScheme::all:{
                        result_queue.enqueue(std::move(Result(ind->get_coeffs_ArrayXd(), ind->get_cost())));
                        break;
                    }
                    default: {
                        throw std::invalid_argument("logging flag is not set; this is an error");
                    }
                }
            }
        }

        /// Evaluate all of the layers
        void evaluate_layers() {
            for (auto &layer : m_layers) {
                for (auto &ind : layer) {
                    evaluate_ind(ind); // A no-op if the individual does not need to be evaluated
                }
            }
        };

        /// Sort all of the layers
        void sort_all_layers() {
            for (auto &layer : m_layers) {
                sort_layer(layer);
            }
        };

        /// Increase the age of all individuals
        void increase_all_ages() {
            for (auto &layer : m_layers) {
                for (auto &ind : layer) {
                    ind->increase_age();
                }
            }
        }

        /// Calculate statistics of the costs of individuals in each layer
        std::vector<std::map<std::string, double> > cost_stats_each_layer() {
            std::vector<std::map<std::string, double> > out;
            for (auto &layer : m_layers) {
                Eigen::ArrayXd cost_layer(layer.size());
                for (auto i = 0; i < layer.size(); ++i) {
                    cost_layer(i) = layer[i]->get_cost();
                }
                std::map<std::string, double> this_layer_map;
                this_layer_map["max"] = cost_layer.maxCoeff();
                this_layer_map["min"] = cost_layer.minCoeff();
                // See https://en.wikipedia.org/wiki/Standard_deviation#Discrete_random_variable
                this_layer_map["stddev"] = sqrt((cost_layer - cost_layer.mean()).square().sum()/cost_layer.size());
                out.push_back(this_layer_map);
            }
            return out;
        }

        /// Sort a given layer
        void sort_layer(Population &pop) {
            auto sort_fcn = [](pIndividual &i1, pIndividual &i2) {
                return i1->get_cost() < i2->get_cost();
            };
            std::sort(pop.begin(), pop.end(), sort_fcn);
        }

        typedef std::vector<std::tuple<std::size_t, pIndividual> > MutantVector;

        void parallel_evaluator(MutantVector::iterator itstart, MutantVector::iterator itend, double &elap_sec) {

            std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
            start = std::chrono::high_resolution_clock::now();
            for (auto it = itstart; it != itend; ++it) {
                auto &ind = std::get<1>(*it);
                evaluate_ind(ind);
            }
            end = std::chrono::high_resolution_clock::now();
            elap_sec = std::chrono::duration<double>(end - start).count();
            return;
        };

        void init_thread_pool(short Nthreads){
            if (!m_pool || m_pool->GetThreads().size() != Nthreads){
                // Make a thread pool for the workers
                m_pool = std::unique_ptr<ThreadPool>(new ThreadPool(Nthreads));
            }
        }

        void evaluate_mutants(MutantVector &mutants) {
            if (!parallel || parallel_threads == 0) {
                MutantVector::iterator itstart = mutants.begin();
                MutantVector::iterator itend = mutants.end();
                double time = 0;
                parallel_evaluator(itstart, itend, time);
            }
            else {
                // Initialize the thread pool, no-op if already initialized and is the right size
                init_thread_pool(static_cast<short>(parallel_threads));
                std::vector< std::future<void> > futures;
                std::vector<std::thread> threads;
                std::size_t Lchunk = mutants.size() / parallel_threads; // Note this is an integer division, so a floor()!
                std::vector<double> times(parallel_threads);

                std::vector<std::size_t> chunksizes(parallel_threads, Lchunk);
                auto Nmax = mutants.size();
                std::size_t remainder = Nmax-Lchunk*parallel_threads;
                // Increase the first remainder chunk sizes
                for (auto i = 0; i < remainder; ++i){
                    chunksizes[i]++;
                }
                // Double-check we get the right sizes
                assert(std::accumulate(chunksizes.begin(), chunksizes.end(), static_cast<std::size_t>(0)) == mutants.size());
                std::size_t isum = 0;
                for (auto j = 0; j < parallel_threads; ++j)
                {
                    std::size_t cs = chunksizes[j];
                    auto itstart = mutants.begin() + isum;
                    auto itend = itstart + cs;
                    isum += cs;
                    double &time = times[j];

                    std::function<void(void)> f = [this, itstart, itend, &time]() {
                        auto startTime = std::chrono::high_resolution_clock::now();
                        parallel_evaluator(itstart, itend, time);
                        auto endTime = std::chrono::high_resolution_clock::now();
                        time = std::chrono::duration<double>(endTime - startTime).count();
                    };
                    m_pool->AddJob(f);
                }
                // Wait until all the threads finish...
                m_pool->WaitAll();
                // Uncomment these lines to print out the times for each chunk; ideally they are all very close
                // to each other.  Similar times means the work per thread is well divided
                if (print_chunk_times){
                    for (auto j = 0; j < parallel_threads; ++j) {
                        std::cout << j << " " << times[j] << std::endl;
                    }
                }
            }
        }

        void evolve_parallel() {
            MutantVector mutants;
            // In serial, generate the mutants; generation of the mutants
            // is very fast.  Store all mutants in a flat vector with the index of the layer
            for (auto i = 0; i < m_layers.size(); ++i) {
                for (auto &&el : m_evolver->evolve_layer(m_layers, i, get_bounds(), get_cost_function())) {
                    mutants.emplace_back(std::make_tuple(i, std::move(el)));
                }
            }

            // Evaluate all the mutants, either in parallel or serial
            evaluate_mutants(mutants);

            // Recollect the mutants into layers, again in serial
            std::vector<Population> new_layers(m_layers.size());
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                new_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }
            // Then determine which ones should be kept based on cost, greedily keeping
            // the one with the lower cost
            for (auto i = 0; i < m_layers.size(); ++i) {
                assert(new_layers[i].size() == m_layers[i].size());
                for (auto j = 0; j < m_layers[i].size(); ++j) {
                    if (new_layers[i][j]->get_cost() < m_layers[i][j]->get_cost()) {
                        std::swap(m_layers[i][j], new_layers[i][j]); // the other one will get thrown away
                    }
                    assert(new_layers[i][j]->get_cost() >= m_layers[i][j]->get_cost());
                }
            }
        }

        /// Evolve a given layer serially
        void evolve_layer(std::size_t i) {

            // Evolve the layer (elements are unevaluated)
            auto new_layer = m_evolver->evolve_layer(m_layers, i, get_bounds(), get_cost_function());

            for (auto j = 0; j < new_layer.size(); ++j) {
                evaluate_ind(new_layer[j]);
                double newcost = new_layer[j]->get_cost(), oldcost = m_layers[i][j]->get_cost();
                // Keep the one with the lower cost (new_layer is going to be the one that is
                // going to be used)
                if (new_layer[j]->get_cost() > m_layers[i][j]->get_cost()) {
                    // Swap because the older one is better, we are collecting values in new_layer
                    std::swap(new_layer[j], m_layers[i][j]);
                }
                assert(new_layer[j]->get_cost() <= m_layers[i][j]->get_cost());
            }

            // How many individuals are missing?
            auto missing_individuals_count = Npop_size - new_layer.size();

            // Then we pad out the population with new random individuals as needed (they start with an age of zero)
            if (missing_individuals_count > 0) {
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                Population random_inds = generator(m_bounds, missing_individuals_count, m_cost_function);
                std::move(random_inds.begin(), random_inds.end(), std::back_inserter(new_layer));
            }

            // And move back to the layer
            std::swap(m_layers[i], new_layer);

            assert(m_layers[i].size() == Npop_size);
        }

        /// Carry out the steps for one generation
        void do_generation() {
            if (m_evolver == nullptr) {
                throw std::invalid_argument("Evolver has not been selected");
            }
            if (m_layers.size() == 0) {
                initialize_layers();
            }
            else if (m_generation % age_gap == 0 && m_layers.size() > 1) {

                // ========
                // Layer #1
                // ========

                // push into the next-from-bottom layer any individuals that dominate an individual in the higher layer
                // --
                // Join them all together into one population
                Population pop;
                // Move them into the population
                std::move(m_layers[1].begin(), m_layers[1].end(), std::back_inserter(pop));
                std::move(m_layers[0].begin(), m_layers[0].end(), std::back_inserter(pop));
                // Sort them in terms of increasing cost
                sort_layer(pop);
                // Remove the ones that are no longer needed;
                // Keep indices [0,Npop_size-1] inclusive
                for (auto i = pop.size()-1; i >= Npop_size; --i){
                    pop.erase(pop.begin()+i);
                }
                // Keep the best ones
                m_layers[1] = std::move(pop);

                // ========
                // Layer #0
                // ========

                // Generate new individuals in serial (fast)
                MutantVector mutants;
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                for (auto && ind : generator(m_bounds, Npop_size, m_cost_function)) {
                    mutants.emplace_back(std::make_pair(0,std::move(ind)));
                }
                // Evaluate the individuals in parallel
                evaluate_mutants(mutants);

                // Put them back into the 0-th layer
                m_layers[0].clear();
                for (auto &&mut : mutants) {
                    auto i = std::get<0>(mut);
                    m_layers[i].emplace_back(std::move(std::get<1>(mut)));
                }
            }
            graduate_elderly_individuals();
            repopulate_layers();
            evaluate_layers();
            if (parallel){
                evolve_parallel();
            }
            else{
                for (int i = static_cast<int>(m_layers.size()) - 1; i >= 0; i--) {
                    evolve_layer(i);
                }
            }
            evaluate_layers();
            increase_all_ages();
            sort_all_layers();
            m_generation++;
        }
        // Get the best individuals from each layer, starting with layer 0
        std::vector<std::tuple<double, const std::vector<T> > > get_best_per_layer(){
            std::vector<std::tuple<double, const std::vector<T> > > out;
            for (const auto &layer : m_layers) {
                const auto c = static_cast<const NumericalIndividual<T>*>(layer[0].get())->get_coefficients();
                out.emplace_back(std::make_tuple(layer[0]->get_cost(), c));
            }
            return out;
        };
        // Get the layer with the individual with the lowest (best) cost
        std::tuple<double, const std::vector<T> > get_best() {
            auto B = get_best_per_layer();
            // The best individual might not be in the highest layer, let's find the best one
            return *std::min_element(B.begin(), B.end(), [](decltype(B.back()) &b1, decltype(B.back()) &b2) { return std::get<0>(b1) < std::get<0>(b2); });
        };
        /** Return a string with some diagnostic information for the best individual in the population
         * \sa get_best
         * \sa get_best_per_layer
         */
        std::string print_diagnostics() {
            auto best = get_best();
            double best_cost; std::vector<T> c;
            std::tie(best_cost, c) = best;
            std::stringstream ss;
            ss << "i: " << static_cast<int>(m_generation - 1) << " best: " << best_cost << " c: " << vec2string(c) << " queue: " << result_queue.size_approx();
            return ss.str();
        }

        /// Get the results that have been logged during the course of this optimization
        std::vector<Result> get_results() {
            std::vector<Result> results(result_queue.size_approx());
            auto Nels = result_queue.try_dequeue_bulk(results.begin(), results.size());
            return results;
        }
    };
    struct ALPSInputValues {
        std::vector<Bound> bounds; ///< The vector of bounds on the variables
        double VTR; ///< Value to reach (terminates on reaching this cost value)
        CostFunction f; ///< The cost function to be minimized
        bool parallel = false; ///< If true, evaluate each layer in a separate thread
        std::size_t max_gen = 1000; ///< Maximum number of generations that are allowed
        std::size_t NP = 40; ///< The number of individuals in a population (per layer)
        std::size_t Nlayer = 1; ///< The number of layers
        std::size_t age_gap = 5; ///< The number of generations between restarting the bottom layer
        bool disp = false; ///< If true, display diagnostics as you go to standard out
    };
    struct ALPSReturnValues {
        double fval; ///< The function value at termination
        double elapsed_sec; ///< The number of seconds to conduct the entire optimization
        std::string termination_reason; ///< Why the optimization stopped
    };

    template<typename T>
    ALPSReturnValues ALPS(ALPSInputValues &in) {
        if (in.bounds.empty()){ throw std::invalid_argument("bounds variable must be provided"); }
        //if (in.VTR) { throw std::invalid_argument("VTR variable must be provided"); }
        if (!in.f) { throw std::invalid_argument("Cost function f must be provided"); }
        ALPSReturnValues out;
        auto D = in.bounds.size();
        auto layers = Layers<T>(in.f, D, in.NP, in.Nlayer, in.age_gap);
        layers.set_bounds(in.bounds);
        layers.parallel = in.parallel;
        layers.set_builtin_evolver(BuiltinEvolvers::differential_evolution);
        auto startTime = std::chrono::system_clock::now();
        for (auto i = 0; i < in.max_gen; ++i) {
            layers.do_generation();
            if (in.disp){ auto diag = layers.print_diagnostics(); std::cout << diag << std::endl; }
            auto best_layer = layers.get_best();
            if (std::get<0>(best_layer) < in.VTR) {
                out.termination_reason = "Reached VTR";
                break;
            }
            if (i == in.max_gen-1) { out.termination_reason = "Reached max # of generations"; }
        }

        auto endTime = std::chrono::system_clock::now();
        out.elapsed_sec = std::chrono::duration<double>(endTime - startTime).count();
        out.fval = std::get<0>(layers.get_best());
        return out;
    }

} /* namespace CEGO */
#endif


// Copyright (c) 2012-2014 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or https://www.opensource.org/licenses/mit-license.php .

#include "scriptnum10.h"
#include "script/script.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>
#include <limits.h>
#include <stdint.h>

BOOST_FIXTURE_TEST_SUITE(scriptnum_tests, BasicTestingSetup)

static const int64_t values[] = \
{ 0, 1, CHAR_MIN, CHAR_MAX, UCHAR_MAX, SHRT_MIN, USHRT_MAX, INT_MIN, INT_MAX, UINT_MAX, INT64_MIN, INT64_MAX };
static const int64_t offsets[] = { 1, 0x79, 0x80, 0x81, 0xFF, 0x7FFF, 0x8000, 0xFFFF, 0x10000};

static bool addition_in_range(int64_t a, int64_t b) {
    // intentionally excludes cases where the result would be INT64_MIN
    return b > 0 ? (a <= INT64_MAX - b) : (a > INT64_MIN - b);
}

static bool subtraction_in_range(int64_t a, int64_t b) {
    // intentionally excludes cases where the result would be INT64_MIN
    return b >= 0 ? (a > INT64_MIN + b) : (a <= INT64_MAX + b);
}

static bool value_in_range(int64_t a) {
    return a != INT64_MIN;
}

static bool verify(const CScriptNum10& bignum, const CScriptNum& scriptnum)
{
    return bignum.getvch() == scriptnum.getvch() && bignum.getint() == scriptnum.getint();
}

static void CheckCreateVch(int64_t num)
{
    CScriptNum10 bignum(num);
    CScriptNum scriptnum(num);
    BOOST_CHECK(verify(bignum, scriptnum));

    CScriptNum10 bignum2(bignum.getvch(), false);
    std::vector<unsigned char> scriptnum_vch = scriptnum.getvch();
    // The 9-byte case is exercised by the 'intmin' test.
    BOOST_CHECK(scriptnum_vch.size() <= 8);
    CScriptNum scriptnum2(scriptnum_vch, false);
    BOOST_CHECK(verify(bignum2, scriptnum2));

    CScriptNum10 bignum3(scriptnum2.getvch(), false);
    CScriptNum scriptnum3(bignum2.getvch(), false);
    BOOST_CHECK(verify(bignum3, scriptnum3));
}

static void CheckCreateInt(int64_t num)
{
    CScriptNum10 bignum(num);
    CScriptNum scriptnum(num);
    BOOST_CHECK(verify(bignum, scriptnum));
    BOOST_CHECK(verify(CScriptNum10(bignum.getint()), CScriptNum(scriptnum.getint())));
    BOOST_CHECK(verify(CScriptNum10(scriptnum.getint()), CScriptNum(bignum.getint())));
    BOOST_CHECK(verify(CScriptNum10(CScriptNum10(scriptnum.getint()).getint()), CScriptNum(CScriptNum(bignum.getint()).getint())));
}

static void CheckAdd(int64_t num1, int64_t num2)
{
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);
    CScriptNum10 bignum3(num1);
    CScriptNum10 bignum4(num1);
    CScriptNum scriptnum3(num1);
    CScriptNum scriptnum4(num1);

    if (addition_in_range(num1, num2))
    {
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum1 + scriptnum2));
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum1 + num2));
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum2 + num1));
    }
}

static void CheckNegate(int64_t num)
{
    const CScriptNum10 bignum(num);
    const CScriptNum scriptnum(num);

    BOOST_CHECK(verify(-bignum, -scriptnum));
}

static void CheckSubtract(int64_t num1, int64_t num2)
{
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);
    bool invalid = false;

    if (subtraction_in_range(num1, num2))
    {
        BOOST_CHECK(verify(bignum1 - bignum2, scriptnum1 - scriptnum2));
        BOOST_CHECK(verify(bignum1 - bignum2, scriptnum1 - num2));
    }

    if (subtraction_in_range(num2, num1))
    {
        BOOST_CHECK(verify(bignum2 - bignum1, scriptnum2 - scriptnum1));
        BOOST_CHECK(verify(bignum2 - bignum1, scriptnum2 - num1));
    }
}

static void CheckCompare(int64_t num1, int64_t num2)
{
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);

    BOOST_CHECK((bignum1 == bignum1) == (scriptnum1 == scriptnum1));
    BOOST_CHECK((bignum1 != bignum1) == (scriptnum1 != scriptnum1));
    BOOST_CHECK((bignum1 <  bignum1) == (scriptnum1 <  scriptnum1));
    BOOST_CHECK((bignum1 >  bignum1) == (scriptnum1 >  scriptnum1));
    BOOST_CHECK((bignum1 >= bignum1) == (scriptnum1 >= scriptnum1));
    BOOST_CHECK((bignum1 <= bignum1) == (scriptnum1 <= scriptnum1));

    BOOST_CHECK((bignum1 == bignum1) == (scriptnum1 == num1));
    BOOST_CHECK((bignum1 != bignum1) == (scriptnum1 != num1));
    BOOST_CHECK((bignum1 <  bignum1) == (scriptnum1 <  num1));
    BOOST_CHECK((bignum1 >  bignum1) == (scriptnum1 >  num1));
    BOOST_CHECK((bignum1 >= bignum1) == (scriptnum1 >= num1));
    BOOST_CHECK((bignum1 <= bignum1) == (scriptnum1 <= num1));

    BOOST_CHECK((bignum1 == bignum2) == (scriptnum1 == scriptnum2));
    BOOST_CHECK((bignum1 != bignum2) == (scriptnum1 != scriptnum2));
    BOOST_CHECK((bignum1 <  bignum2) == (scriptnum1 <  scriptnum2));
    BOOST_CHECK((bignum1 >  bignum2) == (scriptnum1 >  scriptnum2));
    BOOST_CHECK((bignum1 >= bignum2) == (scriptnum1 >= scriptnum2));
    BOOST_CHECK((bignum1 <= bignum2) == (scriptnum1 <= scriptnum2));

    BOOST_CHECK((bignum1 == bignum2) == (scriptnum1 == num2));
    BOOST_CHECK((bignum1 != bignum2) == (scriptnum1 != num2));
    BOOST_CHECK((bignum1 < bignum2)  == (scriptnum1 < num2));
    BOOST_CHECK((bignum1 > bignum2)  == (scriptnum1 > num2));
    BOOST_CHECK((bignum1 >= bignum2) == (scriptnum1 >= num2));
    BOOST_CHECK((bignum1 <= bignum2) == (scriptnum1 <= num2));
}

static void RunCreate(int64_t num)
{
    CheckCreateInt(num);
    CScriptNum scriptnum(num);
    if (scriptnum.getvch().size() <= CScriptNum::nDefaultMaxNumSize)
    {
        CheckCreateVch(num);
    } else {
        BOOST_CHECK_THROW(CheckCreateVch(num), scriptnum10_error);
    }
}

static void RunOperators(int64_t num1, int64_t num2)
{
    CheckAdd(num1, num2);
    CheckSubtract(num1, num2);
    CheckNegate(num1);
    CheckCompare(num1, num2);
}

BOOST_AUTO_TEST_CASE(creation)
{
    for (size_t i = 0; i < sizeof(values) / sizeof(values[0]); ++i)
    {
        for (size_t j = 0; j < sizeof(offsets) / sizeof(offsets[0]); ++j)
        {
            if (value_in_range(values[i])) {
                RunCreate(values[i]);
            }
            if (addition_in_range(values[i], offsets[j])) {
                RunCreate(values[i] + offsets[j]);
            }
            if (subtraction_in_range(values[i], offsets[j])) {
                RunCreate(values[i] - offsets[j]);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(operators)
{
    for (size_t i = 0; i < sizeof(values) / sizeof(values[0]); ++i)
    {
        for (size_t j = 0; j < sizeof(values) / sizeof(values[0]); ++j)
        {
            if (value_in_range(values[i]))
            {
                RunOperators(values[i], values[i]);
                RunOperators(values[i], -values[i]);
            }
            if (value_in_range(values[i]) && value_in_range(values[j]))
            {
               RunOperators(values[i], values[j]);
               RunOperators(values[i], -values[j]);
            }
            if (addition_in_range(values[i], values[j]) && value_in_range(values[j]))
            {
                RunOperators(values[i] + values[j], values[j]);
                RunOperators(values[i] + values[j], -values[j]);
                RunOperators(values[i] + values[j], values[i] + values[j]);
            }
            if (subtraction_in_range(values[i], values[j]) && value_in_range(values[j]))
            {
                RunOperators(values[i] - values[j], values[j]);
                RunOperators(values[i] - values[j], -values[j]);
                RunOperators(values[i] - values[j], values[i] - values[j]);
            }
            if (addition_in_range(values[i], values[j]) && subtraction_in_range(values[i], values[j]))
            {
                RunOperators(values[i] + values[j], values[i] - values[j]);
                RunOperators(values[i] - values[j], values[i] + values[j]);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(intmin)
{
    // INT64_MIN encodes to the buggy encoding.
    const CScriptNum sn(INT64_MIN);
    std::vector<unsigned char> buggy_int64_min_encoding = {0, 0, 0, 0, 0, 0, 0, 128, 128};
    BOOST_CHECK(sn.getvch() == buggy_int64_min_encoding);

    // The buggy INT64_MIN encoding decodes correctly.
    const CScriptNum sn2(buggy_int64_min_encoding, true, 9);
    BOOST_CHECK(sn2 == INT64_MIN);
    BOOST_CHECK(sn2.getvch() == buggy_int64_min_encoding);
    // getint() saturates at the min/max value of the int type
    BOOST_CHECK((sn2.getint()) == std::numeric_limits<int>::min());

    // Should throw for any other 9+ byte encoding.
    std::vector<unsigned char> invalid_nine_bytes = {0, 0, 0, 0, 0, 0, 0, 0, 0};
    BOOST_CHECK_THROW (CScriptNum sn3(invalid_nine_bytes, false, 9), scriptnum_error);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost test for Conan package
// Dmitriy Vetutnev, ODANT, 2018

#include <boost/random.hpp>

#include <ctime>
#include <iostream>
#include <cstdlib>

int main(int, char**) {

    const std::time_t now = std::time(nullptr);
    boost::random::mt19937 gen{static_cast<std::uint32_t>(now)};
    std::cout << "boost::random::mt19937: " << gen() << std::endl;

    return EXIT_SUCCESS;
}


/* Copyright (C) 2017 IBM Corp.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/* test_GGH15.cpp - test program for the GGH15 MMAPs implementation
 */
#include "../GGH15.h"
#include "../obfus.h"
#include "../utils/argmap.h"

#if defined(__unix__) || defined(__unix) || defined(unix)
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <NTL/BasicThreadPool.h>

static void test1(const Mat< mat_l >& S, TDMatrixParams& params);
static void test2(const Mat< mat_l >& S, TDMatrixParams& params);
static void test3(const Mat< mat_l >& S, TDMatrixParams& params);

void test_GGH15(TDMatrixParams& params, long L)
{
    Mat< mat_l > S(INIT_SIZE, L, 2);
    // These are the "plaintext matrices" to be encoded, the row S[i]
    // will holds the matrices that should be encoded relative to the
    // edge i -> i+1. Say that we have at least two matrices in each S[i].

    // Set some values for the S matrices: Each S[i][j] is a matrix with
    // small entries of dimension n-by-n matrix (where n = params.n).
    for (long i=0; i<L; i++)
    {
        setSmall(S[i][0], params.n, params.n, params.r);
        S[i][1] = S[i][0];
#ifdef DEBUG
        cout << "S["<<i<<"]=" << S[i][0] << endl;
#endif
    }

    test1(S, params);
    test2(S, params);
    clear(S[0][0]);

    clear(S[0][0]);
    test3(S, params);

    cout << "\nmaxSigma="<<Gaussian1Dsampler::maxSigma
         << ", maxSample="<<TDMatrix::maxSample << endl;

    cout << "!"<< endl;
    printAllTimers(cout);

#if (defined(__unix__) || defined(__unix) || defined(unix))
    struct rusage rusage;
    getrusage( RUSAGE_SELF, &rusage );
    cout << "  rusage.ru_maxrss="<<rusage.ru_maxrss << endl;
#endif
}

static void test1(const Mat< mat_l >& S, TDMatrixParams& params)
{
    mat_l zeroMat(INIT_SIZE, params.n, params.n);
    clear(zeroMat);

    long N = S.NumRows();
    Mat< GGH15encoding > C(INIT_SIZE, N, S.NumCols());
    GGH15mmaps maps(params, N+1); // an (N+1)-node instance
    TaggedCRTmatrix toA;
    for (long i=0; i<N; i++) for (long j=0; j<S.NumCols(); j++)
        {
            maps.encode(C[i][j], S[i][j], i, i+1, &toA);
            // encode the matrices in S, the encoding matrices are in C
        }
    GGH15encoding zeroEnc;
    maps.encode(zeroEnc, zeroMat, 1, 2);

    // Also generate an encoding of the matrix Prod_i S[i][0]
    // relative to the edge 0 -> N
    mat_l SS = S[0][0];
    for (long i=1; i<N; i++) SS *= S[i][0];
    GGH15encoding cc;
#ifdef DEBUG
    cout << "\\prod S_i=" << SS << endl;
#endif
    maps.encode(cc, SS, 0, N);

    // Compute some operations on the encoded matrices
    GGH15encoding prod1 = C[0][0];
    GGH15encoding prod2 = C[0][0];
    for (long i=1; i<N; i++)
    {
        // forward multiply: C[0][0] *= C[i][0]
        maps.mulEnc(prod1, C[i][0]);

        if (i==zeroEnc.fromNode())
            maps.mulEnc(prod2, zeroEnc);
        else
            maps.mulEnc(prod2, C[i][0]);

        // backward multiply: C[N-i-1][1] *= C[N-i][1]
        maps.mulEnc(C[N-i-1][1], C[N-i][1]);

        // can mult encoding wrt i->j by one wrt j->k,
        // the result is an encoding wrt i->k
    }

    // Test that the two results encode the same thing, and that they
    // both encode the product of matrices

    maps.subEnc(prod1, cc); // prod1 -= cc
    if (maps.zeroTest(prod1)) std::cout << "PASSED encoding test 1" << endl;
    else                      std::cout << "FAILED encoding test 1" << endl;

    maps.subEnc(cc, C[0][1]);
    if (maps.zeroTest(cc)) std::cout << "PASSED encoding test 2" << endl;
    else                   std::cout << "FAILED encoding test 22" << endl;

    // Check that the product itself is nonzero
    if (maps.zeroTest(prod2)) std::cout << "PASSED encoding test 3" << endl;
    else                      std::cout << "FAILED encoding test 3" << endl;

}

static void test2(const Mat< mat_l >& S, TDMatrixParams& params)
{
    long N = S.NumRows();
    Mat< GGH15encoding > C(INIT_SIZE, N, S.NumCols());
    GGH15mmaps maps(params, 2*N); // an 2N-node instance, two parallel chains

    // encode the same matrices on both chains
    maps.encode(C[0][0], S[0][0], 0, 1);
    maps.encode(C[0][1], S[0][0], 0, 2);
    for (long i=1; i<N-1; i++)
    {
        maps.encode(C[i][0], S[i][0], 2*i-1, 2*i+1);
        maps.encode(C[i][1], S[i][0], 2*i, 2*i +2);
    }
    maps.encode(C[N-1][0], S[N-1][0], 2*N-3, 2*N-1);
    maps.encode(C[N-1][1], S[N-1][0], 2*N-2, 2*N-1);

    GGH15encoding prod1 = C[0][0];
    GGH15encoding prod2 = C[0][1];
    for (long i=1; i<N; i++)
    {
        prod1 *= C[i][0];
        prod2 *= C[i][1];
    }
    prod1 -= prod2;
    if (maps.zeroTest(prod1)) std::cout << "PASSED encoding test 4" << endl;
    else                      std::cout << "FAILED encoding test 4" << endl;

    if (maps.zeroTest(prod2)) std::cout << "FAILED encoding tests 4.5" << endl;
}

static void test3(const Mat< mat_l >& S, TDMatrixParams& params)
{
    params.n += 2;
    params.mBar += 4;
    params.wLen += 2 * params.kFactors * params.e;
    params.m += 4 + 2 * params.kFactors * params.e;

    long N = S.NumRows();
    Mat< GGH15encoding > C(INIT_SIZE, N, S.NumCols());
    GGH15mmaps maps(params, 2*N); // an 2N-node instance, two parallel chains

    GGH15ptxt p1, p2;  // encode randomized matrices on both chains
    for (long i=0; i<N; i++)
    {
        randomizeTransitions(p1, p2, S[i][0], i, 0, N, params.n);
#ifdef DEBUG
        cout << "Encoding for edges "
             << p1.getFrom() <<"->"<< p1.getTo() << ", "
             << p2.getFrom() <<"->"<< p2.getTo() << endl;
        cout << "matrices are:\n" << p1.getData() << endl << p2.getData() << endl;
#endif // DEBUG
        maps.encode(C[i][0], p1.getData(), p1.getFrom(), p1.getTo());
        maps.encode(C[i][1], p2.getData(), p2.getFrom(), p2.getTo());
    }

    GGH15encoding prod1 = C[0][0];
    GGH15encoding prod2 = C[0][1];
    for (long i=1; i<N; i++)
    {
        prod1 *= C[i][0];
        prod2 *= C[i][1];
    }
    prod1 -= prod2;
    if (maps.zeroTest(prod1)) std::cout << "PASSED encoding test 5" << endl;
    else                      std::cout << "FAILED encoding test 5" << endl;
}

/************************** main() program below ********************/
/********************************************************************/

//#define NFACTORS 8
//static long tinyFactors[NFACTORS] = { 2, 3, 5, 7, 11, 13, 17, 19 };

int main(int argc, char *argv[])
{
#ifdef DEBUG
    NTL::SetSeed((unsigned char*)"test_GGH15",10);
#endif
    ArgMapping amap;
    long L=3;
    amap.arg("L", L, "the chain length (in edges)");
    long k=0; //0
    amap.arg("k", k, "number of factors", "0: heuristic");
    long n=5;
    amap.arg("n", n, "the small dimension"); // no default info
    long e=3;
    amap.arg("e", e, "the power of each factor");
    long m=0;
    amap.arg("m", m, "the dimension m", "0: heuristic");
    long threads = 4;
    amap.arg("threads", threads, "how many threads to run");

    amap.parse(argc, argv); // parses and overrides initail values
    // for each parameter, one per line,

    GGH15basicParams bp(COMP_GGH15PRMS, n, L, /*sec=*/80, e);
    if (k>0) bp.k=k;
    if (m > 0) bp.m=m;
    if (m < n*(2+k*e)) m = n*(2+k*e);

    TDMatrixParams params(bp.n, bp.k, e, bp.m);

    cout << "L=" << L << ", n=" << bp.n << ", m=" << bp.m << ", k="
         << bp.k     << ", e=" << e << ", sigmaX=" << params.sigmaX
	 << ", q=" << params.getQ() << endl;

    if (threads<1) threads=1;
    else if (threads > numCores()-2)   // numCores() defined in utils/tools.h
    {
        threads = numCores()-2;
        if (threads<1) threads=1;
    }
    NTL::SetNumThreads(threads);
    cout << "Using "<<threads<< " threads\n";

    test_GGH15(params,L);

#ifdef CodeBlocks
    cin.get();
#endif
}


/**
 * @file QuadraticConstraint.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <cmath>
#include <limits>
#include <string>

#include <Eigen/Dense>

#include <ScsEigen/Logger.h>
#include <ScsEigen/Math.h>
#include <ScsEigen/QuadraticConstraint.h>

using namespace ScsEigen;

QuadraticConstraint::QuadraticConstraint()
{
    this->m_lowerBound = ScsEigen::Vector1d::Constant(
        -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity());
}

QuadraticConstraint::QuadraticConstraint(const Eigen::Ref<const Eigen::MatrixXd>& Q,
                                         const Eigen::Ref<const Eigen::MatrixXd>& b,
                                         double upperBound)
    : Constraint((Q.rows() == Q.cols() && Q.rows() == b.rows()) ? Q.rows() : 0,
                 "Quadratic constraint")
{
    if (Q.rows() != Q.cols() || Q.rows() != b.rows())
    {

        log()->error("[QuadraticConstraint::QuadraticConstraint] Q matrix must be square and the "
                     "size of b "
                     "should be coherent with Q");
        assert(false);
    } else
    {
        m_Q = (Q + Q.transpose()) / 2;
        m_b = b;
    }

    // the only admissible lower bound is
    // -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity()
    this->m_lowerBound = ScsEigen::Vector1d::Constant(
        -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity());

    // set the upperbound
    this->setUpperBound(ScsEigen::Vector1d::Constant(upperBound));
}

bool QuadraticConstraint::setQ(const Eigen::Ref<const Eigen::MatrixXd>& Q)
{
    if (m_Q.size() != 0)
    {
        if (Q.size() != m_Q.size())
        {
            log()->error("[QuadraticConstraint::setQ] The size of the matrix 'Q' cannot change.");
            return false;
        }
    } else if (Q.rows() != Q.cols())
    {
        log()->error("[QuadraticConstraint::QuadraticConstraint] Q matrix must be square.");
        return false;
    } else if (!this->setNumberOfVariables(Q.rows()))
    {
        log()->error("[QuadraticConstraint::setQ] Unable to set the number of variables.");
        return false;
    }

    m_Q = (Q + Q.transpose()) / 2;
    return true;
}

bool QuadraticConstraint::setB(const Eigen::Ref<const Eigen::VectorXd>& b)
{
    if (m_b.size() != 0)
    {
        if (b.size() != m_b.size())
        {
            log()->error("[QuadraticConstraint::setB] The size of the vector 'b' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(b.size()))
    {
        log()->error("[QuadraticConstraint::setB] Unable to set the number of variables.");
        return false;
    }

    m_b = b;
    return true;
}

bool QuadraticConstraint::setLowerBound(const Eigen::Ref<const Eigen::VectorXd>& lowerBound)
{
    if (lowerBound.size() != 1)
    {
        log()->error("[QuadraticConstraint::setLowerBound] The lower bound should be a scalar.");
        return false;
    }

    if (lowerBound[0] > 0 || !std::isinf(lowerBound[0]))
    {
        log()->error("[QuadraticConstraint::setLowerBound] The only admissible lowerBound is "
                     "-std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity()");
        return false;
    }

    // the only admissible lowerBound is
    // -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity() and it has been already set in
    // the constructor
    return true;
}

Eigen::Ref<const Eigen::VectorXd> QuadraticConstraint::getB() const
{
    return m_b;
}

Eigen::Ref<const Eigen::MatrixXd> QuadraticConstraint::getQ() const
{
    return m_Q;
}


/*
 * Copyright (c) 2018-2020 Steven Varga, Toronto,ON Canada
 * Author: Varga, Steven <steven@vargaconsulting.ca>
 */
#include <armadillo>
#include <cstdint>
#include <h5cpp/all>
#include <cstddef>

void test_case(const h5::fd_t fd, const std::string& path ){
	arma::mat M = arma::ones(5,6);
	h5::ds_t ds = h5::write(fd, path, M);
	ds[path.data()] = path ;
}

int main(){
	{
		h5::fd_t fd = h5::create("こんにちは世界.h5", H5F_ACC_TRUNC, h5::default_fcpl,
							h5::libver_bounds({H5F_LIBVER_V18, H5F_LIBVER_V18}) );
		std::string utf8 [] = {
			"hello world", "مرحبا بالعالم", "Բարեւ աշխարհ", "Здравей свят","Прывітанне Сусвет","မင်္ဂလာပါကမ္ဘာလောက","你好，世界",
			"Γειά σου Κόσμε","હેલ્લો વિશ્વ","Helló Világ","こんにちは世界","안녕 세상","سلام دنیا","העלא וועלט"};
		for( auto name : utf8 )
			test_case(fd, name);
	}
	{
		try {
			h5::fd_t fd = h5::open("こんにちは世界.h5", H5F_ACC_RDWR);
			auto m = h5::read<arma::mat>(fd, "مرحبا بالعالم" );
		} catch( const h5::error::any& err ){
			std::cerr << "ERROR:" << err.what();
		}
	}
}


////////////////////////////////////////////////////////////////////
//
// Copyright Vicente J. Botet Escriba 2010
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.
//
////////////////////////////////////////////////////////////////////
#ifndef BOOST_MPL_GCD_HPP_INCLUDED
#define BOOST_MPL_GCD_HPP_INCLUDED

#include <boost/mpl/integral_c.hpp>
#include <boost/ratio/detail/mpl/abs.hpp>
#include <boost/mpl/aux_/largest_int.hpp>
#include <boost/mpl/aux_/na_spec.hpp>
#include <boost/mpl/aux_/lambda_support.hpp>
#include <boost/mpl/aux_/config/eti.hpp>
#include <boost/mpl/aux_/config/integral.hpp>
#include <boost/mpl/aux_/config/static_constant.hpp>
#include <boost/mpl/aux_/config/dependent_nttp.hpp>
#include <boost/cstdint.hpp>

#if    !defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2) \
    && !defined(BOOST_MPL_PREPROCESSING_MODE) \
    && ( defined(BOOST_MSVC) \
        || BOOST_WORKAROUND(__EDG_VERSION__, <= 238) \
        )

#   define BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2

#endif

namespace boost { namespace mpl {

template< typename Tag1, typename Tag2 > struct gcd_impl;

template< typename T > struct gcd_tag
{
    typedef typename T::tag type;
};

template<
      typename BOOST_MPL_AUX_NA_PARAM(N1)
    , typename BOOST_MPL_AUX_NA_PARAM(N2)
    >
struct gcd
    : gcd_impl<
          typename gcd_tag<N1>::type
        , typename gcd_tag<N2>::type
        >::template apply<N1, N2>::type
{
    BOOST_MPL_AUX_LAMBDA_SUPPORT(2, gcd, (N1, N2))
};

BOOST_MPL_AUX_NA_SPEC(2, gcd)

template<
      typename T
    , T n1
    , T n2
    >
struct gcd_c
    : gcd<integral_c<T,n1>,integral_c<T,n2> >
{
};

namespace aux {

  // Workaround for error: the type of partial specialization template parameter constant "n2"
    // depends on another template parameter
    // Note: this solution could be wrong for n1 or n2 = [2**63 .. 2**64-1]
#if defined(BOOST_MPL_CFG_NO_DEPENDENT_NONTYPE_PARAMETER_IN_PARTIAL_SPEC)

  template< typename T1, boost::intmax_t n1, bool n1_is_0
                  , typename T2, boost::intmax_t n2, bool n2_is_0 >
      struct gcd_aux
          : gcd_aux<T2, n2, n2==0, T1, (n1 % n2), (n1 % n2)==0>
      {};

      template <typename T1, boost::intmax_t n1, typename T2, boost::intmax_t n2>
      struct gcd_aux<T1, n1, false, T2, n2, true> : integral_c<T1, n1>
      {};

      template <typename T1, boost::intmax_t n1, typename T2, boost::intmax_t n2, bool C>
      struct gcd_aux<T1, n1, true, T2, n2, C> : integral_c<T2, n2>
      {};

#else // defined(BOOST_MPL_CFG_NO_DEPENDENT_NONTYPE_PARAMETER_IN_PARTIAL_SPEC)

    template< typename T1, T1 n1, bool n1_is_0, typename T2, T2 n2, bool n2_is_0 >
    struct gcd_aux

        : gcd_aux<T2, n2, n2==0,
                    typename aux::largest_int<T1, T2>::type,
                    //~ T1,
                    (n1 % n2), (n1 % n2)==0>
    {};

    template <typename T1, T1 n1, typename T2, T2 n2>
    struct gcd_aux<T1, n1, false, T2, n2, true> : integral_c<T1, n1>
    {};

    template <typename T1, T1 n1, typename T2, T2 n2, bool C>
    struct gcd_aux<T1, n1, true, T2, n2, C> : integral_c<T2, n2>
    {};
#endif // defined(BOOST_MPL_CFG_NO_DEPENDENT_NONTYPE_PARAMETER_IN_PARTIAL_SPEC)
}

template<>
struct gcd_impl<integral_c_tag, integral_c_tag>
{
    template< typename N1, typename N2 > struct apply
        : abs<aux::gcd_aux< typename N1::value_type, N1::value, N1::value==0,
                        typename N2::value_type, N2::value, N2::value==0  > >
    {
    };
};

}}

#endif // BOOST_MPL_GCD_HPP_INCLUDED


#include "trajectory.h"
#include <stdexcept>
#include <Eigen/LU>

Trajectory::Trajectory() {}

void Trajectory::push_back(const Mat44& pose, const Id_t id)
{
  assert_unique_id( id );

  Mat44 T_inv = pose.inverse();
  if(!_data.empty())
    _data.push_back( {back()*T_inv, id} );
  else
    _data.push_back( {T_inv, id} );
}

const Mat44& Trajectory::atId(const Id_t id) const
{
  auto it = find_pose_with_id(id);
  if(it == std::end(_data))
    throw std::runtime_error("could not find pose with id");

  return it->pose;
}

Mat44& Trajectory::atId(const Id_t id)
{
  auto it = find_pose_with_id(id);
  if(it == std::end(_data))
    throw std::runtime_error("could not find pose with id");

  return it->pose;
}

void Trajectory::assert_unique_id(const Id_t id) const
{
  if( std::end(_data) != find_pose_with_id(id) )
    throw std::runtime_error("duplicate id in trajectory\n");
}

EigenAlignedContainer_<Mat44> Trajectory::poses() const
{
  EigenAlignedContainer_<Mat44> ret(_data.size());
  for(size_t i = 0; i < ret.size(); ++i)
    ret[i] = _data[i].pose;

  return ret;
}

EigenAlignedContainer_<Vec3> Trajectory::cameraPositions() const
{
  EigenAlignedContainer_<Vec3> ret(_data.size());
  for(size_t i = 0; i < ret.size(); ++i)
    ret[i] = _data[i].pose.block<3,1>(0,3);

  return ret;
}