#include "EpipolarConsistencyRadonIntermediate.h"

#include <Eigen/Dense>

#include <LibUtilsCuda/CudaMemory.h>
#include <LibUtilsCuda/CudaBindlessTexture.h>

#include <GetSet/ProgressInterface.hxx>

// Eigen is considerbly slower for this particular small problem, so we rely on our own numerical recepies code
#include <LibUtilsCuda/culaut/xprojectionmatrix.hxx>

#include "EpipolarConsistencyCommon.hxx"

// The GPU version
void epipolarConsistency(
	int n_x,				//< Width of original images in pixels
	int n_y,				//< Height of original images in pixels
	int num_dtrs,			//< Number of projection images (=# dtrs)
	char* dtrs_d,			//< DTRs as GPU textures cudaTextureObject_t
	int n_alpha,			//< Number of angular bins (y-direction)
	int n_t,				//< Number of line distance bins (x-direction)
	float step_alpha,		//< Angular spacing of radon transform
	float step_t,			//< Line distance spacing of radon transform
	int num_Ps,				//< Number of projection matrices
	float* Cs_d,			//< Source positions 4 * num_Ps, null spaces of projection matrices
	float* PinvTs_d,		//< Projection Matrix inverse transpose, column major 3x4 * num_Ps
	int num_pairs,			//< Explicit assigment of projection matrices and dtrs. Can be zero.
	int *indices_d,			//< num_indices*4 (P0,P1,dtr0,dtr1). Can be null. If not provided, all n*(n-1)*2 pairs are computed.
	float *K01s_d,			//< num_indices*16 for two 2*3 matrices mapping [cos(kappa sin(kappa)] to lines 0/1
	float *out_d,			//< num_indices result values, unless indices is null. Then nxn matrix. Only sub-diagonal elements are written to.
	float object_radius_mm,	//< Approximate size of object in mm (determines angular range, can be zero)
	float dkappa,			//< Angular distance between epipolar planes (can be zero if object_radius_mm and num_samples are given)
	bool isDerivative,		//< If false, normal Radon transform, else its derivative
	bool use_corr,			//< If null, SSD is used as a similarity measure, else correlation coeff is used.
	float *out_corr_d		//< Temporary memory for weights and NCC computation
	);

namespace EpipolarConsistency
{
	MetricRadonIntermediate::MetricRadonIntermediate()
		: Metric()
		, Cs(new UtilsCuda::MemoryBlock<float>())
		, PinvTs(new UtilsCuda::MemoryBlock<float>())
		, tex_dtrs(new UtilsCuda::MemoryBlock<char>())
		, indices(new UtilsCuda::MemoryBlock<int>())
		, K01s(new UtilsCuda::MemoryBlock<float>())
		, out(new UtilsCuda::MemoryBlock<float>())
		, corr_out(new UtilsCuda::MemoryBlock<float>())
		, use_corr(false)
	{}

	MetricRadonIntermediate::MetricRadonIntermediate(const std::vector<ProjectionMatrix>& Ps, const std::vector<RadonIntermediate*>& _dtrs)
		: Metric()
		, Cs(new UtilsCuda::MemoryBlock<float>())
		, PinvTs(new UtilsCuda::MemoryBlock<float>())
		, tex_dtrs(new UtilsCuda::MemoryBlock<char>())
		, indices(new UtilsCuda::MemoryBlock<int>())
		, K01s(new UtilsCuda::MemoryBlock<float>())
		, out(new UtilsCuda::MemoryBlock<float>())
		, corr_out(new UtilsCuda::MemoryBlock<float>())
		, use_corr(false)
	{
		setProjectionMatrices(Ps);
		setRadonIntermediates(_dtrs);
	}

	MetricRadonIntermediate::~MetricRadonIntermediate()
	{
		delete Cs;
		delete PinvTs;
		delete tex_dtrs;
		delete indices;
		delete K01s;
		delete out;
		delete corr_out;
	}

	/// Tell Metric to compute correlation instead of SSD
	MetricRadonIntermediate& MetricRadonIntermediate::useCorrelation(bool corr)
	{
		use_corr=corr;
		return *this;
	}

	/// Move Radon derivataives to texture memory and store reference here. DO NOT delete or change _dtrs during lifetime of MetricRadonIntermediate.
	MetricRadonIntermediate& MetricRadonIntermediate::setRadonIntermediates(const std::vector<RadonIntermediate*>& _dtrs)
	{
		// Store size and parameters of dtrs (assumed to be identical)
		dtrs=_dtrs;
		int n_dtr=(int)_dtrs.size();
		step_alpha=(float)_dtrs[0]->getRadonBinSize(0);
		step_t=(float)_dtrs[0]->getRadonBinSize(1);
		n_alpha=_dtrs[0]->getRadonBinNumber(0);
		n_t=_dtrs[0]->getRadonBinNumber(1);
		n_u=_dtrs[0]->getOriginalImageSize(0);
		n_v=_dtrs[0]->getOriginalImageSize(1);
		m_isDerivative=_dtrs[0]->isDerivative();
		// Download texture ids
		std::vector<cudaTextureObject_t> dtrs_h(n_dtr);
		for (int i=0;i<n_dtr;i++)
			dtrs_h[i]=_dtrs[i]->getTexture()->tex;
		tex_dtrs->allocate(n_dtr*sizeof(cudaTextureObject_t));
		tex_dtrs->download((char*)&dtrs_h[0]);
		return *this;
	}

	/// Access Radon intermediate functions (for visualization and debugging)
	const std::vector<RadonIntermediate*>& MetricRadonIntermediate::getRadonIntermediates() const
	{
		return dtrs;
	}

	//Metric&  MetricRadonIntermediate::setRadonIntermediateBinning(int num_bins_per_180_deg, int num_bins_per_image_diagonal, GetSet::)
	//{
	//	n_alpha =num_bins_per_180_deg;
	//	n_t     =num_bins_per_image_diagonal;
	//}

	Metric& MetricRadonIntermediate::setProjectionImages(const std::vector<UtilsCuda::BindlessTexture2D<float>*>& Is)
	{
		// TODO
		return *this;
	}

	inline float cc(float * xyxxyyxy)
	{
//		return (xyxxyyxy[4]-xyxxyyxy[0]*xyxxyyxy[1])/(sqrt(xyxxyyxy[2]-xyxxyyxy[0]*xyxxyyxy[0])*sqrt(xyxxyyxy[3]-xyxxyyxy[1]*xyxxyyxy[1]));
		return xyxxyyxy[4]/(sqrt(xyxxyyxy[2])*sqrt(xyxxyyxy[3]));
	}

	/// Compute null space and pseudoinverse of projection matrices and convert to float
	Metric& MetricRadonIntermediate::setProjectionMatrices(const std::vector<ProjectionMatrix>& Ps)
	{
		Metric::setProjectionMatrices(Ps);
		// Make sure we have data.
		int n=(int)Ps.size();
		if (n == 0) return *this;
		// Store examplary parameters of first projections matrix, to estimate dkappa, if required.
		auto alphas=Geometry::getCameraFocalLengthPx(Ps.front());
		focal_length_px=std::max(alphas.first,alphas.second);
		// std::cout << "Using CPU inversion.\n";
		std::vector<Eigen::Matrix<float,3,4> > _PinvTs(Ps.size());
		#pragma omp parallel for
		for (int i=0;i<Ps.size();i++)
#ifdef __xprojectionmatrix_hxx
			culaut::projection_matrix_pseudoinverse_transpose(Ps[i].data(),_PinvTs[i].data());
#else
			_PinvTs[i]=Geometry::pseudoInverse(Ps[i]).transpose().cast<float>();
#endif // __xprojectionmatrix_hxx
		PinvTs->download(_PinvTs[0].data(),(int)_PinvTs.size()*3*4);
		std::vector<Eigen::Vector4f> _Cs(Ps.size());
		#pragma omp parallel for
		for (int i=0;i<Ps.size();i++)
#ifdef __xprojectionmatrix_hxx
			culaut::projection_matrix_source_position(Ps[i].data(),_Cs[i].data());
#else
			_Cs[i]=Geometry::getCameraCenter(Ps[i]).cast<float>();
#endif // __xprojectionmatrix_hxx
		Cs->download(_Cs[0].data(),(int)_Cs.size()*4);
		return *this;
	}

	/// Evaluates MetricRadonIntermediate without any transformation of the geometry (out is assumed n*n)
	double MetricRadonIntermediate::evaluate(float * _out)
	{
		int n=(int)Ps.size();
		out->allocate(n*n);
		K01s->allocate(n*(n-1)*8);
		std::vector<float> tmp_data;
		if (!_out) {
			tmp_data = std::vector<float>(n*n, 0.0f);
			_out = &tmp_data[0];
		}

		// Make some space for temporaries
		int n_tmp=use_corr?6:1;
			corr_out->allocate(n_tmp*n*(n-1)/2);
		corr_out->setZero();

		// Download current values of out (so as not to remove values not written to)
		out->download(_out);
		// Compute ECC on the GPU
		epipolarConsistency(
			n_u, n_v, n, *tex_dtrs,
			n_alpha, n_t, step_alpha, step_t,
			n, *Cs, *PinvTs, 0, 0x0, *K01s, *out,
			(float)getObjectRadius(), (float)dkappa, m_isDerivative, use_corr, *corr_out);

		// Readback intermediate data
		NRRD::Image<float> out_sums(n_tmp,n*(n-1)/2);
		corr_out->readback(out_sums);
		//out_sums.save("out_sums.nrrd");// DEBUG

		// Compute weighted sums of either correlation or SSD
		double weighted_sum=0,sum_over_weights=0;
		int l=out_sums.length();
		if (use_corr)
		{
			#pragma omp parallel for reduction (+:weighted_sum,sum_over_weights)
			for (int ij6=0;ij6<l;ij6+=6) {
				short i,j;
				get_ij(ij6/6,n,i,j);
				float corr=cc(&out_sums[ij6]);
				float weight=out_sums[ij6+5];
				weighted_sum+=(_out[i+j*n]=(1.0f-corr)*weight);
				sum_over_weights+=weight;
			}
		}
		else
		{
			// Read back and sum up
			out->readback(_out);
			//#pragma omp parallel for reduction (+:weighted_sum,sum_over_weights)
			for (int ij=0;ij<l;ij++) {
				short i,j;
				get_ij(ij,n,i,j);
				float weight=out_sums[ij];
				weighted_sum+=(_out[i+j*n]*=weight);
				sum_over_weights+=weight;
			}
		}
		return weighted_sum/sum_over_weights;
	}

	/// Evaluates MetricRadonIntermediate for just specific view
	double MetricRadonIntermediate::evaluate(const std::set<int>& views, float * _out)
	{
		std::vector<Eigen::Vector4i> indices;
		for (auto i = views.begin(); i != views.end(); ++i)
			for (auto j = i; j != views.end(); ++j){
				if (i == j) continue;
				indices.push_back(Eigen::Vector4i(*i, *j, *i, *j));
			}
		// Temporary memory, as needed
		std::vector<float> tmp;
		if (!_out)
		{
			tmp.resize(indices.size());
			_out=&tmp[0];
		}
		// Sum up weights and cost
		return evaluate(indices,_out);
	}

	// Might be useful for debugging: if CUDA crashed, you likely messed up the index array
	bool check_indices(const std::vector<Eigen::Vector4i>& _indices, int n_proj, int n_dtr)
	{
		int n=(int)_indices.size();
		for (int i=0;i<n;i++)
		{
			const int *p2_dtr2=_indices[i].data();
			if (p2_dtr2[0]<0||p2_dtr2[0]>=n_proj)
				return false;
			if (p2_dtr2[1]<0||p2_dtr2[1]>=n_proj)
				return false;
			if (p2_dtr2[2]<0||p2_dtr2[2]>=n_dtr)
				return false;
			if (p2_dtr2[3]<0||p2_dtr2[3]>=n_dtr)
				return false;
		}
		return true;
	}

	/// Evaluates MetricRadonIntermediate for specific pairs and projection geometry. Will write indices.size() float values to out.
	double MetricRadonIntermediate::evaluate(const std::vector<Eigen::Vector4i>& _indices, float * _out)
	{
		#ifdef _DEBUG
			if (!check_indices(_indices,(int)Ps.size(),(int)dtrs.size()))
			{
				std::cerr << "EpipolarConsistency::MetricRadonIntermediate::evaluate(...) runtime error: Index array contains invalid indices.\n";
				return 0.0;
			}
		#endif // _DEBUG

		// Compute ECC for specific indices on the GPU
		int n_pairs=(int)_indices.size();
		indices->download(_indices[0].data(),n_pairs*4);
		out->allocate(n_pairs);
		K01s->allocate(n_pairs*16);

		if (use_corr) {
			corr_out->allocate(n_pairs*6);
			corr_out->setZero();
		}
		else  {
			corr_out->allocate(n_pairs);
			corr_out->setZero();
		}

		epipolarConsistency(
			n_u, n_v, (int)dtrs.size(), *tex_dtrs,
			n_alpha, n_t, step_alpha, step_t,
			(int)Ps.size(), *Cs, *PinvTs,
			n_pairs, *indices, *K01s, *out,
			(float)getObjectRadius(), (float)dkappa, m_isDerivative,
			use_corr, *corr_out);

		// Read back intermediate results, if required, compute correlation coefficient
		NRRD::Image<float> out_sums(use_corr?6:1,n_pairs);
		corr_out->readback(out_sums);
		if (use_corr)
		{
			#pragma omp parallel for
			for (int i=0;i<n_pairs;i++)
				_out[i]=1.0f-cc(out_sums+i*6);
		}
		else
			out->readback(_out);

		// Compute weighted sum
		int n_tmp=use_corr?6:1;
		double weighted_sum=0,sum_over_weights=0;
		#pragma omp parallel for reduction (+:weighted_sum,sum_over_weights)
		for (int i=0;i<n_pairs;i++) {
			float weight=out_sums[i*n_tmp+n_tmp-1];
			weighted_sum+=(_out[i]*=weight);
			sum_over_weights+=weight;
		}
		return weighted_sum/sum_over_weights;
	}

	double MetricRadonIntermediate::evaluateForImagePair(int i, int j,
		std::vector<float> *redundant_samples0, std::vector<float> *redundant_samples1,
		std::vector<float> *kappas)
	{
		return evaluateForImagePair(i,j,redundant_samples0,redundant_samples1,kappas,0x0,0x0);
	}

	double MetricRadonIntermediate::evaluateForImagePair(int i, int j,
		std::vector<float> *redundant_samples0, std::vector<float> *redundant_samples1,
		std::vector<float> *kappas, std::vector<std::pair<float,float> > *radon_samples0, std::vector<std::pair<float,float> > *radon_samples1 )
	{
		// Epipolar Geometry
		using namespace Geometry;
		auto& dtr0=*dtrs[i];
		auto& dtr1=*dtrs[j];
		auto P0=Ps[i];
		auto P1=Ps[j];
		auto C0f=getCameraCenter(P0).cast<float>().eval();
		auto C1f=getCameraCenter(P1).cast<float>().eval();
		Eigen::Matrix<float,3,4> P0invTf=pseudoInverse(P0).transpose().cast<float>().eval();
		Eigen::Matrix<float,3,4> P1invTf=pseudoInverse(P1).transpose().cast<float>().eval();
		float K0[8], K1[8];
		computeK01(
			n_u*0.5f,n_v*0.5f,
			C0f.data(),C1f.data(),P0invTf.data(),P1invTf.data(),
			(float)getObjectRadius(),std::sqrtf((float)(n_u*n_u+n_v*n_v)),(float)dkappa,
			K0,K1);
		// Make sure we have data available on CPU
		dtr0.readback();
		dtr1.readback();
		// Figure out plane angle range and step
		float &dkappa(K1[6]);
		float &kappa_max(K1[7]);
		// Reserve some memory for the results (optional)
		double ecc=0;
		int n=(int)(2*kappa_max/dkappa);
		if (redundant_samples0) redundant_samples0->reserve(n);
		if (redundant_samples1) redundant_samples1->reserve(n);
		if (kappas            ) kappas            ->reserve(n);
		if (radon_samples0    ) radon_samples0    ->reserve(n);
		if (radon_samples1    ) radon_samples1    ->reserve(n);

		// Iterate over plane angle kappa
		for (float kappa=-kappa_max+0.5f*dkappa; kappa<kappa_max; kappa+=dkappa)
		{
			// Compute cosine and sine of kappa
			float x0=std::cosf(kappa);
			float x1=std::sinf(kappa);

			// Find corresponding epipolar lines for plane at angle kappa (same as culaut::xgemm<float,3,2,1>(K,x_k,l);)
			float line0[]={K0[0]*x0+K0[3]*x1,K0[1]*x0+K0[4]*x1,K0[2]*x0+K0[5]*x1};
			float line1[]={K1[0]*x0+K1[3]*x1,K1[1]*x0+K1[4]*x1,K1[2]*x0+K1[5]*x1};

			// Sample
			float v0=dtr0.sample(line0);
			float v1=dtr1.sample(line1);

			// Store values for plotting, if desired
			if (redundant_samples0) redundant_samples0->push_back(v0);
			if (redundant_samples1) redundant_samples1->push_back(v1);
			if (kappas            ) kappas            ->push_back(kappa);
			if (radon_samples0    ) radon_samples0    ->push_back(std::make_pair(line0[0], line0[1]));
			if (radon_samples1    ) radon_samples1    ->push_back(std::make_pair(line1[0], line1[1]));

			// Accumulate
			ecc=(v0-v1)*(v0-v1)*dkappa;
		}

		return ecc;
	}

} // namespace EpipolarConsistency


//  Boost sorting_algo library float_sort_test.cpp file  ---------------------------//

//  Copyright Steven Ross 2009. 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)

//  See http://www.boost.org for updates, documentation, and revision history.

#include <boost/algorithm/sorting/spread_sort.hpp>
// Include unit test framework
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/test_tools.hpp>
#include <vector>

using namespace std;
using namespace boost;

//Casting to an integer before bitshifting
struct rightshift {
  int operator()(const float &x, const unsigned offset) const { return float_mem_cast<float, int>(x) >> offset; }
};

struct rightshift_64 {
  boost::int64_t operator()(const double &x, const boost::uint64_t offset) const { return float_mem_cast<double, boost::int64_t>(x) >> offset; }
};

boost::int32_t
rand_32(bool sign = true) {
   boost::int32_t result = rand() | (rand()<< 16);
   if(rand() % 2)
     result |= 1 << 15;
   //Adding the sign bit
   if(sign && (rand() % 2))
     result *= -1;
   return result;
}

void float_test()
{
    // Prepare inputs
    vector<float> base_vec;
  unsigned count = 100000;

  //Generating semirandom numbers; no they're not large, but they'll work for basic testing
  for(unsigned u = 0; u < count; ++u) {
    float val = float(rand_32());
  //As std::sort gives arbitrary results for NaNs and 0.0 vs. -0.0, treat all those as just 0.0 for testing
    if(!(val < 0.0) && !(0.0 < val))
      base_vec.push_back(0.0);
    else
      base_vec.push_back(val);
  }
  vector<float> sorted_vec = base_vec;
  vector<float> test_vec = base_vec;
  std::sort(sorted_vec.begin(), sorted_vec.end());
  //Testing spread_sort version
  test_vec = base_vec;
  spread_sort(test_vec.begin(), test_vec.end());
  BOOST_CHECK(test_vec == sorted_vec);
  //One functor
  test_vec = base_vec;
  float_sort(test_vec.begin(), test_vec.end(), rightshift());
  BOOST_CHECK(test_vec == sorted_vec);
  //Both functors
  test_vec = base_vec;
  float_sort(test_vec.begin(), test_vec.end(), rightshift(), less<float>());
  BOOST_CHECK(test_vec == sorted_vec);

  //Testing for doubles
  vector<double> long_base_vec;
  for(unsigned u = 0; u < base_vec.size(); ++u) {
    double val = double
    ((((boost::int64_t)rand_32()) << ((8 * sizeof(int)) -1)) + rand_32(false));
    //As std::sort gives arbitrary results for NaNs and 0.0 vs. -0.0,
    //treat all those as just 0.0 for testing
    if(!(val < 0.0) && !(0.0 < val))
      long_base_vec.push_back(0.0);
    else
      long_base_vec.push_back(val);
  }
  vector<double> long_sorted_vec = long_base_vec;
  vector<double> long_test_vec = long_base_vec;
  float_sort(long_test_vec.begin(), long_test_vec.end());
  std::sort(long_sorted_vec.begin(), long_sorted_vec.end());
  BOOST_CHECK(long_test_vec == long_sorted_vec);
  //One functor
  long_test_vec = long_base_vec;
  float_sort(long_test_vec.begin(), long_test_vec.end(), rightshift_64());
  BOOST_CHECK(long_test_vec == long_sorted_vec);
  //Both functors
  long_test_vec = long_base_vec;
  float_sort(long_test_vec.begin(), long_test_vec.end(), rightshift_64(), less<double>());
  BOOST_CHECK(long_test_vec == long_sorted_vec);

}

// test main
int test_main( int, char*[] )
{
    float_test();

    return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <random>
#include <chrono>
#include <functional>
#include <cctype>
#include <locale>
#include <algorithm>
#include <regex>
#include <iterator>
#include <vector>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include "perceptron.h"
#include "segmentor.h"
#include "featurizer.h"
#include "naivebayes.h"
#include "util.h"

using namespace std;

int main() {
    auto genesis = std::chrono::system_clock::now();

    NBUnigramBOW* nb = new NBUnigramBOW();

    AveragedPerceptron* perceptron = new AveragedPerceptron();
    Featurizer* feat = new Featurizer();
    Segmentor* seg = new Segmentor(perceptron, feat);
    map<string, int> index_table;
    feat->index_table = index_table;

    vector<string> sentences;
    vector<vector<int> > labels;
    vector<vector<string> > tokens_list;

    ifstream training_data ("../data/train.txt");
    string line;

    if (training_data.is_open()) {
        while (getline(training_data, line)) {
            transform(line.begin(), line.end(), line.begin(), ::tolower);
            replace_if(line.begin(), line.end(), std::not1(std::ptr_fun(ofinterest)), ' ');
            line.erase(remove_if(line.begin(), line.end(), isapos), line.end());
            line = trim(line);

            if (line.empty()) {
                continue;
            }

            vector<int> label;
            vector<string> tokens;

            vector<string> tokenized_strings = split(line, ' ');
            for (auto t: tokenized_strings) {
                if (t.empty()) {
                    continue;
                }
                vector<int> tmp(t.size()-1, -1);
                label.insert(label.end(), tmp.begin(), tmp.end());
                label.insert(label.end(), 1);

                tokens.push_back(t);
            }

            line.erase(remove_if(line.begin(), line.end(), ::isspace), line.end());

            sentences.push_back(line);
            labels.push_back(label);
            tokens_list.push_back(tokens);
        }
    }

    // train Naive Bayes model
    for (auto tokens: tokens_list) {
        nb->fit(tokens);
    }

    cout << "NB unigram count: " << nb->total << endl;

    tokens_list.clear();
    vector<string> query_sentences;
    vector<vector<int> > query_labels;

    ifstream training_query ("../data/train1.qry");
    if (training_query.is_open()) {
        while (getline(training_query, line)) {
            transform(line.begin(), line.end(), line.begin(), ::tolower);
            replace_if(line.begin(), line.end(), std::not1(std::ptr_fun(ofinterest)), ' ');
            line.erase(remove_if(line.begin(), line.end(), isapos), line.end());
            line = trim(line);

            if (line.empty()) {
                continue;
            }

            vector<int> label;
            vector<string> tokens;

            vector<string> tokenized_strings = split(line, ' ');
            int counter = 0;
            for (auto t: tokenized_strings) {
                counter++;
                if (counter == 1) {
                    continue;
                }

                if (t.empty()) {
                    continue;
                }
                vector<int> tmp(t.size()-1, -1);
                label.insert(label.end(), tmp.begin(), tmp.end());
                label.insert(label.end(), 1);

                tokens.push_back(t);
            }
            line.erase(remove_if(line.begin(), line.end(), ::isspace), line.end());

            query_sentences.push_back(line);
            query_labels.push_back(label);
            tokens_list.push_back(tokens);
        }
    }

    for (auto tokens: tokens_list) {
        nb->fit(tokens);
    }

    cout << "NB unigram count: " << nb->total << endl;

    ofstream os("naivebayes.dat", ios::binary);
    boost::archive::binary_oarchive nb_ar(os);
    nb_ar << nb;

    // Averaged perceptron training
    std::random_device rd;

    chrono::milliseconds time_resource (1000 * 60 * 9 + 1000 * 30 - 5000);  // 10 min - 10 sec
    /* chrono::milliseconds time_resource (1000);  // 1s */
    /* chrono::milliseconds time_resource (1000 * 15 - 10000);  // 15 sec - 10 sec = 5 sec */

    for (int i = 0; i < 1000000; ++i) {
        auto showtime = std::chrono::system_clock::now();
        auto time_left = time_resource - chrono::duration_cast<chrono::milliseconds>(showtime - genesis);

        // this loop will terminate if we spent more than given time resource
        bool no_more_time = seg->fit(sentences, labels, time_left);

        if (no_more_time) {
            seg->model->average();
            break;
        }

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        shuffle(sentences.begin(), sentences.end(), std::default_random_engine(seed));
        shuffle(labels.begin(), labels.end(), std::default_random_engine(seed));
    }

    cout << "AP feature count: " << seg->feat->num << endl;

    auto mini_genesis = std::chrono::system_clock::now();
    cout << chrono::duration_cast<chrono::milliseconds> (mini_genesis - genesis).count() << endl;
    /* chrono::milliseconds train_query_time_resource (1000 * 70 - 10000);  // 70 sec - 10 sec = 60 sec */
    chrono::milliseconds train_query_time_resource (1000 * 30 - 5000);  // 10 min - 10 sec
    /* chrono::milliseconds train_query_time_resource (1000);  // 70 sec - 10 sec = 60 sec */

    for (int i = 0; i < 1000000; ++i) {
        cout << i << endl;
        auto showtime = std::chrono::system_clock::now();
        auto time_left = train_query_time_resource - chrono::duration_cast<chrono::milliseconds>(showtime - mini_genesis);

        // this loop will terminate if we spent more than given time resource
        bool no_more_time = seg->fit(query_sentences, query_labels, time_left);

        if (no_more_time) {
            seg->model->average();
            break;
        }

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        shuffle(query_sentences.begin(), query_sentences.end(), std::default_random_engine(seed));
        shuffle(query_labels.begin(), query_labels.end(), std::default_random_engine(seed));
    }

    cout << "AP feature count: " << seg->feat->num << endl;

    auto finished = std::chrono::system_clock::now();
    cout << chrono::duration_cast<chrono::milliseconds> (finished - mini_genesis).count() << endl;

    ofstream ofeature("feature_index.dat", ios::binary);
    boost::archive::binary_oarchive feature_ar(ofeature);
    feature_ar << feat;

    ofstream operceptron("perceptron.dat", ios::binary);
    boost::archive::binary_oarchive perceptron_ar(operceptron);
    perceptron_ar << perceptron;

    return 0;
};


#define BOOST_TEST_MODULE "test_omp_position_restraint_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/math/math.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/RandomNumberGenerator.hpp>
#include <mjolnir/omp/PositionRestraintInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(omp_PositionRestraint)
{
    constexpr double tol = 1e-8;
    mjolnir::LoggerManager::set_default_logger("test_omp_position_restraint_interaction.log");

    using traits_type      = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = typename traits_type::real_type;
    using coordinate_type  = typename traits_type::coordinate_type;
    using boundary_type    = typename traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::PositionRestraintInteraction<traits_type, potential_type>;
    using rng_type         = mjolnir::RandomNumberGenerator<traits_type>;

    using sequencial_system_type      = mjolnir::System<
        mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>>;
    using sequencial_interaction_type = mjolnir::PositionRestraintInteraction<
        mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>,
        potential_type>;

    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << max_number_of_threads);

    const std::size_t N_particle = 64;
    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        potential_type potential(1.0, 0.0);

        rng_type    rng(123456789);
        system_type sys(N_particle, boundary_type{});
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>> positions;

        for(std::size_t i=0; i<sys.size(); ++i)
        {
            const auto i_x = i % 4;
            const auto i_y = i / 4;
            const auto i_z = i / 16;

            sys.mass(i)     = 1.0;
            sys.position(i) = mjolnir::math::make_coordinate<coordinate_type>(i_x*2.0, i_y*2.0, i_z*2.0);
            sys.velocity(i) = mjolnir::math::make_coordinate<coordinate_type>(0, 0, 0);
            sys.force(i)    = mjolnir::math::make_coordinate<coordinate_type>(0, 0, 0);
            sys.name(i)     = "X";
            sys.group(i)    = "TEST";

            positions.emplace_back(i, sys.position(i), potential);
        }
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>> positions2(positions);

        // add perturbation
        for(std::size_t i=0; i<sys.size(); ++i)
        {
            mjolnir::math::X(sys.position(i)) += rng.uniform_real(-0.1, 0.1);
            mjolnir::math::Y(sys.position(i)) += rng.uniform_real(-0.1, 0.1);
            mjolnir::math::Z(sys.position(i)) += rng.uniform_real(-0.1, 0.1);
        }

        // init sequential one with the same coordinates
        sequencial_system_type seq_sys(N_particle, boundary_type{});
        assert(sys.size() == seq_sys.size());
        for(std::size_t i=0; i<sys.size(); ++i)
        {
            seq_sys.mass(i)     = sys.mass(i);
            seq_sys.position(i) = sys.position(i);
            seq_sys.velocity(i) = sys.velocity(i);
            seq_sys.force(i)    = sys.force(i);
            seq_sys.name(i)     = sys.name(i);
            seq_sys.group(i)    = sys.group(i);
        }

        interaction_type            interaction    (std::move(positions));
        sequencial_interaction_type seq_interaction(std::move(positions2));

        interaction    .initialize(sys);
        seq_interaction.initialize(seq_sys);

        // calculate forces with openmp
        interaction.calc_force(sys);
        sys.postprocess_forces();

        // calculate forces without openmp
        seq_interaction.calc_force(seq_sys);

        // check the values are the same
        for(std::size_t i=0; i<sys.size(); ++i)
        {
            BOOST_TEST(mjolnir::math::X(seq_sys.force(i)) == mjolnir::math::X(sys.force(i)),
                       boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(seq_sys.force(i)) == mjolnir::math::Y(sys.force(i)),
                       boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(seq_sys.force(i)) == mjolnir::math::Z(sys.force(i)),
                       boost::test_tools::tolerance(tol));
        }
        BOOST_TEST(interaction.calc_energy(sys) == seq_interaction.calc_energy(seq_sys),
                   boost::test_tools::tolerance(tol));
    }
}


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  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 the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <pluginlib/class_list_macros.h>
#include "polar_nn_fuser.h"

#include <Eigen/Core>
#include <limits>
#include <eigen_conversions/eigen_msg.h>

namespace spencer_detected_person_association
{
    float PolarNNFuserNodelet::computeDistance(const spencer_tracking_msgs::CompositeDetectedPerson& d1, const spencer_tracking_msgs::CompositeDetectedPerson& d2)
    {
        // The distance we compute consists of two parts:
        // (1) The average difference in polar angle between the two detections, but expressed as a Euclidean distance between the angle's end points such that we can sum it up with...
        // (2) The absolute difference in depth, i.e. the absolute difference between their polar radius
        // Usually, the difference in depth is lower-weighted because depth estimates might not be reliable with monocular vision systems

        double phi1 = atan2(d1.pose.pose.position.y, d1.pose.pose.position.x);
        double phi2 = atan2(d2.pose.pose.position.y, d2.pose.pose.position.x);

        double rho1 = hypot(d1.pose.pose.position.x, d1.pose.pose.position.y);
        double rho2 = hypot(d2.pose.pose.position.x, d2.pose.pose.position.y);

        double rho_avg = (rho1 + rho2) / 2.0;

        // Point on the polar ray of detection 1 at the mean radius of the two detections
        double x1_at_rho_avg = rho_avg * cos(phi1);
        double y1_at_rho_avg = rho_avg * sin(phi1);

        // Point on the polar ray of detection 2 at the mean radius of the two detections
        double x2_at_rho_avg = rho_avg * cos(phi2);
        double y2_at_rho_avg = rho_avg * sin(phi2);

        // (1) Average Euclidean distance in polar angle
        double angular_distance = hypot(x1_at_rho_avg - x2_at_rho_avg, y1_at_rho_avg - y2_at_rho_avg);

        // (2) Absolute difference in polar radius
        double radial_distance = std::abs(rho2 - rho1);

        // Gating
        double angular_gating_distance = 0.5; getPrivateNodeHandle().getParamCached("angular_gating_distance", angular_gating_distance);
        if(angular_distance > angular_gating_distance) angular_distance = std::numeric_limits<float>::infinity();

        double radial_gating_distance = 2.5; getPrivateNodeHandle().getParamCached("radial_gating_distance", radial_gating_distance);
        if(radial_distance > radial_gating_distance) radial_distance = std::numeric_limits<float>::infinity();

        // Overall distance is weighted sum of the two components (similar to a 'weighted Manhattan distance', where the two components are angular and radial distances)
        double angular_importance = 0.8; getPrivateNodeHandle().getParamCached("angular_importance", angular_importance);
        double radial_importance = 0.2; getPrivateNodeHandle().getParamCached("radial_importance", radial_importance);

        const float weightSum = angular_importance + radial_importance;
        angular_importance /= weightSum; radial_importance /= weightSum;

        double distance = angular_importance * angular_distance + radial_importance * radial_distance;

        //if(std::isfinite(angular_distance) && std::isfinite(radial_distance))
        //    ROS_INFO("Polar-NN | %zu + %zu:  angular_d=%.1f, radial_d=%.1f, d=%.1f", d1.composite_detection_id, d2.composite_detection_id, angular_distance, radial_distance, distance);

        return distance;
    }

    void PolarNNFuserNodelet::fusePoses(const spencer_tracking_msgs::CompositeDetectedPerson& d1, const spencer_tracking_msgs::CompositeDetectedPerson& d2, geometry_msgs::PoseWithCovariance& fusedPose)
    {
        // Obtain and normalize weights for angular component
        double angular_weight1 = 0.5; getPrivateNodeHandle().getParamCached("fused_angular_weight_for_topic1", angular_weight1);
        double angular_weight2 = 0.5; getPrivateNodeHandle().getParamCached("fused_angular_weight_for_topic2", angular_weight2);

        const float angularWeightSum = angular_weight1 + angular_weight2;
        angular_weight1 /= angularWeightSum; angular_weight2 /= angularWeightSum;

        // Obtain and normalize weights for radial component -- usually one of them should receive a lower weight in practice, when there is high uncertainty in the depth estimates
        double radial_weight1 = 0.5; getPrivateNodeHandle().getParamCached("fused_radial_weight_for_topic1", radial_weight1);
        double radial_weight2 = 0.5; getPrivateNodeHandle().getParamCached("fused_radial_weight_for_topic2", radial_weight2);

        const float radialWeightSum = radial_weight1 + radial_weight2;
        radial_weight1 /= radialWeightSum; radial_weight2 /= radialWeightSum;

        // Obtain and normalize weights for orientation component (if at all relevant, most detections don't have an orientation...)
        double orientation_weight1 = 0.5; getPrivateNodeHandle().getParamCached("fused_orientation_weight_for_topic1", orientation_weight1);
        double orientation_weight2 = 0.5; getPrivateNodeHandle().getParamCached("fused_orientation_weight_for_topic2", orientation_weight2);

        const float orientationWeightSum = orientation_weight1 + orientation_weight2;
        orientation_weight1 /= orientationWeightSum; orientation_weight2 /= orientationWeightSum;

        // Compute the average polar angle (note that averaging angles is not straightforward)
        double phi1 = atan2(d1.pose.pose.position.y, d1.pose.pose.position.x);
        double phi2 = atan2(d2.pose.pose.position.y, d2.pose.pose.position.x);

        double phi_avg = atan2(0.5*sin(phi1) + 0.5*sin(phi2), 0.5*cos(phi1) + 0.5*cos(phi2)); // http://en.wikipedia.org/wiki/Mean_of_circular_quantities

        // Compute the average polar radius
        double rho1 = hypot(d1.pose.pose.position.x, d1.pose.pose.position.y);
        double rho2 = hypot(d2.pose.pose.position.x, d2.pose.pose.position.y);

        double rho_avg = radial_weight1 * rho1 + radial_weight2 * rho2;

        // Compute resulting X and Y coordinates, for Z just take the arithmetic mean
        fusedPose.pose.position.x = rho_avg * cos(phi_avg);
        fusedPose.pose.position.y = rho_avg * sin(phi_avg);

        fusedPose.pose.position.z = 0.5 * d1.pose.pose.position.z + 0.5 * d2.pose.pose.position.z; // FIXME: Make weights configurable if we ever track in 3D

        // Interpolate the orientation using SLERP
        Eigen::Quaterniond q1, q2, qInterpolated;
        tf::quaternionMsgToEigen(d1.pose.pose.orientation, q1);
        tf::quaternionMsgToEigen(d2.pose.pose.orientation, q2);
        qInterpolated = q1.slerp(orientation_weight2, q2);
        tf::quaternionEigenToMsg(qInterpolated, fusedPose.pose.orientation);

        // Take component-wise minimum of the covariance matrices
        for(size_t i = 0; i < d1.pose.covariance.size(); i++) {
            // FIXME: Is this a good idea for tracking? This is similar to using the 'stronger peak' of the two distributions
            // as we hopefully don't get less-informed by fusing the two detections (if our association makes sense)
            // There is probably no 'correct way' as the mixture of two Gaussians is not a Gaussian
            fusedPose.covariance[i] = std::min(d1.pose.covariance[i], d2.pose.covariance[i]);
        }
    }
}

PLUGINLIB_DECLARE_CLASS(spencer_detected_person_association, PolarNNFuserNodelet, spencer_detected_person_association::PolarNNFuserNodelet, nodelet::Nodelet)


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//---------------------------------------------------------------------------//

#include <iostream>

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

#include <nil/marshalling/status_type.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>
#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/pairing/mnt4.hpp>
#include <nil/crypto3/algebra/pairing/mnt6.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>
#include <nil/crypto3/zk/components/disjunction.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/crypto3/marshalling/types/zk/r1cs_gg_ppzksnark/primary_input.hpp>
#include <nil/crypto3/marshalling/types/zk/r1cs_gg_ppzksnark/proof.hpp>
#include <nil/crypto3/marshalling/types/zk/r1cs_gg_ppzksnark/verification_key.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::marshalling;
using namespace nil::crypto3::zk;

template<typename FieldType>
components::blueprint<FieldType> test_disjunction_component(size_t w) {

    using field_type = FieldType;

    std::size_t n = std::log2(w) +
        ((w > (1ul << std::size_t(std::log2(w))))? 1 : 0);

    components::blueprint<field_type> bp;
    components::blueprint_variable<field_type> output;
    output.allocate(bp);

    bp.set_input_sizes(1);

    components::blueprint_variable_vector<field_type> inputs;
    inputs.allocate(bp, n);

    components::disjunction<field_type> d(bp, inputs, output);
    d.generate_r1cs_constraints();

    for (std::size_t j = 0; j < n; ++j) {
        bp.val(inputs[j]) = typename field_type::value_type((w & (1ul << j)) ? 1 : 0);
    }

    d.generate_r1cs_witness();

    assert(bp.val(output) == (w ? field_type::value_type::one() : field_type::value_type::zero()));
    assert(bp.is_satisfied());

    return bp;
}

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

    typedef algebra::curves::bls12<381> curve_type;
    typedef typename curve_type::scalar_field_type scalar_field_type;
    using Endianness = nil::marshalling::option::big_endian;

    typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

    std::size_t num_constraints = 1000, input_size = 100;

    boost::filesystem::path pout, pkout, vkout, piout, viout;
    boost::program_options::options_description options(
        "R1CS Generic Group PreProcessing Zero-Knowledge Succinct Non-interactive ARgument of Knowledge "
        "(https://eprint.iacr.org/2016/260.pdf) CLI Proof Generator");
    // clang-format off
    options.add_options()("help,h", "Display help message")
    ("version,v", "Display version")
    ("generate", "Generate proofs and/or keys")
    ("verify", "verify proofs and/or keys")
    ("proof-output,po", boost::program_options::value<boost::filesystem::path>(&pout)->default_value("proof"))
    ("primary-input-output,pio", boost::program_options::value<boost::filesystem::path>(&piout)->default_value
("pinput"))
    ("proving-key-output,pko", boost::program_options::value<boost::filesystem::path>(&pkout)->default_value("pkey"))
    ("verifying-key-output,vko", boost::program_options::value<boost::filesystem::path>(&vkout)->default_value("vkey"))
    ("verifier-input-output,vio", boost::program_options::value<boost::filesystem::path>(&viout)->default_value("vio"));
    // clang-format on

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help") || argc < 2) {
        std::cout << options << std::endl;
        return 0;
    }

    std::cout << "Blueprint generation started." << std::endl;

    components::blueprint<scalar_field_type> bp = test_disjunction_component<scalar_field_type>(10);

    std::cout << "Blueprint generation finished." << std::endl;

    std::cout << "R1CS generation started." << std::endl;

    std::cout << "R1CS generation finished." << std::endl;

    std::cout << "Starting generator" << std::endl;

    typename scheme_type::keypair_type keypair = zk::snark::generate<scheme_type>(bp.get_constraint_system());

    std::cout << "Starting prover" << std::endl;

    const typename scheme_type::proof_type proof =
        zk::snark::prove<scheme_type>(keypair.first, bp.primary_input(), bp.auxiliary_input());

    using verification_key_marshalling_type = types::r1cs_gg_ppzksnark_verification_key<
        nil::marshalling::field_type<
            Endianness>,
        typename scheme_type::verification_key_type>;

    verification_key_marshalling_type filled_verification_key_val =
        types::fill_r1cs_gg_ppzksnark_verification_key<
            typename scheme_type::verification_key_type,
            Endianness>(keypair.second);

    using proof_marshalling_type = types::r1cs_gg_ppzksnark_proof<
        nil::marshalling::field_type<
            Endianness>,
        typename scheme_type::proof_type>;

    proof_marshalling_type filled_proof_val =
        types::fill_r1cs_gg_ppzksnark_proof<
            typename scheme_type::proof_type,
            Endianness>(proof);

    using primary_input_marshalling_type = types::r1cs_gg_ppzksnark_primary_input<
        nil::marshalling::field_type<
            Endianness>,
        typename scheme_type::primary_input_type>;

    primary_input_marshalling_type filled_primary_input_val =
        types::fill_r1cs_gg_ppzksnark_primary_input<
            typename scheme_type::primary_input_type,
            Endianness>(bp.primary_input());

    std::cout << "Marshalling types filled." << std::endl;

    using unit_type = unsigned char;

    std::vector<unit_type> verification_key_byteblob;
    verification_key_byteblob.resize(filled_verification_key_val.length(), 0x00);
    auto write_iter = verification_key_byteblob.begin();

    typename nil::marshalling::status_type status =
        filled_verification_key_val.write(write_iter,
            verification_key_byteblob.size());

    std::vector<unit_type> proof_byteblob;
    proof_byteblob.resize(filled_proof_val.length(), 0x00);
    write_iter = proof_byteblob.begin();

    status = filled_proof_val.write(write_iter,
            proof_byteblob.size());

    std::vector<unit_type> primary_input_byteblob;

    primary_input_byteblob.resize(filled_primary_input_val.length(), 0x00);
    auto primary_input_write_iter = primary_input_byteblob.begin();

    status = filled_primary_input_val.write(primary_input_write_iter,
            primary_input_byteblob.size());

    std::cout << "Byteblobs filled." << std::endl;

    if (vm.count("verifying-key-output")) {
        boost::filesystem::ofstream out(vkout);
        for (const auto &v : verification_key_byteblob) {
            out << v;
        }
        out.close();
    }

    if (vm.count("proof-output")) {
        boost::filesystem::ofstream out(pout);
        for (const auto &v : proof_byteblob) {
            out << v;
        }
        out.close();
    }

    if (vm.count("primary-input-output")) {
        boost::filesystem::ofstream out(piout);
        for (const auto &v : primary_input_byteblob) {
            out << v;
        }
        out.close();
    }

    // nil::marshalling::status_type provingProcessingStatus = nil::marshalling::status_type::success;
    // typename scheme_type::proving_key_type other =
    //             nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proving_key_process(
    //                 proving_key_byteblob.cbegin(),
    //                 proving_key_byteblob.cend(),
    //                 provingProcessingStatus);

    // assert(keypair.first == other);

    if (vm.count("verifier-input-output")) {
        std::vector<std::uint8_t> verifier_input_output_byteblob(proof_byteblob.begin(), proof_byteblob.end());

        verifier_input_output_byteblob.insert(verifier_input_output_byteblob.end(), primary_input_byteblob.begin(),
                                              primary_input_byteblob.end());
        verifier_input_output_byteblob.insert(verifier_input_output_byteblob.end(), verification_key_byteblob.begin(),
                                              verification_key_byteblob.end());

        boost::filesystem::ofstream poutf(viout);
        for (const auto &v : verifier_input_output_byteblob) {
            poutf << v;
        }
        poutf.close();
    }

    return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/sym_equiv.h>
#include <cctbx/miller/union_of_indices.h>
#include <cctbx/miller/math.h>
#include <boost/python.hpp>
#include <scitbx/boost_python/container_conversions.h>

namespace cctbx { namespace miller { namespace boost_python {

  void wrap_asu();
  void wrap_bins();
  void wrap_change_basis();
  void wrap_expand_to_p1();
  void wrap_index_generator();
  void wrap_index_span();
  void wrap_match_bijvoet_mates();
  void wrap_match_indices();
  void wrap_match_multi_indices();
  void wrap_merge_equivalents();
  void wrap_phase_transfer();
  void wrap_phase_integrator();
  void wrap_sym_equiv();
  void wrap_f_calc_map();
  // miller_lookup_utils
  void wrap_lookup_tensor();
  void wrap_local_neighbourhood();
  void wrap_local_area();
  void wrap_amplitude_normalisation();
  void wrap_randomize_amplitude_and_phase();
  void wrap_slices();

namespace {

  struct union_of_indices_registry_wrappers
  {
    typedef union_of_indices_registry wt;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<wt>("union_of_indices_registry", no_init)
        .def(init<>())
        .def("update", &wt::update, (arg("indices")))
        .def("as_array", &wt::as_array)
      ;
    }
  };

  hendrickson_lattman<>
  as_hendrickson_lattman(
    bool centric_flag,
    std::complex<double> const& phase_integral,
    double const& max_figure_of_merit)
  {
    return hendrickson_lattman<>(
      centric_flag, phase_integral, max_figure_of_merit);
  }

  void register_tuple_mappings()
  {
    using namespace scitbx::boost_python::container_conversions;

    tuple_mapping_variable_capacity<af::shared<sym_equiv_index> >();
  }

  void init_module()
  {
    using namespace boost::python;

    register_tuple_mappings();

    wrap_sym_equiv(); // must be wrapped first to enable use of bases<>
    wrap_asu();
    wrap_bins();
    wrap_change_basis();
    wrap_expand_to_p1();
    wrap_index_generator();
    wrap_index_span();
    wrap_match_bijvoet_mates();
    wrap_match_indices();
    wrap_match_multi_indices();
    wrap_merge_equivalents();
    wrap_phase_integrator();
    wrap_phase_transfer();
    wrap_f_calc_map();

    wrap_lookup_tensor();
    wrap_local_neighbourhood();
    wrap_local_area();
    wrap_amplitude_normalisation();
    wrap_randomize_amplitude_and_phase();
    wrap_slices();

    def("statistical_mean",
      (double(*)(sgtbx::space_group const&,
                 bool,
                 af::const_ref<index<> > const&,
                 af::const_ref<double> const&)) statistical_mean);

    union_of_indices_registry_wrappers::wrap();

    def("as_hendrickson_lattman", as_hendrickson_lattman, (
      arg("centric_flag"),
      arg("phase_integral"),
      arg("max_figure_of_merit")));
  }

} // namespace <anonymous>
}}} // namespace cctbx::miller::boost_python

BOOST_PYTHON_MODULE(cctbx_miller_ext)
{
  cctbx::miller::boost_python::init_module();
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_COMMON_SIMD_FUNCTION_DIVS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIVS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/bitwise_xor.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/genmask.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_zero_else_one.hpp>
#include <boost/simd/function/simd/inc.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/logical_and.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(divs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return a0/a1;
      }
   };

   BOOST_DISPATCH_OVERLOAD(divs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::uint_<A0>, X>
                          , bs::pack_<bd::uint_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        auto iseqza1 = is_eqz(a1);
        const A0 aa1 = if_else(iseqza1, One<A0>(), a1);
        const A0 aa0 = if_else(iseqza1, genmask(a0), a0);
        return aa0/aa1;
      }
   };

   BOOST_DISPATCH_OVERLOAD(divs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::int_<A0>, X>
                          , bs::pack_<bd::int_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using sA0 = bd::scalar_of_t<A0>;
        auto iseqza1 = is_eqz(a1);
        // replace valmin/-1 by (valmin+1)/-1
        A0 x = a0 + if_zero_else_one(inc(a1) | (a0 + Valmin<A0>()));
        // negative -> valmin
        // positive -> valmax
        const A0 x2 = bitwise_xor(Valmax<A0>(), shift_right(x, sizeof(sA0)*CHAR_BIT-1));
        x = if_else(logical_and(iseqza1, is_nez(x)), x2, x);
        const A0 y = if_else(iseqza1, One<A0>(), a1);
        return x/y;
      }
   };

} } }

#endif


#include <drake/systems/plants/RigidBodyIK.h>
#include <drake/systems/plants/RigidBodyTree.h>
#include <drake/systems/plants/constraint/RigidBodyConstraint.h>

#include <drake/systems/plants/IKoptions.h>
#include <iostream>
#include <cstdlib>
#include <limits>
#include <boost/shared_ptr.hpp>
#include <ConciseArgs>

using namespace std;
using namespace Eigen;

#include "lcmtypes/bot_core/robot_state_t.hpp"
#include "lcmtypes/bot_core/pose_t.hpp"
#include <lcm/lcm-cpp.hpp>

struct CommandLineConfig
{
  std::string urdf_filename;
  Eigen::Vector3d gazeGoal;
};

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_);

    ~App(){
    }

    void getRobotState(bot_core::robot_state_t& robot_state_msg, int64_t utime_in, Eigen::VectorXd q, std::vector<std::string> jointNames);

    int getConstraints(Eigen::VectorXd q_star, Eigen::VectorXd &q_sol);

    void solveGazeProblem();

    void robotStateHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::robot_state_t* msg);

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;
    RigidBodyTree model_;

    bot_core::robot_state_t rstate_;
    map<string, int> dofMap_;

};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_):
                       lcm_(lcm_), cl_cfg_(cl_cfg_){

  model_.addRobotFromURDF(cl_cfg_.urdf_filename);
  model_.compile();
  dofMap_ = model_.computePositionNameToIndexMap();

  lcm_->subscribe("EST_ROBOT_STATE",&App::robotStateHandler,this);

}

// Find the joint position indices corresponding to 'name'
vector<int> getJointPositionVectorIndices(const RigidBodyTree &model, const std::string &name) {
  shared_ptr<RigidBody> joint_parent_body = model.findJoint(name);
  int num_positions = joint_parent_body->getJoint().getNumPositions();
  vector<int> ret(static_cast<size_t>(num_positions));

  // fill with sequentially increasing values, starting at joint_parent_body->position_num_start:
  iota(ret.begin(), ret.end(), joint_parent_body->position_num_start);
  return ret;
}

void findJointAndInsert(const RigidBodyTree &model, const std::string &name, vector<int> &position_list) {
  auto position_indices = getJointPositionVectorIndices(model, name);

  position_list.insert(position_list.end(), position_indices.begin(), position_indices.end());
}

Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw) {

  // This conversion function introduces a NaN in Eigen Rotations when:
  // roll == pi , pitch,yaw =0    ... or other combinations.
  // cos(pi) ~=0 but not exactly 0
  // Post DRC Trails: replace these with Eigen's own conversions
  if ( ((roll==M_PI) && (pitch ==0)) && (yaw ==0)){
    return  Eigen::Quaterniond(0,1,0,0);
  }else if( ((pitch==M_PI) && (roll ==0)) && (yaw ==0)){
    return  Eigen::Quaterniond(0,0,1,0);
  }else if( ((yaw==M_PI) && (roll ==0)) && (pitch ==0)){
    return  Eigen::Quaterniond(0,0,0,1);
  }

  double sy = sin(yaw*0.5);
  double cy = cos(yaw*0.5);
  double sp = sin(pitch*0.5);
  double cp = cos(pitch*0.5);
  double sr = sin(roll*0.5);
  double cr = cos(roll*0.5);
  double w = cr*cp*cy + sr*sp*sy;
  double x = sr*cp*cy - cr*sp*sy;
  double y = cr*sp*cy + sr*cp*sy;
  double z = cr*cp*sy - sr*sp*cy;
  return Eigen::Quaterniond(w,x,y,z);
}

void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
  const double q0 = q.w();
  const double q1 = q.x();
  const double q2 = q.y();
  const double q3 = q.z();
  roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
  pitch = asin(2*(q0*q2-q3*q1));
  yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}

VectorXd robotStateToDrakePosition(const bot_core::robot_state_t& rstate,
                                   const map<string, int>& dofMap,
                                   int num_positions)
{
  VectorXd q = VectorXd::Zero(num_positions, 1);
  for (int i=0; i < rstate.num_joints; ++i) {
    auto iter = dofMap.find(rstate.joint_name.at(i));
    if (iter != dofMap.end()) {
      int index = iter->second;
      q(index) = rstate.joint_position[i];
    }
  }

  map<string,int>::const_iterator iter;
  iter = dofMap.find("base_x");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rstate.pose.translation.x;
  }
  iter = dofMap.find("base_y");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rstate.pose.translation.y;
  }
  iter = dofMap.find("base_z");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rstate.pose.translation.z;
  }

  Vector4d quat;
  quat[0] = rstate.pose.rotation.w;
  quat[1] = rstate.pose.rotation.x;
  quat[2] = rstate.pose.rotation.y;
  quat[3] = rstate.pose.rotation.z;
  Vector3d rpy = quat2rpy(quat);

  iter = dofMap.find("base_roll");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rpy[0];
  }

  iter = dofMap.find("base_pitch");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rpy[1];
  }

  iter = dofMap.find("base_yaw");
  if (iter!=dofMap.end()) {
    int index = iter->second;
    q[index] = rpy[2];
  }

  return q;
}

/////////////////////////////////////////////////
void App::getRobotState(bot_core::robot_state_t& robot_state_msg, int64_t utime_in, Eigen::VectorXd q, std::vector<std::string> jointNames){
  robot_state_msg.utime = utime_in;

  // Pelvis Pose:
  robot_state_msg.pose.translation.x =q(0);
  robot_state_msg.pose.translation.y =q(1);
  robot_state_msg.pose.translation.z =q(2);

  Eigen::Quaterniond quat = euler_to_quat( q(3), q(4), q(5));

  robot_state_msg.pose.rotation.w = quat.w();
  robot_state_msg.pose.rotation.x = quat.x();
  robot_state_msg.pose.rotation.y = quat.y();
  robot_state_msg.pose.rotation.z = quat.z();

  robot_state_msg.twist.linear_velocity.x  = 0;
  robot_state_msg.twist.linear_velocity.y  = 0;
  robot_state_msg.twist.linear_velocity.z  = 0;
  robot_state_msg.twist.angular_velocity.x = 0;
  robot_state_msg.twist.angular_velocity.y = 0;
  robot_state_msg.twist.angular_velocity.z = 0;

  // Joint States:
  for (size_t i = 0; i < jointNames.size(); i++)  {
    robot_state_msg.joint_name.push_back( jointNames[i] );
    robot_state_msg.joint_position.push_back( q(i) );
    robot_state_msg.joint_velocity.push_back( 0);
    robot_state_msg.joint_effort.push_back( 0 );
  }
  robot_state_msg.num_joints = robot_state_msg.joint_position.size();
}

int App::getConstraints(Eigen::VectorXd q_star, Eigen::VectorXd &q_sol){
  Vector2d tspan;
  tspan << 0, 1;

  // 0 Pelvis Position and Orientation Constraints
  int pelvis_link = model_.findLinkId("pelvis");
  Vector3d pelvis_pt = Vector3d::Zero();
  Vector3d pelvis_pos0;
  pelvis_pos0(0) = rstate_.pose.translation.x;
  pelvis_pos0(1) = rstate_.pose.translation.y;
  pelvis_pos0(2) = rstate_.pose.translation.z;
  Vector3d pelvis_pos_lb = pelvis_pos0;
  //pelvis_pos_lb(0) += 0.001;
  //pelvis_pos_lb(1) += 0.001;
  //pelvis_pos_lb(2) += 0.001;
  Vector3d pelvis_pos_ub = pelvis_pos_lb;
  //pelvis_pos_ub(2) += 0.001;
  WorldPositionConstraint kc_pelvis_pos(&model_, pelvis_link, pelvis_pt, pelvis_pos_lb, pelvis_pos_ub, tspan);
  Eigen::Vector4d pelvis_quat_des(rstate_.pose.rotation.w , rstate_.pose.rotation.x, rstate_.pose.rotation.y, rstate_.pose.rotation.z);
  double pelvis_tol = 0;//0.0017453292519943296;
  WorldQuatConstraint kc_pelvis_quat(&model_, pelvis_link, pelvis_quat_des, pelvis_tol, tspan);

  // 1 Back Posture Constraint
  PostureConstraint kc_posture_back(&model_, tspan);
  std::vector<int> back_idx;
  findJointAndInsert(model_, "torsoYaw", back_idx);
  findJointAndInsert(model_, "torsoPitch", back_idx);
  findJointAndInsert(model_, "torsoRoll", back_idx);
  VectorXd back_lb = VectorXd::Zero(3);
  VectorXd back_ub = VectorXd::Zero(3);
  kc_posture_back.setJointLimits(3, back_idx.data(), back_lb, back_ub);

  // 2 Upper Neck Constraint
  PostureConstraint kc_posture_neck(&model_, tspan);
  std::vector<int> neck_idx;
  findJointAndInsert(model_, "upperNeckPitch", neck_idx);
  VectorXd neck_lb = VectorXd::Zero(1);
  neck_lb(0) = 0.0;
  VectorXd neck_ub = VectorXd::Zero(1);
  neck_ub(0) = 0.0;
  kc_posture_neck.setJointLimits(1, neck_idx.data(), neck_lb, neck_ub);

  // 3 Look At constraint:
  int head_link = model_.findLinkId("head");
  Eigen::Vector3d gaze_axis = Eigen::Vector3d(1,0,0);
  Eigen::Vector3d target = cl_cfg_.gazeGoal;
  Eigen::Vector3d gaze_origin = Eigen::Vector3d(0,0, 0);//0.45);// inserting this offset almost achieves the required look-at
  double conethreshold = 0;
  WorldGazeTargetConstraint kc_gaze(&model_, head_link, gaze_axis, target, gaze_origin, conethreshold, tspan);

  // Assemble Constraint Set
  std::vector<RigidBodyConstraint *> constraint_array;
  constraint_array.push_back(&kc_pelvis_pos);
  constraint_array.push_back(&kc_pelvis_quat);
  constraint_array.push_back(&kc_gaze);
  constraint_array.push_back(&kc_posture_back); // leave this out to also use the back
  constraint_array.push_back(&kc_posture_neck); // upper neck pitch

  // Solve
  IKoptions ikoptions(&model_);
  int info;
  vector<string> infeasible_constraint;
  inverseKin(&model_, q_star, q_star, constraint_array.size(), constraint_array.data(), q_sol, info, infeasible_constraint, ikoptions);
  printf("INFO = %d\n", info);
  return info;
}

static inline bot_core::pose_t getPoseAsBotPose(Eigen::Isometry3d pose, int64_t utime){
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  return pose_msg;
}

void App::solveGazeProblem(){

  // Publish the query for visualisation in Director
  bot_core::pose_t goalMsg;
  goalMsg.utime = rstate_.utime;
  goalMsg.pos[0] = cl_cfg_.gazeGoal(0);
  goalMsg.pos[1] = cl_cfg_.gazeGoal(1);
  goalMsg.pos[2] = cl_cfg_.gazeGoal(2);
  goalMsg.orientation[0] = 1;
  goalMsg.orientation[1] = 0;
  goalMsg.orientation[2] = 0;
  goalMsg.orientation[3] = 0;
  lcm_->publish("POSE_BODY_ALT", &goalMsg);

  // Solve the IK problem for the neck:
  VectorXd q_star(robotStateToDrakePosition(rstate_, dofMap_, model_.num_positions));
  VectorXd q_sol(model_.num_positions);
  int info = getConstraints(q_star, q_sol);
  if (info != 1) {
    std::cout << "Problem not solved\n";
    return;
  }

  bool mode = 0;
  if (mode==0){ // publish utorso-to-head as orientation, not properly tracking but works with different orientations
    // Get the utorso to head frame:
    int head_link = model_.findLinkId("head");
    int utorso_link = model_.findLinkId("torso");
    KinematicsCache<double> cache = model_.doKinematics(q_sol);
    Eigen::Isometry3d world_to_head = model_.relativeTransform(cache, head_link, 0);
    Eigen::Isometry3d utorso_to_head = model_.relativeTransform(cache, head_link, utorso_link);

    // Apply 180 roll as head orientation control seems to be in z-up frame
    Eigen::Isometry3d rotation_frame;
    rotation_frame.setIdentity();
    Eigen::Quaterniond quat = euler_to_quat( M_PI, 0, 0);
    rotation_frame.rotate(quat);
    utorso_to_head = utorso_to_head*rotation_frame;

    bot_core::pose_t world_to_head_frame_pose_msg =  getPoseAsBotPose(world_to_head, rstate_.utime);
    lcm_->publish("POSE_VICON",&world_to_head_frame_pose_msg);// for debug
    bot_core::pose_t utorso_to_head_frame_pose_msg =  getPoseAsBotPose(utorso_to_head, rstate_.utime);
    lcm_->publish("DESIRED_HEAD_ORIENTATION",&utorso_to_head_frame_pose_msg);// temp
  }else{ // publish neck pitch and yaw joints as orientation. this works ok when robot is facing 1,0,0,0
    // Fish out the two neck joints (in simulation) and send as a command:
    std::vector<string> jointNames;
    for (int i=0 ; i <model_.num_positions ; i++){
      // std::cout << model.getPositionName(i) << " " << i << "\n";
      jointNames.push_back( model_.getPositionName(i) ) ;
    }
    bot_core::robot_state_t robot_state_msg;
    getRobotState(robot_state_msg, 0*1E6, q_sol , jointNames);
    lcm_->publish("CANDIDATE_ROBOT_ENDPOSE",&robot_state_msg);

    std::vector<std::string>::iterator it1 = find(jointNames.begin(),
        jointNames.end(), "lowerNeckPitch");
    int lowerNeckPitchIndex = std::distance(jointNames.begin(), it1);
    float lowerNeckPitchAngle = q_sol[lowerNeckPitchIndex];

    std::vector<std::string>::iterator it2 = find(jointNames.begin(),
        jointNames.end(), "neckYaw");
    int neckYawIndex = std::distance(jointNames.begin(), it2);
    float neckYawAngle = q_sol[neckYawIndex];

    std::cout << lowerNeckPitchAngle << " (" << lowerNeckPitchAngle*180.0/M_PI << ") is lowerNeckPitchAngle\n";
    std::cout << neckYawAngle << " (" << neckYawAngle*180.0/M_PI << ") is neckYawAngle\n";

    bot_core::pose_t headOrientationMsg;
    headOrientationMsg.utime = rstate_.utime;
    headOrientationMsg.pos[0] = 0;
    headOrientationMsg.pos[1] = 0;
    headOrientationMsg.pos[2] = 0;
    Eigen::Quaterniond quat = euler_to_quat(0, lowerNeckPitchAngle, neckYawAngle);
    headOrientationMsg.orientation[0] = quat.w();
    headOrientationMsg.orientation[1] = quat.x();
    headOrientationMsg.orientation[2] = quat.y();
    headOrientationMsg.orientation[3] = quat.z();
    lcm_->publish("DESIRED_HEAD_ORIENTATION",&headOrientationMsg);
    lcm_->publish("POSE_VICON",&headOrientationMsg); // for debug
  }

  //std::cout << "Desired orientation sent, exiting\n";
  //exit(-1);
}

int counter=0;
void App::robotStateHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::robot_state_t* msg){
  rstate_= *msg;
  if (counter%400 == 0 ){
    solveGazeProblem();
  }
  counter++;
}

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

  CommandLineConfig cl_cfg;
  cl_cfg.gazeGoal = Eigen::Vector3d(2,1,1.2); // Position we would like the head to gaze at
  ConciseArgs parser(argc, argv, "simple-fusion");
  parser.add(cl_cfg.urdf_filename, "u", "urdf", "urdf filename");
  parser.add(cl_cfg.gazeGoal(0) , "x", "goal_x", "goal_x");
  parser.add(cl_cfg.gazeGoal(1) , "y", "goal_y", "goal_y");
  parser.add(cl_cfg.gazeGoal(2) , "z", "goal_z", "goal_z");
  parser.parse();

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  App app(lcm, cl_cfg);
  cout << "Ready" << endl << "============================" << endl;
  while(0 == lcm->handle());
}


//==================================================================================================
/**
  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_COMMON_SCALAR_FUNCTION_REC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REC_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/fast.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return One<A0>()/a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename T)
                          , bd::cpu_
                          , bs::fast_tag
                          , bd::scalar_<bd::unspecified_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(const fast_tag &, T a) const BOOST_NOEXCEPT
    {
      return rec(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename T)
                          , bd::cpu_
                          , bs::raw_tag
                          , bd::scalar_<bd::unspecified_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(const raw_tag &, T a) const BOOST_NOEXCEPT
    {
      return rec(a);
    }
  };
} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


/*
 * clustering.hh
 *
 *  Created on: 10/3/2016
 *      Author: Ismael
 */

#ifndef CLUSTERING_HH_
#define CLUSTERING_HH_

#include <cmath>
#include <list>

#include <iostream>

#include <boost/utility.hpp>

//#include "../SMA_Core/complex.hh"

//#include "../SMA_Interface/complex_io.hh"

#include "../SMA_Core/molecule_info.hh"

using namespace std;

using namespace boost;

#define DISTANCE_THRES	0.85

typedef struct Cluster_
{
	list<Point *> 	PointList;
	int				ClusterId;

} Cluster;

#define FREE_EDGE_WEIGHT	1000.

/* ClusterizeComplex
 * 	Given a complex, it applies density-based clustering to merge those points that are close enough to each other to be considered the same.
 * 	Edges are recomputed considering if they connected points within clusters (i.e., there will be an edge between two clusters if there was
 * 	an edge between any point of the first cluster and any point of the second).
 *
 * Inputs:
 * 	TargetComplex - Complex to be clusterized (typically, the Voronoi graph of the molecule).
 *
 * Outputs:
 * 	Clusterized complex.
 *
 */
Complex *ClusterizeComplex (	Complex *TargetComplex,
								Complex *Chemical,
								double DistanceThreshold);

#endif /* CLUSTERING_HH_ */


/*
 * Open Source Movement Analysis Library
 * Copyright (C) 2016, Moveck Solution Inc., 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(s) of the copyright holders 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.
 */

#include "openma/processing.h"
#include "openma/base/timesequence.h"
#include "openma/base/logger.h"
#include "openma/math.h" // ma::math::prepare_window_processing

#include <iostream> // Used by IIRFilterDesign
#include <Eigen_openma/SignalProcessing/IIRFilterDesign.h>
#include <Eigen_openma/SignalProcessing/FiltFilt.h>

void _ma_processing_butterworth_zero_lag_filter(ma::TimeSequence* ts, const Eigen::Matrix<double, Eigen::Dynamic, 1>& b, const Eigen::Matrix<double, Eigen::Dynamic, 1>& a)
{
  Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>> data(ts->data(), ts->samples(), ts->components());
  data = Eigen::filtfilt(b,a,data);
};

void _ma_processing_butterworth_zero_lag_filter_windowed(ma::TimeSequence* ts, const Eigen::Matrix<double, Eigen::Dynamic, 1>& b, const Eigen::Matrix<double, Eigen::Dynamic, 1>& a)
{
  int cpts = ts->components()-1;
  Eigen::Map<Eigen::Array<double,Eigen::Dynamic,1>> resin(ts->data()+cpts*ts->samples(), ts->samples(), 1);
  Eigen::Array<double,Eigen::Dynamic,1> resout;
  unsigned mwlen = 3 * std::max(a.rows(),b.rows()) - 1;
  std::vector<std::array<unsigned,2>> windows;
  ma::math::prepare_window_processing(resout, windows, resin, mwlen);
  Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>> data(ts->data(), ts->samples(), ts->components());
  Eigen::Matrix<double,Eigen::Dynamic,1> temp = Eigen::Matrix<double,Eigen::Dynamic,1>::Zero(data.rows(),1);
  for (int i = 0 ; i < cpts ; ++i)
  {
    for (const auto& w : windows)
      temp.segment(w[0],w[1]) = Eigen::filtfilt(b,a,data.block(w[0],i,w[1],1));
    data.col(i) = temp;
  }
  resin = resout;
};

namespace ma
{
namespace processing
{
  /**
   * Convenient function to simplify the data filtering of time sequences with a zero-lag Butterworth filter
   * Internally, this function does two filtering passes (one forward followed by one backward). To respect the given final cut-off frequency (@a fc) and the final order (@a fn), these ones are adapted in consequence (see Robertson & Dowling [1] and Winter [2]).
   * This function manage each component independently. In case the type of a time sequence is tagged ma::TimeSequence::Reconstructed, the filtering is managed in consequence and realized by window.
   *
   * @important This is the responsability of the developer to use correctly this function. It is not intended to verify if the passed time sequence(s) is(are) physically adapted to this filter. For example, the filter is adapted for time sequence with the types TimeSequence::Position and TimeSequence::Analog but might not with the types TimeSequence::Pose and TimeSequence::Wrench.
   *
   * @par References
   * 1. Robertson D.G., Dowling J.J., Design and responses of Butterworth and critically damped digital filters, Journal of Electromyography and Kinesiology, 2003.
   * 2. Winter D.A., Biomechanics and Motor Control of Human Movement, John Wiley & Sons, Inc., 2009, paragraph 3.4.4.2.
   *
   * @ingroup openma_processing
   */
  bool filter_butterworth_zero_lag(const std::vector<TimeSequence*>& tss, Response type, double fc, int fn)
  {
    if (fc <= 0.0)
    {
      error("Impossible to have a cut-off frequency equal or lower than 0.0.");
      return false;
    }
    if (fn <= 1)
    {
      error("Impossible to have an order equal or lower than 1.");
      return false;
    }
    Eigen::BandType t = Eigen::LowPass;
    switch (type)
    {
    case Response::LowPass:
      break;
    case Response::HighPass:
      t = Eigen::HighPass;
      break;
    case Response::BandPass:
      t = Eigen::BandPass;
      break;
    case Response::BandStop:
      t = Eigen::BandStop;
      break;
    default:
      error("Unknown band type for the Butterworth filter. Filtering aborted.");
      return false;
    }
    for (const auto& ts : tss)
    {
      if (ts->sampleRate() == 0.0)
      {
        error("The time sequence '%s' has a null sample rate.", ts->name().c_str());
        continue;
      }
      double wn = fc / (ts->sampleRate() / 2.0);
      int n = fn;
      Eigen::adjustZeroLagButterworth(n, wn);
      Eigen::Matrix<double, Eigen::Dynamic, 1> a, b;
      Eigen::butter(&b, &a, n, wn, t);
      if ((ts->type() & TimeSequence::Reconstructed) == TimeSequence::Reconstructed)
        _ma_processing_butterworth_zero_lag_filter_windowed(ts,b,a);
      else
        _ma_processing_butterworth_zero_lag_filter(ts,b,a);
    }
    return true;
  };
};
};


#include <boost/test/unit_test.hpp>
// #include "boost/list.hpp"

#include "fhmdot/include/qnum.hpp"

using namespace fhmdot::qnum;

BOOST_AUTO_TEST_CASE(unittest_qnum_sh) {
  BOOST_CHECK(quantum_number_crtp<sh_none>::dim == 1);
  BOOST_CHECK(quantum_number_crtp<sh_none>::sum(2, 3) == 0);
  BOOST_CHECK(quantum_number_crtp<sh_none>::sum(7, 2) == 0);

  BOOST_CHECK(quantum_number_crtp<sh_u1>::dim == 2);
  BOOST_CHECK(quantum_number_crtp<sh_u1>::sum(0, 0) == 0);
  BOOST_CHECK(quantum_number_crtp<sh_u1>::sum(1, 7) == 8);
  BOOST_CHECK(quantum_number_crtp<sh_u1>::sum(4, 9) == 13);

  BOOST_CHECK(quantum_number_crtp<sh_su2>::dim == 1);
  BOOST_CHECK(quantum_number_crtp<sh_su2>::sum(0, 0) == 0);
  BOOST_CHECK(quantum_number_crtp<sh_su2>::sum(6, 7) == 13);
  BOOST_CHECK(quantum_number_crtp<sh_su2>::sum(4, 3) == 7);
}


#include <boost/numeric/ublas/matrix_vector.hpp>