#include "cluster/cluster.hpp"
#undef CHECK
#include "util/dbc.hpp"

#include <stdexcept>
#include <sstream>
#include <numeric>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/split.hpp>

#include <folly/json.h>
namespace bernardo
{
    namespace cluster
    {

        namespace
        {
            const int KD_TREES = 4;
            const int TREE_SEARCH_PARAMS = 128;
            const int NN = 1;
            const int QUERY_SIZE = 1;

        }

        feature_vec::value_type map_number(const folly::dynamic& v)
        {
            return v.asDouble();
        }

        feature_vec::value_type map_enumeration(const folly::dynamic& v, const trait::definition& t)
        {
            auto val = std::find(std::begin(t.values), std::end(t.values), v.getString());
            return val != std::end(t.values) ? std::distance(std::begin(t.values), val) + 1.0 : 0.0;
        }

        feature_vec::value_type map_value(const folly::dynamic& v, const trait::definition& t)
        {
            feature_vec::value_type r = 0.0;
            switch(t.type)
            {
                case trait::kind::number: r = map_number(v); break;
                case trait::kind::enumeration: r = map_enumeration(v, t); break;
                default: CHECK(false && "forgot case");
            }
            return r;
        }

        feature_vec::value_type compile_trait(const trait::definition& t, const folly::dynamic& traits)
        {
            auto trait = traits.find(t.name);
            if(trait == traits.items().end())
            {
                std::stringstream s;
                s << "query is missing trait \"" << t.name << "\"";
                throw std::invalid_argument{s.str()};
            }
            return map_value(trait->second, t);
        }

        const group* group_for_query(const all_groups& all, const query& q)
        {
            auto groups = all.groups.find(q.scope);
            if(groups == all.groups.end()) return nullptr;

            auto group = groups->second.find(q.group_name);
            if(group == groups->second.end()) return nullptr;
            return &(group->second);
        }

        feature_vec compile_traits(const folly::dynamic& traits, const definition& d)
        {
            REQUIRE_FALSE(d.traits.empty());

            feature_vec r;
            r.reserve(d.traits.size());

            std::transform(std::begin(d.traits), std::end(d.traits),
                    std::back_inserter(r),
                    [&traits](const auto& trait_def) { return compile_trait(trait_def, traits); });

            ENSURE_EQUAL(r.size(), d.traits.size());
            return r;
        }

        feature_vec compile_query(const query& q, const group& g)
        {
            const auto& d = g.def;
            REQUIRE_FALSE(d.traits.empty());
            return compile_traits(q.traits, g.def);
        }

        double euclidean_dist(const feature_vec& a, const feature_vec& b)
        {
            REQUIRE_EQUAL(a.size(), b.size());
            return std::inner_product(std::begin(a), std::end(a), std::begin(b), 0.0,
                    std::plus<double>(),
                    [](auto a, auto b)
                    {
                    auto d = a - b;
                    return d*d;
                    });
        }

        double hamming_dist(const feature_vec& a, const feature_vec& b)
        {
            REQUIRE_EQUAL(a.size(), b.size());

            return std::inner_product(std::begin(a), std::end(a), std::begin(b), 0.0,
                    std::plus<double>(),
                    [](auto a, auto b)
                    {
                    return a == b ? 0.0 : 1.0;
                    });
        }

        double cluster_dist(const feature_vec& a, const feature_vec& b, distance_function dist_type)
        {
            double d = std::numeric_limits<double>::max();
            switch(dist_type)
            {
                case distance_function::euclidean: d = euclidean_dist(a, b); break;
                case distance_function::hamming: d = hamming_dist(a, b); break;
                default: CHECK(false && "missed case");
            }

            REQUIRE_GREATER_EQUAL(d, 0.0);
            return d;
        }

        group::group() : def{}, clusters{}, _features{},  _index{} {}
        group::group(const definition& d) : def{d}, clusters{}, _features{},  _index{} {}
        group& group::operator=(const group& o)
        {
            if(&o == this) return *this;
            def = o.def;
            clusters = o.clusters;
            _features = o._features;
            if(o._index) _index = std::make_unique<index>(*o._index);
        }

        void group::add_cluster(const int id, const std::string& reference, const folly::dynamic& traits)
        {
            clusters.emplace_back( cluster { id, reference, traits, compile_traits(traits, def)});
        }

        void group::build_index()
        {
            REQUIRE_FALSE(clusters.empty());

            const auto& first = clusters.front();
            auto feature_count = first.features.size();

            REQUIRE_GREATER(feature_count, 0);

            //construct feature matrix
            auto data_size = clusters.size() * feature_count;
            _features = feature_mat{new double[data_size], clusters.size(), feature_count};

            for(int c = 0; c < clusters.size(); c++)
            {
                const auto& cl = clusters[c];
                CHECK_EQUAL(cl.features.size(), feature_count);
                std::copy(std::begin(cl.features), std::end(cl.features), _features[c]);
            }

            //index features
            _index = std::make_unique<index>(_features, flann::KDTreeIndexParams(KD_TREES));
            _index->buildIndex();

            ENSURE(_index);
        }

        find_result group::find_cluster(const feature_vec& features) const
        {
            REQUIRE_GREATER(clusters.size(), 0);

            if(_index)
            {
                REQUIRE_EQUAL(clusters.size(), _features.rows);
                INVARIANT_EQUAL(QUERY_SIZE, 1);

                //hack here, just use feature vector memory since we have a query size of 1
                feature_mat q{const_cast<double*>(features.data()), QUERY_SIZE, features.size()};

                CHECK_EQUAL(QUERY_SIZE * NN, 1); //make sure we have only closest result for single query.
                //no need to use heap
                int index;
                double dist;
                indices_mat indices{&index, QUERY_SIZE, NN};
                dist_mat dists{&dist, QUERY_SIZE, NN};

                _index->knnSearch(q, indices, dists, NN, flann::SearchParams(TREE_SEARCH_PARAMS));

                CHECK_RANGE(index, 0, clusters.size());

                auto best_cluster = clusters.begin();
                std::advance(best_cluster, index);
                return {best_cluster, dist};
            }
            else
            {
                double smallest_dist = cluster_dist(features, clusters.front().features, def.distance_func);
                auto best_cluster =  std::min_element(std::begin(clusters), std::end(clusters),
                        [&](const auto& c, const auto&) -> bool
                        {
                        auto old_smallest = smallest_dist;
                        auto dist = cluster_dist(features, c.features, def.distance_func);
                        if(dist <= smallest_dist) smallest_dist = dist;
                        return dist < old_smallest;
                        });

                return { best_cluster, smallest_dist};
            }
        }

        distance_function to_distance_func(const std::string& name)
        {
            distance_function f = distance_function::euclidean;
            if(name == "euclidean") f = distance_function::euclidean;
            else if(name == "hamming") f = distance_function::hamming;
            else
            {
                std::stringstream e;
                e << "the distance function " << name << " is not valid" << std::endl;
                throw std::invalid_argument{e.str()};
            }

            return f;
        }

        trait::kind to_trait_kind(const std::string& name)
        {
            trait::kind f = trait::kind::number;
            if(name == "number") f = trait::kind::number;
            else if(name == "enumeration") f = trait::kind::enumeration;
            else
            {
                std::stringstream e;
                e << "the trait kind " << name << " is not valid" << std::endl;
                throw std::invalid_argument{e.str()};
            }

            return f;
        }
        trait::enum_values to_enum_values(const std::string& encoded)
        {
            if(encoded.size() <= 2) return {};

            auto just_values = encoded.substr(1, encoded.size() - 2);
            trait::enum_values r;

            using split_iter = boost::algorithm::split_iterator<std::string::const_iterator>;
            split_iter it(just_values.begin(), just_values.end(),
                    boost::algorithm::first_finder(",", boost::algorithm::is_equal()));
            split_iter end;

            for(;it != end; it++) r.emplace_back(std::string{it->begin(), it->end()});

            ENSURE_GREATER_EQUAL(r.size(), 1);
            return r;
        }

        definition load_definition(pqxx::read_transaction& w, size_t cluster_definition_id)
        {
            std::stringstream def_query;
            def_query << "select distance_func from cluster_definitions where id=" << cluster_definition_id << " limit 1";
            auto cluster_def = w.exec(def_query.str());
            if(cluster_def.size() == 0)
            {
                std::stringstream e;
                e << "unable to find a cluster definition with id " << cluster_definition_id;
                throw std::invalid_argument{e.str()};
            }

            std::string distance_function_name;
            cluster_def[0][0].to(distance_function_name);

            std::cout << "\tdistance function: " << distance_function_name << std::endl;
            definition def;
            def.distance_func = to_distance_func(distance_function_name);

            std::stringstream trait_definition_query;
            trait_definition_query << "select name, kind, enum_values from trait_definitions where cluster_definition_id = " << cluster_definition_id;
            auto trait_definitions = w.exec(trait_definition_query.str());
            for(auto t : trait_definitions)
            {
                CHECK_EQUAL(t.size(), 3);
                std::string name;
                std::string kind;
                std::string enum_values;

                t[0].to(name);
                t[1].to(kind);
                t[2].to(enum_values);

                std::cout << "\t\ttrait: " << name << " kind: " << kind << " vals: " << enum_values << std::endl;

                trait::definition d;
                d.name = name;
                d.type = to_trait_kind(kind);
                d.values = to_enum_values(enum_values);

                def.traits.emplace_back(d);
            }

            if(def.traits.empty())
            {
                std::cerr << "WARNING: Cluster definition " << cluster_definition_id << " does not have any traits";
            }

            return def;
        }

        void load_clusters(pqxx::read_transaction& w, size_t group_id, group& g)
        {
            std::stringstream cluster_query;
            cluster_query << "select id, ref, traits from clusters where group_id = " << group_id;
            auto clusters = w.exec(cluster_query.str());
            for(auto cluster: clusters)
            {
                CHECK_EQUAL(cluster.size(), 3);
                int id;
                std::string ref;
                std::string encoded_traits;
                cluster[0].to(id);
                cluster[1].to(ref);
                cluster[2].to(encoded_traits);

                std::cout << "\t\tcluster: " << ref << " traits: " << encoded_traits << std::endl;
                g.add_cluster(id, ref, folly::parseJson(encoded_traits));
            }
            g.build_index();
        }

        void load_groups_from_db(pqxx::connection& c, all_groups& all)
        {
            pqxx::read_transaction w{c};
            auto groups = w.exec("select id, scope, name, cluster_definition_id from groups");
            for(auto row: groups)
            {
                CHECK_EQUAL(row.size(), 4);

                size_t group_id;
                std::string scope;
                std::string name;
                size_t cluster_definition_id;
                row[0].to(group_id);
                row[1].to(scope);
                row[2].to(name);
                row[3].to(cluster_definition_id);

                std::cout << "group: " << group_id << " scope: " << scope << " name: " << name << " def_id: " << cluster_definition_id << std::endl;

                group g;
                g.def = load_definition(w, cluster_definition_id);
                load_clusters(w, group_id, g);

                all.groups[scope][name] = g;
            }

            if(all.groups.empty())
            {
                std::cerr << "WARNING: NO GROUPS IN DB";
            }
        }
    }
}


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

  Copyright 2016 NumScale SAS

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

namespace ba = boost::alignment;
namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test_periodic(Env& runtime)
{
  using p_t = bs::pack<T,N>;

  T* data = static_cast<T*>(ba::aligned_alloc(p_t::alignment, (sizeof(T)) * N * 3));
  std::array<T,N> ref0, refN, refmN;

  for(std::size_t i = 0;i < 3*N        ; ++i) data[i] = T(i+1);
  for(std::size_t i = 0;i < ref0.size() ; ++i)
  {
    ref0[i]  = data[N+i];
    refN[i]  = data[2*N+i];
    refmN[i] = data[i];
  }

  static const std::ptrdiff_t O = N;

  p_t p0  = bs::aligned_load<bs::pack<T,N>, 0>(&data[N]  );
  p_t pN  = bs::aligned_load<bs::pack<T,N>, O>(&data[N]+O);
  p_t pmN = bs::aligned_load<bs::pack<T,N>,-O>(&data[N]-O);

  STF_ALL_EQUAL( p0 , ref0  );
  STF_ALL_EQUAL( pN , refN  );
  STF_ALL_EQUAL( pmN, refmN );
}

STF_CASE_TPL( "Check aligned_load behavior with periodic misalignment", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test_periodic<T, N>(runtime);
  test_periodic<T, N/2>(runtime);
  test_periodic<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void test_aperiodic(Env& runtime)
{
  using p_t = bs::pack<T,N>;

  T* data = static_cast<T*>(ba::aligned_alloc(p_t::alignment, (sizeof(T)) * N * 3));
  std::array<T,N> ref1, refm2, ref3;

  for(std::size_t i = 0;i < 3*N         ; ++i) data[i] = T(i+1);
  for(std::size_t i = 0;i < ref1.size() ; ++i)
  {
    ref1[i]  = data[N+i+1];
    refm2[i]  = data[N+i-2];
    ref3[i] = data[N+i+3];
  }

  p_t p1  = bs::aligned_load<bs::pack<T,N>,+1>(&data[N]+1);
  p_t pm2 = bs::aligned_load<bs::pack<T,N>,-2>(&data[N]-2);
  p_t p3  = bs::aligned_load<bs::pack<T,N>,+3>(&data[N]+3);

  STF_ALL_EQUAL( p1 , ref1  );
  STF_ALL_EQUAL( pm2, refm2 );
  STF_ALL_EQUAL( p3 , ref3  );
}

STF_CASE_TPL( "Check aligned_load behavior with aperiodic misalignment", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test_aperiodic<T, N>(runtime);
  test_aperiodic<T, N/2>(runtime);
  test_aperiodic<T, N*2>(runtime);
}


#include<iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include<sstream>
#include<iomanip>
#include<string>
#include<fstream>
using namespace std;
using namespace boost::multiprecision;

int main()
{
ifstream infile("store_data.txt");
ofstream outfile("store_data.hex");
int128_t a;
while(infile >> a){
stringstream s;
s << hex << a;
string b(s.str());
outfile << b << endl;
}

return 0;
}


#ifndef HEAT_PARAMETERS_H
#define HEAT_PARAMETERS_H

#pragma once
#include <boost/config.hpp>
#include <boost/program_options.hpp>
#include <exception>
#include <iostream>

#define DEBUG heat_parameters::get_instance().is_debug()

class heat_parameters
{
    double _delta_t;                   // timestep
    double _delta_x;                   // spacestep in x axis
    double _delta_y;                   // spacestep in y axis
    double _delta_z;                   // spacestep in z axis
    double _alpha;                     // thermal diffusivity

    size_t _num_iter;                  // number of iterations per frame
    bool _debug;                       // debug mode for extra input

    int _width;                        // window's width in pixels
    int _height;                       // window's height in pixels

    double _camera_yaw;                // moving "side"
    double _camera_pitch;              // moving "up"
    double _camera_speed;              // speed that cemera moves is direction
    double _camera_mouse_sensitivity;  // how sensitive zooming is
    double _camera_fov;                // equivaletly to zoom

    std::string _capture_name;         // name of output video file
    size_t _fps;                       // fps of capturing
    std::string _source_name;          // name of the sourse of initial state

    boost::program_options::options_description desc;
    heat_parameters();
public:
    static heat_parameters& get_instance();

    void parse_cl(const int argc, const char **argv);

    // getters
    inline double get_dt() const {return _delta_t;}
    inline double get_dx() const {return _delta_x;}
    inline double get_dy() const {return _delta_y;}
    inline double get_dz() const {return _delta_z;}
    inline double get_alpha() const {return _alpha;}
    inline size_t get_num_iter() const {return _num_iter;}
    inline bool is_debug() const {return _debug;}
    inline int get_window_width() const {return _width;}
    inline int get_window_height() const {return _height;}
    inline double get_camera_yaw() const {return _camera_yaw;}
    inline double get_camera_pitch() const {return _camera_pitch;}
    inline double get_camera_speed() const {return _camera_speed;}
    inline double get_camera_fov() const {return _camera_fov;}
    inline double get_camera_mouse_sensitivity() const {return _camera_mouse_sensitivity;}
    inline std::string get_capture_name() const {return _capture_name;}
    inline size_t get_fps() const {return _fps;}
    inline std::string get_source_name() const {return _source_name;}
    // static getters
    static inline double dt() {return heat_parameters::get_instance().get_dt();}
    static inline double dx() {return heat_parameters::get_instance().get_dx();}
    static inline double dy() {return heat_parameters::get_instance().get_dy();}
    static inline double dz() {return heat_parameters::get_instance().get_dz();}
    static inline double alpha() {return heat_parameters::get_instance().get_alpha();}
    static inline size_t num_iter() {return heat_parameters::get_instance().get_num_iter();}
    static inline bool debug() {return heat_parameters::get_instance().is_debug();}
    static inline int window_width() {return heat_parameters::get_instance().get_window_width();}
    static inline int window_height() {return heat_parameters::get_instance().get_window_height();}
    static inline double camera_yaw() {return heat_parameters::get_instance().get_camera_yaw();}
    static inline double camera_pitch() {return heat_parameters::get_instance().get_camera_pitch();}
    static inline double camera_speed() {return heat_parameters::get_instance().get_camera_speed();}
    static inline double camera_fov() {return heat_parameters::get_instance().get_camera_fov();}
    static inline double camera_mouse_sensitivity() {return heat_parameters::get_instance().get_camera_mouse_sensitivity();}
    static inline std::string capture_name() {return heat_parameters::get_instance().get_capture_name();}
    static inline size_t fps() {return heat_parameters::get_instance().get_fps();}
    static inline std::string source_name() {return heat_parameters::get_instance().get_source_name();}

    ~heat_parameters(){};

    heat_parameters(const heat_parameters&) = delete;
    void operator=(const heat_parameters&) = delete;
};

#endif //HEAT_PARAMETERS_H


#include "SPlisHSPlasH/Common.h"
#include <Eigen/Dense>
#include "extern/cxxopts/cxxopts.hpp"
#include "Utilities/Timing.h"
#include "Utilities/OBJLoader.h"
#include "WindingNumbers.h"
#include "Utilities/PartioReaderWriter.h"
#include "Utilities/Version.h"

using namespace SPH;
using namespace Eigen;
using namespace std;
using namespace Utilities;

INIT_TIMING
INIT_LOGGING

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
#define new DEBUG_NEW
#endif
#endif

void sampleObject(TriangleMesh &mesh);
void loadObj(const std::string &filename, TriangleMesh &mesh, const Vector3r &scale);
void partioExport();

struct Region
{
public:
	typedef Real value_type;

	Region() {}
	Region(Real minx, Real miny, Real minz,
		Real maxx, Real maxy, Real maxz)
	{
		m_min = Vector3r(minx, miny, minz);
		m_max = Vector3r(maxx, maxy, maxz);
	}

	Vector3r m_min;
	Vector3r m_max;
};

string exePath;
string inputFile = "";
string outputFile = "";
Real radius = 0.05f;
Real scale = 1.0;
Real diameter = radius*2.0;
int mode = 0;
Region region;
bool useRegion = false;
std::vector<Vector3r> particles;

std::istream& operator >> (std::istream& istream, Region& r)
{
	return istream >> std::skipws >> r.m_min[0] >> r.m_min[1] >> r.m_min[2] >> r.m_max[0] >> r.m_max[1] >> r.m_max[2];
}

std::ostream& operator << (std::ostream& out, const Region& r)
{
	out << r.m_min[0] << ", " << r.m_min[1] << ", " << r.m_min[2] << ", " << r.m_max[0] << ", " << r.m_max[1] << ", " << r.m_max[2];
	return out;
}

// main
int main(int argc, char **argv)
{
	REPORT_MEMORY_LEAKS;

	std::cout << "Git refspec: " << GIT_REFSPEC << std::endl;
	std::cout << "Git SHA1: " << GIT_SHA1 << std::endl;
	std::cout << "Git status: " << GIT_LOCAL_STATUS << std::endl;

	Timing::m_dontPrintTimes = true;

	exePath = string(argv[0]);
	const size_t npos = exePath.rfind("\\");
	if (npos != string::npos)
		exePath = exePath.substr(0, exePath.rfind("\\") + 1);
	else
		exePath = ".";

	try
	{
		cxxopts::Options options(argv[0], "VolumeSampling - Sample a volumetric geometry given by an OBJ file.");

		options.add_options()
			("h,help", "Print help")
			("i,input", "Input file (obj)", cxxopts::value<std::string>())
			("o,output", "Output file (bgeo)", cxxopts::value<std::string>())
			("r,radius", "Particle radius", cxxopts::value<Real>()->default_value("0.025"))
			("s,scale", "Scaling of input geometry", cxxopts::value<Real>()->default_value("1.0"))
			("m,mode", "Mode (regular=0, almost dense=1, dense=2)", cxxopts::value<int>()->default_value("0"))
			("region", "Region to fill with particles (e.g. --region \"0 0 0 1 1 1\"", cxxopts::value<Region>())
			;

		options.parse(argc, argv);

		if (options.count("help"))
		{
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(0);
		}

		if (options.count("input") && options.count("output"))
		{
			inputFile = options["input"].as<std::string>();
			std::cout << "Input = " << inputFile << std::endl;
			outputFile = options["output"].as<std::string>();
			std::cout << "Output = " << outputFile << std::endl;
		}
		else
		{
			std::cout << "Input or output missing!" << std::endl;
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(1);
		}

		if (options.count("radius"))
			radius = options["radius"].as<Real>();
		cout << "Radius: " << radius << endl;

		if (options.count("scale"))
			scale = options["scale"].as<Real>();
		cout << "Scale: " << scale << endl;

		if (options.count("mode"))
			mode = options["mode"].as<int>();
		cout << "Mode: " << mode << endl;

		if (options.count("region"))
		{
			region = options["region"].as<Region>();
			useRegion = true;
			cout << "Region: " << region << endl;
		}
	}
	catch (const cxxopts::OptionException& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}

	diameter = 2.0 * radius;
	TriangleMesh mesh;
	loadObj(inputFile, mesh, scale*Vector3r::Ones());
 	sampleObject(mesh);
	PartioReaderWriter::writeParticles(outputFile, (unsigned int)particles.size(), particles.data(), NULL, radius);

	std::cout << "Generated particles: " << particles.size() << std::endl;

	Timing::printAverageTimes();

	return 0;
}

void loadObj(const std::string &filename, TriangleMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, nullptr, s);

	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	mesh.initMesh(nPoints, nFaces);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		mesh.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
		}

		mesh.addFace(&posIndices[0]);
	}

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}

 void sampleObject(TriangleMesh &mesh)
 {
	 Vector3r *v = mesh.getVertices().data();

 	// compute bounding box
 	Vector3r min, max;
 	min = v[0];
 	max = min;
 	for (unsigned int i = 1; i < mesh.numVertices(); ++i)
 	{
 		const Vector3r& p = v[i];
 		for (unsigned int j = 0; j < 3; ++j) {
 			min[j] = std::min(min[j], p[j]);
 			max[j] = std::max(max[j], p[j]);
 		}
 	}

	if (useRegion)
 	{
		for (unsigned int i=0; i < 3; i++)
		{
			min[i] = std::max(scale * region.m_min[i], min[i]);
			max[i] = std::min(scale * region.m_max[i], max[i]);
		}
 	}

 	// sample object
 	const unsigned int numberOfSamplePoints = (((unsigned int)((1.0f / diameter) * (max[2] - min[2]))) + 1) *
 		(((unsigned int)((1.0f / diameter) * (max[1] - min[1]))) + 1) *
 		(((unsigned int)((1.0f / diameter) * (max[0] - min[0]))) + 1);
 	unsigned int currentSample = 0;
 	Real currentPercent = 0.01;
 	int counter_x = 0;
 	int counter_y = 0;
 	Real xshift = diameter;
 	Real yshift = diameter;

 	if (mode == 1)
 		yshift = sqrt(3.0) * radius;
 	else if (mode == 2)
 	{
 		xshift = sqrt(3.0) * radius;
 		yshift = sqrt(6.0) * diameter / 3.0;
 	}
 	for (Real z = min[2]; z <= max[2]; z += diameter)
 	{
 		for (Real y = min[1]; y <= max[1]; y += yshift)
 		{
 			for (Real x = min[0]; x <= max[0]; x += xshift)
 			{
 				Vector3r particlePosition;
 				if (mode == 1)
 				{
 					if (counter_y % 2 == 0)
 						particlePosition = Vector3r(x, y + radius, z + radius);
 					else
 						particlePosition = Vector3r(x + radius, y + radius, z);
 				}
 				else if (mode == 2)
 				{
 					particlePosition = Vector3r(x, y + radius, z + radius);

 					Vector3r shift_vec(0, 0, 0);
 					if (counter_x % 2)
 					{
 						shift_vec[2] += diameter / (2.0 * (counter_y % 2 ? -1 : 1));
 					}
 					if (counter_y % 2)
 					{
 						shift_vec[0] += xshift / 2.0;
 						shift_vec[2] += diameter / 2.0;
 					}
 					particlePosition += shift_vec;
 				}
 				else
 				{
 					// Use center of voxel
 					particlePosition = Vector3r(x + radius, y + radius, z + radius);
 				}

 				const Real w_p = WindingNumbers::computeGeneralizedWindingNumber(particlePosition, mesh);
 				if (w_p > 0.5f)
 					particles.push_back(particlePosition);
 				currentSample++;

 				if ((Real)currentSample / (Real)numberOfSamplePoints > currentPercent)
 				{
 					std::cout << currentPercent * 100.0 << "%\n";
 					currentPercent += 0.01;
 				}
 				counter_x++;
 			}
 			counter_x = 0;
 			counter_y++;
 		}
 		counter_y = 0;
 	}
 	std::cout << "100%\n";
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file monotonicity_test.cpp
/// \brief Tests for the is_monotonous function for state formulas.

#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/modal_formula/parse.h"

using namespace mcrl2;
using namespace mcrl2::lps;
using namespace mcrl2::state_formulas;

void run_monotonicity_test_case(const std::string& formula, const std::string& lpstext, const bool expect_success = true)
{
  specification lpsspec = remove_stochastic_operators(linearise(lpstext));
  parse_state_formula_options options;
  options.check_monotonicity = false;
  options.resolve_name_clashes = false;
  state_formula f = parse_state_formula(formula, lpsspec, options);
  if (state_formulas::has_state_variable_name_clashes(f))
  {
    std::cerr << "Error: " << state_formulas::pp(f) << " has name clashes" << std::endl;
    f = state_formulas::resolve_state_variable_name_clashes(f);
    std::cerr << "resolved to " << state_formulas::pp(f) << std::endl;
  }
  BOOST_CHECK(is_monotonous(f) == expect_success);
}

BOOST_AUTO_TEST_CASE(test_abp)
{
  std::string lpstext = lps::detail::ABP_SPECIFICATION();

  run_monotonicity_test_case("true", lpstext, true);
  run_monotonicity_test_case("[true*]<true*>true", lpstext, true);
  run_monotonicity_test_case("mu X. !!X", lpstext, true);
  run_monotonicity_test_case("nu X. ([true]X && <true>true)", lpstext, true);
  run_monotonicity_test_case("nu X. ([true]X && forall d:D. [r1(d)] mu Y. (<true>Y || <s4(d)>true))", lpstext, true);
  run_monotonicity_test_case("forall d:D. nu X. (([!r1(d)]X && [s4(d)]false))", lpstext, true);
  run_monotonicity_test_case("nu X. ([true]X && forall d:D. [r1(d)]nu Y. ([!r1(d) && !s4(d)]Y && [r1(d)]false))", lpstext, true);
  run_monotonicity_test_case("mu X. !X", lpstext, false);
  run_monotonicity_test_case("mu X. nu Y. (X => Y)", lpstext, false);
  run_monotonicity_test_case("mu X. X || mu X. X", lpstext, true);
  run_monotonicity_test_case("mu X. (X || mu X. X)", lpstext, true);
  run_monotonicity_test_case("mu X. (X || mu Y. Y)", lpstext, true);
  run_monotonicity_test_case("!(mu X. X || mu X. X)", lpstext, true);
  run_monotonicity_test_case("!(mu X. (X || mu X. X))", lpstext, true);
  run_monotonicity_test_case("!(mu X. (X || mu Y. Y))", lpstext, true);
}

// Test case provided by Jeroen Keiren, 10-9-2010
BOOST_AUTO_TEST_CASE(test_elevator)
{
  std::string lpstext =

    "% Model of an elevator for n floors.                                                                                \n"
    "% Originally described in 'Solving Parity Games in Practice' by Oliver                                                         \n"
    "% Friedmann and Martin Lange.                                                                                \n"
    "%                                                                                \n"
    "% This is the version with a first in first out policy                                                                         \n"
    "                                                                                \n"
    "sort Floor = Pos;                                                                                \n"
    "     DoorStatus = struct open | closed;                                                                                \n"
    "     Requests = List(Floor);                                                                                \n"
    "                                                                                \n"
    "map maxFloor: Floor;                                                                                \n"
    "eqn maxFloor = 3;                                                                                \n"
    "                                                                                \n"
    "map addRequest : Requests # Floor -> Requests;                                                                                \n"
    "                                                                                \n"
    "var r: Requests;                                                                                \n"
    "    f,g: Floor;                                                                                \n"
    "    % FIFO behaviour!                                                                                \n"
    "eqn addRequest([], f) = [f];                                                                                \n"
    "    (f == g) -> addRequest(g |> r, f) = g |> r;                                                                                \n"
    "    (f != g) -> addRequest(g |> r, f) = g |> addRequest(r, f);                                                                 \n"
    "                                                                                \n"
    "map removeRequest : Requests -> Requests;                                                                                \n"
    "var r: Requests;                                                                                \n"
    "    f: Floor;                                                                                \n"
    "eqn removeRequest(f |> r) = r;                                                                                \n"
    "                                                                                \n"
    "map getNext : Requests -> Floor;                                                                                \n"
    "var r: Requests;                                                                                \n"
    "    f: Floor;                                                                                \n"
    "eqn getNext(f |> r) = f;                                                                                \n"
    "                                                                                \n"
    "act isAt: Floor;                                                                                \n"
    "    request: Floor;                                                                                \n"
    "    close, open, up, down;                                                                                \n"
    "                                                                                \n"
    "proc Elevator(at: Floor, status: DoorStatus, reqs: Requests, moving: Bool) =                                                   \n"
    "       isAt(at) . Elevator()                                                                                \n"
    "     + sum f: Floor. (f <= maxFloor) -> request(f) . Elevator(reqs = addRequest(reqs, f))                                      \n"
    "     + (status == open) -> close . Elevator(status = closed)                                                                   \n"
    "     + (status == closed && reqs != [] && getNext(reqs) > at) -> up . Elevator(at = at + 1, moving = true)                     \n"
    "     + (status == closed && reqs != [] && getNext(reqs) < at) -> down . Elevator(at = Int2Pos(at - 1), moving = true)          \n"
    "     + (status == closed && getNext(reqs) == at) -> open. Elevator(status = open, reqs = removeRequest(reqs), moving = false); \n"
    "                                                                                \n"
    "init Elevator(1, open, [], false);                                                                                \n"
    ;

  run_monotonicity_test_case("nu U. [true] U && ((mu V . nu W. !([!request(maxFloor)]!W && [request(maxFloor)]!V)) || (nu X . mu Y. [!isAt(maxFloor)] Y &&  [isAt(maxFloor)]X))", lpstext, true);
  run_monotonicity_test_case("nu U. [true] U && ((nu V . mu W. ([!request(maxFloor)]W && [request(maxFloor)]V)) => (nu X . mu Y. [!isAt(maxFloor)] Y &&  [isAt(maxFloor)]X))", lpstext, true);
  run_monotonicity_test_case("nu U. [true] U && (!(nu V . mu W. ([!request(maxFloor)]W && [request(maxFloor)]V)) || (nu X . mu Y. [!isAt(maxFloor)] Y &&  [isAt(maxFloor)]X))", lpstext, true);
  run_monotonicity_test_case("(nu X . mu Y. X) => true", lpstext, true);
  run_monotonicity_test_case("!(nu X . mu Y. X)", lpstext, true);
  run_monotonicity_test_case("mu X . X", lpstext, true);
  run_monotonicity_test_case("nu X . X", lpstext, true);
  run_monotonicity_test_case("mu X . !X", lpstext, false);
  run_monotonicity_test_case("nu X . !X", lpstext, false);
  run_monotonicity_test_case("!(mu X . X)", lpstext, true);
  run_monotonicity_test_case("!(nu X . X)", lpstext, true);
  run_monotonicity_test_case("(mu X . X) => true", lpstext, true);
  run_monotonicity_test_case("(nu X . X) => true", lpstext, true);
  run_monotonicity_test_case("!(mu X. (mu X. X))", lpstext, true);

  // trac ticket #1320
  run_monotonicity_test_case("!mu X. [true]X && mu X. [true]X", lpstext, true);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return nullptr;
}


#ifndef EDP_CONSTRUCTSPARSEMAT_HPP
#define EDP_CONSTRUCTSPARSEMAT_HPP
#include <Eigen/Sparse>
#include <tbb/tbb.h>
namespace edp
{
	template< class ColFunc>
	auto constructSparseMat(uint64_t dim, ColFunc&& colFunc)
	{
		using T = typename std::result_of_t<ColFunc(uint32_t)>::mapped_type;
		using TripletT = Eigen::Triplet<T>;
		std::vector<TripletT> tripletList;
		tripletList.reserve(3*dim);
		for(uint64_t col = 0; col < dim; ++col)
		{
			auto m = colFunc(col);
			for(const auto& v: m)
			{
				tripletList.emplace_back(v.first, col, v.second);
			}
		}

		Eigen::SparseMatrix<T> res(dim, dim);
		res.setFromTriplets(tripletList.begin(), tripletList.end());
		return res;
	}

	/**
	 * @basis: Random access iterable container for the basis
	 */
	template<typename ColFunc, typename RandomIterable>
	auto constructSubspaceMat(ColFunc&& t, RandomIterable&& basis)
	{
		const uint32_t n = basis.size();
		using T = typename std::result_of_t<ColFunc(uint32_t)>::mapped_type;

		using TripletT = Eigen::Triplet<T>;
		tbb::concurrent_vector<TripletT> tripletList;
		tbb::parallel_for(0u, n,
			[&](uint32_t idx)
		{
			std::map<uint32_t, T> m = t(basis[idx]);
			auto iter = basis.begin();
			for(auto& kv: m)
			{
				iter = std::lower_bound(iter, basis.end(), kv.first);
				if(iter == basis.end())
					break;
				auto j = std::distance(basis.begin(), iter);
				tripletList.emplace_back(idx, j, kv.second);
			}
		});

		Eigen::SparseMatrix<T> res(n, n);
		res.setFromTriplets(tripletList.begin(), tripletList.end());
		return res;
	}

}

#endif//EDP_CONSTRUCTSPARSEMAT_HPP


#pragma once
#include <boost/multiprecision/cpp_int.hpp>

namespace sandbox {
using long_type = boost::multiprecision::checked_cpp_int;
}


/*******************************<SCHWARZ LIB LICENSE>***********************
Copyright (c) 2019, the SCHWARZ LIB authors
All rights reserved.

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

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

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

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

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

#ifndef initialization_hpp
#define initialization_hpp

#include <cmath>
#include <functional>
#include <memory>
#include <random>
#include <vector>

#include <schwarz/config.hpp>

#if SCHW_HAVE_DEALII
#include <deal.II/lac/sparse_matrix.h>
#endif

#include <partition_tools.hpp>
#include <settings.hpp>

namespace schwz {

/**
 * The initialization class that provides methods for initialization of the
 * solver.
 *
 * @tparam ValueType  The type of the floating point values.
 * @tparam IndexType  The type of the index type values.
 *
 * @ref init
 * @ingroup init
 */
template <typename ValueType = gko::default_precision,
          typename IndexType = gko::int32>
class Initialize : public Settings, public Metadata<ValueType, IndexType> {
public:
    Initialize(Settings &settings, Metadata<ValueType, IndexType> &metadata);

    /**
     * The partition indices containing the subdomains to which each row(vertex)
     * of the matrix(graph) belongs to.
     */
    std::vector<unsigned int> partition_indices;

    /**
     * The cell weights for the partition algorithm.
     */
    std::vector<unsigned int> cell_weights;

    /**
     * Generates the right hand side vector
     *
     * @param rhs  The rhs vector.
     */
    void generate_rhs(std::vector<ValueType> &rhs);

    /**
     * Generates the 2D global laplacian matrix.
     *
     * @param oned_laplacian_size  The size of the one d laplacian grid.
     * @param global_matrix  The global matrix.
     */
#if SCHW_HAVE_DEALII
    void setup_global_matrix(
        const std::string &filename, const gko::size_type &oned_laplacian_size,
        const dealii::SparseMatrix<ValueType> &matrix,
        std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>> &global_matrix);
#else
    void setup_global_matrix(
        const std::string &filename, const gko::size_type &oned_laplacian_size,
        std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>> &global_matrix);
#endif

    /**
     * The partitioning function. Allows the partition of the global matrix
     * depending with METIS and a regular 1D decomposition.
     *
     * @param settings  The settings struct.
     * @param metadata  The metadata struct.
     * @param global_matrix  The global matrix.
     * @param partition_indices  The partition indices [OUTPUT].
     */
    void partition(const Settings &settings,
                   const Metadata<ValueType, IndexType> &metadata,
                   const std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>>
                       &global_matrix,
                   std::vector<unsigned int> &partition_indices);

    /**
     * Setup the vectors with default values and allocate mameory if not
     * allocated.
     *
     * @param settings  The settings struct.
     * @param metadata  The metadata struct.
     * @param local_rhs  The local right hand side vector in the subdomain.
     * @param global_rhs  The global right hand side vector.
     * @param local_solution  The local solution vector in the subdomain.
     */
    void setup_vectors(
        const Settings &settings,
        const Metadata<ValueType, IndexType> &metadata,
        std::vector<ValueType> &rhs,
        std::shared_ptr<gko::matrix::Dense<ValueType>> &local_rhs,
        std::shared_ptr<gko::matrix::Dense<ValueType>> &global_rhs,
        std::shared_ptr<gko::matrix::Dense<ValueType>> &local_solution);

    /**
     * Sets up the local and the interface matrices from the global matrix and
     * the partition indices.
     *
     * @param settings  The settings struct.
     * @param metadata  The metadata struct.
     * @param partition_indices  The array containing the partition indices.
     * @param global_matrix The global system matrix.
     * @param local_matrix The local system matrix.
     * @param interface_matrix The interface matrix containing the interface and
     *                         the overlap data mainly used for exchanging
     *                         values between different sub-domains.
     * @param local_perm  The local permutation, obtained through RCM or METIS.
     */
    virtual void setup_local_matrices(
        Settings &settings, Metadata<ValueType, IndexType> &metadata,
        std::vector<unsigned int> &partition_indices,
        std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>> &global_matrix,
        std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>> &local_matrix,
        std::shared_ptr<gko::matrix::Csr<ValueType, IndexType>>
            &interface_matrix) = 0;

private:
    Settings &settings;

    Metadata<ValueType, IndexType> &metadata;
};

}  // namespace schwz

#endif  // initialization.hpp


// boost heap: heap node helper classes
//
// Copyright (C) 2010 Tim Blechmann
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HEAP_DETAIL_HEAP_COMPARISON_HPP
#define BOOST_HEAP_DETAIL_HEAP_COMPARISON_HPP

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/concept/assert.hpp>
#include <boost/heap/heap_concepts.hpp>

#ifdef BOOST_HEAP_SANITYCHECKS
#define BOOST_HEAP_ASSERT BOOST_ASSERT
#else
#define BOOST_HEAP_ASSERT(expression)
#endif

namespace boost  {
namespace heap   {
namespace detail {

template <typename Heap1, typename Heap2>
bool value_equality(Heap1 const & lhs, Heap2 const & rhs,
                    typename Heap1::value_type lval, typename Heap2::value_type rval)
{
    typename Heap1::value_compare const & cmp = lhs.value_comp();
    bool ret = !(cmp(lval, rval)) && !(cmp(rval, lval));

    // if this assertion is triggered, the value_compare objects of lhs and rhs return different values
    BOOST_ASSERT((ret == (!(rhs.value_comp()(lval, rval)) && !(rhs.value_comp()(rval, lval)))));

    return ret;
}

template <typename Heap1, typename Heap2>
bool value_compare(Heap1 const & lhs, Heap2 const & rhs,
                    typename Heap1::value_type lval, typename Heap2::value_type rval)
{
    typename Heap1::value_compare const & cmp = lhs.value_comp();
    bool ret = cmp(lval, rval);

    // if this assertion is triggered, the value_compare objects of lhs and rhs return different values
    BOOST_ASSERT((ret == rhs.value_comp()(lval, rval)));
    return ret;
}

struct heap_equivalence_copy
{
    template <typename Heap1, typename Heap2>
    bool operator()(Heap1 const & lhs, Heap2 const & rhs)
    {
        BOOST_CONCEPT_ASSERT((boost::heap::PriorityQueue<Heap1>));
        BOOST_CONCEPT_ASSERT((boost::heap::PriorityQueue<Heap2>));

        // if this assertion is triggered, the value_compare types are incompatible
        BOOST_STATIC_ASSERT((boost::is_same<typename Heap1::value_compare, typename Heap2::value_compare>::value));

        if (Heap1::constant_time_size && Heap2::constant_time_size)
            if (lhs.size() != rhs.size())
                return false;

        if (lhs.empty() && rhs.empty())
            return true;

        Heap1 lhs_copy(lhs);
        Heap2 rhs_copy(rhs);

        while (true) {
            if (!value_equality(lhs_copy, rhs_copy, lhs_copy.top(), rhs_copy.top()))
                return false;

            lhs_copy.pop();
            rhs_copy.pop();

            if (lhs_copy.empty() && rhs_copy.empty())
                return true;

            if (lhs_copy.empty())
                return false;

            if (rhs_copy.empty())
                return false;
        }
    }
};

struct heap_equivalence_iteration
{
    template <typename Heap1, typename Heap2>
    bool operator()(Heap1 const & lhs, Heap2 const & rhs)
    {
        BOOST_CONCEPT_ASSERT((boost::heap::PriorityQueue<Heap1>));
        BOOST_CONCEPT_ASSERT((boost::heap::PriorityQueue<Heap2>));

        // if this assertion is triggered, the value_compare types are incompatible
        BOOST_STATIC_ASSERT((boost::is_same<typename Heap1::value_compare, typename Heap2::value_compare>::value));

        if (Heap1::constant_time_size && Heap2::constant_time_size)
            if (lhs.size() != rhs.size())
                return false;

        if (lhs.empty() && rhs.empty())
            return true;

        typename Heap1::ordered_iterator it1 = lhs.ordered_begin();
        typename Heap1::ordered_iterator it1_end = lhs.ordered_end();
        typename Heap1::ordered_iterator it2 = rhs.ordered_begin();
        typename Heap1::ordered_iterator it2_end = rhs.ordered_end();
        while (true) {
            if (!value_equality(lhs, rhs, *it1, *it2))
                return false;

            ++it1;
            ++it2;

            if (it1 == it1_end && it2 == it2_end)
                return true;

            if (it1 == it1_end || it2 == it2_end)
                return false;
        }
    }
};

template <typename Heap1,
          typename Heap2
         >
bool heap_equality(Heap1 const & lhs, Heap2 const & rhs)
{
    const bool use_ordered_iterators = Heap1::has_ordered_iterators && Heap2::has_ordered_iterators;

    typedef typename boost::mpl::if_c<use_ordered_iterators,
                                      heap_equivalence_iteration,
                                      heap_equivalence_copy
                                     >::type equivalence_check;

    equivalence_check eq_check;
    return eq_check(lhs, rhs);
}

struct heap_compare_iteration
{
    template <typename Heap1,
              typename Heap2
             >
    bool operator()(Heap1 const & lhs, Heap2 const & rhs)
    {
        typename Heap1::size_type left_size = lhs.size();
        typename Heap2::size_type right_size = rhs.size();
        if (left_size < right_size)
            return true;

        if (left_size > right_size)
            return false;

        typename Heap1::ordered_iterator it1 = lhs.ordered_begin();
        typename Heap1::ordered_iterator it1_end = lhs.ordered_end();
        typename Heap1::ordered_iterator it2 = rhs.ordered_begin();
        typename Heap1::ordered_iterator it2_end = rhs.ordered_end();
        while (true) {
            if (value_compare(lhs, rhs, *it1, *it2))
                return true;

            if (value_compare(lhs, rhs, *it2, *it1))
                return false;

            ++it1;
            ++it2;

            if (it1 == it1_end && it2 == it2_end)
                return true;

            if (it1 == it1_end || it2 == it2_end)
                return false;
        }
    }
};

struct heap_compare_copy
{
    template <typename Heap1,
              typename Heap2
             >
    bool operator()(Heap1 const & lhs, Heap2 const & rhs)
    {
        typename Heap1::size_type left_size = lhs.size();
        typename Heap2::size_type right_size = rhs.size();
        if (left_size < right_size)
            return true;

        if (left_size > right_size)
            return false;

        Heap1 lhs_copy(lhs);
        Heap2 rhs_copy(rhs);

        while (true) {
            if (value_compare(lhs_copy, rhs_copy, lhs_copy.top(), rhs_copy.top()))
                return true;

            if (value_compare(lhs_copy, rhs_copy, rhs_copy.top(), lhs_copy.top()))
                return false;

            lhs_copy.pop();
            rhs_copy.pop();

            if (lhs_copy.empty() && rhs_copy.empty())
                return false;
        }
    }
};

template <typename Heap1,
          typename Heap2
         >
bool heap_compare(Heap1 const & lhs, Heap2 const & rhs)
{
    const bool use_ordered_iterators = Heap1::has_ordered_iterators && Heap2::has_ordered_iterators;

    typedef typename boost::mpl::if_c<use_ordered_iterators,
                                      heap_compare_iteration,
                                      heap_compare_copy
                                     >::type compare_check;

    compare_check check;
    return check(lhs, rhs);
}

} /* namespace detail */
} /* namespace heap */
} /* namespace boost */

#undef BOOST_HEAP_ASSERT

#endif // BOOST_HEAP_DETAIL_HEAP_COMPARISON_HPP


#ifndef plane_calibration_SRC_INPUT_FILTER_HPP_
#define plane_calibration_SRC_INPUT_FILTER_HPP_

#include <memory>
#include <mutex>
#include <Eigen/Dense>

#include "camera_model.hpp"
#include "calibration_parameters.hpp"
#include "visualizer_interface.hpp"

namespace plane_calibration
{

class InputFilter
{
public:
  class Config
  {
  public:
    Config()
    {
      max_error = 0.0;
      threshold_from_ground = 0.0;

      max_nan_ratio = 0.0;
      max_zero_ratio = 0.0;
      min_data_ratio = 0.0;
    }

    double max_error;
    double threshold_from_ground;

    double max_nan_ratio;
    double max_zero_ratio;
    double min_data_ratio;
  };

  //TODO automatic threshold from max_deviation
  InputFilter(const CameraModel& camera_model, const CalibrationParametersPtr& parameters,
              const VisualizerInterfacePtr& depth_visualizer, const Config& config);

  void updateConfig(const Config& config);
  void updateBorders();
  void filter(Eigen::MatrixXf& matrix, bool debug = false);
  bool dataIsUsable(const Eigen::MatrixXf& data, bool debug = false);

protected:
  void updateBorders_();

  mutable std::mutex mutex_;
  CameraModel camera_model_;
  CalibrationParametersPtr parameters_;
  Config config_;

  Eigen::MatrixXf min_plane_;
  Eigen::MatrixXf max_plane_;

  VisualizerInterfacePtr depth_visualizer_;
};
typedef std::shared_ptr<InputFilter> InputFilterPtr;

} /* end namespace */

#endif


#ifndef FAST_GICP_FAST_VGICP_IMPL_HPP
#define FAST_GICP_FAST_VGICP_IMPL_HPP

#include <atomic>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <sophus/so3.hpp>
#include <fast_gicp/so3/so3.hpp>
#include <fast_gicp/opt/gauss_newton.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>

namespace fast_gicp {

template<typename PointSource, typename PointTarget>
FastVGICP<PointSource, PointTarget>::FastVGICP() {
#ifdef _OPENMP
  num_threads_ = omp_get_max_threads();
#else
  num_threads_ = 1;
#endif

  reg_name_ = "FastVGICP";
  max_iterations_ = 64;
  k_correspondences_ = 20;
  rotation_epsilon_ = 2e-3;
  transformation_epsilon_ = 5e-4;
  // corr_dist_threshold_ = 1.0;
  regularization_method_ = PLANE;
  corr_dist_threshold_ = std::numeric_limits<float>::max();

  voxel_resolution_ = 1.0;
  search_method_ = DIRECT1;
  voxel_mode_ = ADDITIVE;

  source_kdtree.reset(new pcl::search::KdTree<PointSource>);
  target_kdtree.reset(new pcl::search::KdTree<PointTarget>);
}

template<typename PointSource, typename PointTarget>
FastVGICP<PointSource, PointTarget>::~FastVGICP() {}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setRotationEpsilon(double eps) {
  rotation_epsilon_ = eps;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setNumThreads(int n) {
  num_threads_ = n;

#ifdef _OPENMP
  if(n == 0) {
    num_threads_ = omp_get_max_threads();
  }
#endif
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setResolution(double resolution) {
  voxel_resolution_ = resolution;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
  k_correspondences_ = k;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
  regularization_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method) {
  search_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setVoxelAccumulationMode(VoxelAccumulationMode mode) {
  voxel_mode_ = mode;
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::swapSourceAndTarget() {
  input_.swap(target_);
  source_kdtree.swap(target_kdtree);
  source_covs.swap(target_covs);

  if(target_) {
    create_voxelmap(target_);
  }
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::clearSource() {
  input_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::clearTarget() {
  target_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
  if(input_ == cloud) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
  calculate_covariances(cloud, *source_kdtree, source_covs);
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
  if(target_ == cloud) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
  calculate_covariances(cloud, *target_kdtree, target_covs);
  create_voxelmap(cloud);
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::create_voxelmap(const PointCloudTargetConstPtr& cloud) {
  voxels.clear();
  for(int i = 0; i < cloud->size(); i++) {
    Eigen::Vector3i coord = voxel_coord(cloud->at(i).getVector4fMap());

    auto found = voxels.find(coord);
    if(found == voxels.end()) {
      GaussianVoxel::Ptr voxel;
      switch(voxel_mode_) {
        case ADDITIVE:
        case ADDITIVE_WEIGHTED:
          voxel = std::make_shared<AdditiveGaussianVoxel>();
          break;
        case MULTIPLICATIVE:
          voxel = std::make_shared<MultiplicativeGaussianVoxel>();
          break;
      }
      found = voxels.insert(found, std::make_pair(coord, voxel));
    }

    auto& voxel = found->second;
    voxel->append(cloud->at(i).getVector4fMap(), target_covs[i]);
  }

  for(auto& voxel : voxels) {
    voxel.second->finalize();
  }
}

template<typename PointSource, typename PointTarget>
void FastVGICP<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  Eigen::Matrix<float, 6, 1> x0;
  Eigen::Quaterniond init_quat = Eigen::Quaternionf(guess.template block<3, 3>(0, 0)).cast<double>();
  x0.head<3>() = Sophus::SO3f(init_quat.normalized().toRotationMatrix().cast<float>()).log();
  x0.tail<3>() = guess.template block<3, 1>(0, 3);

  // prevent stacking at zero
  if(x0.head<3>().norm() < 1e-2) {
    x0.head<3>() = (Eigen::Vector3f::Random()).normalized() * 1e-2;
  }

  converged_ = false;
  GaussNewton<double, 6> solver;

  for(int i = 0; i < max_iterations_; i++) {
    nr_iterations_ = i;

    Eigen::MatrixXf J;
    Eigen::VectorXf loss = loss_ls(x0, &J);

    Eigen::Matrix<float, 6, 1> delta = solver.delta(loss.cast<double>(), J.cast<double>()).cast<float>();

    Eigen::Isometry3f x0_ = Eigen::Isometry3f::Identity();
    x0_.linear() = Sophus::SO3f::exp(x0.head<3>()).matrix();
    x0_.translation() = x0.tail<3>();

    Eigen::Isometry3f delta_ = Eigen::Isometry3f::Identity();
    delta_.linear() = Sophus::SO3f::exp(delta.head<3>()).matrix();
    delta_.translation() = delta.tail<3>();

    Eigen::Isometry3f x1_ = delta_.inverse() * x0_;
    x1_.linear() = Eigen::Quaternionf(x1_.linear()).cast<double>().normalized().toRotationMatrix().cast<float>();

    x0.head<3>() = Sophus::SO3f(x1_.linear()).log();
    x0.tail<3>() = x1_.translation();

    if(is_converged(delta)) {
      converged_ = true;
      break;
    }
  }

  final_transformation_.setIdentity();
  final_transformation_.template block<3, 3>(0, 0) = Sophus::SO3f::exp(x0.head<3>()).matrix();
  final_transformation_.template block<3, 1>(0, 3) = x0.tail<3>();

  pcl::transformPointCloud(*input_, output, final_transformation_);
}

template<typename PointSource, typename PointTarget>
std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> FastVGICP<PointSource, PointTarget>::neighbor_offsets() const {
  switch(search_method_) {
    // clang-format off
    default:
      std::cerr << "here must not be reached" << std::endl;
      abort();
    case DIRECT1:
      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
        Eigen::Vector3i(0, 0, 0)
      };
    case DIRECT7:
      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
        Eigen::Vector3i(0, 0, 0),
        Eigen::Vector3i(1, 0, 0),
        Eigen::Vector3i(-1, 0, 0),
        Eigen::Vector3i(0, 1, 0),
        Eigen::Vector3i(0, -1, 0),
        Eigen::Vector3i(0, 0, 1),
        Eigen::Vector3i(0, 0, -1)
      };
    // clang-format on
  }

  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> offsets27;
  for(int i = 0; i < 3; i++) {
    for(int j = 0; j < 3; j++) {
      for(int k = 0; k < 3; k++) {
        offsets27.push_back(Eigen::Vector3i(i, j, k));
      }
    }
  }
  return offsets27;
}

template<typename PointSource, typename PointTarget>
Eigen::Vector3i FastVGICP<PointSource, PointTarget>::voxel_coord(const Eigen::Vector4f& x) const {
  return (x.array() / voxel_resolution_ - 0.5).floor().template cast<int>().template head<3>();
}

template<typename PointSource, typename PointTarget>
Eigen::Vector4f FastVGICP<PointSource, PointTarget>::voxel_origin(const Eigen::Vector3i& coord) const {
  Eigen::Vector3f origin = (coord.cast<float>().array() + 0.5) * voxel_resolution_;
  return Eigen::Vector4f(origin[0], origin[1], origin[2], 1.0f);
}

template<typename PointSource, typename PointTarget>
GaussianVoxel::Ptr FastVGICP<PointSource, PointTarget>::lookup_voxel(const Eigen::Vector3i& x) const {
  auto found = voxels.find(x);
  if(found == voxels.end()) {
    return nullptr;
  }

  return found->second;
}

template<typename PointSource, typename PointTarget>
bool FastVGICP<PointSource, PointTarget>::is_converged(const Eigen::Matrix<float, 6, 1>& delta) const {
  Eigen::Matrix3f R = Sophus::SO3f::exp(delta.head<3>()).matrix() - Eigen::Matrix3f::Identity();
  Eigen::Vector3f t = delta.tail<3>();

  Eigen::Matrix3f r_delta = 1.0 / rotation_epsilon_ * R.array().abs();
  Eigen::Vector3f t_delta = 1.0 / transformation_epsilon_ * t.array().abs();

  return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1;
}

template<typename PointSource, typename PointTarget>
Eigen::VectorXf FastVGICP<PointSource, PointTarget>::loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const {
  Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
  trans.block<3, 3>(0, 0) = Sophus::SO3f::exp(x.head<3>()).matrix();
  trans.block<3, 1>(0, 3) = x.tail<3>();

  auto offsets = neighbor_offsets();

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> losses(input_->size() * offsets.size());
  // use row-major arrangement for ease of repacking
  std::vector<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>, Eigen::aligned_allocator<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>>> Js(input_->size() * offsets.size());

  std::atomic_int count(0);

#pragma omp parallel for num_threads(num_threads_)
  for(int i = 0; i < input_->size(); i++) {
    const auto& mean_A = input_->at(i).getVector4fMap();
    const auto& cov_A = source_covs[i];

    Eigen::Vector4f transed_mean_A = trans * mean_A;
    Eigen::Vector3i coord = voxel_coord(transed_mean_A);

    bool is_RCR_computed = false;
    Eigen::Matrix4f RCR;
    Eigen::Matrix4f skew_mean_A;

    for(const auto& offset : offsets) {
      auto voxel = lookup_voxel(coord + offset);

      if(voxel == nullptr) {
        continue;
      }

      if(!is_RCR_computed) {
        RCR = trans * cov_A * trans.transpose();
        RCR(3, 3) = 1;
        skew_mean_A.setZero();
        skew_mean_A.block<3, 3>(0, 0) = skew(transed_mean_A.head<3>());
        is_RCR_computed = true;
      }

      const auto& mean_B = voxel->mean;
      const auto& cov_B = voxel->cov;

      Eigen::Vector4f d = mean_B - transed_mean_A;
      Eigen::Matrix4f RCR_inv = (cov_B + RCR).inverse();

      int n = count++;
      losses[n] = (RCR_inv * d).head<3>();
      Js[n].block<3, 3>(0, 0) = (RCR_inv * skew_mean_A).block<3, 3>(0, 0);
      Js[n].block<3, 3>(0, 3) = -RCR_inv.block<3, 3>(0, 0);

      if(voxel_mode_ == ADDITIVE_WEIGHTED) {
        losses[n] *= voxel->num_points;
      }
    }
  }

  int final_size = count;
  *J = Eigen::Map<Eigen::MatrixXf>(Js.front().data(), 6, final_size * 3).transpose();

  return Eigen::Map<Eigen::VectorXf>(losses.front().data(), final_size * 3);
}

template<typename PointSource, typename PointTarget>
template<typename PointT>
bool FastVGICP<PointSource, PointTarget>::calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances) {
  kdtree.setInputCloud(cloud);
  covariances.resize(cloud->size());

#pragma omp parallel for num_threads(num_threads_)
  for(int i = 0; i < cloud->size(); i++) {
    std::vector<int> k_indices;
    std::vector<float> k_sq_distances;
    kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances);

    Eigen::Matrix<float, 4, -1> data(4, k_correspondences_);

    for(int j = 0; j < k_indices.size(); j++) {
      data.col(j) = cloud->at(k_indices[j]).getVector4fMap();
    }

    data.colwise() -= data.rowwise().mean().eval();
    Eigen::Matrix4f cov = data * data.transpose();

    if(regularization_method_ == FROBENIUS) {
      double lambda = 1e-6;
      Eigen::Matrix3f C = cov.block<3, 3>(0, 0) + lambda * Eigen::Matrix3f::Identity();
      Eigen::Matrix3f C_inv = C.inverse();
      covariances[i].setZero();
      covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse();
    } else {
      Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::Vector3f values;

      switch(regularization_method_) {
        default:
          std::cerr << "here must not be reached" << std::endl;
          abort();
        case PLANE:
          values = Eigen::Vector3f(1, 1, 1e-2);
          break;
        case MIN_EIG:
          values = svd.singularValues().array().max(1e-2);
          break;
        case NORMALIZED_MIN_EIG:
          values = svd.singularValues().normalized().array().max(1e-2);
          break;
      }

      covariances[i].setZero();
      covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
    }
  }

  return true;
}

}  // namespace fast_gicp

#endif


#include <iomanip>
#include <cassert>
#include "boost/format.hpp"

#include "amilab_messages.h"
#include "DefineClass.hpp"
#include "Variable.hpp"
#include <math.h>
#include <iostream>

#define NEW_SMARTPTR(type, var, value) \
  boost::shared_ptr<type> var(new type(value));

#define RETURN_VARPTR(type,  value) \
  boost::shared_ptr<type> newval(new type(value)); \
  return Variable<type>::ptr( new Variable<type>(newval));

#include <boost/numeric/conversion/cast.hpp>

std::vector<std::string> AMILabType<float>::conversion_types =
        {   "bool", "double",
            "long", "int", "short",
            "unsigned long", "unsigned short",
            "unsigned char" };

//------------------------------------------------------
//------- Variable<float>
//------------------------------------------------------

/// Copy contents to new variable
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::NewCopy() const
{
  float_ptr newval( new float(Value()));
  Variable<float>::ptr newvar(new Variable<float>(newval));
  return newvar;
}

// Arithmetic operators

/// +a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator +()
{  RETURN_VARPTR(float,Value());}

/// prefix ++ operator ++a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator ++()
{
  //std::cout << "**" << std::endl;
  RETURN_VARPTR(float,++RefValue());
}

/// postfix ++ operator a++
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator ++(int)
{
  //std::cout << "**" << std::endl;
  RETURN_VARPTR(float,RefValue()++);
}

/// -a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator -()
{   RETURN_VARPTR(float,-Value());}

/// prefix -- operator --a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator --()
{  RETURN_VARPTR(float,--RefValue()); }

/// postfix -- operator a--
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator --(int)
{  RETURN_VARPTR(float,RefValue()--);  }

/// a+b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator +(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(float,Value()+b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a+=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator +=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() += b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a-b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator -(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(float,Value()-b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a-=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator -=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() -= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a*b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator *(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(float,Value()*b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a*=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator *=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() *= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a/b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator /(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(float,Value()/b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a/=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator /=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() /= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a%b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator %(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(float, ((int) round(Value())) % ((int) round(b->GetValueAsDouble())));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a%=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator %=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() =  ((int) round(Value())) % ((int) round(b->GetValueAsDouble()));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

//  Comparison Operators

/// a<b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator <(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()<b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a<=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator <=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()<=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a>b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator >(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()>b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a>=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator >=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()>=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a!=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator !=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()!=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a==b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator ==(const BasicVariable::ptr& b)
{
  //std::cout << __func__ << std::endl;
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()==b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

// Logical operators

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator !()
{
  RETURN_VARPTR(bool,!(Value()>0.5));
}

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator &&(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,(Value()>0.5)&& (bool) (b->GetValueAsDouble()>0.5));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::operator ||(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,(Value()>0.5) || (bool) (b->GetValueAsDouble()>0.5));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

// Mathematical functions
#define VAR_IMPL_FUNC(type,fname,func) \
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<type>::m_##fname() \
{ \
    RETURN_VARPTR(float, func(Value())); \
}

VAR_IMPL_FUNC(float,  sin,  sin)
VAR_IMPL_FUNC(float,  cos,  cos)
VAR_IMPL_FUNC(float,  tan,  tan)
VAR_IMPL_FUNC(float,  asin, asin)
VAR_IMPL_FUNC(float,  acos, acos)
VAR_IMPL_FUNC(float,  atan, atan)
VAR_IMPL_FUNC(float,  fabs, fabs)
VAR_IMPL_FUNC(float,  round,round)
VAR_IMPL_FUNC(float,  floor,floor)
VAR_IMPL_FUNC(float,  exp,  exp)
VAR_IMPL_FUNC(float,  log,  1.0/log(10.0)*log)
VAR_IMPL_FUNC(float,  ln,   log)
VAR_IMPL_FUNC(float,  norm, fabs)
VAR_IMPL_FUNC(float,  sqrt, sqrt)
//VAR_IMPL_FUNC(float,  pow,  pow)

//---------------------------------------------------
template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<float>::TryCast(
    const std::string& type_string) const
{
  try
  {
    // cast to double
    if (type_string==AMILabType<double>::name_as_string()) {
      RETURN_VARPTR(double, boost::numeric_cast<double>(Value()));
    } else
    // cast to int
    if (type_string==AMILabType<int>::name_as_string()) {
      RETURN_VARPTR(int, boost::numeric_cast<int>(Value()));
    } else
    // cast to long
    if (type_string==AMILabType<long>::name_as_string()) {
      RETURN_VARPTR(long, boost::numeric_cast<long>(Value()));
    } else
    // cast to unsigned long
    if (type_string==AMILabType<unsigned long>::name_as_string()) {
      RETURN_VARPTR(unsigned long, boost::numeric_cast<unsigned long>(Value()));
    } else
    // cast to short
    if (type_string==AMILabType<short>::name_as_string()) {
      RETURN_VARPTR(short, boost::numeric_cast<short>(Value()));
    } else
    // cast to unsigned short
    if (type_string==AMILabType<unsigned short>::name_as_string()) {
      RETURN_VARPTR(unsigned short, boost::numeric_cast<unsigned short>(Value()));
    } else
    // cast to unsigned char
    if (type_string==AMILabType<unsigned char>::name_as_string()) {
      RETURN_VARPTR(unsigned char, boost::numeric_cast<unsigned char>(Value()));
    } else
    // cast to bool
    if (type_string==AMILabType<bool>::name_as_string()) {
      RETURN_VARPTR(bool, boost::numeric_cast<bool>(Value()));
    } else
    {
      // make default conversion to double??
      CLASS_ERROR((boost::format("No conversion available for variable %1% from float to %2%") % _name % type_string).str().c_str());
    }
  } catch (std::bad_cast &e)
  {
    CLASS_ERROR((boost::format("%1%, for variable %2% from float to %3%") % e.what() % _name % type_string).str().c_str());
    return BasicVariable::ptr();
  }
  return BasicVariable::ptr();
}

//
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<float>::BasicCast(const int& type)
{
#define NUMCAST(amitype,type) \
      case amitype:  { RETURN_VARPTR(type, boost::numeric_cast<type>(Value())); }

  try
  {
    switch((WORDTYPE)type) {
      NUMCAST( WT_UNSIGNED_CHAR,  unsigned char )
      NUMCAST( WT_SIGNED_SHORT,   short         )
      NUMCAST( WT_UNSIGNED_SHORT, unsigned short)
      NUMCAST( WT_SIGNED_INT,     int           )
      NUMCAST( WT_SIGNED_LONG,    long          )
      NUMCAST( WT_FLOAT,          float         )
      NUMCAST( WT_DOUBLE,         double        )
      case WT_UNSIGNED_INT: { RETURN_VARPTR(float, (unsigned int) Value()); }
      default:
        CLASS_ERROR(( boost::format("Conversion to type %1% not available")%((WORDTYPE)type)).str().c_str());
    }
  } catch (std::bad_cast &e)
  {
    CLASS_ERROR((boost::format("%1%, for variable %2% from float to WORDTYPE %3%") % e.what() % _name % (WORDTYPE)type ).str().c_str());
    return BasicVariable::ptr();
  }

  RETURN_VARPTR(float, Value());

}

//
template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<float>::TernaryCondition(const BasicVariable::ptr& v1, const BasicVariable::ptr&v2)
{

  if (Value()>0.5) {
    return v1->NewReference();
  } else {
    return v2->NewReference();
  }
  return NewReference();
}

#if !defined(ARRAY_SIZE)
    #define ARRAY_SIZE(x) (sizeof((x)) / sizeof((x)[0]))
#endif

template<> AMI_DLLEXPORT
  BasicVariable::ptr Variable<float>::operator[](const BasicVariable::ptr& v)
{
  if (v->IsNumeric()) {
    float* pointer = this->Pointer().get();
    //std::cout << "Size of array " << ARRAY_SIZE(pointer) << std::endl;
    // at the user own risk
    float res = pointer[(int)(v->GetValueAsDouble()+0.5)];
    return AMILabType<float>::CreateVar(res);
  } else
    CLASS_ERROR("operator[] only takes a numerical parameter");
  return NewReference();
}

template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<float>::operator =(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() = b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return NewReference();
}


#ifndef STAN_MATH_TORSTEN_TWOCPT_EFFCPT_HPP
#define STAN_MATH_TORSTEN_TWOCPT_EFFCPT_HPP

#include <Eigen/Dense>
#include <stan/math/prim/err/check_greater_or_equal.hpp>
#include <stan/math/torsten/meta.hpp>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/pmx_solve_cpt.hpp>
#include <stan/math/torsten/ev_solver.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_twocpt_effcpt_model.hpp>
#include <stan/math/torsten/pmx_population_check.hpp>
#include <stan/math/prim/meta/return_type.hpp>
#include <string>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a one-compartment coupled with effective-compartment
 * model with analytical solution.
 *
 * @tparam Ts types of parameters, see pmx_solve_cpt for
 *         details.
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 */
  template <typename... Ts>
  auto pmx_solve_twocpt_effcpt(Ts... args) {
    return PMXSolveCPT<PMXTwocptEffCptModel>::solve(args...);
  }

/**
 * Overload function to allow user to pass an std::vector for
 * pMatrix/bioavailability/tlag
 */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  pmx_solve_twocpt_effcpt(const std::vector<T0>& time,
                          const std::vector<T1>& amt,
                          const std::vector<T2>& rate,
                          const std::vector<T3>& ii,
                          const std::vector<int>& evid,
                          const std::vector<int>& cmt,
                          const std::vector<int>& addl,
                          const std::vector<int>& ss,
                          const std::vector<T_par>& pMatrix,
                          const std::vector<T_biovar>& biovar,
                          const std::vector<T_tlag>& tlag) {
    auto param_ = torsten::to_array_2d(pMatrix);
    auto biovar_ = torsten::to_array_2d(biovar);
    auto tlag_ = torsten::to_array_2d(tlag);

    return pmx_solve_twocpt_effcpt(time, amt, rate, ii, evid, cmt, addl, ss,
                                   param_, biovar_, tlag_);
  }

  /**
   * For population models, we follow the call signature
   * but add the arrays of the length of each individual's data.
   * The size of that vector is the size of
   * the population.
   */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6>
Eigen::Matrix<typename  EventsManager<NONMENEventsRecord<T0, T1, T2, T3>, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6> > >::T_scalar, // NOLINT
              Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_group_twocpt_effcpt(const std::vector<int>& len,
                       const std::vector<T0>& time,
                       const std::vector<T1>& amt,
                       const std::vector<T2>& rate,
                       const std::vector<T3>& ii,
                       const std::vector<int>& evid,
                       const std::vector<int>& cmt,
                       const std::vector<int>& addl,
                       const std::vector<int>& ss,
                       const std::vector<std::vector<T4> >& pMatrix,
                       const std::vector<std::vector<T5> >& biovar,
                       const std::vector<std::vector<T6> >& tlag) {
  using ER = NONMENEventsRecord<T0, T1, T2, T3>;
  using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6> >>;

  int nCmt = torsten::PMXTwocptEffCptModel<double>::Ncmt;
  ER events_rec(nCmt, len, time, amt, rate, ii, evid, cmt, addl, ss);

  static const char* caller("pmx_solve_group_twocpt_effcpt");
  torsten::pmx_population_check(len, time, amt, rate, ii, evid, cmt, addl, ss,
                                pMatrix, biovar, tlag, caller);

  using model_type = torsten::PMXTwocptEffCptModel<typename EM::T_par>;
   EventSolver<model_type, EM> pr;

  Eigen::Matrix<typename EM::T_scalar, -1, -1> pred(events_rec.total_num_event_times, nCmt);

  pr.pred(events_rec, pred, dsolve::PMXAnalyiticalIntegrator(), pMatrix, biovar, tlag);

  return pred;
}

}
#endif


#include "zonesigner.hpp"
#include "gtest/gtest.h"
#include <cstring>
#include <iostream>
#include <boost/log/trivial.hpp>

class DNSKEYTest : public ::testing::Test
{

public:
    virtual void SetUp()
    {
    }

    virtual void TearDown()
    {
    }
};

const char *KSK_DNSKEY_BASE64 =
    "AwEAAdYg5E46kVXXLNnVZYGutuUr8WuBOw+D/La0MnQYIn6POg61eMtN"
    "5JBKZBmO+fP4Pw9Rddt4xVjN4OmxUkcRJ/rdi/SzMTo4/kqUuHKm3uRX"
    "CKtwkfDCOgN1jm/ORGkVb7vFAmRYyAO6LxkcRZHBpqtHp10uPt+Qeh4+"
    "TodQo688AC54ldJSt0NZIMRYgcfodfzs+zmIBvB5lNaNlsOtTCr9W+7A"
    "m2J5H79YFYqPaNz7ON2u8c1hpSb55gQYKBVXuIG84D5xAWboc7a/tpk0"
    "EnwXCvhCj5m40xVjJOrgD5TrwMTrJml2KwgyGcIxABxji+N6kCHZkma9"
    "gtgAyD++xw0=";

const char *PUBLIC_EXPONENT_BASE64 = "AQAB";

const char *MODULUS_BASE64 =
    "1iDkTjqRVdcs2dVlga625Svxa4E7D4P8trQydBgifo86DrV4y03kkEpk"
    "GY758/g/D1F123jFWM3g6bFSRxEn+t2L9LMxOjj+SpS4cqbe5FcIq3CR"
    "8MI6A3WOb85EaRVvu8UCZFjIA7ovGRxFkcGmq0enXS4+35B6Hj5Oh1Cj"
    "rzwALniV0lK3Q1kgxFiBx+h1/Oz7OYgG8HmU1o2Ww61MKv1b7sCbYnkf"
    "v1gVio9o3Ps43a7xzWGlJvnmBBgoFVe4gbzgPnEBZuhztr+2mTQSfBcK"
    "+EKPmbjTFWMk6uAPlOvAxOsmaXYrCDIZwjEAHGOL43qQIdmSZr2C2ADI"
    "P77HDQ==";

TEST_F( DNSKEYTest, CompareDNSKEY )
{
    std::vector<uint8_t> ksk_dnskey, expected, public_exponent, modulus;
    decodeFromBase64( KSK_DNSKEY_BASE64,      expected );
    decodeFromBase64( PUBLIC_EXPONENT_BASE64, public_exponent );
    decodeFromBase64( MODULUS_BASE64,         modulus );

    dns::RSAPublicKey public_key( public_exponent, modulus );
    ksk_dnskey = public_key.getDNSKEYFormat();
    EXPECT_EQ( expected, ksk_dnskey );
}

int main( int argc, char **argv )
{
    ::testing::InitGoogleTest( &argc, argv );
    return RUN_ALL_TESTS();
}


///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2008-2012  Artyom Beilis (Tonkikh) <artyomtnk@yahoo.com>
//
//  See accompanying file COPYING.TXT file for licensing details.
//
///////////////////////////////////////////////////////////////////////////////
#define CPPCMS_SOURCE
#include <cppcms/urandom.h>
#include <cppcms/crypto.h>
#include <cppcms/config.h>
#include <booster/backtrace.h>
#include <string.h>

#if defined(CPPCMS_HAVE_OPENSSL)
	#include <openssl/aes.h>
	#define CPPCMS_HAVE_AES
#endif

#if defined(CPPCMS_HAVE_GCRYPT)
	#include <gcrypt.h>
	#define CPPCMS_HAVE_AES

	#ifdef CPPCMS_WIN_NATIVE
	#	define CPPCMS_GCRYPT_USE_BOOSTER_THREADS
	#endif

	#ifdef CPPCMS_GCRYPT_USE_BOOSTER_THREADS

		#include <booster/thread.h>

		extern "C" {
			static int nt_mutex_init(void **p)
			{
				try {
					*p=new booster::mutex();
					return 0;
				}
				catch(...)
				{
					return 1;
				}
			}
			static int nt_mutex_destroy(void **p)
			{
				booster::mutex **m=reinterpret_cast<booster::mutex **>(p);
				delete *m;
				*m=0;
				return 0;
			}

			static int nt_mutex_lock(void **p)
			{
				booster::mutex *m=reinterpret_cast<booster::mutex *>(*p);
				try {
					m->lock();
					return 0;
				}
				catch(...)
				{
					return 1;
				}
			}

			static int nt_mutex_unlock(void **p)
			{
				booster::mutex *m=reinterpret_cast<booster::mutex *>(*p);
				try {
					m->unlock();
					return 0;
				}
				catch(...)
				{
					return 1;
				}
			}

			static struct gcry_thread_cbs threads_nt = {
				GCRY_THREAD_OPTION_USER,
				0,
				nt_mutex_init,
				nt_mutex_destroy,
				nt_mutex_lock,
				nt_mutex_unlock,
				0,0,0,0,
				0,0,0,0
			};

			static void set_gcrypt_cbs()
			{
				gcry_control (GCRYCTL_SET_THREAD_CBS, &threads_nt);
			}
		} // extern "C"

	#else // pthreads

		#include <pthread.h>
		#include <errno.h>

		extern "C" {

			GCRY_THREAD_OPTION_PTHREAD_IMPL;

			static void set_gcrypt_cbs()
			{
				gcry_control (GCRYCTL_SET_THREAD_CBS, &gcry_threads_pthread);
			}
		} // exten C

	#endif

	// Static library initialization
	namespace {
		class load {
			public:
			load() {
				set_gcrypt_cbs();
				gcry_check_version(NULL);
			}
		} loader;
	} // anon namespace

#endif // defined(CPPCMS_HAVE_GCRYPT)

namespace cppcms {
namespace crypto {

#if defined CPPCMS_HAVE_GCRYPT

	class gcrypt_aes_encryptor : public cbc {
	public:
		gcrypt_aes_encryptor(unsigned type) :
			enc_(0),
			dec_(0),
			iv_initialized_(false)
		{
			memset(iv_,0,sizeof(iv_));
			type_ = type;
		}

		void check()
		{
			if(key_.size() == 0)
				throw booster::runtime_error("cppcms::crypto::aes: attempt to use cbc without key");
			if(!iv_initialized_)
				throw booster::runtime_error("cppcms::crypto::aes: attempt to use cbc without initial vector set");
		}
		unsigned block_size() const
		{
			return 16;
		}
		unsigned key_size() const
		{
			return type_ / 8;
		}
		void set_key(key const &k)
		{
			if(key_.size()!=0)
				booster::runtime_error("cppcms::crypto::aes can't set key more then once");
			if(k.size() != key_size())
				throw booster::invalid_argument("cppcms::crypto::aes Invalid key size");
			key_ = k;
		}
		void set_iv(void const *ptr,size_t size)
		{
			if(size != sizeof(iv_))
				throw booster::invalid_argument("cppcms::crypto::aes: Invalid IV size");
			memcpy(iv_,ptr,size);
			iv_initialized_ = true;
			reset_iv();
		}
		void set_nonce_iv()
		{
			urandom_device rnd;
			rnd.generate(iv_,sizeof(iv_));
			iv_initialized_ = true;
			reset_iv();
		}

		void reset_iv()
		{
			if(enc_) {
				if(gcry_cipher_setiv(enc_,iv_,16)) {
					throw booster::runtime_error("cppcms::crypto::aes: failed to reset iv");
				}
			}
			if(dec_) {
				if(gcry_cipher_setiv(dec_,iv_,16)) {
					throw booster::runtime_error("cppcms::crypto::aes: failed to reset iv");
				}
			}
		}

		virtual ~gcrypt_aes_encryptor()
		{
			if(enc_)
				gcry_cipher_close(enc_);
			if(dec_)
				gcry_cipher_close(dec_);
			memset(iv_,0,sizeof(iv_));
		}
		virtual void encrypt(void const *in,void *out,unsigned len)
		{
			init(enc_);
			if(gcry_cipher_encrypt(enc_,out,len,in,len)!=0) {
				throw booster::runtime_error("Encryption failed");
			}
		}
		virtual void decrypt(void const *in,void *out,unsigned len)
		{
			init(dec_);
			if(gcry_cipher_decrypt(dec_,out,len,in,len)!=0) {
				throw booster::runtime_error("Decryption failed");
			}
		}
	private:
		void init(gcry_cipher_hd_t &h)
		{
			if(h)
				return;
			check();

			int algo=0;
			switch(type_) {
			case 128:
				algo = GCRY_CIPHER_AES128;
				break;
			case 192:
				algo = GCRY_CIPHER_AES192;
				break;
			case 256:
				algo = GCRY_CIPHER_AES256;
				break;
			default:
				throw booster::invalid_argument("Invalid encryption method");
			}

			if(	gcry_cipher_open(&h,algo,GCRY_CIPHER_MODE_CBC,0)
				|| gcry_cipher_setkey(h,key_.data(),key_.size())
				|| gcry_cipher_setiv(h,iv_,16)
			  )
			{
				if(h) {
					gcry_cipher_close(h);
					h=0;
				}
				throw booster::runtime_error("Failed to create AES encryptor");
			}
		}

		gcry_cipher_hd_t enc_;
		gcry_cipher_hd_t dec_;
		key key_;
		char iv_[16];
		bool iv_initialized_;
		unsigned type_;
	};

	typedef gcrypt_aes_encryptor aes_encryption_provider;

#elif defined CPPCMS_HAVE_OPENSSL

	class openssl_aes_encryptor : public cbc {
	public:
		openssl_aes_encryptor(unsigned bits)
		{
			switch(bits) {
			case 128:
			case 192:
			case 256:
				type_  = bits;
				break;
			default:
				throw booster::invalid_argument("cppcms::crypto::aes invalid algorithm");
			}
			reset();
		}
		void reset()
		{
			key_.reset();
			memset(&key_enc_,0,sizeof(key_enc_));
			memset(&key_dec_,0,sizeof(key_dec_));
			memset(iv_dec_,0,sizeof(iv_dec_));
			memset(iv_enc_,0,sizeof(iv_dec_));
			encryption_initialized_ = false;
			decryption_initialized_ = false;
			iv_initialized_ = false;
		}
		void check()
		{
			if(key_.size() == 0)
				throw booster::runtime_error("cppcms::crypto::aes: attempt to use cbc without key");
			if(!iv_initialized_)
				throw booster::runtime_error("cppcms::crypto::aes: attempt to use cbc without initial vector set");
		}
		unsigned block_size() const
		{
			return 16;
		}
		unsigned key_size() const
		{
			return type_ / 8;
		}
		void set_key(key const &k)
		{
			if(key_.size()!=0)
				booster::runtime_error("cppcms::crypto::aes can't set key more then once");
			if(k.size() != key_size())
				throw booster::invalid_argument("cppcms::crypto::aes Invalid key size");
			key_ = k;
		}
		void set_iv(void const *ptr,size_t size)
		{
			if(size != sizeof(iv_enc_))
				throw booster::invalid_argument("cppcms::crypto::aes: Invalid IV size");
			memcpy(iv_enc_,ptr,size);
			memcpy(iv_dec_,ptr,size);
			iv_initialized_ = true;
		}
		void set_nonce_iv()
		{
			urandom_device rnd;
			rnd.generate(iv_enc_,sizeof(iv_enc_));
			rnd.generate(iv_dec_,sizeof(iv_dec_));
			iv_initialized_ = true;
		}
		virtual void encrypt(void const *in,void *out,unsigned len)
		{
			check();
			if(!encryption_initialized_) {
				AES_set_encrypt_key(reinterpret_cast<unsigned char const *>(key_.data()), type_, &key_enc_);
				encryption_initialized_ = true;
			}
			AES_cbc_encrypt(reinterpret_cast<unsigned char const *>(in),
					reinterpret_cast<unsigned char *>(out), len,
					&key_enc_,
					iv_enc_,
					AES_ENCRYPT);
		}
		virtual void decrypt(void const *in,void *out,unsigned len)
		{
			check();
			if(!decryption_initialized_) {
				AES_set_decrypt_key(reinterpret_cast<unsigned char const *>(key_.data()), type_, &key_dec_);
				decryption_initialized_ = true;
			}

			AES_cbc_encrypt(reinterpret_cast<unsigned char const *>(in),
					reinterpret_cast<unsigned char *>(out), len,
					&key_dec_,
					iv_dec_,
					AES_DECRYPT);
		}
		virtual ~openssl_aes_encryptor()
		{
			reset();
		}
	private:
		key key_;
		unsigned type_;
		AES_KEY key_enc_;
		AES_KEY key_dec_;
		unsigned char iv_enc_[16];
		unsigned char iv_dec_[16];
		bool encryption_initialized_;
		bool decryption_initialized_;
		bool iv_initialized_;
	};

	typedef openssl_aes_encryptor aes_encryption_provider;

#endif

std::auto_ptr<cbc> cbc::create(std::string const &name)
{
	std::auto_ptr<cbc> res;
	if(name=="aes" || name=="AES" || name=="aes128" || name=="aes-128" || name=="AES128" || name=="AES-128")
		res = cbc::create(aes128);
	else if(name=="aes192" || name=="aes-192" || name=="AES192" || name=="AES-192")
		res = cbc::create(aes192);
	else if(name=="aes256" || name=="aes-256" || name=="AES256" || name=="AES-256")
		res = cbc::create(aes256);

	return res;
}

std::auto_ptr<cbc> cbc::create(cbc::cbc_type type)
{
	std::auto_ptr<cbc> res;
	switch(type) {
#ifdef CPPCMS_HAVE_AES
	case aes128:
		res.reset(new aes_encryption_provider(128));
		break;
	case aes192:
		res.reset(new aes_encryption_provider(192));
		break;
	case aes256:
		res.reset(new aes_encryption_provider(256));
		break;
#endif
	default:
		;
	}
	return res;
}

} // crypto
} //cppcms