/*
  SPDX-License-Identifier: MIT

*/

#ifndef TOKEN_H
#define TOKEN_H

/**
 * @file
 * Token scanner.
 * This file declares the token scanner class, tokenizer_t, plus the
 * different possible tokens that it can return.
 * @par
 * The token scanner interface is trivial, it only has a constructor and
 * a method for providing the next token, if any.
 */

#include <memory>
#include <system_error>
#include "environment.hh"
#include "sbucket.hh"
#include "position.hh"
#include "mmap_file.hh"
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/gmp.hpp>

/**
 * Multi-precision integer type.
 * This type is used to store Sisdel integers, which are by default multi-
 * precision integers. This means that the integer can be as large as the
 * available memory allows.
 */
typedef boost::multiprecision::number<boost::multiprecision::gmp_int> mp_int;

/**
 * Multi-precision floating point type.
 * This type is used to store Sisdel floating point numbers, which are by
 * default multi-precision floating point numbers. This means that the
 * value can be as large, and with selectable precision, as the available
 * memory allows.
 */
typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<0> > mp_float;

class token_t;

/**
 * Token scanner.
 * Reads the given file and split it into a stream of tokens.
 *
 * @todo Implement support for standard input as source.
 */
class tokenizer_t {
public:

	/**
	 * Construct the scanner.
	 *
	 * @todo When environment object has been made thread local, the
	 *       env parameter should be removed.
	 */
	tokenizer_t(
		environment_t& env, /**< Reference to environment object. */
		const char* file    /**< Name of file in UTF8 format. */
		);

	/**
	 * Return next token.
	 *
	 * @returns Next token in the token stream, or NULL if end of file.
	 */
	const token_t* next(void);

private:
	void get_number(mp_int& nr, char base,
			const char *valid_digits, size_t& nr_digits);

	environment_t& m_env;
	mmap_file_t m_file;
	position_t m_startofline;
};

/**
 * Base class for tokens.
 * This class provides the interface provided by all inherited tokens.
 * This is a virtual class, so no token is of this type.
 * @par
 * Use constructs like "typeid(returned_token) == typeid(token_eol_t)"
 * to determine what kind of token this is, and then use
 * dynamic_cast to cast it to the correct type.
 */
class token_t {
public:
	/**
	 * Destructor.
	 * Virtual destructor since this class will be inherited by other
	 * classes.
	 */
	virtual ~token_t() noexcept {};

	/**
	 * Determine where in the input stream this token was found.
	 *
	 * @returns Position in input stream.
	 */
	virtual const position_t& position(void) const noexcept = 0;
};

/**
 * End of line token.
 * This token is returned when an end of line character is found.
 * Any multiple end of line characters are skipped, as are empty lines or
 * lines with only comments.
 * The token provides indentation level, i.e. how many tab characters that
 * were found right after the end of line character.
 */
class token_eol_t : public token_t {
public:
	/**
	 * Constructor, used by the scanner.
	 */
	token_eol_t(
		const position_t& pos, /**< Position in input stream where
					* this token was found. */
		size_t indent_level    /**< Indentation level, i.e. number of
					* tab characters right after the
					* end of line character. */
		)
		: m_position(pos), m_indent_level(indent_level) {}

	/**
	 * Return indentation level.
	 *
	 * @returns The indentation level, i.e. number of tab characters,
	 *          right after the end of line character.
	 */
	constexpr size_t indent_level(void) const noexcept
		{ return m_indent_level; }

	//
	// Inherited from token_t
	//

	const position_t& position(void) const noexcept
		{ return m_position; }

private:
	position_t m_position;
	size_t m_indent_level;
};

/**
 * String token.
 * This token represents a string immediate value.
 */
class token_string_t : public token_t {
public:
	/**
	 * Constructor, used by scanner.
	 */
	token_string_t(
		const position_t& pos, /**< Position in input stream where
					* this token was found. */
		string_idx_t str       /**< String content as a string_idx_t. */
		)
		: m_position(pos), m_string(str) {}

	constexpr string_idx_t string(void) const noexcept
		{ return m_string; }

	//
	// Inherited from token_t
	//

	const position_t& position(void) const noexcept
		{ return m_position; }

private:
	position_t m_position;
	string_idx_t m_string;
};

class token_identifier_t : public token_t {
public:
	token_identifier_t(const position_t& pos, string_idx_t name)
		: m_position(pos), m_name(name) {}

	constexpr string_idx_t name(void) const noexcept
		{ return m_name; }

	//
	// Inherited from token_t
	//

	const position_t& position(void) const noexcept
		{ return m_position; }

private:
	position_t m_position;
	string_idx_t m_name;
};

class token_integer_t : public token_t {
public:
	token_integer_t(const position_t& pos, const mp_int& value)
		: m_position(pos), m_integer(value) {}

	constexpr const mp_int& value(void) const noexcept
		{ return m_integer; }

	//
	// Inherited from token_t
	//

	const position_t& position(void) const noexcept
		{ return m_position; }

private:
	position_t m_position;
	mp_int m_integer;
};

class token_float_t : public token_t {
public:
	token_float_t(const position_t& pos, const mp_float& floating)
		: m_position(pos), m_float(floating) {}

	constexpr const mp_float& value(void) const noexcept
		{ return m_float; }

	//
	// Inherited from token_t
	//

	const position_t& position(void) const noexcept
		{ return m_position; }

private:
	position_t m_position;
	mp_float m_float;
};

std::ostream& operator<<(std::ostream& os, const token_t& t);

#endif /* TOKEN_H */


#include "relativegazelearner.h"

#include <boost/lexical_cast.hpp>

using namespace std;

RelativeGazeLearner::RelativeGazeLearner(TrainingParameters &params) : AbstractLearner(params)
{

}

RelativeGazeLearner::~RelativeGazeLearner()
{

}

boost::optional<dlib::matrix<double,0,1>> RelativeGazeLearner::getFeatureVector(GazeHyp& ghyp) {
    auto result = boost::optional<dlib::matrix<double,0,1>>();
    if (ghyp.horizGazeFeatures.size() && ghyp.faceFeatures.size() && ghyp.eyeHogFeatures.size()) {
        switch (trainParams.featureSet.get_value_or(FeatureSetConfig::ALL)) {
        case FeatureSetConfig::ALL:
            result = dlib::join_cols(dlib::join_cols(ghyp.horizGazeFeatures, ghyp.faceFeatures), ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::POSREL:
            result = dlib::join_cols(ghyp.horizGazeFeatures, ghyp.faceFeatures);
            break;
        case FeatureSetConfig::RELATIONAL:
            result = ghyp.horizGazeFeatures;
            break;
        case FeatureSetConfig::HOG:
            result = ghyp.eyeHogFeatures;
            break;
        case FeatureSetConfig::HOGREL:
            result = dlib::join_cols(ghyp.horizGazeFeatures, ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::HOGPOS:
            result = dlib::join_cols(ghyp.faceFeatures, ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::POSITIONAL:
            result = ghyp.faceFeatures;
            break;
        default:
            throw std::runtime_error("feature-set not implemented");
        }
    }
    return result;
}

void RelativeGazeLearner::classify(GazeHyp& ghyp){
    _classify(ghyp, learned_function, ghyp.horizontalGazeEstimation);
    //cerr << ghyp.relativeGazeClassification << endl;
}

void RelativeGazeLearner::train(const string& outfilename) {
    dlib::svr_trainer<kernel_type> trainer;
    _train(outfilename, learned_function, trainer);
}

void RelativeGazeLearner::visualize(GazeHyp& ghyp, double mutualGazeTolerance)
{
    if (!ghyp.horizontalGazeEstimation.is_initialized()) return;
    double relativeGazeEst = ghyp.horizontalGazeEstimation.get();
    if (!std::isfinite(relativeGazeEst)) {
        return;
    }
    auto fr = ghyp.pupils.faceRect();
    double angle = relativeGazeEst - 90;
    double length = fr.width/3;
    cv::Point p1 (fr.tl().x + fr.width / 2, fr.tl().y);
    cv::Point p2;
    p2.x = round(p1.x + length * cos(angle * CV_PI / 180.0));
    p2.y = round(p1.y + length * sin(angle * CV_PI / 180.0));
    //cv::line(ghyp.parentHyp.frame, p1, p2, cv::Scalar(255, 150, 150), 2, 'A');
    cv::line(ghyp.parentHyp.frame, p1, p2, cv::Scalar(255, 0, 0), 2, 'A');
    cv::Mat facereg = ghyp.pupils.faceRegion();
    int limita = facereg.cols*(0.5+mutualGazeTolerance/90.0);
    int limitb = facereg.cols*(0.5-mutualGazeTolerance/90.0);
    cv::rectangle(facereg, cv::Rect(cv::Point(limita-1, 0), cv::Size(2, 14)), cv::Scalar(255, 100, 100), -1, 'A');
    cv::rectangle(facereg, cv::Rect(cv::Point(limitb-1, 0), cv::Size(2, 14)), cv::Scalar(255, 100, 100), -1, 'A');
    int offset = facereg.cols*(0.5+relativeGazeEst/90.0);
    cv::rectangle(facereg, cv::Rect(cv::Point(offset-3, 0), cv::Size(6, 10)), cv::Scalar(255, 0, 0), -1, 'A');
}

void RelativeGazeLearner::loadClassifier(const std::string &filename)
{
    _loadClassifier(filename, learned_function);
}

string RelativeGazeLearner::getId()
{
    return "RelativeGazeLearner";
}


/*
 * gpuCSICoilEstimationGadget.cpp
 *
 *  Created on: Nov 12, 2014
 *      Author: dch
 */

#include "gpuCSICoilEstimationGadget.h"
#include <utility>
#include <ismrmrd/xml.h>
#include "cuNDArray.h"
#include "cuNFFT.h"
#include "b1_map.h"
#include "vector_td_utilities.h"
#include "../../toolboxes/nfft/NFFTOperator.h"
#include "cuCgSolver.h"
#include "cuNDArray_fileio.h"
#include "trajectory_utils.h"
#include <boost/make_shared.hpp>
#include "hoNDArray_math.h"

namespace Gadgetron {

gpuCSICoilEstimationGadget::gpuCSICoilEstimationGadget() {

}

gpuCSICoilEstimationGadget::~gpuCSICoilEstimationGadget() {
	// TODO Auto-generated destructor stub
}

int gpuCSICoilEstimationGadget::process_config(ACE_Message_Block* mb) {
	ISMRMRD::IsmrmrdHeader h;
	ISMRMRD::deserialize(mb->rd_ptr(),h);

	if (h.encoding.size() != 1){
		GDEBUG("Coil estimation gadget only supports encoding spaces of size 1\n");
		return GADGET_FAIL;
	}
	img_size = {h.encoding[0].reconSpace.matrixSize.x,h.encoding[0].reconSpace.matrixSize.y};
	kernel_width_ = kernel_width.value();

	coils = h.acquisitionSystemInformation->receiverChannels;
	skip_lines_ = skip_lines.value();
	return GADGET_OK;
}

int gpuCSICoilEstimationGadget::process(
		GadgetContainerMessage<IsmrmrdReconData>* m1) {
	IsmrmrdReconData* bucket = m1->getObjectPtr();

	auto cm1 = new GadgetContainerMessage<cuSenseData>();
	auto senseData = cm1->getObjectPtr();

	coils = bucket->rbit_.front().data_.headers_[0].active_channels;
	GDEBUG("Active channels %i \n",coils);

	{

		hoNDArray<std::complex<float>> * ho_data = &bucket->rbit_.front().data_.data_;
		hoNDArray<float>* ho_traj = &bucket->rbit_.front().data_.trajectory_.value();

		if (skip_lines_ > 0){

			auto separated = split_calibration_lines<std::complex<float>>(*ho_data,skip_lines_,1);

			senseData->freq_calibration = boost::make_shared<cuNDArray<float_complext>>((hoNDArray<float_complext>*)std::get<0>(separated).get());
			senseData->freq_calibration->squeeze();
			senseData->data = boost::make_shared<cuNDArray<float_complext>>((hoNDArray<float_complext>*)std::get<1>(separated).get());
		} else {

			senseData->data = boost::make_shared<cuNDArray<float_complext>>(reinterpret_cast<hoNDArray<float_complext>*>(ho_data));
		}

		if (ho_traj->get_size(0) > 2){ //We have dcw
			auto traj_dcw = separate_traj_and_dcw(ho_traj);
			senseData->traj = boost::make_shared<cuNDArray<floatd2>>(*std::get<0>(traj_dcw));
			senseData->dcw = boost::make_shared<cuNDArray<float>>(*std::get<1>(traj_dcw));
		} else {
			std::vector<size_t> tdims = *ho_traj->get_dimensions();
			std::vector<size_t> tmp_dim(tdims.begin()+1,tdims.end());
			hoNDArray<floatd2> tmp(tmp_dim,reinterpret_cast<floatd2*>(ho_traj->get_data_ptr()));
			senseData->traj = boost::make_shared<cuNDArray<floatd2>>(tmp);
		}

	}

	//Remove Initial Spirals

	boost::shared_ptr< cuNDArray<float_complext> >  ref_data;
	boost::shared_ptr< cuNDArray<floatd2> > ref_traj;
	boost::shared_ptr<cuNDArray<float> > ref_dcw;

	if (!bucket->rbit_.front().ref_){
		GDEBUG("Setting reference data to real data\n");
		ref_data = senseData->data;
		ref_traj = senseData->traj;
		ref_dcw = senseData->dcw;
	} else {

		auto & ref = *bucket->rbit_.front().ref_;

		hoNDArray<std::complex<float>> * ho_data = &ref.data_;
		hoNDArray<float>* ho_traj = &ref.trajectory_.value();

		ref_data = boost::make_shared<cuNDArray<float_complext>>(reinterpret_cast<hoNDArray<float_complext>*>(ho_data));
		if (ho_traj->get_size(0) > 2){
			auto traj_dcw = separate_traj_and_dcw(ho_traj);
			ref_traj =boost::make_shared<cuNDArray<floatd2>>(*std::get<0>(traj_dcw));
			ref_dcw = boost::make_shared<cuNDArray<float>>(*std::get<1>(traj_dcw));
		} else {
			std::vector<size_t> tdims = *ho_traj->get_dimensions();
			std::vector<size_t> tmp_dim(tdims.begin()+1,tdims.end());
			hoNDArray<floatd2> tmp(tmp_dim,reinterpret_cast<floatd2*>(ho_traj->get_data_ptr()));
			ref_traj = boost::make_shared<cuNDArray<floatd2>>(tmp);
		}
	}

	senseData->csm = calculate_CSM(ref_data.get(),ref_traj.get(),ref_dcw.get());

	if (this->next()->putq(cm1) == GADGET_FAIL){
		GERROR("Failed to put message on que\n");
		return GADGET_FAIL;
	}

	return GADGET_OK;

}

boost::shared_ptr<cuNDArray<float_complext> > gpuCSICoilEstimationGadget::calculate_CSM(
		cuNDArray<float_complext>* data, cuNDArray<floatd2>* traj, cuNDArray<float>* dcw ) {

	std::vector<size_t> csm_dims = img_size;
	csm_dims.push_back(coils);
	std::vector<size_t> flat_dims = {traj->get_number_of_elements()};
	cuNDArray<floatd2> flat_traj(flat_dims,traj->get_data_ptr());
	std::vector<size_t> spiral_dims{data->get_size(0)*data->get_size(1)*data->get_size(2),data->get_size(3)}; //Trajectories, coils
	cuNDArray<complext<float>> second_spiral(spiral_dims,data->get_data_ptr());
	std::vector<size_t> spiral_traj_dims{spiral_dims[0]};
	cuNDArray<floatd2> spiral_traj(spiral_traj_dims,traj->get_data_ptr());
	if (dcw) { //We have density compensation, so we can get away with gridding

		cuNFFT_impl<float,2> plan(from_std_vector<size_t,2>(img_size),from_std_vector<size_t,2>(img_size)*size_t(2),kernel_width_);
		cuNDArray<float_complext> tmp(csm_dims);
		GDEBUG("Coils %i \n\n",tmp.get_size(2));

		cuNDArray<float> spiral_dcw(spiral_traj_dims,dcw->get_data_ptr());

		GDEBUG("Preprocessing\n\n");
		plan.preprocess(spiral_traj,NFFT_prep_mode::NC2C);
		GDEBUG("Computing\n\n");

		auto tmp_abs = abs(&tmp);

		return boost::make_shared<cuNDArray<float_complext>>(estimate_b1_map<float,2>(tmp));

	} else { //No density compensation, we have to do iterative reconstruction.

		auto E = boost::make_shared<NFFTOperator<cuNDArray,float,2>>();

		E->setup(from_std_vector<size_t,2>(img_size),from_std_vector<size_t,2>(img_size)*size_t(2),kernel_width_);

		E->set_codomain_dimensions(&spiral_dims);
		E->set_domain_dimensions(&csm_dims);
		cuCgSolver<float_complext> solver;
		solver.set_max_iterations(20);
		solver.set_encoding_operator(E);

		E->preprocess(&spiral_traj);

		auto tmp = solver.solve(&second_spiral);
		auto tmp_abs = abs(tmp.get());

		auto res = estimate_b1_map<float,2>(*tmp);
		//fill(res.get(),float_complext(1,0));
		//auto res= boost::make_shared<cuNDArray<float_complext>>(csm_dims);
		//fill(res.get(),float_complext(1,0));
		return boost::make_shared<cuNDArray<float_complext>>(res);

	}

}

std::tuple<boost::shared_ptr<hoNDArray<floatd2 > >, boost::shared_ptr<hoNDArray<float >>> gpuCSICoilEstimationGadget::separate_traj_and_dcw(
		hoNDArray<float >* traj_dcw) {
	std::vector<size_t> dims = *traj_dcw->get_dimensions();
	std::vector<size_t> reduced_dims(dims.begin()+1,dims.end()); //Copy vector, but leave out first dim
	auto  dcw = boost::make_shared<hoNDArray<float>>(reduced_dims);

	auto traj = boost::make_shared<hoNDArray<floatd2>>(reduced_dims);

	auto dcw_ptr = dcw->get_data_ptr();
	auto traj_ptr = traj->get_data_ptr();
	auto ptr = traj_dcw->get_data_ptr();

	for (size_t i = 0; i < traj_dcw->get_number_of_elements()/3; i++){
		traj_ptr[i][0] = ptr[i*3];
		traj_ptr[i][1] = ptr[i*3+1];
		dcw_ptr[i] = ptr[i*3+2];
	}

	return std::make_tuple(traj,dcw);

}

template<class T> std::tuple<boost::shared_ptr<hoNDArray<T>>, boost::shared_ptr<hoNDArray<T>> > gpuCSICoilEstimationGadget::split_calibration_lines(hoNDArray<T>& data, int ncal_lines, int dim){

	auto dimensions = *data.get_dimensions();

	size_t cal_dim = dimensions[dim];
	if (ncal_lines >= dimensions[dim]){
		throw std::runtime_error("Number of calibration lines is longer or equal to data size. No data left for reconstruction!");
	}
	auto cal_dims = dimensions;
	cal_dims[dim] = ncal_lines;
	dimensions[dim] -= ncal_lines;

	auto calibration_data = boost::make_shared<hoNDArray<T>>(cal_dims);
	auto recon_data = boost::make_shared<hoNDArray<T>>(dimensions);

	size_t smaller_dims = 1;
	for (auto i = 0u; i < dim;i++)
		smaller_dims *= dimensions[i];

	size_t larger_dims = 1;
	for (auto i = dim+1; i < dimensions.size(); i++)
		larger_dims *= dimensions[i];

	auto cal_ptr = calibration_data->get_data_ptr();
	auto recon_ptr = recon_data->get_data_ptr();
	auto data_ptr = data.get_data_ptr();

	for (auto k = 0u; k < larger_dims; k++){
		for (auto n = 0u; n < ncal_lines; n++){
			for (auto m = 0u; m < smaller_dims; m++){
				cal_ptr[m+n*smaller_dims+k*ncal_lines*smaller_dims] = data_ptr[m+n*smaller_dims+k*smaller_dims*cal_dim];
			}
		}
		for (auto n = 0u; n < dimensions[dim]; n++){
			for (auto m = 0u; m < smaller_dims; m++){
				recon_ptr[m+n*smaller_dims+k*dimensions[dim]*smaller_dims] = data_ptr[m+(n+ncal_lines)*smaller_dims+k*smaller_dims*cal_dim];
			}
		}
	}

	return std::make_tuple(calibration_data,recon_data);

}

GADGET_FACTORY_DECLARE(gpuCSICoilEstimationGadget)

} /* namespace Gadgetron */


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SHIFT_LEFT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SHIFT_LEFT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_ornot.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/if_else_allbits.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/detail/make_dependent.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::ints8_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      using int_t =  detail::make_dependent_t<std::uint16_t,A0>;
      using gen_t = pack<int_t, A0::static_size/2>;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left ints8 sse2: a shift is out of range");
      A0 const Mask1 = bitwise_cast<A0>( gen_t(0x00ff));
      A0 const Mask2 = bitwise_cast<A0>( gen_t(0xff00));
      A0 tmp  = bitwise_and(a0, Mask1);
      A0 tmp1 = _mm_slli_epi16(tmp, int(a1));
      tmp1 = bitwise_and(tmp1, Mask1);
      tmp = bitwise_and(a0, Mask2);
      A0 tmp3 = _mm_slli_epi16(tmp, int(a1));
      return bitwise_or(tmp1, bitwise_and(tmp3, Mask2));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left ints16 sse2: a shift is out of range");
      return _mm_slli_epi16(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left ints32 sse2: a shift is out of range");
      return _mm_slli_epi32(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0,typename A1 )
                          , bs::sse2_
                          , bs::pack_<bd::ints64_<A0>, bs::sse_>
                          , bd::scalar_<bd::integer_<A1>>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A1 & a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left ints64 sse2: a shift is out of range");
      return _mm_slli_epi64(a0, int(a1));
    }
  };
} } }

#endif


#ifndef DOODAD_H
#define DOODAD_H

#include <set>
#include <glm/glm.hpp>
#include <GL/glew.h>
#include <boost/tokenizer.hpp>
#include <sstream>
#include <iomanip>
#include "SceneGraph.hpp"
#include "Texture.hpp"
#include "Shader.hpp"
#include "Util.hpp"

static const char * ddVertPath = "shader/doodad.vert";
static const char * ddFragPath = "shader/doodad.frag";

enum class DoodadMount
{
  center = 0, topRight = 1, topLeft = 2, bottomRight = 3, bottomLeft = 4
};

class Segment : public Drawable
{
public:
  void draw();
  static std::shared_ptr<Segment> getSegment(float length, float topScale,
                                             float bottomScale,
                                             float topLength,
                                             float bottomLength,
                                             std::shared_ptr<Shader> inShader);
  std::shared_ptr<Shader> shader;
  glm::mat4 getMountPoint(DoodadMount where);
  static void clearMemo()
  {
    memo = std::map<std::string, std::shared_ptr<Segment> >();
  }
private:
  Segment(float length, float topScale, float bottomScale,
          float topLength, float bottomLength,
          std::shared_ptr<Shader> inShader);
  static std::map<std::string, std::shared_ptr<Segment> > memo;
  CubeMap tex;
  GLuint VAO;
  GLuint VBO;
  GLuint EBO;

  size_t indices;

  glm::vec3 center;
  glm::vec3 topLeft;
  glm::vec3 topRight;
  glm::vec3 bottomLeft;
  glm::vec3 bottomRight;

  glm::vec3 centerNorm;
  glm::vec3 topLeftNorm;
  glm::vec3 topRightNorm;
  glm::vec3 bottomLeftNorm;
  glm::vec3 bottomRightNorm;
};

class Doodad : public Group
{ // a Doodad is a thing that can be generated by L-systems
public:

  Doodad(float length, float topScale, float bottomScale,
         float topLength, float bottomLength,
         std::shared_ptr<Shader> inShader);

  virtual void insert(std::shared_ptr<Node> what)
  {insert(what, DoodadMount::center);}

  virtual void insert(std::shared_ptr<Node> what,
                      DoodadMount where,
                      std::function<void(glm::mat4 &, double)> uFn = doNothing);

  virtual void getLights(const glm::mat4 & inM,
                         std::vector<std::pair<std::shared_ptr<Light>,
                                               glm::mat4> > & vec) const;

  virtual void getCameras(const glm::mat4 & inM,
                          std::vector<std::pair<std::shared_ptr<Camera>,
                                                glm::mat4> > & vec) const;

  virtual void getDrawables(const glm::mat4 & inM,
                            std::vector<std::pair<std::shared_ptr<Drawable>,
                                                  glm::mat4> > & vec) const;

  virtual void update(double time);

private:
  std::shared_ptr<Segment> model;
  std::shared_ptr<Transform> center;
  std::shared_ptr<Transform> topRight;
  std::shared_ptr<Transform> topLeft;
  std::shared_ptr<Transform> bottomRight;
  std::shared_ptr<Transform> bottomLeft;

};

typedef struct _Grammar
{
  char ctor;
  std::vector<float> args;
  std::shared_ptr<struct _Grammar> lhs;
  std::shared_ptr<struct _Grammar> rhs;
} Grammar;

std::shared_ptr<Grammar> parse(std::string str);
std::string toString(std::shared_ptr<Grammar> g);
std::shared_ptr<Grammar> iterate(size_t n, std::shared_ptr<Grammar> g);
std::shared_ptr<Group> eval(std::shared_ptr<Grammar> g,
                            int seed,
                            std::shared_ptr<Shader> shader);

/*
  V = { D(len, topScale, bottomScale, topLen, bottomLen), -- Doodad
        C(len, topScale, bottomScale, topLen, bottomLen), -- Doodad(center only)
        S(scale), -- scepter
        T(minSegs) } -- angryTentacle
  S = { F(theta), -- fanout in 3 directions
        K(theta), -- fork in 2 directions
        A(r, s) -- for all directions produced by r, do subrule s }
  ω = { A(C(6,1,1,1,1), D(1.0f, 3.0f, 2.5f, 0.5f, 1.0f)) }
  ω = { A(C(6,1,1,1,1), A(F(1), S(1))) }
  ω = { A(S(1), A(K(1), T(3)) }

 */

std::shared_ptr<Transform> scepter(int seed,
                                   float scaleFactor,
                                   std::shared_ptr<Shader> shader,
                                   std::shared_ptr<Transform> & p1,
                                   std::shared_ptr<Transform> & p2,
                                   std::shared_ptr<Transform> & p3,
                                   std::shared_ptr<Transform> & p4);

std::shared_ptr<Doodad> angryTentacle(int seed,
                                      size_t minSegs,
                                      std::shared_ptr<Shader> shader,
                                      std::shared_ptr<Doodad> & p1);

std::shared_ptr<Group> fanout(int seed,
                              float theta,
                              std::shared_ptr<Transform> & p1,
                              std::shared_ptr<Transform> & p2,
                              std::shared_ptr<Transform> & p3);

std::shared_ptr<Group> fork(int seed,
                            float theta,
                            std::shared_ptr<Transform> & l,
                            std::shared_ptr<Transform> & r);

std::string baseOne();
std::string baseTwo();
std::string baseThree();
#endif


//
// Copyright Jason Rice 2016
// 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 NBDL_UID_HPP
#define NBDL_UID_HPP

#include <nbdl/concept/Buffer.hpp>
#include <nbdl/detail/hash_combine.hpp>
#include <nbdl/entity.hpp>
#include <nbdl/key.hpp>

#include <array>
#include <boost/hana/ext/std/array.hpp>
#include <cstdint>
#include <iterator>
#include <functional>
#include <tuple>

#ifdef EMSCRIPTEN
#include <emscripten.h>
#else
#include <random>
#endif

namespace nbdl
{
  struct make_uid_generator_fn
  {
    auto operator()() const;
  };

  constexpr make_uid_generator_fn make_uid_generator{};

  struct uid
  {
    using value_type = uint8_t;

    uid() = default;

    constexpr auto size()         { return storage.size(); };
    constexpr auto size() const   { return storage.size(); };
    constexpr auto data()         { return storage.data(); };
    constexpr auto data() const   { return storage.data(); };
    constexpr auto begin()        { return storage.begin(); };
    constexpr auto end()          { return storage.end(); };
    constexpr auto begin() const  { return storage.begin(); };
    constexpr auto end()   const  { return storage.end(); };

    inline decltype(auto) operator[](std::size_t i) const { return storage[i]; };
    inline decltype(auto) operator[](std::size_t i)       { return storage[i]; };

    inline bool operator==(nbdl::uid const& o) const
    { return storage == o.storage; }
    inline bool operator!=(nbdl::uid const& o) const
    { return storage != o.storage; }

  private:
    using storage_t = std::array<value_type, 16>;
    friend make_uid_generator_fn;
    friend void swap(nbdl::uid&, nbdl::uid&);

    uid(storage_t&& s)
      : storage(std::move(s))
    { }

    storage_t storage;
  };

  // swap

  inline void swap(nbdl::uid& x, nbdl::uid& y)
  {
    std::swap(x.storage, y.storage);
  }

  template <typename T>
  using unique_key = key<T, uid>;

#ifdef EMSCRIPTEN
  inline auto make_uid_generator_fn::operator()() const
  {
    return []
    {
      nbdl::uid xs{};
      for (auto& x : xs)
      {
        x = (uint8_t) (emscripten_random() * 256);
      }
      return xs;
    };
  };
#else
  inline auto make_uid_generator_fn::operator()() const
  {
    static_assert(sizeof(typename uid::storage_t) == sizeof(std::array<uint32_t, 4>));
    std::random_device rd{};
    std::mt19937 mt_{rd()};

    return [mt = std::move(mt_)]() mutable
    {
      std::array<uint32_t, 4> temp{{
        (uint32_t) mt()
      , (uint32_t) mt()
      , (uint32_t) mt()
      , (uint32_t) mt()
      }};
      return uid{std::move(*reinterpret_cast<typename uid::storage_t*>(temp.data()))};
    };
  };
#endif
}

namespace std
{
  // hash

  template <>
  class hash<nbdl::uid>
  {
  public:
    std::size_t operator()(nbdl::uid const& uid) const
    {
      std::size_t digest = 0;
      for (auto const& x : uid)
      {
        nbdl::detail::hash_combine(digest, x);
      }
      return digest;
    }
  };

  // tuple_size

  template <>
  class tuple_size<nbdl::uid>
    : public std::integral_constant<std::size_t, 16>
  { };

}

#endif


#include "path.h"

#include <boost/log/trivial.hpp>

namespace WorldEngine
{

struct Node;
typedef std::shared_ptr<Node> NodePtr;
typedef std::list<NodePtr>    NodeList;

const uint32_t MAX_PATH_ITERATIONS = 10000u;

/**
 * @brief The basic unit, pixel or location
 */
struct Node
{
   Point   location_;     /**< Where the node is located */
   float   movementCost_; /**< Total move cost to reach this node */
   float   score_;        /**< Calculated score for this node */
   NodePtr parent_;       /**< Parent node */

   Node(Point location, float movementCost, NodePtr parent = nullptr) :
       location_(location),
       movementCost_(movementCost),
       score_(0.0f),
       parent_(parent)
   {
   }
};

struct Path
{
   NodeList nodes_;
   float    totalCost_;

   Path(NodeList nodes, float totalCost) : nodes_(nodes), totalCost_(totalCost)
   {
   }
};

bool operator==(const Node& lhs, const Node& rhs)
{
   return lhs.location_ == rhs.location_;
}

/**
 * @brief A simple square map implementation
 */
struct SQMapHandler
{
   const ElevationArrayType& mapData_;
   const int32_t             width_;
   const int32_t             height_;

   SQMapHandler(const ElevationArrayType& mapData,
                int32_t                   width,
                int32_t                   height) :
       mapData_(mapData), width_(width), height_(height)
   {
   }

   NodeList GetAdjacentNodes(NodePtr currentNode, Point destination) const
   {
      NodeList result;

      const Point& cl = currentNode->location_;
      const Point& dl = destination;

      NodePtr n;

      n = HandleNode(cl.first + 1, cl.second, currentNode, dl.first, dl.second);
      if (n != nullptr)
      {
         result.push_back(n);
      }

      n = HandleNode(cl.first - 1, cl.second, currentNode, dl.first, dl.second);
      if (n != nullptr)
      {
         result.push_back(n);
      }

      n = HandleNode(cl.first, cl.second + 1, currentNode, dl.first, dl.second);
      if (n != nullptr)
      {
         result.push_back(n);
      }

      n = HandleNode(cl.first, cl.second - 1, currentNode, dl.first, dl.second);
      if (n != nullptr)
      {
         result.push_back(n);
      }

      return result;
   }

   NodePtr GetNode(Point location) const
   {
      int32_t x = location.first;
      int32_t y = location.second;
      if (x < 0 || x >= width_ || y < 0 || y >= height_)
      {
         return nullptr;
      }
      float d = mapData_[y][x];
      return std::make_shared<Node>(location, d);
   }

   NodePtr HandleNode(int32_t x,
                      int32_t y,
                      NodePtr fromNode,
                      int32_t destX,
                      int32_t destY) const
   {
      NodePtr n = GetNode({x, y});

      if (n != nullptr)
      {
         int32_t dx     = std::max(x, destX) - std::min(x, destX);
         int32_t dy     = std::max(y, destY) - std::min(y, destY);
         int32_t emCost = dx + dy;
         n->movementCost_ += fromNode->movementCost_;
         n->score_  = n->movementCost_ + emCost;
         n->parent_ = fromNode;
      }

      return n;
   }
};

/**
 * @brief The A* Search Algorithm
 *
 * https://en.wikipedia.org/wiki/A*_search_algorithm
 *
 * TODO: Use Boost implementation
 */
struct AStar
{
   const SQMapHandler& mapHandler_;
   NodeList            openSet;
   NodeList            closedSet;

   AStar(const SQMapHandler& mapHandler) : mapHandler_(mapHandler) {}

   NodePtr GetBestOpenNode()
   {
      NodePtr bestNode = nullptr;

      for (NodePtr n : openSet)
      {
         if (bestNode == nullptr || n->score_ <= bestNode->score_)
         {
            bestNode = n;
         }
      }

      return bestNode;
   }

   static Path TracePath(NodePtr n)
   {
      NodeList nodes;
      NodePtr  p         = n->parent_;
      float    totalCost = n->movementCost_;

      nodes.push_front(n);

      while (p->parent_ != nullptr)
      {
         nodes.push_front(p);
         p = p->parent_;
      }

      return Path(nodes, totalCost);
   }

   NodePtr HandleNode(NodePtr node, Point end)
   {
      openSet.remove_if([&node](const NodePtr o) { return *node == *o; });
      closedSet.push_back(node);

      NodeList nodes = mapHandler_.GetAdjacentNodes(node, end);

      NodeList::iterator openNode;

      for (NodePtr n : nodes)
      {
         if (n->location_ == end)
         {
            // Reached the destination
            return n;
         }
         else if (std::find_if(closedSet.begin(), //
                               closedSet.end(),
                               [&n](const NodePtr o) { return *n == *o; }) !=
                  closedSet.end())
         {
            // Already in closed set, skip this
            continue;
         }
         else if ((openNode = std::find_if(openSet.begin(), //
                                           openSet.end(),
                                           [&n](const NodePtr o) {
                                              return *n == *o;
                                           })) != openSet.end())
         {
            // Already in open set, check if better score
            if (n->movementCost_ < (*openNode)->movementCost_)
            {
               openSet.erase(openNode);
               openSet.push_back(n);
            }
         }
         else
         {
            // New node, append to open set
            openSet.push_back(n);
         }
      }

      return nullptr;
   }

   Path FindPath(Point fromLocation, Point toLocation)
   {
      NodePtr nextNode = mapHandler_.GetNode(fromLocation);
      openSet.push_back(nextNode);

      uint32_t counter = 0;

      while (nextNode != nullptr)
      {
         if (counter > MAX_PATH_ITERATIONS)
         {
            BOOST_LOG_TRIVIAL(warning) << "FindPath: Exceeded trial limit";
            break;
         }

         NodePtr finish = HandleNode(nextNode, toLocation);
         if (finish != nullptr)
         {
            return TracePath(finish);
         }

         nextNode = GetBestOpenNode();
         counter++;
      }

      return Path(NodeList(), 0.0f);
   }
};

std::list<Point>
FindPath(const ElevationArrayType& elevation, Point source, Point destination)
{
   std::list<Point> path;

   const int32_t width  = static_cast<int32_t>(elevation.shape()[1]);
   const int32_t height = static_cast<int32_t>(elevation.shape()[0]);

   SQMapHandler mapHandler(elevation, width, height);
   AStar        pathFinder(mapHandler);
   Path         p = pathFinder.FindPath(source, destination);

   for (NodePtr n : p.nodes_)
   {
      path.push_back(n->location_);
   }

   return path;
}

} // namespace WorldEngine


#ifndef _GATE_EST_GATE_EST_NODE_HPP_
#define _GATE_EST_GATE_EST_NODE_HPP_

#include <opencv2/opencv.hpp>

#include <Eigen/Core>
#include <sensor_msgs/CameraInfo.h>
#include <flightgoggles/IRMarkerArray.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <nav_msgs/Odometry.h>

#include <vector>
#include <memory>
#include <map>
#include <deque>

#include "base_node.hpp"
#include "frame.hpp"

namespace gate_est {

  class GateEst : public xros::RunnableNode<GateEst> {
  public:
    GateEst(XROS_CONSTRUCTOR_DEFAULT_PARAMETERS)
     : XROS_CONSTRUCTOR_INIT_RUNNABLE,
       tf_listener_(buffer_),
       gate_offsets_{0} {}

    virtual void Init();

  public:
    static constexpr std::size_t NUM_GATES = 23;

  private:
    void LoadGateNominalInformation();
    void LoadInitialPose();
    void CameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& ptr);
    void IRMarkerArrayCallback(const flightgoggles::IRMarkerArrayConstPtr& ptr);
    void OdometryCallback(const nav_msgs::OdometryConstPtr& ptr);

    void BackProject(double x, double y, double depth, Eigen::Vector3d& out);
    void Pixel2Camera(double x, double y, Eigen::Vector2d& out);
    void Pixel2Camera(double x, double y, cv::Point2d& out);
    cv::Point2d Pixel2Camera(double x, double y);

    Frame::Ptr p_ref_;

    // gate offset estimation
    // each gate we need to store the x, y & z offset
    double gate_offsets_[NUM_GATES][3];

    // camera matrix stored as eigen matrix
    Eigen::Matrix3d cm_;

    // storing the (nominal) position of the gates (corners)
    std::vector<std::vector<Eigen::Vector3d>> gate_corners_;

    Landmark3D gates_;

    // FIXME: don't hard-coded the nearest gate here.
    std::size_t reference_gate_idx_ = 10;
    Eigen::Vector3d initial_position_;
    Eigen::Quaterniond initial_orientation_;

    // camera matrix
    cv::Mat k_;

    ros::Subscriber camera_info_sub_;
    ros::Subscriber ir_beacons_sub_;
    ros::Subscriber odometry_sub_;

    ros::Publisher corners_viz_pub_;

    tf2_ros::Buffer buffer_;
    tf2_ros::TransformListener tf_listener_;
  };

}

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_NEXTAFTER_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_NEXTAFTER_HPP_INCLUDED
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/properties.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/eps_related.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/next.hpp>
#include <nt2/include/functions/prev.hpp>
#include <nt2/include/functions/is_finite.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/negif.hpp>
#include <nt2/include/functions/copysign.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::nextafter_, tag::cpu_,
                            (A0)(X),
                            ((simd_<arithmetic_<A0>,X>))
                            ((simd_<arithmetic_<A0>,X>))
                           );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::nextafter_(tag::simd_<tag::arithmetic_, X> ,
                              tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      return seladd(is_not_equal(a0,a1),a0,seladd(gt(a1,a0),-One<A0>(),Two<A0>()));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is unsigned_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::nextafter_, tag::cpu_,
                            (A0)(X),
                            ((simd_<unsigned_<A0>,X>))
                            ((simd_<unsigned_<A0>,X>))
                           );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::nextafter_(tag::simd_<tag::unsigned_, X> ,
                              tag::simd_<tag::unsigned_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      return sel(is_equal(a0,a1),a0,sel(gt(a1,a0),a0+One<A0>(),a0-One<A0>()));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::nextafter_, tag::cpu_,
                            (A0)(X),
                            ((simd_<real_<A0>,X>))
                            ((simd_<real_<A0>,X>))
                           );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::nextafter_(tag::simd_<tag::real_, X> ,
                              tag::simd_<tag::real_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename meta::as_integer<A0, signed>::type itype;
      A0 m;
      itype expon;
      boost::fusion::tie(m, expon) = fast_frexp(a0);
      expon =  seladd(is_equal(m, Mhalf<A0>()), expon, Mone<itype>());
      A0 diff =  fast_ldexp(One<A0>(), expon-Nbdigits<A0>());
      diff = b_and(sel(is_eqz(diff)||is_eqz(a0),  Mindenormal<A0>(), diff), is_finite(a0));
      A0 r = copysign(sel(is_equal(a0, Minf<A0>()), Valmin<A0>(), a0), a0);
      diff   =  b_and(negif(gt(a0, a1), diff), is_not_equal(a0, a1));
      return r+diff;
      //      return sel(islt(a0, a1), next(a0), sel(iseq(a0, a1),  a0, prev(a0)));
    }
  };
} }

#endif
// modified by jt the 04/01/2011


// Copyright (C) 2005-2006 Douglas Gregor <doug.gregor@gmail.com>

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

// A test of the is_mpi_op functionality.
#include <boost/mpicxx/operations.hpp>
#include <boost/mpicxx/environment.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include "check_test.hpp"

namespace mpi = boost::mpicxx;
using namespace std;
using boost::is_base_and_derived;

int main(int argc, char* argv[])
{
  boost::mpicxx::environment env(argc, argv);
  mpi::communicator comm;
  // Check each predefined MPI_Op type that we support directly.
  check_test(comm, (mpi::is_mpi_op<mpi::maximum<int>, int>::op() == MPI_MAX));
  check_test(comm, (mpi::is_mpi_op<mpi::minimum<float>, float>::op() == MPI_MIN));
  check_test(comm, (mpi::is_mpi_op<std::plus<double>, double>::op() == MPI_SUM));
  check_test(comm, (mpi::is_mpi_op<std::multiplies<long>, long>::op() == MPI_PROD));
  check_test(comm, (mpi::is_mpi_op<std::logical_and<int>, int>::op() == MPI_LAND));
  check_test(comm, (mpi::is_mpi_op<mpi::bitwise_and<int>, int>::op() == MPI_BAND));
  check_test(comm, (mpi::is_mpi_op<std::logical_or<int>, int>::op() == MPI_LOR));
  check_test(comm, (mpi::is_mpi_op<mpi::bitwise_or<int>, int>::op() == MPI_BOR));
  check_test(comm, (mpi::is_mpi_op<mpi::logical_xor<int>, int>::op() == MPI_LXOR));
  check_test(comm, (mpi::is_mpi_op<mpi::bitwise_xor<int>, int>::op() == MPI_BXOR));

  return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/constant/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/constant.hpp>
using namespace boost::hana;

struct UDT {
    struct Type1 { };
    struct Type2 { };
    Type1 member1;
    Type2 member2;
};

template <typename mcd, typename T, typename U>
void test() {
    BOOST_HANA_CONSTANT_ASSERT( equal(detail::minimal::constant<T, 0, mcd>, detail::minimal::constant<U, 0, mcd>));
    BOOST_HANA_CONSTANT_ASSERT(!equal(detail::minimal::constant<T, 0, mcd>, detail::minimal::constant<U, 1, mcd>));

    // Incomparable constants must appear as unequal.
    BOOST_HANA_CONSTANT_ASSERT(!equal(
        detail::minimal::constant<T, 0, mcd>,
        detail::minimal::constant<decltype(&UDT::member1), &UDT::member1, mcd>
    ));
    BOOST_HANA_CONSTANT_ASSERT(!equal(
        detail::minimal::constant<decltype(&UDT::member1), &UDT::member1, mcd>,
        detail::minimal::constant<decltype(&UDT::member2), &UDT::member2, mcd>
    ));
}

int main() {
    test<Constant::mcd, int, int>();
    test<Constant::mcd, int, long>();
}


#ifndef SPS_PLANE_HPP
#define SPS_PLANE_HPP

#include <Eigen/Core>
#include <array>
#include <utility>
#include <vector>

namespace sps
{
    class AbstractPlane
    {
    public:
        /// \brief Index for grid cells on a plane.
        ///
        /// \details (0, 0) is the center.
        using GridCellIndex = std::pair<int, int>;

        const Eigen::VectorXd&                GetCenter() const { return m_center; }
        const std::array<Eigen::VectorXd, 4>& GetVertices() const { return m_vertices; }

        /// \brief Calculate N-d parameters corresponding to the specified grid coordinates (i.e., [0, 1]^2).
        Eigen::VectorXd CalcParameters(const Eigen::Vector2d& coords) const;

        /// \brief Calculate N-d parameters corresponding to the specified settings and user interactions.
        Eigen::VectorXd CalcGridParameters(const GridCellIndex&              grid_cell,
                                           const unsigned                    num_candidates,
                                           const double                      inter_level_scale,
                                           const std::vector<GridCellIndex>& prev_grid_cells) const;

        double CalcArea() const;

    protected:
        Eigen::VectorXd                m_center;
        std::array<Eigen::VectorXd, 4> m_vertices;
    };

    class SimplePlane : public AbstractPlane
    {
    public:
        SimplePlane(const Eigen::VectorXd& x_base, const Eigen::VectorXd& x_opposite, const Eigen::VectorXd& cross_vec)
        {
            m_center      = 0.5 * (x_base + x_opposite);
            m_vertices[0] = x_base;
            m_vertices[1] = m_center + cross_vec;
            m_vertices[2] = x_opposite;
            m_vertices[3] = m_center - cross_vec;
        }

        const Eigen::VectorXd& GetBaseVertex() const { return m_vertices[0]; }
        Eigen::VectorXd        GetCrossVec() const { return m_vertices[1] - m_center; }
    };

    class PiecewisePlanarPlane : public AbstractPlane
    {
    public:
        PiecewisePlanarPlane(const Eigen::VectorXd& center,
                             const Eigen::VectorXd& vertex_0,
                             const Eigen::VectorXd& vertex_1,
                             const Eigen::VectorXd& vertex_2,
                             const Eigen::VectorXd& vertex_3)
        {
            m_center      = center;
            m_vertices[0] = vertex_0;
            m_vertices[1] = vertex_1;
            m_vertices[2] = vertex_2;
            m_vertices[3] = vertex_3;
        }
    };
} // namespace sps

#endif // SPS_PLANE_HPP


#include <cmath>
#include <algorithm>
#include <armadillo>
#include <thread>
#include "RHF.hpp"
#include "RHFGrad.hpp"

using namespace std;
using namespace libint2;

namespace willow {  namespace qcmol {

static unsigned int num_threads;

static arma::vec compute_nuclear_gradient (const vector<Atom>& atoms,
					   const vector<QAtom>& atoms_Q,
					   arma::vec& grd_Q);

void compute_2body_deriv_ints (const int thread_id,
			       const BasisSet& obs,
			       const vector<Atom>& atoms,
			       const arma::mat& Dm,
			       const arma::mat& Schwartz,
			       arma::vec& result);

template <libint2::Operator obtype>
void compute_1body_deriv_ints (const int thread_id,
			       const BasisSet& obs,
			       const vector<Atom>& atoms,
			       const arma::mat& Dm,
			       arma::vec& result);

void compute_1body_deriv_nuclear (const int thread_id,
				  const BasisSet& obs,
				  const vector<Atom>& atoms,
				  const arma::mat& Dm,
				  const vector<QAtom>& atoms_Q,
				  arma::vec& grd,
				  arma::vec& grd_Q);

//-----
//
//----
RHFGrad::RHFGrad (const vector<Atom>& atoms,
		  const BasisSet& bs,
		  const Integrals& ints,
		  const int qm_chg,
		  const bool l_print,
		  const vector<QAtom>& atoms_Q )
  : RHF (atoms, bs, ints, qm_chg, l_print, atoms_Q)
{

  num_threads = std::thread::hardware_concurrency();
  if (num_threads == 0) num_threads = 1;

  arma::mat D = densityMatrix ();
  arma::mat W = energyWeightDensityMatrix ();

  const auto natom = atoms.size();
  const auto nbf   = bs.nbf();

  vector<arma::vec> grd_kin_t (num_threads);
  vector<arma::vec> grd_nuc_t (num_threads);
  vector<arma::vec> grd_nuc_Q_t(num_threads);
  vector<arma::vec> frc_orth_t(num_threads);

  vector<std::thread> t_grd_kin (num_threads);
  vector<std::thread> t_grd_nuc (num_threads);
  vector<std::thread> t_frc_orth(num_threads);

  vector<QAtom> bq_Q;

  for (int id = 0; id < num_threads; ++id) {
    // Kinetic
    t_grd_kin[id] =
      std::thread (compute_1body_deriv_ints<libint2::Operator::kinetic>,
		   id,
		   std::cref(bs),
		   std::cref(atoms),
		   std::cref(D),
		   std::ref(grd_kin_t[id]) );
    // Nuclear Der
    t_grd_nuc[id] =
      std::thread (compute_1body_deriv_nuclear,
		   id,
		   std::cref(bs),
		   std::cref(atoms),
		   std::cref(D),
		   std::cref(atoms_Q),
		   std::ref(grd_nuc_t[id]),
		   std::ref(grd_nuc_Q_t[id]) );

    // Overlap
    t_frc_orth[id] =
      std::thread (compute_1body_deriv_ints<libint2::Operator::overlap>,
		   id,
		   std::cref(bs),
		   std::cref(atoms),
		   std::cref(W),
		   std::ref(frc_orth_t[id]) );
  }

  // Join
  for (auto id = 0; id < num_threads; ++id) {
    t_grd_kin[id].join();
    t_grd_nuc[id].join();
    t_frc_orth[id].join();
  }

  for (auto id = 1; id < num_threads; ++id) {
    grd_kin_t[0]  += grd_kin_t[id];
    grd_nuc_t[0]  += grd_nuc_t[id];
    grd_nuc_Q_t[0] += grd_nuc_Q_t[id];
    frc_orth_t[0] += frc_orth_t[id];
  }

  // Nuclear Repulsion
  arma::vec grd_rep_Q (3*atoms_Q.size(), arma::fill::zeros);
  arma::vec grd_rep  = compute_nuclear_gradient (atoms, atoms_Q, grd_rep_Q);

  // Two electron Integral
  vector<arma::vec>   grd_eri_t (num_threads);
  vector<std::thread> t_grd_eri (num_threads);

  for (auto id = 0; id < num_threads; ++id) {
    t_grd_eri[id] =
      std::thread (compute_2body_deriv_ints,
		   id,
		   std::cref(bs),
		   std::cref(atoms),
		   std::cref(D),
		   std::cref(ints.Km),
		   std::ref(grd_eri_t[id]) );
  }

  for (auto id = 0; id < num_threads; ++id)
    t_grd_eri[id].join();

  for (auto id = 1; id < num_threads; ++id)
    grd_eri_t[0] += grd_eri_t[id];

  m_grad = grd_kin_t[0] + grd_nuc_t[0] - frc_orth_t[0] + grd_rep + grd_eri_t[0];
  m_grad_Q = grd_nuc_Q_t[0] + grd_rep_Q;

  if (l_print) {
    cout << "Total Gradient (QM): " << endl;

    for (auto i = 0; i < natom; ++i)
      cout << (i + 1)
	   << "  " << m_grad(3*i)
	   << "  " << m_grad(3*i+1)
	   << "  " << m_grad(3*i+2) << endl;

    auto natom_Q = atoms_Q.size();

    if (natom_Q > 0) {
      cout << "Total Gradient (BQ): " << endl;
      for (auto i = 0; i < natom_Q; ++i)
	cout << (i + 1)
	     << "  " << m_grad_Q(3*i)
	     << "  " << m_grad_Q(3*i+1)
	     << "  " << m_grad_Q(3*i+2) << endl;
    }

  }
}

template<libint2::Operator obtype>
void compute_1body_deriv_ints (const int thread_id,
			       const BasisSet& bs,
			       const vector<Atom>& atoms,
			       const arma::mat& Dm,
			       arma::vec& grd)
{
  const auto nshl = bs.size();
  const auto nbf  = bs.nbf();
  const auto natom = atoms.size();

  const unsigned deriv_order = 1;

  constexpr auto nopers = libint2::operator_traits<obtype>::nopers;

  const auto nresults = nopers * libint2::num_geometrical_derivatives (natom, deriv_order);

  grd = arma::vec(nresults, arma::fill::zeros);

  libint2::Engine engine (obtype,
			  bs.max_nprim(),
			  bs.max_l(),
			  deriv_order);

  const auto& buf = engine.results();

  auto shell2bf = bs.shell2bf();
  auto shell2atom = bs.shell2atom(atoms);

  for (auto s1 = 0, s12 = 0; s1 != nshl; ++s1){
    auto bf1  = shell2bf[s1];
    auto nbf1 = bs[s1].size();
    auto at1  = shell2atom[s1];

    assert (at1 != -1);

    for (auto s2 = 0; s2 <= s1; ++s2, ++s12){

      if (s12 % num_threads != thread_id) continue;

      auto bf2  = shell2bf[s2];
      auto nbf2 = bs[s2].size();
      auto at2  = shell2atom[s2];

      auto nbf12 = nbf1*nbf2;

      engine.compute (bs[s1], bs[s2]);

      assert(deriv_order == 1);

      // 1. Process derivatives with respect to the Gaussian origins first
      //
      for (unsigned int d = 0; d != 6; ++d) { // 2 centers x 3 axes
	auto iat = d < 3 ? at1 : at2;
	auto op_start = (3*iat + d%3)*nopers;
	auto op_fence = op_start + nopers;
	const auto* buf_idx = buf[d];
	if (buf_idx == nullptr) continue;

	for (unsigned int op = op_start; op != op_fence; ++op) {

	  for (auto ib = 0, ij = 0; ib < nbf1; ib++)
	    for (auto jb = 0; jb < nbf2; jb++, ij++) {
	      const auto val = buf_idx[ij];

	      grd(op) += Dm(bf1+ib,bf2+jb)*val;

	      if (s1 != s2) {
		grd(op) += Dm(bf2+jb,bf1+ib)*val;
	      }

	    }
	} // exit op
      } // exit d

    }
  }

}

void compute_1body_deriv_nuclear (const int thread_id,
				  const BasisSet& bs,
				  const vector<Atom>& atoms,
				  const arma::mat& Dm,
				  const vector<QAtom>& atoms_Q,
				  arma::vec& grd,
				  arma::vec& grd_Q)
{
  const auto nshl = bs.size();
  const auto nbf  = bs.nbf();
  const auto natom = atoms.size();
  const auto natom_Q = atoms_Q.size();

  const unsigned deriv_order = 1;
  constexpr auto nopers = libint2::operator_traits<Operator::nuclear>::nopers;

  const auto nresults = nopers * libint2::num_geometrical_derivatives (natom, deriv_order);

  grd   = arma::vec(nresults, arma::fill::zeros);
  grd_Q = arma::vec(atoms_Q.size()*3, arma::fill::zeros);

  libint2::Engine engine (Operator::nuclear,
			  bs.max_nprim(),
			  bs.max_l(),
			  deriv_order);

  const auto& buf = engine.results ();

  // nuclear attraction ints engine
  vector<pair<double, array<double,3>>> q;
  for (const auto& atom : atoms){
    q.push_back( {static_cast<double> (atom.atomic_number),
	  {{atom.x, atom.y, atom.z}}} );
  }

  for (auto iq = 0; iq < natom_Q; ++iq) {
    q.push_back ( {atoms_Q[iq].charge,
	  {{atoms_Q[iq].x, atoms_Q[iq].y, atoms_Q[iq].z}}} );
  }

  engine.set_params(q);

  auto shell2bf = bs.shell2bf();
  auto shell2atom = bs.shell2atom(atoms);

  for (auto s1 = 0, s12 = 0; s1 != nshl; ++s1){
    auto bf1  = shell2bf[s1];
    auto nbf1 = bs[s1].size();
    auto at1  = shell2atom[s1];

    assert (at1 != -1);

    for (auto s2 = 0; s2 <= s1; ++s2, ++s12){

      if (s12 % num_threads != thread_id) continue;

      auto bf2  = shell2bf[s2];
      auto nbf2 = bs[s2].size();
      auto at2  = shell2atom[s2];

      auto nbf12 = nbf1*nbf2;

      engine.compute (bs[s1], bs[s2]);

      assert(deriv_order == 1);

      // 1. Process derivatives with respect to the Gaussian origins first
      //
      for (unsigned int d=0; d != 6; ++d){ // 2 centers x 3 axes = 6 cartesian geometric derivatives

	auto iat = d < 3 ? at1 : at2;
	auto op_start = (3*iat + d%3)*nopers;
	auto op_fence = op_start + nopers;

	const auto* buf_idx = buf[d];
	if (buf_idx == nullptr) continue;

	for (unsigned int op = op_start; op != op_fence; ++op) {

	  for (auto ib = 0, ij = 0; ib < nbf1; ib++)
	    for (auto jb = 0; jb < nbf2; jb++, ij++) {
	      const auto val = buf_idx[ij];

	      grd(op) += Dm(bf1+ib,bf2+jb)*val;

	      if (s1 != s2) {
		grd(op) += Dm(bf2+jb,bf1+ib)*val;
	      }

	    }
	}

      } // d

      // 2. Process derivatives of nuclear Coulomb operators,
      for (unsigned int iat = 0; iat != natom; ++iat) {
	for (unsigned int ixyz = 0; ixyz != 3; ++ixyz) {

	  const auto* buf_idx = buf[6 + iat*3 + ixyz];

	  auto op_start = (3*iat + ixyz) * nopers;
	  auto op_fence = op_start + nopers;

	  for (unsigned int op = op_start;
	       op != op_fence; ++op) {

	    for (auto ib = 0, ij = 0; ib < nbf1; ib++)
	      for (auto jb = 0; jb < nbf2; jb++, ij++) {
		const double val = buf_idx[ij];

		grd(op) += Dm(bf1+ib,bf2+jb)*val;
		if (s1 != s2)
		  grd(op) += Dm(bf2+jb,bf1+ib)*val;

	      }
	  }
	}
      } // iat =

      for (unsigned int iq = 0; iq != natom_Q; ++iq) {
	for (unsigned int ixyz = 0; ixyz != 3; ++ixyz) {

	  const auto* buf_idx = buf[6 + (natom + iq)*3 + ixyz];
	  auto op_start = (3*iq + ixyz) * nopers;
	  auto op_fence = op_start + nopers;

	  for (unsigned int op = op_start;
	       op != op_fence; ++op) {

	    for (auto ib = 0, ij = 0; ib < nbf1; ib++)
	      for (auto jb = 0; jb < nbf2; jb++, ij++) {
		const double val = buf_idx[ij];

		grd_Q(op) += Dm(bf1+ib,bf2+jb)*val;
		if (s1 != s2)
		  grd_Q(op) += Dm(bf2+jb,bf1+ib)*val;

	      }
	  }
	}
      } // iQ =

    }

  }

}

arma::vec compute_nuclear_gradient (const vector<Atom>& atoms,
				    const vector<QAtom>& atoms_Q,
				    arma::vec& grd_Q)
{
  arma::vec grd (3*atoms.size(), arma::fill::zeros);
  if (atoms_Q.size() > 0)
    grd_Q.zeros();

  for (auto i = 0; i < atoms.size(); ++i) {
    double chg_i = (double)atoms[i].atomic_number;
    double xi = atoms[i].x;
    double yi = atoms[i].y;
    double zi = atoms[i].z;

    for (auto j = 0; j < i; ++j) {
      double chg_j = (double)atoms[j].atomic_number;
      // calculate distance

      double dx = xi - atoms[j].x;
      double dy = yi - atoms[j].y;
      double dz = zi - atoms[j].z;

      double rij2 = dx*dx + dy*dy + dz*dz;
      double rij3 = sqrt(rij2)*rij2;

      arma::vec tmp(3);

      tmp(0) = chg_i*chg_j/rij3*dx;
      tmp(1) = chg_i*chg_j/rij3*dy;
      tmp(2) = chg_i*chg_j/rij3*dz;

      grd(3*i  ) -= tmp(0);
      grd(3*i+1) -= tmp(1);
      grd(3*i+2) -= tmp(2);
      grd(3*j  ) += tmp(0);
      grd(3*j+1) += tmp(1);
      grd(3*j+2) += tmp(2);
    }

    for (auto j = 0; j < atoms_Q.size(); ++j) {
      double chg_j = atoms_Q[j].charge;
      // calculate distance

      double dx = xi - atoms_Q[j].x;
      double dy = yi - atoms_Q[j].y;
      double dz = zi - atoms_Q[j].z;

      double rij2 = dx*dx + dy*dy + dz*dz;
      double rij3 = sqrt(rij2)*rij2;

      arma::vec tmp(3);

      tmp(0) = chg_i*chg_j/rij3*dx;
      tmp(1) = chg_i*chg_j/rij3*dy;
      tmp(2) = chg_i*chg_j/rij3*dz;

      grd(3*i  ) -= tmp(0);
      grd(3*i+1) -= tmp(1);
      grd(3*i+2) -= tmp(2);

      grd_Q(3*j  ) += tmp(0);
      grd_Q(3*j+1) += tmp(1);
      grd_Q(3*j+2) += tmp(2);
    }

  }

  return grd;

}

// --

void compute_2body_deriv_ints (const int thread_id,
			       const BasisSet& bs,
			       const vector<Atom>& atoms,
			       const arma::mat& Dm,
			       const arma::mat& Schwartz,
			       arma::vec& grd)
{

  const auto nbf     = bs.nbf();
  const auto nshells = bs.size();
  const auto natom   = atoms.size();
  const auto shell2atom = bs.shell2atom(atoms);

  grd =  arma::vec(3*natom, arma::fill::zeros);

  libint2::Engine engine  (libint2::Operator::coulomb,
			   bs.max_nprim(),
			   bs.max_l(),
			   1);

  const auto precision = numeric_limits<double>::epsilon();
  engine.set_precision (precision);
  const auto& buf = engine.results();

  auto shell2bf = bs.shell2bf();

  // loop over permutationally-unique set of shells
  for (auto s1 = 0, s1234 = 0; s1 != nshells; ++s1) {
    auto bf1_first = shell2bf[s1];
    auto nbf1      = bs[s1].size();
    auto iat       = shell2atom[s1];

    for (auto s2 = 0; s2 <= s1; ++s2) {
      auto bf2_first = shell2bf[s2];
      auto nbf2      = bs[s2].size();
      auto jat       = shell2atom[s2];
      auto s12_cut   = Schwartz(s1,s2);

      for (auto s3 = 0; s3 <= s1; ++s3) {
	auto bf3_first = shell2bf[s3];
	auto nbf3      = bs[s3].size();
	auto kat       = shell2atom[s3];

	const auto s4_max = (s1 == s3) ? s2 : s3;
	for (auto s4 = 0; s4 <= s4_max; ++s4, ++s1234) {

	  if (s1234 % num_threads != thread_id) continue;

	  auto bf4_first = shell2bf[s4];
	  auto nbf4      = bs[s4].size();
	  auto lat       = shell2atom[s4];
	  auto s34_cut   = Schwartz(s3,s4);

	  if (s12_cut*s34_cut < precision) {
	    continue;
	  }

	  auto s12_deg = (s1 == s2) ? 1.0 : 2.0;
	  auto s34_deg = (s3 == s4) ? 1.0 : 2.0;
	  auto s13_s24_deg = (s1 == s3) ? ((s2 == s4) ? 1.0 : 2.0) : 2.0;
	  auto s1234_deg = s12_deg*s34_deg*s13_s24_deg;

	  engine.compute2<Operator::coulomb,BraKet::xx_xx,1>
	    (bs[s1], bs[s2], bs[s3], bs[s4]);

	  arma::vec tmp(12, arma::fill::zeros);

	  for (auto di = 0; di != 12; di++) {
	    const auto shset = buf[di];

	    if (shset == nullptr) continue;

	    double sum = 0.0;
	    for (auto f1 = 0, f1234 = 0; f1 != nbf1; ++f1) {
	      const auto bf1 = f1 + bf1_first;

	      for (auto f2 = 0; f2 != nbf2; ++f2) {
		const auto bf2 = f2 + bf2_first;

		for (auto f3 = 0; f3 != nbf3; ++f3) {
		  const auto bf3 = f3 + bf3_first;

		  for (auto f4 = 0; f4 != nbf4; ++f4, ++f1234) {
		    const auto bf4 = f4 + bf4_first;

		    const auto eri4  = shset[f1234];
		    const auto value = eri4*s1234_deg;

		    sum += 2.0*Dm(bf1,bf2)*Dm(bf3,bf4)*value;
		    sum -= 0.5*Dm(bf1,bf3)*Dm(bf2,bf4)*value;
		    sum -= 0.5*Dm(bf1,bf4)*Dm(bf2,bf3)*value;
		  }
		}
	      }
	    }

	    tmp(di) = 0.25*sum;
	  }// end di

	  // store the gradient

	  grd(3*iat  ) += tmp(0);
	  grd(3*iat+1) += tmp(1);
	  grd(3*iat+2) += tmp(2);
	  grd(3*jat  ) += tmp(3);
	  grd(3*jat+1) += tmp(4);
	  grd(3*jat+2) += tmp(5);
	  grd(3*kat  ) += tmp(6);
	  grd(3*kat+1) += tmp(7);
	  grd(3*kat+2) += tmp(8);
	  grd(3*lat  ) += tmp(9);
	  grd(3*lat+1) += tmp(10);
	  grd(3*lat+2) += tmp(11);

	}
      }
    }
  }

}

} }  // namespace willow::qcmol