#include <ros/ros.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <vector>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <nodelet/nodelet.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <image_geometry/pinhole_camera_model.h>
#include <geometry_msgs/Pose.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>

#include <localizer/Localize.h>

namespace localizer {

using namespace message_filters::sync_policies;

// Encapsulate differences between processing float and uint16_t depths
template<typename T> struct DepthTraits {};

template<>
struct DepthTraits<uint16_t>
{
	static inline bool valid(uint16_t depth) { return depth != 0; }
	static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
	static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
	static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
};

template<>
struct DepthTraits<float>
{
	static inline bool valid(float depth) { return std::isfinite(depth); }
	static inline float toMeters(float depth) { return depth; }
	static inline float fromMeters(float depth) { return depth; }

	static inline void initializeBuffer(std::vector<uint8_t>& buffer)
	{
		float* start = reinterpret_cast<float*>(&buffer[0]);
		float* end = reinterpret_cast<float*>(&buffer[0] + buffer.size());
		std::fill(start, end, std::numeric_limits<float>::quiet_NaN());
	}
};

const std::string stripSlashTF2(const std::string& str) {

	if (str.find("/") == 0) {
		return std::string(str, 1, std::string::npos);
	} else
		return str;

}

class LocalizerNodelet : public nodelet::Nodelet {
private:
	// Subscriptions
	message_filters::Subscriber<sensor_msgs::Image> sub_depth_image;
	message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info;
	boost::shared_ptr<tf2_ros::Buffer> tf_buffer;
	boost::shared_ptr<tf2_ros::TransformListener> tf_listener;
	typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
	typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
	boost::shared_ptr<Synchronizer> synchronized_listener;

	ros::ServiceServer localize_service;
	boost::mutex request_mutex;
	image_geometry::PinholeCameraModel saved_depth_model;
	sensor_msgs::ImageConstPtr saved_depth_msg;

	template<typename T>
	bool estimate(const sensor_msgs::ImageConstPtr& depth_msg, const geometry_msgs::Point& point, int radius, const Eigen::Affine3d& target_to_depth, geometry_msgs::Pose& pose);

public:
	virtual void onInit();

	void imageCallback(const sensor_msgs::ImageConstPtr& depth_image_msg,
		       const sensor_msgs::CameraInfoConstPtr& depth_info_msg);

	bool localize(localizer::Localize::Request &req, localizer::Localize::Response &res);

};

bool LocalizerNodelet::localize(localizer::Localize::Request &req, localizer::Localize::Response &res) {

	boost::lock_guard<boost::mutex> lock(request_mutex);

	res.pose.position.x = 0;
	res.pose.position.y = 0;
	res.pose.position.z = 0;

	res.pose.orientation.x = 0;
	res.pose.orientation.y = 0;
	res.pose.orientation.z = 1;
	res.pose.orientation.w = 0;

	if (!saved_depth_msg) return true;

	Eigen::Affine3d target_to_depth;
	try
	{
		geometry_msgs::TransformStamped transform = tf_buffer->lookupTransform (
				              stripSlashTF2(saved_depth_msg->header.frame_id), stripSlashTF2(req.header.frame_id),
				              saved_depth_msg->header.stamp);

		tf::transformMsgToEigen(transform.transform, target_to_depth);
	}
	catch (tf2::TransformException& ex)
	{
		NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what());
		return false;
	}

	if (saved_depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
		estimate<uint16_t>(saved_depth_msg, req.point, req.scope, target_to_depth, res.pose);
	}
	else if (saved_depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
		estimate<float>(saved_depth_msg, req.point, req.scope, target_to_depth, res.pose);
	}
	else {
		NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", saved_depth_msg->encoding.c_str());
		return false;
	}

	return true;

}

void LocalizerNodelet::onInit()
{

	ros::NodeHandle& nh = getNodeHandle();
	ros::NodeHandle& private_nh = getPrivateNodeHandle();
	tf_buffer.reset( new tf2_ros::Buffer );
	tf_listener.reset( new tf2_ros::TransformListener(*tf_buffer) );

	saved_depth_msg.reset();

	// Read parameters.
	int queue_size;
	private_nh.param("queue_size", queue_size, 10);

    sub_depth_image.subscribe(nh, "depth/image", 1);
  	sub_depth_info.subscribe(nh, "depth/camera_info", 1);

	synchronized_listener.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image, sub_depth_info) );
	synchronized_listener->registerCallback(boost::bind(&LocalizerNodelet::imageCallback, this, _1, _2));

	std::string service_name = ros::this_node::getName() + std::string("/localize");
	localize_service = nh.advertiseService<LocalizerNodelet, localizer::Localize::Request, localizer::Localize::Response>(service_name, &LocalizerNodelet::localize, this);

}

void LocalizerNodelet::imageCallback(const sensor_msgs::ImageConstPtr& depth_image_msg,
                              const sensor_msgs::CameraInfoConstPtr& depth_info_msg) {

	boost::lock_guard<boost::mutex> lock(request_mutex);
	if (!depth_image_msg) return;

	saved_depth_model.fromCameraInfo(depth_info_msg);
	saved_depth_msg = depth_image_msg;

}

template<typename T>
bool LocalizerNodelet::estimate(const sensor_msgs::ImageConstPtr& depth_msg, const geometry_msgs::Point& point, int scope, const Eigen::Affine3d& target_to_depth, geometry_msgs::Pose& pose) {

	// Extract all the parameters we need
	double depth_fx = saved_depth_model.fx();
	double depth_fy = saved_depth_model.fy();
	double depth_cx = saved_depth_model.cx(), depth_cy = saved_depth_model.cy();
	double depth_Tx = saved_depth_model.Tx(), depth_Ty = saved_depth_model.Ty();

	Eigen::Vector4d xyz_target;
	xyz_target << point.x, point.y, point.z, 1;

	// Transform to depth camera frame
	Eigen::Vector4d xyz_depth = target_to_depth * xyz_target;

	// Project to (u,v) in depth image
	double inv_Z = 1.0 / xyz_depth.z();
	int u = (depth_fx * xyz_depth.x() + depth_Tx) * inv_Z + depth_cx + 0.5;
	int v = (depth_fy * xyz_depth.y() + depth_Ty) * inv_Z + depth_cy + 0.5;

    if (u < 0 || u >= (int)depth_msg->width || v < 0 || v >= (int)depth_msg->height)
        return false;

	const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
	int row_step = depth_msg->step / sizeof(T);

	double depth = 1;

	// in case we need a more robust estimate
	if (scope > 1) {
		scope = MIN(scope, 30); // Some reasonable bounds

		std::vector<double> values;

		for (int i = u - scope; i < u + scope; i++)
		for (int j = v - scope; j < v + scope; j++) {
			if (i < 0 || i >= (int)depth_msg->width || j < 0 || j >= (int)depth_msg->height)
				continue;

			T raw_depth = depth_row[i + row_step * j];
			if (!DepthTraits<T>::valid(raw_depth))
				continue;

			values.push_back(DepthTraits<T>::toMeters(raw_depth));

		}

		if (values.size() == 0) return false;

		if (values.size() == 1) {
			depth = values[0];
		}

		// Computing a median of values
		std::sort(values.begin(), values.end());

		if (values.size() % 2 == 0)
			depth = (values[values.size() / 2 - 1] + values[values.size() / 2]) / 2;
		else
			depth = values[values.size() / 2];

	} else {

		T raw_depth = depth_row[u + row_step * v];
		if (!DepthTraits<T>::valid(raw_depth))
			return false;

		depth = DepthTraits<T>::toMeters(raw_depth);

	}

	xyz_depth << ((u - depth_cx) * depth - depth_Tx) / depth_fx,
		 ((v - depth_cy) * depth - depth_Ty) / depth_fy, depth, 1;

	// Transform to target camera frame
	xyz_target = target_to_depth.inverse() * xyz_depth;

	pose.position.x = xyz_target.x();
	pose.position.y = xyz_target.y();
	pose.position.z = xyz_target.z();

	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = 1;
	pose.orientation.w = 0;

	return true;

}

}

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(localizer::LocalizerNodelet,nodelet::Nodelet);


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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_UNITS_HOMOGENEOUS_SYSTEM_HPP_INCLUDED
#define BOOST_UNITS_HOMOGENEOUS_SYSTEM_HPP_INCLUDED

#include <boost/mpl/bool.hpp>

#include <boost/units/config.hpp>
#include <boost/units/static_rational.hpp>

#ifdef BOOST_UNITS_CHECK_HOMOGENEOUS_UNITS

#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/not.hpp>

#include <boost/units/detail/linear_algebra.hpp>

#endif

namespace boost {

namespace units {

/// A system that can uniquely represent any unit
/// which can be composed from a linearly independent set
/// of base units.  It is safe to rebind a unit with
/// such a system to different dimensions.
///
/// Do not construct this template directly.  Use
/// make_system instead.
template<class L>
struct homogeneous_system {
    /// INTERNAL ONLY
    typedef L type;
};

template<class T, class E>
struct static_power;

template<class T, class R>
struct static_root;

/// INTERNAL ONLY
template<class L, long N, long D>
struct static_power<homogeneous_system<L>, static_rational<N,D> >
{
    typedef homogeneous_system<L> type;
};

/// INTERNAL ONLY
template<class L, long N, long D>
struct static_root<homogeneous_system<L>, static_rational<N,D> >
{
    typedef homogeneous_system<L> type;
};

namespace detail {

template<class System, class Dimensions>
struct check_system;

#ifdef BOOST_UNITS_CHECK_HOMOGENEOUS_UNITS

template<class L, class Dimensions>
struct check_system<homogeneous_system<L>, Dimensions> :
    boost::mpl::not_<
        boost::is_same<
            typename calculate_base_unit_exponents<
                L,
                Dimensions
            >::type,
            inconsistent
        >
    > {};

#else

template<class L, class Dimensions>
struct check_system<homogeneous_system<L>, Dimensions> : mpl::true_ {};

#endif

} // namespace detail

} // namespace units

} // namespace boost

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::homogeneous_system, (class))

#endif

#endif


//
// Copyright (c) Leonid Seniukov. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for details.
//

#include "absolutePoseEstimation/rotationAveraging/RotationAverager.h"
#include "poseGraph/ConnectedComponent.h"
#include "parametrization/RelativeSE3.h"
#include "parametrization/Point3d.h"
#include "absolutePoseEstimation/rotationAveraging/RotationRobustOptimizerLogSO3.h"
#include "absolutePoseEstimation/translationAveraging/TranslationAverager.h"

#include <fstream>
#include <boost/filesystem.hpp>

namespace gdr {

    ConnectedComponentPoseGraph::ConnectedComponentPoseGraph(
            const std::vector<VertexPose> &absolutePosesToSet,
            const std::vector<std::vector<RelativeSE3>> &edgesLocalIndicesRelativePoses,
            const KeyPointMatches &inlierPointCorrespondencesToSet,
            int componentNumberToSet) :
            inlierPointCorrespondences(inlierPointCorrespondencesToSet),
            componentNumber(componentNumberToSet) {

        poseGraph = PoseGraph(absolutePosesToSet, edgesLocalIndicesRelativePoses);
        assert(getNumberOfPoses() > 0);
    }

    int ConnectedComponentPoseGraph::getNumberOfPoses() const {
        return poseGraph.size();
    }

    std::set<int> ConnectedComponentPoseGraph::initialIndices() const {
        std::set<int> initialIndices;
        for (const auto &pose: poseGraph.getPoseVertices()) {
            initialIndices.insert(pose.getInitialIndex());
        }
        return initialIndices;
    }

    int
    ConnectedComponentPoseGraph::printRelativeRotationsToFile(const std::string &pathToFileRelativeRotations) const {

        std::ofstream file(pathToFileRelativeRotations);

        if (file.is_open()) {

            int numPoses = getNumberOfPoses();

            for (int i = 0; i < numPoses; ++i) {
                std::string s1 = "VERTEX_SE3:QUAT ";
                std::string s2 = std::to_string(i) + " 0.000000 0.000000 0.000000 0.0 0.0 0.0 1.0\n";
                file << s1 + s2;
            }

            for (int i = 0; i < poseGraph.size(); ++i) {
                for (int j = 0; j < poseGraph.getNumberOfAdjacentVertices(i); ++j) {

                    const auto &transformation = poseGraph.getRelativePose(i, j);
                    if (i >= transformation.getIndexTo()) {
                        continue;
                    }
                    std::string noise = "   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000";

                    int indexTo = transformation.getIndexTo();
                    int indexFrom = i;
                    //order of vertices in the EDGE_SE3:QUAT representation is reversed (bigger_indexTo less_indexFrom)(gtsam format)
                    file << "EDGE_SE3:QUAT " << indexFrom << ' ' << indexTo << ' ';
                    auto translationVector = transformation.getRelativeTranslation();
                    file << ' ' << std::to_string(translationVector.col(0)[0]) << ' '
                         << std::to_string(translationVector.col(0)[1]) << ' '
                         << std::to_string(translationVector.col(0)[2]) << ' ';

                    const auto &qR = transformation.getRelativeRotation();
                    file << qR << noise << '\n';
                }
            }
        } else {
            return 1;
        }

        return 0;
    }

    std::vector<SE3> ConnectedComponentPoseGraph::getPoses() const {
        std::vector<SE3> poses;
        poses.reserve(getNumberOfPoses());

        for (const auto &pose: poseGraph.getPoseVertices()) {
            poses.emplace_back(pose.getAbsolutePoseSE3());
        }

        assert(poses.size() == getNumberOfPoses());
        return poses;
    }

    const KeyPointMatches &
    ConnectedComponentPoseGraph::getInlierObservedPoints() const {
        return inlierPointCorrespondences;
    }

    const VertexPose &ConnectedComponentPoseGraph::getVertex(int vertexNumber) const {
        return poseGraph.getPoseVertex(vertexNumber);
    }

    void ConnectedComponentPoseGraph::setRotation(int poseIndex, const SO3 &rotationSO3) {
        poseGraph.setRotationSO3(poseIndex, rotationSO3);
    }

    const std::vector<VertexPose> &ConnectedComponentPoseGraph::getVertices() const {
        return poseGraph.getPoseVertices();
    }

    const std::vector<RelativeSE3> &ConnectedComponentPoseGraph::getConnectionsFromVertex(int vertexNumber) const {

        return poseGraph.getRelativePosesFrom(vertexNumber);
    }

    bool ConnectedComponentPoseGraph::poseIndexIsValid(int poseIndex) const {
        bool isValid = poseIndex < poseGraph.size() && poseIndex >= 0;

        assert(isValid);
        return true;
    }

    void ConnectedComponentPoseGraph::setTranslation(int poseIndex, const Eigen::Vector3d &translation) {
        poseGraph.setTranslationV3(poseIndex, translation);
    }

    void ConnectedComponentPoseGraph::setPoseSE3(int poseIndex, const SE3 &poseSE3) {
        poseGraph.setPoseSE3(poseIndex, poseSE3);
    }

    int ConnectedComponentPoseGraph::getComponentNumber() const {
        assert(componentNumber >= 0 && "component number not initialized");

        return componentNumber;
    }

    std::vector<SE3> ConnectedComponentPoseGraph::getAbsolutePoses() const {
        std::vector<SE3> absolutePosesToReturn;
        absolutePosesToReturn.reserve(getNumberOfPoses());

        for (const auto &vertex: poseGraph.getPoseVertices()) {
            absolutePosesToReturn.emplace_back(vertex.getAbsolutePoseSE3());
        }

        return absolutePosesToReturn;
    }

    int ConnectedComponentPoseGraph::getPoseIndexWithMaxConnectivity() const {
        return poseGraph.getPoseIndexWithMaxConnectivity();
    }

    const PoseGraph &ConnectedComponentPoseGraph::getPoseGraph() const {
        return poseGraph;
    }

    PosesForEvaluation ConnectedComponentPoseGraph::getPosesForEvaluation() const {

        const auto &fixedToZeroPose = poseGraph.getPoseVertex(poseGraph.getPoseIndexWithMaxConnectivity());

        assert(!getVertices().empty());

        return getPosesForEvaluation(SE3(fixedToZeroPose.getAbsolutePoseSE3().inverse()));
    }

    PosesForEvaluation ConnectedComponentPoseGraph::getPosesForEvaluation(const SE3 &poseFixedZeroInversed) const {

        return PosesForEvaluation(getVertices(), poseFixedZeroInversed);
    }
}


#include <boost/test/unit_test.hpp>

#include "SharedSecretFactory.hpp"
#include "ethcrypto/key/KeyGenerator.hpp"

using Ethereum::KeyGenerator;

BOOST_AUTO_TEST_SUITE(SharedSecretTest)

BOOST_AUTO_TEST_CASE(generate)
{

    Ethereum::KeyGenerator generator;
    Ethereum::KeyPair scan = generator.generate();
    Ethereum::KeyPair ephemeral = generator.generate();

    Ethereum::Stealth::SharedSecretFactory secretFactory;
    Ethereum::Stealth::SharedSecret secret = secretFactory.createFromScanPublic(scan.getPublicKey(), ephemeral.getPrivateKey());
    Ethereum::Stealth::SharedSecret secret2 = secretFactory.createFromScanSecret(scan.getPrivateKey(), ephemeral.getPublicKey());

    for(size_t i=0; i<secret.size(); i++)
    {
        BOOST_REQUIRE_EQUAL(secret[i], secret2[i]);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/** Boost */
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unordered_set.hpp>
#include <boost/serialization/utility.hpp>
#include <boost/serialization/vector.hpp>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

/** STL */
#include <chrono>
#include <condition_variable>
#include <future>
#include <mutex>
#include <thread>
#include <vector>

/** Ligero */
#include "Common.hpp"

#include "EncryptedClient.hpp"
#include "EncryptedCoordinator.hpp"

#include "ZkArgument.hpp"
#include "protocol/ExpressNPStatement.hpp"

#include "FiniteFields.hpp"
#include "Hash.hpp"
#include "SecretSharingNTT.hpp"

using namespace ligero;
using namespace std::chrono_literals;

/** Parameters RSA Ceremony */
/** ======================= */
constexpr auto degree = 1 << 16;
constexpr auto p = 9;
constexpr auto q = 21;
constexpr auto sigma = 8;
constexpr auto lambda = 128;
constexpr auto tau_limit_bit = 175;
constexpr auto tau = 1000;
constexpr auto numCandidates = 2048;

/** throughput parties */
constexpr auto wait_timeout = 300 * 1000; /* wait 5 min when start */
constexpr auto timeout = 25 * 1000;       /* a little longer than test */
constexpr auto data_size = 32 * 1024;     /* 32 KB */
constexpr auto nb_max_send = 1024;
constexpr auto parties = 1;

/** throughtput coordinator */
constexpr auto pbs = 1000;
constexpr auto duration = 20s;
constexpr auto wait_time = 120s;
constexpr auto cleanup_time = 5s;
constexpr auto throughput_cutoff = 100 * 1024; /* 200 KB/s */

constexpr auto receive_timeout = 20min;

/** Parameters Zero-Knowledge */
/** ========================= */

/** Settings Zero-Knowledge */
RegularInteger p_zk(10514644122014433281ULL);
typedef Fpp_Fixed<p_zk> FieldT;

zksnark::InterleavedRSCode irs;        /** Reed-Solomon Interleaved Code */
ligero::MTParameters<FieldT> mtParams; /** Merkle Tree Parameters */
constexpr size_t l = 4096;             /** Bloc size */
constexpr size_t k = 8192;             /** Degree */
constexpr size_t n = 16384;            /** # of Shares */

/** Columns to Open Up (By default half the columns) */
constexpr size_t t = 256;
constexpr auto ptNumberEvaluation = 3;

/** Random Generator */
const hash::HashIntegerSeedGeneratorF<FieldT>& fieldGenerator =
    hash::blake2bIntegerSeedGenerator<FieldT>;
const hash::HashStringSeedGeneratorF& stringGenerator = hash::blake2bStringHash;

/** Threading */
constexpr size_t maxNumberThreads = 50;
/** [ a potential target machine, m5.metal has 96 threads over 48 cores] */

/** Selecting a port for distributed verifying */
constexpr char* port = "5556";

/** Depositing the data here */
PublicData* pd = new PublicData;
PublicData& pdata = *pd;
SigmaProtocolPublicData spdata;
std::vector<ligero::zksnark::FullFSTranscript<FieldT>> transcripts;
size_t availableTranscriptsIndex = 0;
bool firstIteration = true;

std::string localIpAddr = "UNKNOWN";

/** Stitching Proofs */
constexpr int nbIterations = 3;
constexpr int nbVarsStitched = 4;
constexpr int nbBlocksPerVar = degree / l;

/** Data */
std::string hashedPublicData;

expected<Unit> verify(std::string basename) {
  /** Verifying */
  /** ====================================================================== */
  FieldT::modulusIdx_ = 0;
  size_t aggregateSuccess = 0;

  Statements statement = Statements::RSACeremony;
  Statements localStatement = statement;

  LOG(INFO) << "Fetching Sigma Data";
  fetchFromFile<SigmaProtocolPublicData>(spdata,
                                         basename + std::string(".sigmadata"));

  LOG(INFO) << "Fetching Public Data";
  fetchFromFile<PublicData>(pdata, basename + std::string(".publicdata"));

  LOG(INFO) << "Processing";
  /** Hashing Public Data */
  pdata.modulusIdx = 0;
  pdata.roots.clear();
  pdata.ptNumberEvaluation = ptNumberEvaluation;
  hashedPublicData = hash::hashPublicData(stringGenerator, pdata, spdata);

  /** Primitive Roots of Unity for All Moduli */
  nfl::poly_p<uint64_t, degree, q> roots({0});
  for (size_t idx = 0; idx < q; idx++) {
    roots(idx, 1) = 1ull;
  }
  roots.ntt_pow_phi();

  SecretData sdata;

  LOG(INFO) << "Preparing Scalars";
  /** Dimensioning ai scalars */
  sdata.ai.resize(nbIterations);
  for (size_t iter = 0; iter < nbIterations; iter++) {
    sdata.ai[iter].resize(nbVarsStitched);
    for (size_t var = 0; var < nbVarsStitched; var++) {
      sdata.ai[iter][var].resize(nbBlocksPerVar);
      for (size_t block = 0; block < nbBlocksPerVar; block++) {
        sdata.ai[iter][var][block].resize(l);
      }
    }
  }

  /** Storing the results of the verification per modulus */
  std::vector<int> report(q, 0);

  /** CRT representation to check integrity for the proofs */
  std::vector<mpz_class> stitchingCRT;
  mpz_class stitching0;

  for (size_t modulusIdx = 0; modulusIdx < q; modulusIdx++) {
    ligero::zksnark::FullFSTranscript<FieldT> transcript;

    LOG(INFO) << "Fetching Proof " << modulusIdx;
    fetchFromFile<ligero::zksnark::FullFSTranscript<FieldT>>(
        transcript,
        basename + std::string(".proof." + std::to_string(modulusIdx)));

    bool success = true;

    LOG(INFO) << "Verifying modulus idx:" << modulusIdx;

    // Zero-Knowledge
    p_zk = nfl::params<uint64_t>::P[modulusIdx];
    FieldT::modulusIdx_ = modulusIdx;
    FieldT::preprocessing(log2(n) + 1);

    pdata.modulusIdx = modulusIdx;

    /** For portions 7 to 12, only run the first 9 moduli */
    if ((localStatement == Statements::RSACeremony) && (pdata.modulusIdx > 8))
      localStatement = Statements::RSACeremony_Rounds3to6;

    /** Roots of Unity */
    pdata.roots.assign(
        roots.poly_obj().data() + pdata.modulusIdx * (size_t)roots.degree,
        roots.poly_obj().data() +
            (pdata.modulusIdx + 1) * (size_t)roots.degree);
    pdata.ptNumberEvaluation = ptNumberEvaluation;

    zksnark::Verifier<FieldT> verifier(
        localStatement, irs, t, l, fieldGenerator, stringGenerator, mtParams,
        pdata, spdata, sdata, permut<k>::compute, permut<n>::compute,
        hashedPublicData);

    try {
      verifier.verifyProof(transcript);

      LOG(INFO) << "Verifier mod " << modulusIdx << ":" << verifier._stitching;
      if (modulusIdx == 0) {
        stitching0 = verifier._stitching;
      } else {
        stitchingCRT.push_back(verifier._stitching);
      }
    } catch (failedTest& e) {
      DBG("Failed Test: " << e.what());
      success = false;
    }

    if (success) {
      LOG(INFO) << "Verification for modulus idx:" << modulusIdx
                << " succeeded";
      report[modulusIdx] = 1;
      aggregateSuccess++;
    } else {
      LOG(INFO) << "Verification for modulus idx:" << modulusIdx << " failed";
      report[modulusIdx] = 0;
    }
  }

  /** Cross-Moduli Verification */
  {
    bool success = false;
    std::vector<mpz_class> mods, coefs;
    for (size_t i = 1; i < q; i++) {
      mods.emplace_back(params<uint64_t>::P[i]);
    }

    success = (math::mod(stitching0 -
                             math::crt_reconstruct(stitchingCRT, coefs, mods),
                         mpz_class(params<uint64_t>::P[0])) == mpz_class(0));

    if (success) {
      LOG(INFO) << "Verification for cross-moduli integrity succeeded";
    } else {
      LOG(INFO) << "Verification for cross-moduli integrity failed";
      for (size_t idx = 0; idx < q; idx++) {
        report[idx] = 0;
      }
      aggregateSuccess = 0;
    }
  }

  if (aggregateSuccess == q) {
    LOG(INFO) << "All proofs successfully verified.";
  } else {
    LOG(INFO) << "Failed to verify some of the proofs.";
  }
}

int main(int argc, char** argv) {
  std::string proof;

  po::options_description desc("Allowed options");
  desc.add_options()("help", "produce help message")(
      "proof", po::value<std::string>(), "base filename for the proof");

  po::variables_map vm;
  po::positional_options_description p;

  po::store(
      po::command_line_parser(argc, argv).options(desc).positional(p).run(),
      vm);

  po::notify(vm);

  if (vm.count("help")) {
    LOG(INFO) << desc << "\n";
    LOG(INFO) << "USAGE: --proof <Proof basename>";
    return 0;
  }

  if (vm.count("proof")) {
    proof = vm["proof"].as<std::string>();
  }

  /** Set Zero-Knowledge Parameters */
  irs.k = k; /** Degree of the Polynomial */
  irs.n = n; /** Number of Shares */

  mtParams.digestLength = 32;
  mtParams.zkByteSize = 8;
  mtParams.hashInnerNodeF = hash::blake2bTwoToOneHash;
  mtParams.hashLeafContentF = hash::blake2bFieldElementHash<FieldT>;
  mtParams.hashZKRandomnessF = hash::blake2bZKElementHash;
  mtParams.leavesNumber = irs.n;

  //** Configure easylogging */
  configureEasyLogging("manual_verifier");

  if (hasError(verify(proof))) {
    LOG(TRACE) << "Failed to process the proofs, exiting now.";
    return EXIT_FAILURE;
  }

  return 0;
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#define BOOST_TEST_MODULE test_od_serialization

#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <cstdio>
#include <ml/neural_net/model_spec.hpp>
#include <toolkits/coreml_export/mlmodel_include.hpp>
#include <toolkits/object_detection/od_serialization.hpp>

namespace turi {
namespace object_detection {
namespace {

constexpr size_t NUM_ANCHOR_BOXES = 15;

BOOST_AUTO_TEST_CASE(test_init_darknet_yolo) {
  neural_net::model_spec nn_spec;
  const size_t num_classes = 10;
  init_darknet_yolo(nn_spec, num_classes, NUM_ANCHOR_BOXES, "image");

  const CoreML::Specification::NeuralNetwork& nn = nn_spec.get_coreml_spec();
  TS_ASSERT_EQUALS(nn.layers_size(), 32);
  /* we totally should have 9 conv layers
   * (conv_0 -> conv_8), 8 batch normalization layer (batch_0 -> batch_7),
   * 6 max pooling layers (pool_0 -> pool_5), and 8 relu layers (leakyrelu_0 ->
   * leakyrelu_7) so totally 31 layers.
   */
  int layer_num = 1;  // Skip the scale layer at the beginning.
  int num_features = 3;
  const std::map<int, int> layer_num_to_channels = {
      {0, 16},  {1, 32},  {2, 64},   {3, 128},
      {4, 256}, {5, 512}, {6, 1024}, {7, 1024}};
  for (auto const& x : layer_num_to_channels) {
    const auto& convlayer_ = nn.layers(layer_num);
    TS_ASSERT(convlayer_.has_convolution());
    TS_ASSERT_EQUALS(convlayer_.name(),
                     "conv" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(convlayer_.convolution().outputchannels(), x.second);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelchannels(), num_features);
    TS_ASSERT_EQUALS(convlayer_.convolution().stride(0), 1);
    TS_ASSERT_EQUALS(convlayer_.convolution().stride(1), 1);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(0), 3);
    TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(1), 3);
    TS_ASSERT(convlayer_.convolution().has_same());

    const auto& batchnormlayer_ = nn.layers(layer_num + 1);
    TS_ASSERT(batchnormlayer_.has_batchnorm());
    TS_ASSERT_EQUALS(batchnormlayer_.name(),
                     "batchnorm" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(batchnormlayer_.batchnorm().channels(), x.second);
    TS_ASSERT_EQUALS(batchnormlayer_.batchnorm().epsilon(), 0.00001f);

    const auto& relulayer_ = nn.layers(layer_num + 2);
    TS_ASSERT(relulayer_.has_activation());
    TS_ASSERT_EQUALS(relulayer_.name(),
                     "leakyrelu" + std::to_string(x.first) + "_fwd");
    TS_ASSERT_EQUALS(relulayer_.activation().leakyrelu().alpha(), 0.1f);

    // since the yolo model only has max pooling layers for layer 0 - 5
    int num_max_pooling_layers_in_yolo = 5;
    if (x.first <= num_max_pooling_layers_in_yolo) {
      const auto& poolinglayer_ = nn.layers(layer_num + 3);
      TS_ASSERT(poolinglayer_.has_pooling());
      TS_ASSERT_EQUALS(poolinglayer_.name(),
                       "pool" + std::to_string(x.first) + "_fwd");
      TS_ASSERT_EQUALS(poolinglayer_.pooling().kernelsize(0), 2);
      TS_ASSERT_EQUALS(poolinglayer_.pooling().kernelsize(1), 2);
      if (x.first == num_max_pooling_layers_in_yolo) {
        TS_ASSERT_EQUALS(poolinglayer_.pooling().stride(0), 1);
        TS_ASSERT_EQUALS(poolinglayer_.pooling().stride(1), 1);
        TS_ASSERT(poolinglayer_.pooling().has_same());
        TS_ASSERT(poolinglayer_.pooling().avgpoolexcludepadding());
      } else {
        TS_ASSERT_EQUALS(poolinglayer_.pooling().stride(0), 2);
        TS_ASSERT_EQUALS(poolinglayer_.pooling().stride(1), 2);
        TS_ASSERT(poolinglayer_.pooling().has_valid());
        TS_ASSERT(poolinglayer_.pooling().valid().has_paddingamounts());
        TS_ASSERT_EQUALS(poolinglayer_.pooling()
                             .valid()
                             .paddingamounts()
                             .borderamounts_size(),
                         2);
      }
      layer_num = layer_num + 4;
    } else {
      layer_num = layer_num + 3;
    }
    num_features = x.second;
  }
  // add check for the last layer
  const auto& convlayer_ = nn.layers(layer_num);
  TS_ASSERT(convlayer_.has_convolution());
  TS_ASSERT_EQUALS(convlayer_.name(),
                     "conv8_fwd");
  const size_t num_predictions =
      5 + num_classes;  // Per anchor box, including predict classes and
                        // bounding box regression
  const size_t conv8_c_out = NUM_ANCHOR_BOXES * num_predictions;
  TS_ASSERT_EQUALS(convlayer_.convolution().outputchannels(), conv8_c_out);
  TS_ASSERT_EQUALS(convlayer_.convolution().kernelchannels(), num_features);
  TS_ASSERT_EQUALS(convlayer_.convolution().stride(0), 1);
  TS_ASSERT_EQUALS(convlayer_.convolution().stride(1), 1);
  TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(0), 1);
  TS_ASSERT_EQUALS(convlayer_.convolution().kernelsize(1), 1);
  TS_ASSERT(convlayer_.convolution().has_same());
}

BOOST_AUTO_TEST_CASE(test_save_load) {
  // Create Test Model
  neural_net::model_spec nn_spec_1;
  std::map<std::string, variant_type> state1 = {
      {"num_classes", 10}, {"model", "darknet_yolo"}, {"max_iterations", 5}};
  init_darknet_yolo(nn_spec_1,
                    variant_get_value<size_t>(state1.at("num_classes")),
                    NUM_ANCHOR_BOXES, "image");

  // Save it
  dir_archive archive_write;
  archive_write.open_directory_for_write("serialized_save_load_tests");
  turi::oarchive oarc(archive_write);
  _save_impl(oarc, state1, nn_spec_1.export_params_view());
  archive_write.close();

  // Load it
  dir_archive archive_read;
  archive_read.open_directory_for_read("serialized_save_load_tests");
  turi::iarchive iarc(archive_read);
  size_t version = 1;
  neural_net::float_array_map weights;
  neural_net::model_spec nn_spec_2;
  std::map<std::string, variant_type> state2 = {
      {"num_classes", 10}, {"model", "darknet_yolo"}, {"max_iterations", 5}};
  _load_version(iarc, version, &state2, &weights);
  archive_read.close();
  init_darknet_yolo(nn_spec_2,
                    variant_get_value<size_t>(state2.at("num_classes")),
                    NUM_ANCHOR_BOXES, "image");
  nn_spec_2.update_params(weights);

  // Compare saved and loaded models
  const CoreML::Specification::NeuralNetwork& nn_saved =
      nn_spec_1.get_coreml_spec();
  const CoreML::Specification::NeuralNetwork& nn_loaded =
      nn_spec_2.get_coreml_spec();
  TS_ASSERT(nn_saved.SerializeAsString() == nn_loaded.SerializeAsString());
}

}  // namespace
}  // namespace object_detection
}  // namespace turi


//---------------------------------------------------------------------------//
//!
//! \file   tstElectronState.cpp
//! \author Luke Kersting
//! \brief  Electron state unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#include <boost/shared_ptr.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "Utility_PhysicalConstants.hpp"

//---------------------------------------------------------------------------//
// Tests.
//---------------------------------------------------------------------------//
// Get the particle type
TEUCHOS_UNIT_TEST( ElectronState, getParticleType )
{
  MonteCarlo::ElectronState particle( 1ull );

  TEST_EQUALITY_CONST( particle.getParticleType(), MonteCarlo::ELECTRON );
}

//---------------------------------------------------------------------------//
// Get the particle speed
TEUCHOS_UNIT_TEST( ElectronState, getSpeed )
{
  MonteCarlo::ElectronState particle( 1ull );

  particle.setEnergy( 1.0 );

  double speed_of_light = Utility::PhysicalConstants::speed_of_light;
  double rest_mass = Utility::PhysicalConstants::electron_rest_mass_energy;

  TEST_FLOATING_EQUALITY( particle.getSpeed(),
                          speed_of_light * sqrt( 1.0 - rest_mass * rest_mass /
                          ((1.0 + rest_mass) * (1.0 + rest_mass))),
                          1e-12 );
}

//---------------------------------------------------------------------------//
// Set the particle speed
TEUCHOS_UNIT_TEST( ElectronState, setSpeed )
{
  MonteCarlo::ElectronState particle( 1ull );

  double speed_of_light = Utility::PhysicalConstants::speed_of_light;
  double rest_mass = Utility::PhysicalConstants::electron_rest_mass_energy;

  particle.setSpeed( speed_of_light * sqrt( 1 - rest_mass * rest_mass /
                     ((1 + rest_mass) * (1 + rest_mass))) );

  TEST_FLOATING_EQUALITY( particle.getEnergy(), 1.0, 1e-12 );
}

//---------------------------------------------------------------------------//
// Advance the particle along its direction by a specified distance
TEUCHOS_UNIT_TEST( ElectronState, advance )
{
  const double position[3] = {1.0, 1.0, 1.0};
  const double direction[3] = {0.5773502691896258,
			       0.5773502691896258,
			       0.5773502691896258};

  MonteCarlo::ElectronState particle( 1ull );
  particle.setPosition( position );
  particle.setDirection( direction );
  particle.setEnergy( 1.0 );
  particle.setTime( 0.0 );

  particle.advance( 1.7320508075688772 );

  double speed_of_light = Utility::PhysicalConstants::speed_of_light;
  double rest_mass = Utility::PhysicalConstants::electron_rest_mass_energy;

  TEST_FLOATING_EQUALITY( particle.getXPosition(), 2.0, 1e-12 );
  TEST_FLOATING_EQUALITY( particle.getYPosition(), 2.0, 1e-12 );
  TEST_FLOATING_EQUALITY( particle.getZPosition(), 2.0, 1e-12 );
  TEST_FLOATING_EQUALITY( particle.getTime(),
                          1.7320508075688772 / ( speed_of_light * sqrt( 1 - rest_mass * rest_mass /
                          ((1 + rest_mass) * (1 + rest_mass)))),
                          1e-12 );
}

//---------------------------------------------------------------------------//
// Check that the electron state can be cloned
TEUCHOS_UNIT_TEST( ElectronState, clone )
{
  boost::shared_ptr<MonteCarlo::ParticleState> particle(
				       new MonteCarlo::ElectronState( 0ull ) );
  particle->setPosition( 1.0, 1.0, 1.0 );
  particle->setDirection( 0.0, 0.0, 1.0 );
  particle->setEnergy( 1.0 );
  particle->setTime( 0.5 );
  particle->setWeight( 0.25 );

  boost::shared_ptr<MonteCarlo::ParticleState> particle_clone(
							   particle->clone() );

  TEST_EQUALITY_CONST( particle_clone->getXPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getYPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getZPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getXDirection(), 0.0 );
  TEST_EQUALITY_CONST( particle_clone->getYDirection(), 0.0 );
  TEST_EQUALITY_CONST( particle_clone->getZDirection(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getEnergy(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getTime(), 0.5 );
  TEST_EQUALITY_CONST( particle_clone->getCollisionNumber(), 0 );
  TEST_EQUALITY_CONST( particle_clone->getGenerationNumber(), 0 );
  TEST_EQUALITY_CONST( particle_clone->getWeight(), 0.25 );
  TEST_EQUALITY_CONST( particle_clone->getHistoryNumber(), 0ull );
  TEST_EQUALITY_CONST( particle_clone->getParticleType(),
		       MonteCarlo::ELECTRON );
}

//---------------------------------------------------------------------------//
// Check that the electron state can be cloned with a new history number
TEUCHOS_UNIT_TEST( ElectronState, clone_new_hist )
{
  boost::shared_ptr<MonteCarlo::ParticleState> particle(
				       new MonteCarlo::ElectronState( 0ull ) );
  particle->setPosition( 1.0, 1.0, 1.0 );
  particle->setDirection( 0.0, 0.0, 1.0 );
  particle->setEnergy( 1.0 );
  particle->setTime( 0.5 );
  particle->setWeight( 0.25 );

  boost::shared_ptr<MonteCarlo::ParticleState> particle_clone(
						    particle->clone( 10ull ) );

  TEST_EQUALITY_CONST( particle_clone->getXPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getYPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getZPosition(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getXDirection(), 0.0 );
  TEST_EQUALITY_CONST( particle_clone->getYDirection(), 0.0 );
  TEST_EQUALITY_CONST( particle_clone->getZDirection(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getEnergy(), 1.0 );
  TEST_EQUALITY_CONST( particle_clone->getTime(), 0.5 );
  TEST_EQUALITY_CONST( particle_clone->getCollisionNumber(), 0 );
  TEST_EQUALITY_CONST( particle_clone->getGenerationNumber(), 0 );
  TEST_EQUALITY_CONST( particle_clone->getWeight(), 0.25 );
  TEST_EQUALITY_CONST( particle_clone->getHistoryNumber(), 10ull );
  TEST_EQUALITY_CONST( particle_clone->getParticleType(),
		       MonteCarlo::ELECTRON );
}

//---------------------------------------------------------------------------//
// Archive an electron state
TEUCHOS_UNIT_TEST( ElectronState, archive )
{
  // Create and archive an electron
  {
    MonteCarlo::ElectronState particle( 1ull );
    particle.setPosition( 1.0, 1.0, 1.0 );
    particle.setDirection( 0.0, 0.0, 1.0 );
    particle.setEnergy( 1.0 );
    particle.setTime( 0.5 );
    particle.incrementCollisionNumber();
    particle.setWeight( 0.25 );

    std::ofstream ofs( "test_electron_state_archive.xml" );

    boost::archive::xml_oarchive ar(ofs);
    ar << BOOST_SERIALIZATION_NVP( particle );
  }

  // Load the archived particle
  MonteCarlo::ElectronState loaded_particle;

  std::ifstream ifs( "test_electron_state_archive.xml" );

  boost::archive::xml_iarchive ar(ifs);
  ar >> boost::serialization::make_nvp( "particle", loaded_particle );

  TEST_EQUALITY_CONST( loaded_particle.getXPosition(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getYPosition(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getZPosition(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getXDirection(), 0.0 );
  TEST_EQUALITY_CONST( loaded_particle.getYDirection(), 0.0 );
  TEST_EQUALITY_CONST( loaded_particle.getZDirection(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getEnergy(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getTime(), 0.5 );
  TEST_EQUALITY_CONST( loaded_particle.getCollisionNumber(), 1.0 );
  TEST_EQUALITY_CONST( loaded_particle.getGenerationNumber(), 0.0 );
  TEST_EQUALITY_CONST( loaded_particle.getWeight(), 0.25 );
  TEST_EQUALITY_CONST( loaded_particle.getHistoryNumber(), 1ull );
  TEST_EQUALITY_CONST( loaded_particle.getParticleType(),
		       MonteCarlo::ELECTRON );
}

//---------------------------------------------------------------------------//
// Create new particles
TEUCHOS_UNIT_TEST( ElectronState, copy_constructor )
{
  MonteCarlo::ElectronState particle_gen_a( 1ull );
  particle_gen_a.setPosition( 1.0, 1.0, 1.0 );
  particle_gen_a.setDirection( 0.0, 0.0, 1.0 );
  particle_gen_a.setEnergy( 1.0 );
  particle_gen_a.setTime( 1.0 );
  particle_gen_a.incrementCollisionNumber();
  particle_gen_a.setWeight( 0.5 );

  MonteCarlo::ElectronState particle_gen_a_copy( particle_gen_a );

  TEST_EQUALITY( particle_gen_a_copy.getXPosition(),
		 particle_gen_a.getXPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getYPosition(),
		 particle_gen_a.getYPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getZPosition(),
		 particle_gen_a.getZPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getXDirection(),
		 particle_gen_a.getXDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getYDirection(),
		 particle_gen_a.getYDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getZDirection(),
		 particle_gen_a.getZDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getEnergy(),
		 particle_gen_a.getEnergy() );
  TEST_EQUALITY( particle_gen_a_copy.getSpeed(),
		 particle_gen_a.getSpeed() );
  TEST_EQUALITY( particle_gen_a_copy.getTime(),
		 particle_gen_a.getTime() );
  TEST_EQUALITY( particle_gen_a_copy.getCollisionNumber(),
		 particle_gen_a.getCollisionNumber() );
  TEST_EQUALITY( particle_gen_a_copy.getGenerationNumber(),
		 particle_gen_a.getGenerationNumber() );
  TEST_EQUALITY( particle_gen_a_copy.getWeight(),
		 particle_gen_a.getWeight() );

  //Create a second generation particle with the same collision number
  MonteCarlo::ElectronState particle_gen_b( particle_gen_a, true );

  TEST_EQUALITY( particle_gen_b.getXPosition(),
		 particle_gen_a.getXPosition() );
  TEST_EQUALITY( particle_gen_b.getYPosition(),
		 particle_gen_a.getYPosition() );
  TEST_EQUALITY( particle_gen_b.getZPosition(),
		 particle_gen_a.getZPosition() );
  TEST_EQUALITY( particle_gen_b.getXDirection(),
		 particle_gen_a.getXDirection() );
  TEST_EQUALITY( particle_gen_b.getYDirection(),
		 particle_gen_a.getYDirection() );
  TEST_EQUALITY( particle_gen_b.getZDirection(),
		 particle_gen_a.getZDirection() );
  TEST_EQUALITY( particle_gen_b.getEnergy(),
		 particle_gen_a.getEnergy() );
  TEST_EQUALITY( particle_gen_b.getSpeed(),
		 particle_gen_a.getSpeed() );
  TEST_EQUALITY( particle_gen_b.getTime(),
		 particle_gen_a.getTime() );
  TEST_EQUALITY( particle_gen_b.getCollisionNumber(),
		 particle_gen_a.getCollisionNumber() );
  TEST_EQUALITY( particle_gen_b.getGenerationNumber(),
		 particle_gen_a.getGenerationNumber()+1u );
  TEST_EQUALITY( particle_gen_b.getWeight(),
		 particle_gen_a.getWeight() );

  // Create a third generation particle and reset the collision counter
  MonteCarlo::ElectronState particle_gen_c( particle_gen_b, true, true );

  TEST_EQUALITY( particle_gen_c.getXPosition(),
		 particle_gen_b.getXPosition() );
  TEST_EQUALITY( particle_gen_c.getYPosition(),
		 particle_gen_b.getYPosition() );
  TEST_EQUALITY( particle_gen_c.getZPosition(),
		 particle_gen_b.getZPosition() );
  TEST_EQUALITY( particle_gen_c.getXDirection(),
		 particle_gen_b.getXDirection() );
  TEST_EQUALITY( particle_gen_c.getYDirection(),
		 particle_gen_b.getYDirection() );
  TEST_EQUALITY( particle_gen_c.getZDirection(),
		 particle_gen_b.getZDirection() );
  TEST_EQUALITY( particle_gen_c.getEnergy(),
		 particle_gen_b.getEnergy() );
  TEST_EQUALITY( particle_gen_c.getSpeed(),
		 particle_gen_b.getSpeed() );
  TEST_EQUALITY( particle_gen_c.getTime(),
		 particle_gen_b.getTime() );
  TEST_EQUALITY_CONST( particle_gen_c.getCollisionNumber(), 0u );
  TEST_EQUALITY( particle_gen_c.getGenerationNumber(),
		 particle_gen_b.getGenerationNumber()+1u );
  TEST_EQUALITY( particle_gen_c.getWeight(),
		 particle_gen_b.getWeight() );
}

//---------------------------------------------------------------------------//
// Create new particles
TEUCHOS_UNIT_TEST( ElectronState, assignment_operator )
{
  MonteCarlo::ElectronState particle_gen_a( 1ull );
  particle_gen_a.setPosition( 1.0, 1.0, 1.0 );
  particle_gen_a.setDirection( 0.0, 0.0, 1.0 );
  particle_gen_a.setEnergy( 1.0 );
  particle_gen_a.setTime( 1.0 );
  particle_gen_a.incrementCollisionNumber();
  particle_gen_a.setWeight( 0.5 );

  MonteCarlo::ElectronState particle_gen_a_copy = particle_gen_a;

  TEST_EQUALITY( particle_gen_a_copy.getXPosition(),
		 particle_gen_a.getXPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getYPosition(),
		 particle_gen_a.getYPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getZPosition(),
		 particle_gen_a.getZPosition() );
  TEST_EQUALITY( particle_gen_a_copy.getXDirection(),
		 particle_gen_a.getXDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getYDirection(),
		 particle_gen_a.getYDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getZDirection(),
		 particle_gen_a.getZDirection() );
  TEST_EQUALITY( particle_gen_a_copy.getEnergy(),
		 particle_gen_a.getEnergy() );
  TEST_EQUALITY( particle_gen_a_copy.getTime(),
		 particle_gen_a.getTime() );
  TEST_EQUALITY( particle_gen_a_copy.getCollisionNumber(),
		 particle_gen_a.getCollisionNumber() );
  TEST_EQUALITY( particle_gen_a_copy.getGenerationNumber(),
		 particle_gen_a.getGenerationNumber() );
  TEST_EQUALITY( particle_gen_a_copy.getWeight(),
		 particle_gen_a.getWeight() );
}

//---------------------------------------------------------------------------//
// end tstElectronState.cpp
//---------------------------------------------------------------------------//


#include <bits/stdc++.h>
#include <boost/date_time.hpp>
#include <boost/date_time/date.hpp>
#include "boost/date_time/posix_time/posix_time.hpp"

using namespace std;
using namespace boost;
using namespace boost::posix_time;
using namespace boost::gregorian;

#define error(args...) { string _s = #args; \
replace(_s.begin(), _s.end(), ',', ' '); \
stringstream _ss(_s); \
istream_iterator<string> _it(_ss); \
err(_it, args); }

void err(istream_iterator<string> it) {}

template<typename T, typename... Args>
void err(istream_iterator<string> it, T a, Args... args) {
    cerr << *it << " = " << a << endl;
    err(++it, args...);
}

template<typename... Args>
auto Func(Args... args) {
    return (args + ...);
}

void stupidSort(vector<int> &nums) {
    vector<int> idx(nums.size());
    iota(idx.begin(), idx.end(), 0);
    sort(idx.begin(), idx.end(), [&](int i, int k) {
        return floor(log10(nums.at(i))) < floor(log10(nums[k])) || (i < k);
    });
    vector<int> temp;
    temp.reserve(nums.size());
    for (auto i : idx) {
        temp.emplace_back(nums[i]);
    }
    std::copy(temp.begin(), temp.end(), nums.begin());
}

int findLongestChain(vector<pair<int, int>> pairs) {
    sort(pairs.begin(), pairs.end(), [](pair<int, int> p1, pair<int, int> p2) {
        return p1.first < p2.first || p1.second < p2.second;
    });
    vector<int> dp;
    dp.reserve(pairs.size());

    int i = 0;
    for (auto &p : pairs) {
        auto p2 = lower_bound(dp.begin(), dp.end(), p.second);
        if (p2 == dp.end() || p2 == dp.begin()) {
            dp.emplace_back(1);
        } else {
            dp.emplace_back(*(p2 - 1) + 1);
        }
    }
    return dp[pairs.size() - 1];
}

int main(int argc, char **argv) {
    cout << "Hello World!" << endl;
    return 0;
}


#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>
#include <boost/functional/hash.hpp>

int main(int argc, char **argv) {
  ros::init(argc, argv, "publish_velocity");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);

  // initialize rand with current time and node name
  std::string name = ros::this_node::getName();
  const int seed = time(NULL) + boost::hash<std::string>()(name);
  srand(seed);

  ros::Rate rate(2);

  while(ros::ok()) {
    geometry_msgs::Twist msg;

    // assign random numbers in the range [-1,1] to x and y velocity
    msg.linear.x = 2 * double(rand())/double(RAND_MAX) - 1;
    msg.linear.y = 2 * double(rand())/double(RAND_MAX) - 1;

    pub.publish(msg);
    // ROS_INFO_STREAM("x= " << msg.linear.x << "y= " << msg.linear.y);
    rate.sleep();
  }
}


//
// Created by 关鑫 on 2018/9/15.
//

#include <cstdio>
#include <iostream>
#include <cstdint>
#include <cstdlib>
#include <functional>
#include <memory>
#include <vector>
#include <set>
#include <map>
#include <queue>
#include <unordered_map>
#include <chrono>

#include <boost/heap/fibonacci_heap.hpp>

namespace st = std::chrono;

typedef int32_t UNIT;
typedef int8_t UNIT2;

static size_t PEAK;

template <typename _Tp>
class MyAllocator : public std::allocator< _Tp > {
public:
    typedef size_t            size_type;
    typedef ptrdiff_t         difference_type;
    typedef _Tp*              pointer;
    typedef const _Tp*        const_pointer;
    typedef _Tp&              reference;
    typedef const _Tp&        const_reference;
    typedef _Tp               value_type;

    template <class U1> struct rebind { typedef MyAllocator<U1> other; };

    MyAllocator() : total(0), peak(0) {}
    MyAllocator( const std::allocator<_Tp>& alloc ) : total(0), peak(0) {
    }
    ~MyAllocator() {
        PEAK += get_peak();
    }

    pointer allocate(size_type __n, std::allocator<void>::const_pointer = 0)
    {
        size_type cur = __n * sizeof(_Tp);
        total += cur;
        if( total > peak ) peak = total;
        // printf( "alloc %zd bytes, now total %zd bytes\n", cur, total );
        return std::allocator< _Tp >::allocate( __n );
    }

    void deallocate(pointer __p, size_type __n )
    {
        size_type cur = __n * sizeof(_Tp);
        total -= cur;
        // printf( "dealloc %zd bytes, now total %zd bytes\n", cur, total );
        return std::allocator< _Tp >::deallocate( __p, __n );
    }

    const size_type get_peak() { return peak; }

private:
    size_type total;
    size_type peak;
};

void benchmark( const std::function< void() > &func ) {
    PEAK = 0;
    auto beg = st::high_resolution_clock::now();
    func();
    auto end = st::high_resolution_clock::now();
    printf( "memory_peak:%10zd time:%10lld\n", PEAK, st::duration_cast< st::microseconds >(end - beg).count() );
}

void testVector( int N ) {
    std::vector< int, MyAllocator< int > > v1;
    for( int i = 0; i < N; ++i ) v1.push_back( i );
    for( int i = 0; i < N * 10; ++i ) auto x = v1[rand() % 100];
    for( int i = 0; i < N; ++i ) v1.pop_back();
}

void testMap( int N ) {
    std::map< UNIT, UNIT2, std::less<UNIT>, MyAllocator< std::pair< const UNIT, UNIT2 > > > m1;
    for( int i = 0; i < N; ++i ) m1.insert( std::make_pair( i, i ) );
    for( int i = 0; i < N * 10; ++i ) auto x = m1[rand() % 100];
    for( int i = 0; i < N; ++i ) m1.erase( i );
}

void testUnorderedmap( int N ) {
    std::unordered_map< UNIT, UNIT2, std::hash<UNIT>, std::equal_to<UNIT>, MyAllocator< std::pair< const UNIT, UNIT2 > > > m2;
    for( int i = 0; i < N; ++i ) m2.insert( std::make_pair( i, i ) );
    for( int i = 0; i < N * 10; ++i ) auto x = m2[rand() % 100];
    for( int i = 0; i < N; ++i ) m2.erase( i );
}

void testSet( int N ) {
    std::set< UNIT, std::less< UNIT >, MyAllocator< UNIT > > s1;
    for( int i = 0; i < N; ++i ) s1.insert( i );
    for( int i = 0; i < N * 10; ++i ) auto x = *s1.cbegin();
    for( int i = 0; i < N; ++i ) s1.erase( i );
}

void testPriorityQueue( int N ) {
    std::priority_queue< UNIT, std::vector< UNIT, MyAllocator< UNIT > > > q1;

    for( int i = 0; i < N; ++i ) q1.push( i );
    for( int i = 0; i < N * 10; ++i ) auto x = q1.top();
    for( int i = 0; i < N; ++i ) q1.pop();
}

void testFibonacci( int N ) {
    boost::heap::fibonacci_heap< UNIT, MyAllocator< UNIT > > h1;

    for( int i = 0; i < N; ++i ) h1.push( i );
    for( int i = 0; i < N * 10; ++i ) auto x = h1.top();
    for( int i = 0; i < N; ++i ) h1.pop();
}

template <typename T>
int sign(T v)
{
    if (v > 0)
        return 1;
    else if (v < 0)
        return -1;
    else
        return 0;
}

void TraverseLine(int32_t xs, int32_t ys, int32_t xe, int32_t ye, std::function<void (int32_t , int32_t , bool&)> handle8Connect, std::function<void (int32_t , int32_t , bool&)> handle4Connect)
{
    int32_t x = xs, y = ys;
    int32_t dx = abs(xe - xs);
    int32_t dy = abs(ye - ys);
    int32_t sx = sign(xe - xs);
    int32_t sy = sign(ye - ys);
    bool flag;
    if (dy > dx)
    {
        std::swap(dx, dy);
        flag = 1;
    }
    else
        flag = 0;

    int32_t nerror = 2 * dy - dx;

    bool stopTraverse = false;
    for (int32_t i = 0; i <= dx; i++)
    {
        handle8Connect(x, y, stopTraverse);
        if(stopTraverse)
            break;

        if (nerror >= 0)
        {
            if (flag)
                x = x + sx;
            else
                y = y + sy;
            nerror = nerror - 2 * dx;

            if (handle4Connect != nullptr)
            {
                handle4Connect(x, y, stopTraverse);
                if(stopTraverse)
                    break;
            }
        }
        if (flag)
            y = y + sy;
        else
            x = x + sx;
        nerror = nerror + 2 * dy;
    }
}

int main() {
    srand( time( nullptr ) );
    std::vector<int> arr{ 1, 10, 100, 1000, 10000, 100000, 1000000 };
    for( auto n : arr ) {
        printf( "========== TEST %d ELEMENTS ============\n", n );
        printf( "std::vector                 " ); benchmark( [=]() { testVector( n ); } );

        printf( "std::map                    " ); benchmark( [=]() { testMap( n ); } );
        printf( "std::unordered_map          " ); benchmark( [=]() { testUnorderedmap( n ); } );

        printf( "std::set                    " ); benchmark( [=]() { testSet( n ); } );
        printf( "std::priority_queue         " ); benchmark( [=]() { testPriorityQueue( n ); } );
        printf( "boost::heap::fibonacci_heap " ); benchmark( [=]() { testFibonacci( n ); } );
        printf( "\n" );
    }

//    printf( "handle size %zd\n", sizeof(typename boost::heap::fibonacci_heap< UNIT, MyAllocator< UNIT > >::handle_type) );

//    auto func8 = [](int32_t x, int32_t y, bool&) {
//        printf( "(%d,%d) 8\n", x, y );
//    };
//
//    auto func4 = [](int32_t x, int32_t y, bool&) {
//        printf( "(%d,%d) 4\n", x, y );
//    };
//
//    TraverseLine( 1, 1, 5, 8, func8, func4 );

}


/**
 *  Project: The Stock Libraries
 *
 *  File: image.hpp
 *  Created: Jun 18, 2012
 *
 *  Author: Abhinav Sarje <abhinav.sarje@gmail.com>
 *
 *  Copyright (c) 2012-2017 Abhinav Sarje
 *  Distributed under the Boost Software License.
 *  See accompanying LICENSE file.
 */

#ifndef __IMAGE_HPP__
#define __IMAGE_HPP__

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/numeric/affine.hpp>

#include "globals.hpp"
#include "colormap.hpp"
#include "typedefs.hpp"

namespace stock {

	/**
	 * The main image class
	 */
	class Image {
		//template <typename ChannelValue, typename Layout> struct pixel;
		//typedef pixel<bits8, rgb_layout_t> rgb8_pixel_t;

		private:
			unsigned int nx_;				/* x dimension - used in 3D image construction */
			unsigned int ny_;				/* y dimension */
			unsigned int nz_;				/* z dimension */
			boost::gil::rgb8_pixel_t* image_buffer_;	/* this will hold the final rgb values */
			ColorMap8 color_map_8_;			/* defines mapping to colors in the defined palette */
			ColorMap color_map_;			/* better color mapping */

			bool convert_to_rgb_pixels(unsigned int, unsigned int, real_t*);
			bool convert_to_rgb_palette(unsigned int, unsigned int, real_t*);
			bool slice(Image* &img, unsigned int xval = 0);	/* obtain a slice at given x in case of 3D data */

			bool translate_pixels_to_positive(unsigned int nx, unsigned int ny, real_t* &data);
			bool normalize_pixels(unsigned int nx, unsigned int ny, real_t* &data);
			vector2_t minmax(unsigned int n, real_t* data);

		public:
			Image(unsigned int ny, unsigned int nz);					/* initialize a 2D image object */
			Image(unsigned int ny, unsigned int nz, char* palette);
			Image(unsigned int ny, unsigned int nz, std::string palette);
			Image(unsigned int ny, unsigned int nz, unsigned int r, unsigned int g, unsigned int b);
			Image(unsigned int nx, unsigned int ny, unsigned int nz);	/* initialize a 3D image object */
			Image(unsigned int nx, unsigned int ny, unsigned int nz, char* palette);
			Image(unsigned int nx, unsigned int ny, unsigned int nz, std::string palette);
			Image(unsigned int nx, unsigned int ny, unsigned int nz,
					unsigned int r, unsigned int g, unsigned int b);
			~Image();

			bool construct_image(const real_t* data, int slice);
			bool construct_log_image(real_t* data);
			bool construct_image(real_t* data);
			bool construct_palette(real_t* data);
			bool save(std::string filename);			/* save the current image buffer */
			bool save(std::string filename, int xval);	/* save slice xval */
			bool save(char* filename, int xval);
			bool save(std::string filename, int xbegin, int xend);	/* save slices from xbegin to xend */
			bool save(char* filename, int xbegin, int xend);

	}; // class Image

	bool scale_image(int, int, int, int, real_t*, real_t*&);
	bool resample_pixels(int, int, real_t*, int, int, real_t*&, const boost::gil::matrix3x2<real_t>&);

} // namespace stock

#endif /* __IMAGE_HPP__ */


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/core/lightweight_test.hpp>

using namespace boost;

template < typename Graph > void reset_edge_index(Graph& g)
{
    typename property_map< Graph, edge_index_t >::type index
        = get(edge_index, g);
    typename graph_traits< Graph >::edge_iterator ei, ei_end;
    typename graph_traits< Graph >::edges_size_type cnt = 0;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(index, *ei, cnt++);
}

template < typename Graph > void make_line_graph(Graph& g, int size)
{
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;

    vertex_t prev_vertex = add_vertex(g);

    for (int i = 1; i < size; ++i)
    {
        vertex_t curr_vertex = add_vertex(g);
        add_edge(curr_vertex, prev_vertex, g);
        prev_vertex = curr_vertex;
    }
}

struct UpdateVertexIndex
{
    template < typename Graph > void update(Graph& g)
    {
        typename property_map< Graph, vertex_index_t >::type index
            = get(vertex_index, g);
        typename graph_traits< Graph >::vertex_iterator vi, vi_end;
        typename graph_traits< Graph >::vertices_size_type cnt = 0;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
            put(index, *vi, cnt++);
    }
};

struct NoVertexIndexUpdater
{
    template < typename Graph > void update(Graph&) {}
};

template < typename Graph, typename VertexIndexUpdater >
void test_line_graph(VertexIndexUpdater vertex_index_updater, int size)
{

    Graph g;
    make_line_graph(g, size);
    vertex_index_updater.update(g);
    reset_edge_index(g);

    typedef std::vector< typename graph_traits< Graph >::edge_descriptor >
        edge_vector_t;
    typedef std::vector< edge_vector_t > embedding_storage_t;
    typedef iterator_property_map< typename embedding_storage_t::iterator,
        typename property_map< Graph, vertex_index_t >::type >
        embedding_t;

    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

    typename graph_traits< Graph >::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        std::copy(out_edges(*vi, g).first, out_edges(*vi, g).second,
            std::back_inserter(embedding[*vi]));

    BOOST_TEST(biconnected_components(
                    g, make_vector_property_map< int >(get(edge_index, g)))
        > 1);
    BOOST_TEST(boyer_myrvold_planarity_test(g));
    make_biconnected_planar(g, embedding);
    reset_edge_index(g);
    BOOST_TEST(biconnected_components(
                    g, make_vector_property_map< int >(get(edge_index, g)))
        == 1);
    BOOST_TEST(boyer_myrvold_planarity_test(g));
}

int main(int, char*[])
{
    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        VVgraph_t;

    typedef adjacency_list< vecS, listS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        VLgraph_t;

    typedef adjacency_list< listS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        LVgraph_t;

    typedef adjacency_list< listS, listS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        LLgraph_t;

    typedef adjacency_list< setS, setS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        SSgraph_t;

    test_line_graph< VVgraph_t >(NoVertexIndexUpdater(), 10);
    test_line_graph< VVgraph_t >(NoVertexIndexUpdater(), 50);

    test_line_graph< VLgraph_t >(UpdateVertexIndex(), 3);
    test_line_graph< VLgraph_t >(UpdateVertexIndex(), 30);

    test_line_graph< LVgraph_t >(NoVertexIndexUpdater(), 15);
    test_line_graph< LVgraph_t >(NoVertexIndexUpdater(), 45);

    test_line_graph< LLgraph_t >(UpdateVertexIndex(), 8);
    test_line_graph< LLgraph_t >(UpdateVertexIndex(), 19);

    test_line_graph< SSgraph_t >(UpdateVertexIndex(), 13);
    test_line_graph< SSgraph_t >(UpdateVertexIndex(), 20);

    return boost::report_errors();
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_UTILITY_MT19937STREAM_HPP_INCLUDED
#define NT2_CORE_UTILITY_MT19937STREAM_HPP_INCLUDED

#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/details/rands.hpp>
#include <nt2/core/utility/randstream.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace nt2
{
  class mt19937stream_ : public randstream_
  {
    public:
    NT2_DEFINE_RAND(mt19937stream_);

    private:
    boost::random::mt19937 generator_;
  };

  NT2_CORE_RANDOM_DECL nt2::randstream_* mt19937stream();
}

#endif


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern feature spatial_attribute
#include <boost/test/unit_test.hpp>
#include "fern/language/feature/core/attributes.h"

namespace fl = fern::language;

BOOST_AUTO_TEST_CASE(int_per_box)
{
    using FieldAttribute = fl::SpatialAttribute<fl::FieldDomain, int>;

    // // An integer value is stored per box.
    fl::d2::Point south_west;
    fern::set<0>(south_west, 1.1);
    fern::set<1>(south_west, 2.2);
    fl::d2::Point north_east;
    fern::set<0>(north_east, 3.3);
    fern::set<1>(north_east, 4.4);
    fl::d2::Box box(south_west, north_east);

    int value = 5;

    FieldAttribute attribute;
    BOOST_CHECK(attribute.empty());

    FieldAttribute::GID gid = attribute.add(box, value);
    BOOST_CHECK(!attribute.empty());
    BOOST_CHECK_EQUAL(attribute.domain().size(), 1u);
    BOOST_CHECK_EQUAL(attribute.values().value(gid), value);
}

BOOST_AUTO_TEST_CASE(array_per_box)
{
    // A pointer to a 2D array is stored as value per box.
    using Value = fl::d2::ArrayValue<int>;
    using ValuePtr = fl::d2::ArrayValuePtr<int>;
    using FieldAttribute = fl::SpatialAttribute<fl::FieldDomain, ValuePtr>;

    fl::d2::Point south_west;
    fern::set<0>(south_west, 1.1);
    fern::set<1>(south_west, 2.2);
    fl::d2::Point north_east;
    fern::set<0>(north_east, 3.3);
    fern::set<1>(north_east, 4.4);
    fl::d2::Box box(south_west, north_east);

    size_t const nr_rows = 3;
    size_t const nr_cols = 2;
    ValuePtr grid(std::make_shared<Value>(fern::extents[nr_rows][nr_cols]));
    (*grid)[0][0] = -2;
    (*grid)[0][1] = -1;
    (*grid)[1][0] = 0;
    (*grid)[1][1] = 999;
    (*grid)[2][0] = 1;
    (*grid)[2][1] = 2;

    FieldAttribute attribute;
    BOOST_CHECK(attribute.empty());

    FieldAttribute::GID gid = attribute.add(box, grid);
    BOOST_CHECK(!attribute.empty());
    BOOST_CHECK_EQUAL(attribute.domain().size(), 1u);
    // TODO BOOST_CHECK(*attribute.values().value(gid) == *grid);
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "BinarySystem.h"

#include "DensityOperator.h"
#include "Interaction.h"
#include "StateVector.h"

#include "Algorithm.h"
#include "SliceIterator.tcc"

#include "BlitzTiny.h"

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/algorithm_ext/for_each.hpp>

using composite::SubSystemFree;

using namespace structure;

using cppqedutils::sliceiterator::fullRange;

namespace {

#include "details_BinaryHelper.h"

}

//////////
//      //
// Base //
//      //
//////////

binary::Base::Base(InteractionPtr ia)
  : QuantumSystem<2>(Dimensions(ia->getFrees()[0]->getDimension(),ia->getFrees()[1]->getDimension())),
    free0_(ia->getFrees()[0]), free1_(ia->getFrees()[1]), ia_(ia)
{
}

double binary::Base::highestFrequency_v() const
{
  using std::max;
  return max(ia_.get()->highestFrequency(),max(free0_.get()->highestFrequency(),free1_.get()->highestFrequency()));
}

std::ostream& binary::Base::streamParameters_v(std::ostream& os) const
{
  using namespace std;
  os<<"Binary System\nDimensions: "<<getDimensions()<<". Total: "<<getTotalDimension()
    <<"\n\nSubsystem Nr. 0\n";     free0_.get()->streamParameters(os);
  os<<    "Subsystem Nr. 1\n";     free1_.get()->streamParameters(os);
  os<<"0 - 1 - Interaction\n";
  return ia_.get()->streamParameters(os);
}

#define SUCCESSIVE_Ranges(f0,f1,ia) ptrdiff_t l=-1, u;  \
  if ((u=l+free0_.nAvr<LA_Av>())>l) {                          \
    PROCESS_Range( averages(blitz::Range(l+1,u)) , f0)  \
  }                                                     \
  if ((l=u+free1_.nAvr<LA_Av>())>u) {                          \
    PROCESS_Range( averages(blitz::Range(u+1,l)) , f1)  \
  }                                                     \
  if ((u=l+ia_.nAvr<LA_Av>())>l) {                             \
    PROCESS_Range( averages(blitz::Range(l+1,u)) , ia)  \
  }                                                     \

void binary::Base::process_v(Averages& averages) const
{
#define PROCESS_Range(av,ss) Averages temp(av); ss.process(temp);

  SUCCESSIVE_Ranges(free0_,free1_,ia_) ;

#undef  PROCESS_Range

}

std::ostream& binary::Base::stream_v(const Averages& averages, std::ostream& os, int precision) const
{
  const auto
    av0 =free0_.getAv(),
    av1 =free1_.getAv();
  const auto
    av01=   ia_.getAv();

#define PROCESS_Range(av,ss) ss->stream(av,os,precision);

  SUCCESSIVE_Ranges(av0,av1,av01) ;

#undef  PROCESS_Range

  return os;

}

#undef  SUCCESSIVE_Ranges

////////////////////////////
//                        //
// Averaged - Liouvillean //
//                        //
////////////////////////////

namespace binary {

// These possibilities get instantiated through compilation, so that explicit instantiation is not necessary here.

template<LiouvilleanAveragedTag LA>
std::ostream& streamKey(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia)
{
  os<<"Binary system\n";
  free0.streamKey<LA>(os,i);
  free1.streamKey<LA>(os,i);
  return ia.streamKey<LA>(os,i);
}
template std::ostream& streamKey<LA_Li>(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia);
template std::ostream& streamKey<LA_Av>(std::ostream& os, size_t& i, const SSF& free0, const SSF& free1, const SSI& ia);

template<LiouvilleanAveragedTag LA>
size_t nAvr(const SSF& free0, const SSF& free1, const SSI& ia)
{
  return free0.nAvr<LA>() + free1.nAvr<LA>() + ia.nAvr<LA>();
}
template size_t nAvr< LA_Li >(const SSF& free0, const SSF& free1, const SSI& ia); // explicit instantiation
template size_t nAvr< LA_Av >(const SSF& free0, const SSF& free1, const SSI& ia); // explicit instantiation

template<LiouvilleanAveragedTag LA>
const Averages average(double t, const LazyDensityOperator& ldo, const SSF& free0, const SSF& free1, const SSI& ia, size_t numberAvr)
{
  using boost::copy;

  const Averages
    a0 {quantumdata::partialTrace<V0>(ldo,[&](const auto& m){return free0.average<LA>(t,m);})},
    a1 {quantumdata::partialTrace<V1>(ldo,[&](const auto& m){return free1.average<LA>(t,m);})},
    a01{ia.average<LA>(t,ldo)};

  Averages a(numberAvr);

  copy(a01,copy(a1,copy(a0,a.begin())));

  return a;
}

} // binary

///////////
//       //
// Exact //
//       //
///////////

bool binary::Exact::applicableInMaster_v() const
{
  return free0_.applicableInMaster() && free1_.applicableInMaster() && ia_.applicableInMaster();
}

void binary::Exact::actWithU_v(double t, StateVectorLow& psi, double t0) const
{
  if (const auto ex=free0_.getEx()) for(auto& psiS : fullRange<V0>(psi)) ex->actWithU(t,psiS,t0);
  if (const auto ex=free1_.getEx()) for(auto& psiS : fullRange<V1>(psi)) ex->actWithU(t,psiS,t0);

  ia_.actWithU(t,psi,t0);

}

/////////////////
//             //
// Hamiltonian //
//             //
/////////////////

void binary::Hamiltonian::addContribution_v(double t, const StateVectorLow& psi, StateVectorLow& dpsidt, double t0) const
{
  const auto lambda=[=](auto ha) {
    return [=](const auto& psiS, auto& dpsidtS) {
      ha->addContribution(t,psiS,dpsidtS,t0);
    };
  };

  if (const auto ha=free0_.getHa()) boost::range::for_each(fullRange<V0>(psi),fullRange<V0>(dpsidt),lambda(ha));
  if (const auto ha=free1_.getHa()) boost::range::for_each(fullRange<V1>(psi),fullRange<V1>(dpsidt),lambda(ha));

  ia_.addContribution(t,psi,dpsidt,t0);

}

/////////////////
//             //
// Liouvillean //
//             //
/////////////////

void binary::Liouvillean::actWithJ_v(double t, StateVectorLow& psi, size_t i) const
{
  const auto
    li0 =free0_.getLi(),
    li1 =free1_.getLi();

  size_t n=free0_.nAvr<LA_Li>();
  if (li0 && i<n) {
    for(auto& psiS : fullRange<V0>(psi)) li0->actWithJ(t,psiS,i);
    return;
  }

  i-=n;
  if (li1 && i<(n=free1_.nAvr<LA_Li>())) {
    for(auto& psiS : fullRange<V1>(psi)) li1->actWithJ(t,psiS,i);
    return;
  }

  i-=n;
  if (i<ia_.nAvr<LA_Li>())
    ia_.actWithJ(t,psi,i);

}

void binary::Liouvillean::actWithSuperoperator_v(double t, const DensityOperatorLow& rho, DensityOperatorLow& drhodt, size_t i) const
{
  const auto lambda=[=,&i](auto li) {
    return [=,&i](const auto& rhoS, auto& drhodtS) {
      li->actWithSuperoperator(t,rhoS,drhodtS,i);
    };
  };

  typedef tmptools::Vector<0,2> V0;
  typedef tmptools::Vector<1,3> V1;

  const auto
    li0 =free0_.getLi(),
    li1 =free1_.getLi();

  size_t n=free0_.nAvr<LA_Li>();
  if (li0 && i<n) {
    boost::range::for_each(fullRange<V0>(rho),fullRange<V0>(drhodt),lambda(li0));
    return;
  }

  i-=n;
  if (li1 && i<(n=free1_.nAvr<LA_Li>())) {
    boost::range::for_each(fullRange<V1>(rho),fullRange<V1>(drhodt),lambda(li1));
    return;
  }

  i-=n;
  if (i<ia_.nAvr<LA_Li>())
    ia_.actWithSuperoperator(t,rho,drhodt,i);

}

//////////////////
//              //
// Constructors //
//              //
//////////////////

#define BASE_ctor(Class) Class##Base(getFree0(),getFree1(),getIA())

template<bool IS_EX, bool IS_HA, bool IS_LI>
BinarySystem<IS_EX,IS_HA,IS_LI>::BinarySystem(binary::InteractionPtr ia)
: binary::Base(ia),
  BASE_ctor(Exact),
  BASE_ctor(Hamiltonian),
  BASE_ctor(Liouvillean)
{
}

#undef BASE_ctor

namespace {

using structure::SystemCharacteristics;

const SystemCharacteristics querySystemCharacteristics(binary::InteractionPtr ia)
{
  using namespace structure;

  const QuantumSystemPtr<1>
    free0=ia->getFrees()[0],
    free1=ia->getFrees()[1];

  return SystemCharacteristics{
    std::dynamic_pointer_cast<const Exact<1>>(free0) || std::dynamic_pointer_cast<const Exact<1>>(free1) || std::dynamic_pointer_cast<const Exact<2>>(ia),
    std::dynamic_pointer_cast<const Hamiltonian<1>>(free0) || std::dynamic_pointer_cast<const Hamiltonian<1>>(free1) || std::dynamic_pointer_cast<const Hamiltonian<2>>(ia),
    std::dynamic_pointer_cast<const Liouvillean<1>>(free0) || std::dynamic_pointer_cast<const Liouvillean<1>>(free1) || std::dynamic_pointer_cast<const Liouvillean<2>>(ia)};
}

}

#define DISPATCHER(EX,HA,LI) (all(querySystemCharacteristics(ia)==SystemCharacteristics{EX,HA,LI})) return std::make_shared<BinarySystem<EX,HA,LI> >(ia)

const binary::Ptr binary::make(InteractionPtr ia)
{
  if      DISPATCHER(true ,true ,true ) ;
  else if DISPATCHER(true ,true ,false) ;
  else if DISPATCHER(true ,false,true ) ;
  else if DISPATCHER(true ,false,false) ;
  else if DISPATCHER(false,true ,true ) ;
  else if DISPATCHER(false,true ,false) ;
  else if DISPATCHER(false,false,true ) ;
  else return std::make_shared<BinarySystem<false,false,false> >(ia);
}

#undef DISPATCHER

template class BinarySystem<true ,true ,true >;
template class BinarySystem<true ,true ,false>;
template class BinarySystem<true ,false,true >;
template class BinarySystem<true ,false,false>;
template class BinarySystem<false,true ,true >;
template class BinarySystem<false,true ,false>;
template class BinarySystem<false,false,true >;
template class BinarySystem<false,false,false>;


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "variant/type/variant.hpp"
#include "caller/candidateVariantBank.hpp"
#include "unittest/vcf/VCFTestUtils.hpp"

using namespace wecall::variant;
using namespace wecall::caller;
using wecall::caller::Region;
using wecall::utils::ReferenceSequence;

BOOST_AUTO_TEST_CASE( shouldComputePriorFromAlleleFrequenceString )
{
    wecall::vcf::Info info;

    info.emplace_back( std::make_pair< std::string, std::vector< std::string > >(
        std::string( "AF" ), {std::string( "0.4" ), std::string( "0.5" )} ) );
    info.emplace_back( std::make_pair< std::string, std::vector< std::string > >(
        std::string( "DP" ), {std::string( "0.4" ), std::string( "0.1" )} ) );

    const auto expectedResults = {0.4, 0.5};
    const auto results = getPriorsFromInfo( info );

    BOOST_CHECK_EQUAL_COLLECTIONS( expectedResults.begin(), expectedResults.end(), results.begin(), results.end() );
}

//-------------------------------------------------------------------------------------------------


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_POINT3D_HPP
#define UTILITIES_GEOMETRY_POINT3D_HPP

#include "../UtilitiesAPI.hpp"
#include "../data/Vector.hpp"
#include "../core/Logger.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  // forward declaration
  class Vector3d;

  class UTILITIES_API Point3d{
  public:

    /// default constructor creates point at 0, 0, 0
    Point3d();

    /// constructor with x, y, z
    Point3d(double x, double y, double z);

    /// copy constructor
    Point3d(const Point3d& other);

    /// get x
    double x() const;

    /// get y
    double y() const;

    /// get z
    double z() const;

    /// point plus a vector is a new point
    Point3d operator+(const Vector3d& vec) const;

    /// point plus a vector is a new point
    Point3d& operator+=(const Vector3d& vec);

    /// point minus another point is a vector
    Vector3d operator-(const Point3d& other) const;

    /// check equality
    bool operator==(const Point3d& other) const;

  private:

    REGISTER_LOGGER("utilities.Point3d");
    Vector m_storage;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Point3d& point);

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const std::vector<Point3d>& pointVector);

  // optional Point3d
  typedef boost::optional<Point3d> OptionalPoint3d;

  // vector of Point3d
  typedef std::vector<Point3d> Point3dVector;

} // openstudio

#endif //UTILITIES_GEOMETRY_POINT3D_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2011 - 2013 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_SSE2_EXTRACT_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_SSE_SSE2_EXTRACT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/memory/functions/extract.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/simd/sdk/config/enforce_precision.hpp>
#include <boost/simd/sdk/config/compiler.hpp>
#include <boost/simd/sdk/config/arch.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< ints8_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      int v = _mm_extract_epi16(a0, A1::value / 2);
      return result_type((v >> (8*(A1::value % 2)))& 0xFF);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< ints16_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      return result_type(_mm_extract_epi16(a0, A1::value));
    }
  };

// workaround for bug with GCC 32-bit x87, causing weird issues with splat int,sse -> complex<float>,sse
#if !(defined(BOOST_SIMD_COMPILER_GCC) && !defined(BOOST_SIMD_ARCH_X86_64) && defined(BOOST_SIMD_HAS_X87))
  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< ints32_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      return result_type(_mm_cvtsi128_si32(_mm_srli_si128(a0, A1::value * 4)));
    }
  };
#endif

#ifdef BOOST_SIMD_ARCH_X86_64
  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< ints64_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      return result_type(_mm_cvtsi128_si64(_mm_srli_si128(a0, A1::value * 8)));
    }
  };
#endif

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< single_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      return _mm_cvtss_f32(_mm_castsi128_ps(_mm_srli_si128(_mm_castps_si128(a0), A1::value * 4)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_< double_<A0>, boost::simd::tag::sse_ >))
                                      (mpl_integral_< scalar_< integer_<A1> > >)
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1) const
    {
      return _mm_cvtsd_f64(_mm_castsi128_pd(_mm_srli_si128(_mm_castpd_si128(a0), A1::value * 8)));
    }
  };

} } }

#endif
#endif


//=============================================================================================================
/**
* @file     mnemath.cpp
* @author   Christoph Dinh <chdinh@nmr.mgh.harvard.edu>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     July, 2012
*
* @section  LICENSE
*
* Copyright (C) 2012, Christoph Dinh and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors 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 OWNER 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.
*
*
* @brief    Definition of the MNEMath Class.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "mnemath.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Eigen>

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

#include <QDebug>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

VectorXd* MNEMath::combine_xyz(const VectorXd& vec)
{
    if (vec.size() % 3 != 0)
    {
        printf("Input must be a row or a column vector with 3N components\n");
        return NULL;
    }

    MatrixXd tmp = MatrixXd(vec.transpose());
    SparseMatrix<double>* s = make_block_diag(tmp,3);

    SparseMatrix<double> sC = *s*s->transpose();
    VectorXd* comb = new VectorXd(sC.rows());

    for(qint32 i = 0; i < sC.rows(); ++i)
        (*comb)[i] = sC.coeff(i,i);

    delete s;
    return comb;
}

//*************************************************************************************************************

double MNEMath::getConditionNumber(const MatrixXd& A, VectorXd &s)
{
    JacobiSVD<MatrixXd> svd(A);
    s = svd.singularValues();

    double c = s.maxCoeff()/s.minCoeff();

    return c;
}

//*************************************************************************************************************

double MNEMath::getConditionSlope(const MatrixXd& A, VectorXd &s)
{
    JacobiSVD<MatrixXd> svd(A);
    s = svd.singularValues();

    double c = s.maxCoeff()/s.mean();

    return c;
}

//*************************************************************************************************************

void MNEMath::get_whitener(MatrixXd &A, bool pca, QString ch_type, VectorXd &eig, MatrixXd &eigvec)
{
    // whitening operator
    SelfAdjointEigenSolver<MatrixXd> t_eigenSolver(A);//Can be used because, covariance matrices are self-adjoint matrices.

    eig = t_eigenSolver.eigenvalues();
    eigvec = t_eigenSolver.eigenvectors().transpose();

    MNEMath::sort<double>(eig, eigvec, false);
    qint32 rnk = MNEMath::rank(A);

    for(qint32 i = 0; i < eig.size()-rnk; ++i)
        eig(i) = 0;

    printf("Setting small %s eigenvalues to zero.\n", ch_type.toUtf8().constData());
    if (!pca)  // No PCA case.
        printf("Not doing PCA for %s\n", ch_type.toUtf8().constData());
    else
    {
        printf("Doing PCA for %s.",ch_type.toUtf8().constData());
        // This line will reduce the actual number of variables in data
        // and leadfield to the true rank.
        eigvec = eigvec.block(eigvec.rows()-rnk, 0, rnk, eigvec.cols());
    }
}

//*************************************************************************************************************

VectorXi MNEMath::intersect(const VectorXi &v1, const VectorXi &v2, VectorXi &idx_sel)
{
    std::vector<int> tmp;

    std::vector< std::pair<int,int> > t_vecIntIdxValue;

    //ToDo:Slow; map VectorXi to stl container
    for(qint32 i = 0; i < v1.size(); ++i)
        tmp.push_back(v1[i]);

    std::vector<int>::iterator it;
    for(qint32 i = 0; i < v2.size(); ++i)
    {
        it = std::search(tmp.begin(), tmp.end(), &v2[i], &v2[i]+1);
        if(it != tmp.end())
            t_vecIntIdxValue.push_back(std::pair<int,int>(v2[i], it-tmp.begin()));//Index and int value are swapped // to sort using the idx
    }

    std::sort(t_vecIntIdxValue.begin(), t_vecIntIdxValue.end(), MNEMath::compareIdxValuePairSmallerThan<int>);

    VectorXi p_res(t_vecIntIdxValue.size());
    idx_sel = VectorXi(t_vecIntIdxValue.size());

    for(quint32 i = 0; i < t_vecIntIdxValue.size(); ++i)
    {
        p_res[i] = t_vecIntIdxValue[i].first;
        idx_sel[i] = t_vecIntIdxValue[i].second;
    }

    return p_res;
}

//*************************************************************************************************************

//    static inline MatrixXd extract_block_diag(MatrixXd& A, qint32 n)
//    {

//        //
//        // Principal Investigators and Developers:
//        // ** Richard M. Leahy, PhD, Signal & Image Processing Institute,
//        //    University of Southern California, Los Angeles, CA
//        // ** John C. Mosher, PhD, Biophysics Group,
//        //    Los Alamos National Laboratory, Los Alamos, NM
//        // ** Sylvain Baillet, PhD, Cognitive Neuroscience & Brain Imaging Laboratory,
//        //    CNRS, Hopital de la Salpetriere, Paris, France
//        //
//        // Copyright (c) 2005 BrainStorm by the University of Southern California
//        // This software distributed  under the terms of the GNU General Public License
//        // as published by the Free Software Foundation. Further details on the GPL
//        // license can be found at http://www.gnu.org/copyleft/gpl.html .
//        //
//        //FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
//        // UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
//        // WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
//        // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
//        // LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
//        //
//        // Author: John C. Mosher 1993 - 2004
//        //
//        //
//        // Modifications for mne Matlab toolbox
//        //
//        //   Matti Hamalainen
//        //   2006

//          [mA,na] = size(A);		% matrix always has na columns
//          % how many entries in the first column?
//          bdn = na/n;			% number of blocks
//          ma = mA/bdn;			% rows in first block

//          % blocks may themselves contain zero entries.  Build indexing as above
//          tmp = reshape([1:(ma*bdn)]',ma,bdn);
//          i = zeros(ma*n,bdn);
//          for iblock = 1:n,
//            i((iblock-1)*ma+[1:ma],:) = tmp;
//          end

//          i = i(:); 			% row indices foreach sparse bd

//          j = [0:mA:(mA*(na-1))];
//          j = j(ones(ma,1),:);
//          j = j(:);

//          i = i + j;

//          bd = full(A(i)); 	% column vector
//          bd = reshape(bd,ma,na);	% full matrix

//    }

//*************************************************************************************************************

bool MNEMath::issparse(VectorXd &v)
{
    //ToDo: Figure out how to accelerate MNEMath::issparse(VectorXd &v)

    qint32 c = 0;
    qint32 n = v.rows();
    qint32 t = n/2;

    for(qint32 i = 0; i < n; ++i)
    {
        if(v(i) == 0)
            ++c;
        if(c > t)
            return true;
    }

    return false;
}

//*************************************************************************************************************

MatrixXd MNEMath::legendre(qint32 n, const VectorXd &X, QString normalize)
{
    MatrixXd y;

    Q_UNUSED(y);

    Q_UNUSED(n);
    Q_UNUSED(X);
    Q_UNUSED(normalize);

    //ToDo

    return y;
}

//*************************************************************************************************************

SparseMatrix<double>* MNEMath::make_block_diag(const MatrixXd &A, qint32 n)
{

    qint32 ma = A.rows();
    qint32 na = A.cols();
    float bdn = ((float)na)/n;      // number of submatrices

//    std::cout << std::endl << "ma " << ma << " na " << na << " bdn " << bdn << std::endl;

    if(bdn - floor(bdn))
    {
        printf("Width of matrix must be even multiple of n\n");
        return NULL;
    }

    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletList;
    tripletList.reserve(bdn*ma*n);

    qint32 current_col, current_row, i, r, c;
    for(i = 0; i < bdn; ++i)
    {
        current_col = i * n;
        current_row = i * ma;

        for(r = 0; r < ma; ++r)
            for(c = 0; c < n; ++c)
                tripletList.push_back(T(r+current_row, c+current_col, A(r, c+current_col)));
    }

    SparseMatrix<double>* bd = new SparseMatrix<double>((int)floor((float)ma*bdn+0.5),na);
//    SparseMatrix<double> p_Matrix(nrow, ncol);
    bd->setFromTriplets(tripletList.begin(), tripletList.end());

    return bd;
}

//*************************************************************************************************************

int MNEMath::nchoose2(int n)
{

    //nchoosek(n, k) with k = 2, equals n*(n-1)*0.5

    int t_iNumOfCombination = (int)(n*(n-1)*0.5);

    return t_iNumOfCombination;
}

//*************************************************************************************************************

qint32 MNEMath::rank(const MatrixXd& A, double tol)
{
    JacobiSVD<MatrixXd> t_svdA(A);//U and V are not computed
    VectorXd s = t_svdA.singularValues();
    double t_dMax = s.maxCoeff();
    t_dMax *= tol;
    qint32 sum = 0;
    for(qint32 i = 0; i < s.size(); ++i)
        sum += s[i] > t_dMax ? 1 : 0;
    return sum;
}

//*************************************************************************************************************

MatrixXd MNEMath::rescale(const MatrixXd &data, const RowVectorXf &times, QPair<QVariant,QVariant> baseline, QString mode)
{
    MatrixXd data_out = data;
    QStringList valid_modes;
    valid_modes << "logratio" << "ratio" << "zscore" << "mean" << "percent";
    if(!valid_modes.contains(mode))
    {
        qWarning() << "\tWarning: mode should be any of : " << valid_modes;
        return data_out;
    }
    printf("\tApplying baseline correction ... (mode: %s)\n", mode.toUtf8().constData());

    qint32 imin = 0;
    qint32 imax = times.size();

    if(!baseline.first.isValid())
        imin = 0;
    else
    {
        float bmin = baseline.first.toFloat();
        for(qint32 i = 0; i < times.size(); ++i)
        {
            if(times[i] >= bmin)
            {
                imin = i;
                break;
            }
        }
    }
    if (!baseline.second.isValid())
        imax = times.size();
    else
    {
        float bmax = baseline.second.toFloat();
        for(qint32 i = times.size()-1; i >= 0; --i)
        {
            if(times[i] <= bmax)
            {
                imax = i+1;
                break;
            }
        }
    }

    VectorXd mean = data_out.block(0, imin,data_out.rows(),imax-imin).rowwise().mean();
    if(mode.compare("mean") == 0)
    {
        data_out -= mean.rowwise().replicate(data.cols());
    }
    else if(mode.compare("logratio") == 0)
    {
        for(qint32 i = 0; i < data_out.rows(); ++i)
            for(qint32 j = 0; j < data_out.cols(); ++j)
                data_out(i,j) = log10(data_out(i,j)/mean[i]); // a value of 1 means 10 times bigger
    }
    else if(mode.compare("ratio") == 0)
    {
        data_out = data_out.cwiseQuotient(mean.rowwise().replicate(data_out.cols()));
    }
    else if(mode.compare("zscore") == 0)
    {
        MatrixXd std_mat = data.block(0, imin, data.rows(), imax-imin) - mean.rowwise().replicate(imax-imin);
        std_mat = std_mat.cwiseProduct(std_mat);
        VectorXd std_v = std_mat.rowwise().mean();
        for(qint32 i = 0; i < std_v.size(); ++i)
            std_v[i] = sqrt(std_v[i] / (float)(imax-imin));

        data_out -= mean.rowwise().replicate(data_out.cols());
        data_out = data_out.cwiseQuotient(std_v.rowwise().replicate(data_out.cols()));
    }
    else if(mode.compare("percent") == 0)
    {
        data_out -= mean.rowwise().replicate(data_out.cols());
        data_out = data_out.cwiseQuotient(mean.rowwise().replicate(data_out.cols()));
    }

    return data_out;
}

//*************************************************************************************************************