// erik reed
// erikreed@cmu.edu

#include <dai/alldai.h>
#include <iostream>
#include <fstream>
#include <string>
#include <time.h>
#include <fstream>
#include <math.h>
#include <stdio.h>
#include "include/hadoop/Pipes.hh"
#include "include/hadoop/TemplateFactory.hh"
#include "include/hadoop/StringUtils.hh"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/string.hpp>

using namespace std;
using namespace dai;
using namespace HadoopPipes;

#define INF_TYPE "JTREE"

const Real LIB_EM_TOLERANCE = 1e-9;
const size_t EM_MAX_ITER = 100;

struct EMdata {
	string emFile;
	string fgFile;
	string tabFile;
	size_t iter;
	Real likelihood;
	vector<MaximizationStep> msteps;
	int bnID;

	friend class boost::serialization::access;
	template<class Archive>
	void serialize(Archive & ar, const unsigned int version) {
		ar & emFile;
		ar & fgFile;
		ar & tabFile;
		ar & iter;
		ar & likelihood;
		ar & msteps;
		ar & bnID;
	}
};

inline string emToString(const EMdata &em) {
	ostringstream s;
	s << std::scientific;
	boost::archive::text_oarchive oa(s);
	oa << em;
	return s.str();
}

inline EMdata stringToEM(const string &s) {
	istringstream ss(s);
	boost::archive::text_iarchive ia(ss);
	EMdata em;
	ia >> em;
	return em;
}

string convertInt(int number) {
	stringstream ss;//create a stringstream
	ss << number;//add number to the stream
	return ss.str();//return a string with the contents of the stream
}

string readFile(const char* path) {

	int length;
	char * buffer;

	ifstream is;
	is.open(path, ios::binary);

	// get length of file:
	is.seekg(0, ios::end);
	length = is.tellg();
	is.seekg(0, ios::beg);
	// allocate memory:
	buffer = new char[length];

	// read data as a block:
	is.read(buffer, length);
	is.close();
	string out = buffer;
	delete[] buffer;
	return out;
}

bool str_replace(std::string& str, const std::string& from,
		const std::string& to) {
	size_t start_pos = str.find(from);
	if (start_pos == std::string::npos)
		return false;
	str.replace(start_pos, from.length(), to);
	return true;
}

void randomize_fg(FactorGraph* fg) {
	vector<Factor> factors = fg->factors();
	size_t size = factors.size();
	for (size_t i = 0; i < size; i++) {
		Factor f = fg->factor(i);
		f.randomize();
		fg->setFactor(i, f, false);
	}
}

vector<std::string> &str_split(const std::string &s, char delim, std::vector<
		std::string> &elems) {
	std::stringstream ss(s);
	std::string item;
	while (std::getline(ss, item, delim))
		elems.push_back(item);
	return elems;
}

std::vector<std::string> str_split(const std::string &s, char delim) {
	std::vector<std::string> elems;
	return str_split(s, delim, elems);
}

void doEm(char* fgIn, char* tabIn, char* emIn, int init) {
	string fixedIn = emIn;

	FactorGraph fg;
	fg.ReadFromFile(fgIn);

	// Prepare junction-tree object for doing exact inference for E-step
	PropertySet infprops;
	infprops.set("verbose", (size_t) 1);
	infprops.set("updates", string("HUGIN"));
	infprops.set("log_z_tol", LIB_EM_TOLERANCE);
	infprops.set("LOG_Z_TOL_KEY", LIB_EM_TOLERANCE);
	infprops.set(EMAlg::LOG_Z_TOL_KEY, LIB_EM_TOLERANCE);

	InfAlg* inf = newInfAlg(INF_TYPE, fg, infprops);
	inf->init();

	// Read sample from file
	Evidence e;
	ifstream estream(tabIn);
	e.addEvidenceTabFile(estream, fg);
	cout << "Number of samples: " << e.nrSamples() << endl;

	// Read EM specification
	ifstream emstream(emIn);
	EMAlg em(e, *inf, emstream);

	// Iterate EM until convergence
	while (!em.hasSatisfiedTermConditions()) {
		em.iterate();
	}

	stringstream ss;
	ss << inf->fg() << endl;

	// Clean up
	delete inf;
	//	return ss.str();

}

Real EM_estep(MaximizationStep &mstep, const Evidence &evidence, InfAlg &inf) {
	Real logZ = 0;
	Real likelihood = 0;

	inf.run();
	logZ = inf.logZ();

	// Expectation calculation
	for (Evidence::const_iterator e = evidence.begin(); e != evidence.end(); ++e) {
		InfAlg* clamped = inf.clone();
		// Apply evidence
		for (Evidence::Observation::const_iterator i = e->begin(); i
				!= e->end(); ++i)
			clamped->clamp(clamped->fg().findVar(i->first), i->second);
		clamped->init();
		clamped->run();

		likelihood += clamped->logZ() - logZ;

		mstep.addExpectations(*clamped);

		delete clamped;
	}

	// Maximization of parameters
	//    mstep.maximize( _estep.fg() );

	return likelihood;
}

bool emHasSatisfiedTermConditions(size_t iter, Real previous, Real current) {
    if( iter >= EM_MAX_ITER )
        return true;
    else if( iter < 3 )
        // need at least 2 to calculate ratio
        // Also, throw away first iteration, as the parameters may not
        // have been normalized according to the estimation method
        return false;
    else {
        if( previous == 0 )
            return false;
        Real diff = current - previous;
        if( diff < 0 ) {
            std::cerr << "Error: in EM log-likehood decreased from " << previous << " to " << current << std::endl;
            return true;
        }
        return (diff / fabs(previous)) <= LIB_EM_TOLERANCE;
    }
}

//TODO: sum likelihood when evidence is split up
Real hadoop_iterate(vector<MaximizationStep>& msteps, const Evidence &e,
		InfAlg &inf) {
	Real likelihood = 0;
	for (size_t i = 0; i < msteps.size(); ++i)
		likelihood = EM_estep(msteps[i], e, inf);
	//    _lastLogZ.push_back( likelihood );
	//    ++_iters;
	return likelihood;
}

string testEM(char* fgIn, char* tabIn, char* emIn, bool serial) {

	FactorGraph fg;
	fg.ReadFromFile(fgIn);

	// Prepare junction-tree object for doing exact inference for E-step
	PropertySet infprops;
	infprops.set("verbose", (size_t) 1);
	infprops.set("updates", string("HUGIN"));
	infprops.set("log_z_tol", LIB_EM_TOLERANCE);
	infprops.set("LOG_Z_TOL_KEY", LIB_EM_TOLERANCE);
	infprops.set(EMAlg::LOG_Z_TOL_KEY, LIB_EM_TOLERANCE);

	InfAlg* inf = newInfAlg(INF_TYPE, fg, infprops);
	inf->init();

	// Read sample from file
	Evidence e;
	ifstream estream(tabIn);
	e.addEvidenceTabFile(estream, fg);
	cout << "Number of samples: " << e.nrSamples() << endl;

	// Read EM specification
	ifstream emstream(emIn);
	EMAlg em(e, *inf, emstream);

	// Iterate EM until convergence
	while (!em.hasSatisfiedTermConditions()) {
		Real likelihood = hadoop_iterate(em._msteps, em._evidence, em._estep);
		em._iters++;
		em._lastLogZ.push_back(likelihood);

		if (serial) {
			ostringstream s;
			vector<MaximizationStep> m = em._msteps;

			boost::archive::text_oarchive oa(s);
			oa << m;

			istringstream s2(s.str());

			boost::archive::text_iarchive ia(s2);
			// read class state from archive
			vector<MaximizationStep> m2;
			ia >> m2;
			em._msteps = m2;

		}
		for (size_t i = 0; i < em._msteps.size(); i++)
			em._msteps[i].maximize(em._estep.fg());
		cout << em._iters << '\t' << likelihood << endl;
	}

	stringstream ss;
	ss << inf->fg() << endl;

	// Clean up
	delete inf;
	return ss.str();

}

string mapper(const string& in) {

	EMdata dat = stringToEM(in);

	FactorGraph fg;

	istringstream fgStream(dat.fgFile);
	fgStream >> fg;

	// Prepare junction-tree object for doing exact inference for E-step
	PropertySet infprops;
	infprops.set("verbose", (size_t) 1);
	infprops.set("updates", string("HUGIN"));
	infprops.set("log_z_tol", LIB_EM_TOLERANCE);
	infprops.set("LOG_Z_TOL_KEY", LIB_EM_TOLERANCE);
	infprops.set(EMAlg::LOG_Z_TOL_KEY, LIB_EM_TOLERANCE);

	InfAlg* inf = newInfAlg(INF_TYPE, fg, infprops);
	inf->init();

	// Read sample from file
	Evidence e;
	istringstream estream(dat.tabFile);
	e.addEvidenceTabFile(estream, fg);
	cout << "Number of samples: " << e.nrSamples() << endl;

	// Read EM specification
	istringstream emstream(dat.emFile);
	EMAlg em(e, *inf, emstream);

	// perform 1 iteration e-step
	Real likelihood = hadoop_iterate(em._msteps, em._evidence, em._estep);

	dat.likelihood = likelihood;
	dat.msteps = em._msteps;

	// Clean up
	delete inf;

	return emToString(dat);
}

// TODO: key val will be BN_ID:E1,E2,E3 where E_N corresponds to evidence set N
string em_reduce(vector<string>& in) {
	// using first EMdata to store e-step counts and create fg
	EMdata dat = stringToEM(in[0]);
	FactorGraph fg;
	istringstream fgStream(dat.fgFile);
	fgStream >> fg;

	// collect stats for each set of evidence
	for (size_t i=1; i<in.size(); i++) {
		EMdata next = stringToEM(in[i]);
		DAI_ASSERT(dat.msteps.size() == next.msteps.size());

		for (size_t j = 0; j < dat.msteps.size(); j++) {
			dat.msteps[j].addExpectations(next.msteps[j]);
			dat.likelihood += next.likelihood;
		}
	}

	// m-step
	for (size_t i=0; i<dat.msteps.size(); i++)
		dat.msteps[i].maximize(fg);

	ostringstream newFg;
	newFg << fg;

	// save new fg, clear counts, increment iter
	dat.fgFile = newFg.str();
	dat.msteps.clear();
	dat.iter++;

	return emToString(dat);
}

class DaiEM_RecordReader: public HadoopPipes::RecordReader {
public:

	int num_mappers;
	int current;

	string emFile;
	string fgFile;
	string tabFile;
	string tabHeader;
	vector<string> tabLines;
	size_t samplesPerMapper;
	vector<string> evidencePerMapper;

	size_t t;
	EMdata datForMapper;

	DaiEM_RecordReader(HadoopPipes::TaskContext& context) {
		current = 0;
		const HadoopPipes::JobConf* job = context.getJobConf();
		num_mappers = job->getInt("mapred.map.tasks");
		cout << "num mappers: " << num_mappers << endl;

		emFile = readFile("in/em");
		fgFile = readFile("in/fg");
		tabFile = readFile("in/tab_content");
		tabHeader = readFile("in/tab_header");

		tabLines = str_split(tabFile, '\n');
		if (num_mappers > tabLines.size()) {
			cerr << "more mappers than evidence samples..." << endl;
			throw;
		}
		evidencePerMapper.reserve(num_mappers);
		samplesPerMapper = (tabLines.size())  / num_mappers;

		t = 0;

		datForMapper.iter = 0;
		datForMapper.likelihood = 0;
		datForMapper.emFile = emFile;
		datForMapper.fgFile = fgFile;

		// read evidence
		for (int i = 0; i < num_mappers; i++) {
			ostringstream evidence;
			// 2 header lines: labels followed by blank line
			evidence << tabHeader << '\n';
			for (size_t j = 0; j < samplesPerMapper && t < tabLines.size(); j++) {
				evidence << tabLines[t++] << '\n';
			}
			evidencePerMapper.push_back(evidence.str());
		}

	}

	bool next(std::string& key, std::string& value) {
		datForMapper.tabFile = evidencePerMapper[current];
		datForMapper.bnID = current;
		key = HadoopUtils::toString(current);
		value = emToString(datForMapper);
		return (++current <= num_mappers);
	}

	float getProgress() {
		return ((float) current) / num_mappers;
	}
};

class DaiEM_Map: public HadoopPipes::Mapper {
public:
	DaiEM_Map(HadoopPipes::TaskContext& context){}

	void map(HadoopPipes::MapContext& context) {

		string in = context.getInputKey();
		string out = mapper(in);
		//todo: make key BN_id
		context.emit(string("1"),out);
	}
};

class DaiEM_Reduce: public HadoopPipes::Reducer {
public:
	DaiEM_Reduce(HadoopPipes::TaskContext& context){}
	void reduce(HadoopPipes::ReduceContext& context) {

		vector<string> in;
		while (context.nextValue())
			in.push_back(context.getInputValue());

		string out = em_reduce(in);
		context.emit("1",stringToEM(out).fgFile);
	}
};

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

	bool hadoop = false;
	bool tests = true;

	if (!hadoop) {

		size_t numMappers = 5;

		string emFile = readFile("dat/em");
		string fgFile = readFile("dat/fg");
		string tabFile = readFile("dat/tab");

		vector<string> tabLines = str_split(tabFile, '\n');

		if (numMappers > tabLines.size() - 2) {
			cerr << "more mappers than evidence samples..." << endl;
			throw;
		}

		size_t samplesPerMapper = (tabLines.size() - 2)  / numMappers;
		vector<string> evidencePerMapper;

		size_t t = 3;
		EMdata datForMapper;
		datForMapper.iter = 0;
		datForMapper.likelihood = 0;
		datForMapper.emFile = emFile;
		datForMapper.fgFile = fgFile;

		// read evidence
		for (size_t i = 0; i < numMappers; i++) {

			ostringstream evidence;
			// 2 header lines: labels followed by blank line
			evidence << tabLines[0] << "\n\n";
			for (size_t j = 0; j < samplesPerMapper && t < tabLines.size(); j++) {
				evidence << tabLines[t++] << '\n';
			}
			evidencePerMapper.push_back(evidence.str());
		}

		if (tests) {
			Real lastLikelihood = 0;
			Real likelihood = 0;
			while (!emHasSatisfiedTermConditions(datForMapper.iter,lastLikelihood,likelihood)) {
				lastLikelihood = datForMapper.likelihood;
				// map-phase
				vector<string> datForReducer;
				for (size_t i = 0; i < numMappers; i++) {
					datForMapper.tabFile = evidencePerMapper[i];
					string mapperIn = emToString(datForMapper);
					string mapperOut = mapper(mapperIn);
					datForReducer.push_back(mapperOut);
				}
				// reduce-phase
				string reducerOut = em_reduce(datForReducer);
				datForMapper = stringToEM(reducerOut);
				likelihood = datForMapper.likelihood;
				cout << datForMapper.iter << '\t' << likelihood << endl;
			}
			string s1 = testEM("dat/fg", "dat/tab", "dat/em", false);
			string s2 = testEM("dat/fg", "dat/tab", "dat/em", true);
			cout << likelihood << endl;
			if (s1 != s2)
				throw;
		}
	}
	else
		return HadoopPipes::runTask(
			HadoopPipes::TemplateFactory<DaiEM_Map,DaiEM_Reduce,void,void,DaiEM_RecordReader>());

//	return 0;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(inside_aabb_test)
{

  {
    V const min_coord = V::make(-1.0, -1.0, -1.0);
    V const max_coord = V::make( 1.0,  1.0,  1.0);

    geometry::AABB<V> A = geometry::make_aabb(min_coord, max_coord);

    V const p0 = V::make( 0.0,  0.0,  0.0);
    V const p1 = V::make( 1.0,  1.0,  1.0);
    V const p2 = V::make( 2.0,  2.0,  2.0);

    BOOST_CHECK( geometry::inside_aabb(p0, A) == true );
    BOOST_CHECK( geometry::inside_aabb(p1, A) == true );
    BOOST_CHECK( geometry::inside_aabb(p2, A) == false );

  }

}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <iostream>
#include <fstream>

#include <stdint.h>
#include <vector>
#include <Eigen/Dense>

#include <boost/shared_ptr.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/gamma_distribution.hpp> // for gamma_distribution.
#include <boost/math/special_functions/gamma.hpp>

#include <dpMM/cat.hpp>
#include <dpMM/dpMM.hpp>
#include <dpMM/lrCluster.hpp>
#include <dpMM/dir.hpp>
#include <dpMM/dirBaseMeasure.hpp>

using namespace Eigen;
using std::endl;
using std::cout;
using boost::shared_ptr;
//using boost::math::lgamma;

#undef CUDA

template <class B, typename T>
class DpSubclusterMM : public DpMM<T>
{
public:
  DpSubclusterMM(const T alpha, const shared_ptr<LrCluster<B,T> >& theta,
    uint32_t K0, boost::mt19937 *pRndGen);
  virtual ~DpSubclusterMM();

  virtual void initialize(const shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx);

  void relinearize();
  void relinearize(uint32_t k);

  virtual void sampleLabels();
  virtual void sampleParameters_(); // only parameters
  virtual void sampleParameters() // also does splits and merges
  { sampleParameters_();
//    cout<<"  counts= "<<counts(z_,2*K_).transpose()<<endl;
//    cout<<" ----------------------- proposing merges ----------------------"<<endl;
//    proposeMerges();
//    cout<<"  counts= "<<counts(z_,2*K_).transpose()<<endl;
//    cout<<" ----------------------- proposing splits ----------------------"<<endl;
//    proposeSplits();
//    cout<<"  counts= "<<counts(z_,2*K_).transpose()<<endl;

//    for(uint32_t k=0; k< getK(); ++k)
//      get(k)->print();
  };
  void proposeSplits();
  void proposeMerges();

  // call this right before sampleParameters()
  void resampleFromBase(uint32_t Kmax);

  void proposeRandomSplits();

  virtual double logJoint();

  virtual const VectorXu & getLabels(){ return z_;};
  virtual uint32_t getK() const { return K_;};

  shared_ptr<LrCluster<B,T> > get(uint32_t k) { return thetas_[k];};

  MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes);

  virtual double evalLogLikelihood(const Matrix<T,Dynamic,1>& x);

  Matrix<T,Dynamic,1> getCounts();

  void dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs);

protected:

  bool checkLabelMap(const VectorXi & labelMap, uint32_t Knew);

//  void sampleSuperClusters();

  uint32_t K_;
  uint32_t N_;
  uint32_t D_;
  T alpha_;
  shared_ptr<LrCluster<B,T> > theta0_;

  vector<shared_ptr<LrCluster<B,T> > > thetas_;

  shared_ptr<Matrix<T,Dynamic,Dynamic> > spq_; // points on the sphere
  shared_ptr<Matrix<T,Dynamic,Dynamic> > spxMu_; // points in T_muS
  shared_ptr<Matrix<T,Dynamic,Dynamic> > spx_; // points in T_northS
  VectorXu z_;
  VectorXu lr_;

  Matrix<T,Dynamic,1> sticks_;

  boost::mt19937 *pRndGen_;

#ifdef CUDA
  SamplerGpu<T>* sampler_;
#else
  Sampler<T>* sampler_;
#endif
  Matrix<T,Dynamic,Dynamic> pdfsUpper_;
  Matrix<T,Dynamic,2> pdfsLR_;

  Matrix<T,Dynamic,Dynamic> logLikeZcls_; // K*K matrix with the probabilities of sampling a merge between two clusters

};

// --------------------------------- impl -------------------------------------
template <class B, typename T>
DpSubclusterMM<B,T>::DpSubclusterMM(const T alpha,
    const shared_ptr<LrCluster<B,T> >& theta, uint32_t K0, boost::mt19937 *pRndGen)
  : K_(K0), alpha_(alpha), theta0_(theta), sticks_(K0), pRndGen_(pRndGen)
{
};

template <class B, typename T>
DpSubclusterMM<B,T>::~DpSubclusterMM()
{
  if (sampler_ != NULL) delete sampler_;
};

template <class B, typename T>
Matrix<T,Dynamic,1> DpSubclusterMM<B,T>::getCounts()
{
  Matrix<T,Dynamic,1> counts(K_);
  for(uint32_t k=0; k<K_; ++k)
    counts(k) = thetas_[k]->count();
  return counts;
};

template <class B, typename T>
void DpSubclusterMM<B,T>::initialize(
  const shared_ptr<Matrix<T,Dynamic,Dynamic> >& spq)
{
//  assert(K_ == 1);
  spq_ = spq;
  N_ = spq_->cols();
  D_ = spq_->rows();

#ifdef CUDA
  sampler_ = new SamplerGpu<T>(D_,K_,pRndGen_);
  assert(false); //TODO: need to support changing K on GPU sampler!
#else
  sampler_ = new Sampler<T>(pRndGen_);
#endif
  z_.setZero(N_);
  lr_.setZero(N_);

  Matrix<T,Dynamic,1> alpha(2); alpha.setOnes(2);
//  alpha *= alpha_;
  Dir<Cat<T>, T> dirLr(alpha,pRndGen_);
  Cat<T> piLr = dirLr.sample();
  piLr.sample(lr_);
  if(K_ > 1)
  {
    alpha.setOnes(K_);
//    alpha *= alpha_;
    Dir<Cat<T>, T> dir(alpha,pRndGen_);
    Cat<T> pi = dir.sample();
    cout<<"init pi="<<pi.pdf().transpose()<<endl;
    pi.sample(z_);
  }
#pragma omp parallel for
  for(uint32_t i=0; i<z_.size(); ++i)
    z_(i) = z_(i)*2 + lr_(i);

//  cout<<z_.transpose()<<" #labels="<<z_.size()<<endl;
  cout<<"counts = "<<counts<T,uint32_t>(z_,2*K_).transpose()<<endl;

  for(uint32_t k=0; k<K_; ++k)
  {
    thetas_.push_back(shared_ptr<LrCluster<B,T> >(theta0_->copy()));
//    thetas_[k]->posterior(*spx_,z_,2*k);
  }
  pdfsLR_.resize(N_,2);

  uint32_t totalCount=0;
  for(uint32_t k=0; k<K_; ++k)
  {
    assert(thetas_[k] != NULL);
    relinearize(k);
    // compute posteriors and sample parameters
    thetas_[k]->posterior(*spx_,z_,2*k);
    cout<<thetas_[k]->countL()<<" + "<<thetas_[k]->countR()<<" = "<<thetas_[k]->count()<<endl;
    assert(thetas_[k]->countL()+thetas_[k]->countR() == thetas_[k]->count());
    totalCount += thetas_[k]->count();
  }
  cout<<totalCount<<endl;
  assert(totalCount == N_);
//  sticks_.setOnes(K_);
//  sticks_ *= 1./K_;
//  sampleParameters_();
//  dump();
};

template <class B, typename T>
void DpSubclusterMM<B,T>::dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
{
  const uint32_t Kmax = 500;
  Matrix<T,Dynamic,Dynamic> means(D_,Kmax*3); means.setZero();
  for(uint32_t k=0; k<K_; ++k)
  {
    means.col(3*k) = thetas_[k]->getL()->getMean();
    means.col(3*k+1) = thetas_[k]->getR()->getMean();
    means.col(3*k+2) = thetas_[k]->getUpper()->getMean();
  }
  fOutMeans << means<<endl;

  uint32_t D = thetas_[0]->getL()->Sigma().cols();
  Matrix<T,Dynamic,Dynamic> covs(D,Kmax*3*D); covs.setZero();
  for(uint32_t k=0; k<K_; ++k)
  {
    covs.middleCols(3*k*D,D) = thetas_[k]->getL()->Sigma();
    covs.middleCols((3*k+1)*D,D) = thetas_[k]->getR()->Sigma();
    covs.middleCols((3*k+2)*D,D) = thetas_[k]->getUpper()->Sigma();
  }
  fOutCovs << covs<<endl;
}

template <>
void DpSubclusterMM<DirMultSampledd,double>::dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
{
//  const uint32_t Kmax = 500;
//  Matrix<T,Dynamic,Dynamic> means(D_,Kmax*3); means.setZero();
//  for(uint32_t k=0; k<K_; ++k)
//  {
//    means.col(3*k) = thetas_[k]->getL()->getMean();
//    means.col(3*k+1) = thetas_[k]->getR()->getMean();
//    means.col(3*k+2) = thetas_[k]->getUpper()->getMean();
//  }
//  fOutMeans << means<<endl;
//
  for(uint32_t k=0; k<K_; ++k)
  {
    fOutMeans << thetas_[k]->getL()->pdf().transpose()<<endl;
    fOutMeans << thetas_[k]->getR()->pdf().transpose()<<endl;
    fOutMeans << thetas_[k]->getUpper()->pdf().transpose()<<endl;
  }
}

template <>
void DpSubclusterMM<DirMultSampledf,float>::dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
{
  for(uint32_t k=0; k<K_; ++k)
  {
    fOutMeans << thetas_[k]->getL()->pdf().transpose()<<endl;
    fOutMeans << thetas_[k]->getR()->pdf().transpose()<<endl;
    fOutMeans << thetas_[k]->getUpper()->pdf().transpose()<<endl;
  }
}

template <class B, typename T>
void DpSubclusterMM<B,T>::relinearize()
{
  // just use spq as spq - no relinearization is performed
  if(spx_.get() != spq_.get())
    spx_ = spq_;
}

template <class B, typename T>
void DpSubclusterMM<B,T>::relinearize(uint32_t k)
{
  // just use spq as spq - no relinearization is performed
  if(spx_.get() != spq_.get())
    spx_ = spq_;
}

/* specialization for spherical data */
/* TODO: just for testing */
template <>
void DpSubclusterMM<NiwSphered, double>::relinearize(uint32_t k)
{
  if(spx_.get() == NULL)
  {
    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_,N_));
    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
  }
  assert(spx_.get() != NULL);
  // find karcher means  for left and right
  Matrix<double,Dynamic,1> pL = thetas_[k]->getL()->getMean();
  pL = karcherMean<double>(pL,*spq_, *spxMu_, z_, 2*k,100);
  Matrix<double,Dynamic,1> pR = thetas_[k]->getR()->getMean();
  pR = karcherMean<double>(pR,*spq_, *spxMu_, z_, 2*k+1,100);

  // set all means to the same value
  thetas_[k]->getL()->setMean(pL);
  thetas_[k]->getR()->setMean(pR);
  // linearize around the mean and rotate to tangent plane
  Sphere<double> S(D_);
  S.rotate_p2north(pL,*spxMu_,*spx_,z_,2*k);
  S.rotate_p2north(pR,*spxMu_,*spx_,z_,2*k+1);
}

template <>
void DpSubclusterMM<NiwSphereFulld, double>::relinearize(uint32_t k)
{
  if(spx_.get() == NULL)
  {
    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_,N_));
    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
  }
  assert(spx_.get() != NULL);
  // find karcher means  for left and right
  Matrix<double,Dynamic,1> pL = thetas_[k]->getL()->getMean();
  pL = karcherMean<double>(pL,*spq_, *spxMu_, z_, 2*k,100);
  Matrix<double,Dynamic,1> pR = thetas_[k]->getR()->getMean();
  pR = karcherMean<double>(pR,*spq_, *spxMu_, z_, 2*k+1,100);
  Matrix<double,Dynamic,1> p = thetas_[k]->getUpper()->getMean();
  p = karcherMean<double>(p,*spq_, *spxMu_, z_, k,100,2);

#ifndef NDEBUG
  cout<<"\x1b[34m ne linearization point for "<<k<<endl
    <<"U: "<<p.transpose()<<endl
    <<"L: "<<pL.transpose()<<endl
    <<"R: "<<pR.transpose()<<"\x1b[0m"<<endl;
#endif

//  Matrix<double,Dynamic,1> p = rotationFromAtoB<T>(pL,pR,
//      thetas_[k]->countR()/(thetas_[k]->))

  // set all means
  thetas_[k]->getL()->setMean(pL);
  thetas_[k]->getR()->setMean(pR);
  thetas_[k]->getUpper()->setMean(p);

  thetas_[k]->getL()->setMeanKarch(pL); // pL
  thetas_[k]->getR()->setMeanKarch(pR); // pR
  thetas_[k]->getUpper()->setMeanKarch(p);
  // linearize around the mean and rotate to tangent plane
  Sphere<double> S(D_);
  S.Log_p(pL,*spq_,z_,2*k,*spxMu_);
  S.Log_p(pR,*spq_,z_,2*k+1,*spxMu_);
  S.rotate_p2north(pL,*spxMu_,*spx_,z_,2*k);
  S.rotate_p2north(pR,*spxMu_,*spx_,z_,2*k+1);
}

//template <>
//void DpSubclusterMM<NiwTangentd, double>::relinearize(uint32_t k)
//{
//  if(spx_.get() == NULL)
//  {
//    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
//        new Matrix<double,Dynamic,Dynamic>(D_,N_));
//    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
//        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
//  }
//  assert(spx_.get() != NULL);
//  //TODO: find the karcher mean of both L and R at the same time (from union of
//  // point sets).
//  // find karcher means  for left and right
//  Matrix<double,Dynamic,1> p = thetas_[k]->getUpper()->getMean();
//  p = karcherMean<double>(p,*spq_, *spxMu_, z_, k,100,2);
//  cout<<"p upper "<<p.transpose()<<endl;
//  // set all means to the same value
//  thetas_[k]->getL()->setMean(p);
//  thetas_[k]->getR()->setMean(p);
//  thetas_[k]->getUpper()->setMean(p);
//  // linearize around the mean and rotate to tangent plane
//  Sphere<double> S(D_);
//  S.Log_p(p,*spq_,z_,2*k,*spxMu_);
//  S.Log_p(p,*spq_,z_,2*k+1,*spxMu_);
//  S.rotate_p2north(p,*spxMu_,*spx_,z_,2*k);
//  S.rotate_p2north(p,*spxMu_,*spx_,z_,2*k+1);
//}

template <>
void DpSubclusterMM<NiwSphered, double>::relinearize()
{
  cout<<" ------ relinearizing ----------"<<endl;
  Sphere<double> S(D_);
  if(spx_.get() == NULL)
  {
    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_,N_));
    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
  }
  Matrix<double,Dynamic,Dynamic> p0s(D_,2*K_);
  Matrix<double,Dynamic,Dynamic> ps(D_,2*K_);

  for(uint32_t k=0; k<K_; ++k)
  {
    p0s.col(2*k)   = thetas_[k]->getL()->getMean();
    p0s.col(2*k+1) = thetas_[k]->getR()->getMean();
  }
#ifndef NDEBUG
  cout<< p0s<<endl;
#endif
  ps = karcherMeanMultiple(p0s, *spq_, *spxMu_, z_, 2*K_, 100);
#ifndef NDEBUG
  for(uint32_t i=0; i<N_; ++i)
  {
    assert((ps.col(z_(i)).transpose() * spxMu_->col(i)).sum()<1e6);
  }
  cout<< ps<<endl;
#endif
  S.rotate_p2north(ps, *spxMu_, *spx_, z_, 2*K_);
  for(uint32_t k=0; k<K_; ++k)
  {
    thetas_[k]->getL()->setMean(ps.col(2*k));
    thetas_[k]->getR()->setMean(ps.col(2*k+1));
  }
}

//template <>
//void DpSubclusterMM<NiwTangentd, double>::relinearize()
//{
//  cout<<" ------ linearizing ----------"<<endl;
//  Sphere<double> S(D_);
//  if(spx_.get() == NULL)
//  {
//    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
//        new Matrix<double,Dynamic,Dynamic>(D_,N_));
//    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
//        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
//  }
////  Matrix<double,Dynamic,Dynamic> p0s(D_,2*K_);
//  Matrix<double,Dynamic,Dynamic> ps(D_,2*K_);
//
//  for(uint32_t k=0; k<K_; ++k)
//  {
//    ps.col(2*k)   = thetas_[k]->getL()->getMean();
//    ps.col(2*k+1) = thetas_[k]->getR()->getMean();
//  }
//#ifndef NDEBUG
//  cout<< ps<<endl;
//#endif
////  ps = karcherMeanMultiple(p0s, *spq_, *spxMu_, z_, 2*K_, 100);
////#ifndef NDEBUG
////  for(uint32_t i=0; i<N_; ++i)
////  {
////    assert((ps.col(z_(i)).transpose() * spxMu_->col(i)).sum()<1e6);
////  }
////  cout<< ps<<endl;
////#endif
//  S.rotate_p2north(ps, *spxMu_, *spx_, z_, 2*K_);
//  // only bring points to that
////  for(uint32_t k=0; k<K_; ++k)
////  {
////    thetas_[k]->getL()->setMean(ps.col(2*k));
////    thetas_[k]->getR()->setMean(ps.col(2*k+1));
////  }
//}

template <>
void DpSubclusterMM<NiwSpheref, float>::relinearize()
{
  cout<<" ------ relinearizing ----------"<<endl;
  Sphere<float> S(D_);
  if(spx_.get() == NULL)
  {
    spxMu_ = shared_ptr<Matrix<float,Dynamic,Dynamic> >(
        new Matrix<float,Dynamic,Dynamic>(D_,N_));
    spx_ = shared_ptr<Matrix<float,Dynamic,Dynamic> >(
        new Matrix<float,Dynamic,Dynamic>(D_-1,N_));
 }

  Matrix<float,Dynamic,Dynamic> p0s(D_,2*K_);
  Matrix<float,Dynamic,Dynamic> ps(D_,2*K_);

  for(uint32_t k=0; k<K_; ++k)
  {
    p0s.col(2*k)   = thetas_[k]->getL()->getMean();
    p0s.col(2*k+1) = thetas_[k]->getR()->getMean();
  }
  ps = karcherMeanMultiple(p0s, *spq_, *spxMu_, z_, 2*K_, 100);
  S.rotate_p2north(ps,*spxMu_, *spx_, z_, 2*K_);
  for(uint32_t k=0; k<K_; ++k)
  {
    thetas_[k]->getL()->setMean(ps.col(2*k));
    thetas_[k]->getR()->setMean(ps.col(2*k+1));
  }
}

template <>
void DpSubclusterMM<NiwSphereFulld, double>::relinearize()
{
  cout<<" ------ relinearizing ----------"<<endl;
  Sphere<double> S(D_);
  if(spx_.get() == NULL)
  {
    spxMu_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_,N_));
    spx_ = shared_ptr<Matrix<double,Dynamic,Dynamic> >(
        new Matrix<double,Dynamic,Dynamic>(D_-1,N_));
  }
  Matrix<double,Dynamic,Dynamic> p0s(D_,2*K_);
  Matrix<double,Dynamic,Dynamic> ps(D_,2*K_);

  for(uint32_t k=0; k<K_; ++k)
  {
    p0s.col(2*k)   = thetas_[k]->getL()->getMean();
    p0s.col(2*k+1) = thetas_[k]->getR()->getMean();
  }
  ps = karcherMeanMultiple(p0s, *spq_, *spxMu_, z_, 2*K_, 100);
#ifndef NDEBUG
//  cout<< p0s<<endl;
  for(uint32_t i=0; i<N_; ++i)
  {
    assert((ps.col(z_(i)).transpose() * spxMu_->col(i)).sum()<1e6);
  }
  cout<< ps<<endl;
#endif
//  S.rotate_p2north(ps, *spxMu_, *spx_, z_, 2*K_);
  for(uint32_t k=0; k<K_; ++k)
  {
    thetas_[k]->getL()->setMeanKarch(ps.col(2*k));
    thetas_[k]->getR()->setMeanKarch(ps.col(2*k+1));
  }
  // linearize around the sampled means!
//  S.Log_ps(p0s,*spq_,  z_, *spxMu_);
//  S.rotate_p2north(p0s, *spxMu_, *spx_, z_, 2*K_);
//  //TODO
  S.Log_ps(ps, *spq_,  z_, *spxMu_);
  S.rotate_p2north(ps, *spxMu_, *spx_, z_, 2*K_);

#ifndef NDEBUG
  cout<<"\x1b[34m linearizing around "<<endl
    <<p0s<<"\x1b[0m"<<endl;
#endif
}

template <class B, typename T>
void DpSubclusterMM<B,T>::sampleParameters_()
{
//  relinearize();

  uint32_t totalCount=0;
//#pragma omp parallel for
  for(uint32_t k=0; k<K_; ++k)
  {
    cout<<"count "<<k<<"/"<<K_<<": "<<thetas_[k]->count()<<endl;
    // compute posteriors and sample parameters
//    thetas_[k]->posterior(*spx_,z_,2*k);
    thetas_[k]->sample();
    // use counts to sample stick breaks
    if(thetas_[k]->count() > 0){
      boost::random::gamma_distribution<> gamma(thetas_[k]->count());
      sticks_(k) = gamma(*pRndGen_);
    }else{
      sticks_(k) = 0.0;
      // I think this is propper:
      boost::random::gamma_distribution<> gamma(alpha_);
      sticks_(k) = gamma(*pRndGen_);
    }
    totalCount += thetas_[k]->count();
  }
  assert(totalCount == N_);

//  for(uint32_t k=0; k<K_; ++k)
//    // to decide internally about splitability
//    thetas_[k]->updateAvgLogLikeData();

  boost::random::gamma_distribution<> gammaRest(alpha_);
  T total = sticks_.sum() + gammaRest(*pRndGen_);
  sticks_ = (sticks_.array().log() - log(total)).matrix();

  assert(fabs(logSumExp(sticks_)) <1e-6);

//  dump();
};

template <class B, typename T>
void DpSubclusterMM<B,T>::resampleFromBase(uint32_t Kmax)
{
  for(uint32_t k=K_; k<Kmax; ++k)
  {
    thetas_.push_back(shared_ptr<LrCluster<B,T> >(theta0_->copy()));
//    thetas_[k]->posterior(*spx_,z_,2*k);
  }
  K_ = Kmax;
  sticks_.conservativeResize(K_);
  logLikeZcls_.conservativeResize(K_,K_);
};

template <class B, typename T>
void DpSubclusterMM<B,T>::sampleLabels()
{
#ifndef NDEBUG
  cout<<"sticks = "<<sticks_.array().exp().matrix().transpose()
    <<" || "<<sticks_.array().exp().matrix().sum()<<endl;
#endif
  assert(fabs(logSumExp(sticks_)) <1e-6);

  // sample association to top upper clusters
  // TODO: will have to take care of left right clusters ie left is even
  // and right is odd cluster
  pdfsUpper_.resize(N_,K_);
#pragma omp parallel for
  for(uint32_t i=0; i<z_.size(); ++i)
  {
//    VectorXd logPdf_z = sticks_;
    pdfsUpper_.row(i) = sticks_;
    for(uint32_t k=0; k<K_; ++k)
      if(thetas_[k]->count() > 0)
      {
        // ! need to use q here and to relinearize since we are going through all
        // clusters and we only have linearization for a single one !
        pdfsUpper_(i,k) += thetas_[k]->logLikelihood(spq_->col(i));
//        if(pdfsUpper_(i,k) != pdfsUpper_(i,k)) // check for nan
//        {
//          cout<<pdfsUpper_(i,k)<<endl;
//          cout<<"sticks "<<sticks_.transpose()<<endl;
////          cout<<thetas_[k]->logLikelihood(spx_->col(i))<<endl;
//          thetas_[k]->getUpper()->print();
//          thetas_[k]->getL()->print();
//          thetas_[k]->getR()->print();
//          cout<<"---------------"<<endl;
//          thetas_[k].reset();
//        }
        assert(pdfsUpper_(i,k) == pdfsUpper_(i,k));
      }
    // make pdf sum to 1. and exponentiate
//    pdfsUpper_.row(i) =(logPdf_z.array()-logSumExp<T>(logPdf_z)).exp().matrix().transpose();
  }
  // sample all z_i
  sampler_->sampleDiscLogPdfUnNormalized(pdfsUpper_,z_);

//  MatrixXd A(N_,K_+1);
//  A<< pdfsUpper_, z_.cast<T>();
//  cout<<A<<endl;

  // aggregate the probability of sampling a merge between any two clusters
  logLikeZcls_.setZero(K_,K_);
  VectorXi toDelete(K_); toDelete.setOnes(K_);
#pragma omp parallel for
  for(uint32_t i=0; i<z_.size(); ++i)
  {
    Matrix<T,1,Dynamic> logLikeZcls_i(K_);
    for(uint32_t k=0; k<K_; ++k)
      if(k!=z_(i))
        logLikeZcls_i(k) = logsumexp(pdfsUpper_(i,z_(i)),pdfsUpper_(i,k));
      else
        logLikeZcls_i(k) = pdfsUpper_(i,z_(i)); // on the diagonal
//    cout<<"------"<<endl;
//    cout<<pdfsUpper_.row(i)<<endl;
//    cout<<logLikeZcls_i<<endl;
    // to sample association to subclusters
    pdfsLR_.row(i) = thetas_[z_(i)]->getSubclusterLogPdf(spq_->col(i));
#pragma omp critical
    {
      logLikeZcls_.row(z_(i)) += logLikeZcls_i;
      // compute counts to determine empty clusters
      --toDelete(z_(i));
    }
  }

  // sample all z_i
  sampler_->sampleDiscLogPdfUnNormalized(pdfsLR_,lr_);

//  cout<<" ----- 0 ------ "<<endl;
//  thetas_[0]->getL()->print();
//  cout<<" ----- 1 ------ "<<endl;
//  thetas_[0]->getR()->print();
//
//  MatrixXd C(N_,3);
//  C<< pdfsLR_, lr_.cast<T>();
//  cout<<C<<endl;

//TODO do this in GPU -> should be simple
  VectorXd logLikeZ(K_); // agregate probability of this subcluster sample
  logLikeZ.setZero();
  VectorXd logLikeData(K_); logLikeData.setZero();
//  VectorXd Nk(K_);  Nk.setZero();
//#pragma omp parallel for
  for(uint32_t i=0; i<z_.size(); ++i)
  {
//#pragma omp critical
    {
      logLikeZ(z_(i)) += pdfsLR_(i,lr_(i)) - logsumexp(pdfsLR_(i,0),pdfsLR_(i,1));
//      cout<<exp(pdfsLR_(i,lr_(i)) - logsumexp(pdfsLR_(i,0),pdfsLR_(i,1)))<<" "
//        <<exp(pdfsLR_(i,(lr_(i)+1)%2) - logsumexp(pdfsLR_(i,0),pdfsLR_(i,1)))
//        <<endl;
      logLikeData(z_(i)) += pdfsLR_(i,lr_(i));
//      ++ Nk(z_(i));
    }
  }
  for(uint32_t k=0; k<K_; ++k)
    thetas_[k]->logLikelihoodOfSubclusterSplit() = logLikeZ(k);
//  // to decide internally about splitability
//  for(uint32_t k=0; k<K_; ++k)
//    thetas_[k]->updateAvgLogLikeData(logLikeData(k)/Nk(k));
  // compute labelMap to remove empty clusters
#ifndef NDEBUG
  cout<<"toDelete: "<<toDelete.transpose()<<endl;
#endif
  VectorXi labelMap(2*K_);
  for(uint32_t k=0; k<2*K_; ++k)
    labelMap(k) = k;
  int32_t nDelete = toDelete(0) == 1?1:0;
  for(uint32_t k=1; k<K_; ++k)
  {
    labelMap(2*k) = 2*(k-nDelete);
    labelMap(2*k+1) = 2*(k-nDelete) +1;
    if(toDelete(k) == 1)
    {
      labelMap(2*k) -= 2;
      labelMap(2*k+1) -= 2;
      nDelete ++;
    }
  }

  // delete clusters
  for(int32_t k=K_-1; k>=0; --k)
    if(toDelete(k) == 1)
      thetas_.erase(thetas_.begin() + k);
  // delete respective cols and rows from logLikeZcls_
  Matrix<T,Dynamic,Dynamic> logLikeZclsAfter(K_-nDelete, K_-nDelete);
  logLikeZclsAfter.setZero();
#ifndef NDEBUG
//  Matrix<T,Dynamic,Dynamic> logLikeZcls(K_, K_);
//  for(int32_t k=0; k<K_; ++k)
//    for(int32_t j=0; j<K_; ++j)
//    {
//      logLikeZcls(k,j)= j+K_*k;
//    }
  cout<<"before"<<endl<<logLikeZcls_<<endl;
#endif
  int32_t lk=0;
  int32_t lj=0;
  for(int32_t k=0; k<static_cast<int32_t>(K_); ++k)
    if(toDelete(k) <1)
    {
      lj = 0;
      for(int32_t j=0; j<static_cast<int32_t>(K_); ++j)
        if(toDelete(j) <1)
          logLikeZclsAfter(j-lj,k-lk) = logLikeZcls_(j,k);
        else
          lj ++;
//      cout<<"@"<<k<<endl<<logLikeZclsAfter<<endl;
    }else
      lk ++;
  logLikeZcls_ = logLikeZclsAfter;
#ifndef NDEBUG
  cout<<"after"<<endl<<logLikeZcls_<<endl;
  cout<<" deleted : "<<nDelete<<" -> Knew = "<<K_ - nDelete<< " from "<<K_<<endl;
  cout<<"labelMap: "<<labelMap.transpose()<<endl;
  cout<<"counts before:   "<<counts<T,uint32_t>(z_,K_).transpose()<<endl;
  cout<<"sticks before: "<<sticks_.transpose()<<endl;
#endif
  Matrix<T,Dynamic,1> sticksNew(K_ - nDelete);
  for(uint32_t k=0; k<K_; ++k)
    sticksNew(labelMap(2*k)/2) = sticks_(k);
  K_ = K_ - nDelete;
  sticks_.resize(K_);
  sticks_ = sticksNew;
  // subtract of LR assignments since we resampled them
  for(uint32_t k=0; k<labelMap.size(); ++k)
    labelMap(k) -= labelMap(k)%2;
#ifndef NDEBUG
  cout<<"sticks after:  "<<sticks_.transpose()<<endl;
  cout<<"Knew = "<<K_<<" #thetas = "<<thetas_.size()
    <<" sticks = "<<sticks_.transpose()<<endl;
//  assert(checkLabelMap(labelMap,2*K_));
  cout<<"labelMap after: "<<labelMap.transpose()<<endl;
//  cout<<"z:  "<<z_.transpose()<<endl;
//  cout<<"lr: "<<lr_.transpose()<<endl;
#endif

#pragma omp parallel for
  for(uint32_t i=0; i<z_.size(); ++i)
    z_(i) = labelMap(2*z_(i)) + lr_(i);
//  cout<<"z:  "<<z_.transpose()<<endl;
#ifndef NDEBUG
  cout<<"counts after:    "<<counts<T,uint32_t>(z_,2*K_).transpose()<<endl;
#endif

  // relinearize and compute posteriors
  relinearize();

//#pragma omp parallel for
//  for(uint32_t i=0; i<z_.size(); ++i)
//  {
//
//  }

//#pragma omp parallel for
  for(uint32_t k=0; k<K_; ++k)
  {
    // compute posteriors and sample parameters
    thetas_[k]->posterior(*spx_,z_,2*k);
    // to decide internally about splitability
    thetas_[k]->updateAvgLogLikeData();
  }
};

template <class B, typename T>
void DpSubclusterMM<B,T>::proposeMerges()
{
  uint32_t nMerges = 0;
//  vector<int32_t> mergeWidth(K_,-1);
  VectorXi toDelete(K_); toDelete.setZero();
  VectorXi labelMap(2*K_);
  for(uint32_t jj=0; jj<2*K_; ++jj)
    labelMap(jj) = jj;
#ifndef NDEBUG
  cout<<"labelMap before: "<<labelMap.transpose()<<endl;
#endif
  for(uint32_t k=0; k<K_; ++k) if(thetas_[k]->splittable()) cout<<" cluster "<<k<<" splittable"<<endl; else cout<<" cluster "<<k<<" NOT splittable"<<endl;
  for(uint32_t k=0; k<K_; ++k) if(thetas_[k]->splittable())
    for(uint32_t j=k+1; j<K_; ++j) if(thetas_[j]->splittable())
      if(labelMap(2*k) != labelMap(2*k+1) && labelMap(2*j) != labelMap(2*j+1))
      {
//        cout<<k<<" "<<j<<endl;
        uint32_t Nk = thetas_[k]->count();
        uint32_t Nj = thetas_[j]->count();

        // construct the merge here and sample it then decide to accept or
        // reject it
// TODO we sample inside the merge method since it may be a specific proposal
        shared_ptr<LrCluster<B,T> > merged(thetas_[k]->merge(thetas_[j]));
//        merged->sample();
        // Hastings Ratio
        //TODO deterministic merge
        T logJointAfter =
            merged->dataLogLikelihoodMarginalized()
          + boost::math::lgamma(alpha_) + boost::math::lgamma(Nk+Nj)
          + boost::math::lgamma(0.5*alpha_+Nk)
          + boost::math::lgamma(0.5*alpha_+Nj);
        T logJointBefore =
            thetas_[k]->dataLogLikelihoodMarginalized()
          + thetas_[j]->dataLogLikelihoodMarginalized()
          + log(alpha_) + 2*boost::math::lgamma(0.5*alpha_)
          + boost::math::lgamma(alpha_+Nk+Nj)
          + boost::math::lgamma(Nk)
          + boost::math::lgamma(Nj);
        T qBefore = thetas_[k]->qRandomParamProposal()
          + thetas_[j]->qRandomParamProposal();
        T qAfter = merged->qRandomParamProposal();
// ------------- old
//        double HR =  boost::math::lgamma(alpha_) + boost::math::lgamma(Nk+Nj);
//        HR += -log(alpha_)-2*boost::math::lgamma(0.5*alpha_)
//          -boost::math::lgamma(alpha_+Nk+Nj);
//        HR += merged->dataLogLikelihoodMarginalized();
////        HR += thetas_[k]->dataLogLikelihoodMarginalizedMerged(thetas_[j]);
//        HR -= thetas_[k]->dataLogLikelihoodMarginalized();
//        HR -= thetas_[j]->dataLogLikelihoodMarginalized();
// -----------------
        //TODO inverse of local subcluster split
//       T logJointAfter = merged->dataLogLikelihoodMarginalized()
//          + boost::math::lgamma(Nk+Nj);
//       T logJointBefore = thetas_[k]->dataLogLikelihoodMarginalized()
//         + thetas_[j]->dataLogLikelihoodMarginalized()
//         + log(alpha_) + boost::math::lgamma(Nk)
//         + boost::math::lgamma(Nj);
//       T qBefore = logLikeZcls_(k,k) + logLikeZcls_(j,j)
//         + thetas_[k]->qRandomParamProposal()
//         + thetas_[j]->qRandomParamProposal();
//       T qAfter =  logLikeZcls_(j,k) + logLikeZcls_(k,j)
//         + merged->qRandomParamProposal();
// --------------------- old
//        double HR = -log(alpha_) - boost::math::lgamma(Nk);
//        HR += -boost::math::lgamma(Nj) + boost::math::lgamma(Nk+Nj);
//        HR += merged->dataLogLikelihoodMarginalized();
////        HR += thetas_[k]->dataLogLikelihoodMarginalizedMerged(thetas_[j]);
//        HR -= thetas_[k]->dataLogLikelihoodMarginalized();
//        HR -= thetas_[j]->dataLogLikelihoodMarginalized();
//        HR += logLikeZcls_(k,k) + logLikeZcls_(j,j);
//        HR -= logLikeZcls_(j,k) + logLikeZcls_(k,j);
// ------------------------

      T HR = logJointAfter - logJointBefore + qBefore - qAfter;
#ifndef NDEBUG
    cout<<"\x1b[32m p after  = "<< logJointAfter <<"\x1b[0m"<<endl;
    cout<<"\x1b[32m p before = "<< logJointBefore <<"\x1b[0m"<<endl;
    cout<<"\x1b[32m q after  = "<< qAfter <<"\x1b[0m"<<endl;
    cout<<"\x1b[32m q before = "<< qBefore <<"\x1b[0m"<<endl;
//      << "( "
//      << logLikeZcls_(k,k)<<" + "<<logLikeZcls_(j,j) << " - "
//      << logLikeZcls_(j,k)<<" - "<<logLikeZcls_(k,j)<<") "
//      <<"\x1b[0m"<<endl;

      cout<<"\x1b[32m log(alpha) = "<<log(alpha_)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(Nk)) = "<<boost::math::lgamma(Nk)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(Nj)) = "<<boost::math::lgamma(Nj)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(N)) = "<<boost::math::lgamma(Nk+Nj)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike k:  "
        <<thetas_[k]->logPdfUnderPriorMarginalized()<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike j: "
        <<thetas_[j]->logPdfUnderPriorMarginalized()<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike joint: "
        <<merged->dataLogLikelihoodMarginalized()<<"\x1b[0m"<<endl;
//        <<thetas_[k]->dataLogLikelihoodMarginalizedMerged(thetas_[j])<<"\x1b[0m"<<endl;
#endif

//        cout<<k<<" "<<j<<endl;
        boost::uniform_01<> unif_;
        if( HR > 0 || unif_(*pRndGen_) < exp(HR))
        { // accept merge
          labelMap(2*k) = 2*(k) + (labelMap(2*k)-2*k); // becomes left cluster
          labelMap(2*k+1) = 2*(k) + (labelMap(2*k)-2*k); // becomes left cluster
          labelMap(2*j) = 2*(k)+1 + (labelMap(2*k)-2*k); // becomes right cluster
          labelMap(2*j+1) = 2*(k)+1 + (labelMap(2*k)-2*k); // becomes right cluster
          toDelete(j) = 1;
          nMerges ++;
          for(uint32_t jj=j+1; jj<K_; ++jj)
            if((labelMap(2*jj) != labelMap(2*jj+1))
              || ((labelMap(2*jj) == labelMap(2*jj+1))
                  &&(labelMap(2*jj)/2 > static_cast<int32_t>(k))))
            {
              labelMap(2*jj) -= 2;
              labelMap(2*jj+1) -= 2;
            }

          // TODO may need to relinearize the merged one?
//          shared_ptr<LrCluster<B,T> > merged(thetas_[k]->merge(thetas_[j]));
          thetas_[k] = merged; // replace k with the merged one
          thetas_[k]->stickL() = sticks_(k);
          thetas_[k]->stickR() = sticks_(j);
          thetas_[k]->resetSplittable();
          sticks_(k) = logsumexp(sticks_(k),sticks_(j));
          sticks_(j) = sticks_(k); // so that it gets updated correctly below
          //thetas_[k]->sampleUpper(); // already done when merging
          cout<<"\x1b[31m merging "<<k<<" and "<<j<<" HR="<<HR<<"\x1b[0m"<<endl;
#ifndef NDEBUG
          cout<<" k:"<<k<<endl;
          thetas_[k]->getUpper()->print();
          cout<<" j:"<<j<<endl;
          thetas_[j]->getUpper()->print();
          cout<<" new left"<<endl;
          merged->getL()->print();
          cout<<" new right"<<endl;
          merged->getR()->print();
#endif
          break;
        }else{
          cout<<"NOT merging "<<k<<" and "<<j<<" HR="<<HR<<endl;
        }
      }
  if(nMerges > 0)
  {
    uint32_t Knew = K_ - nMerges;
#ifndef NDEBUG
    cout<<"labelMap before: "<<labelMap.transpose()<<endl;
    cout<<"K_before = "<<K_<<endl;
    cout<<"K_new = "<<Knew<<endl;
    assert(checkLabelMap(labelMap,2*Knew));
#endif
    // remove thetas, that are no needed anymore
    for(int32_t k=K_-1; k>=0; --k)
      if(toDelete(k)==1)
      { // second condition ensures that only the "j" cluster gets erased
#ifndef NDEBUG
        cout<<"Erasing "<< k<<endl;
#endif
        thetas_.erase(thetas_.begin() + k);
      }

    Matrix<T,Dynamic,1> sticksNew(Knew);
    for(uint32_t k=0; k<K_; ++k)
      sticksNew(labelMap(2*k)/2) = sticks_(k);
    sticks_.resize(Knew);
    //TODO: resize sticks and renormalize
    sticks_ = sticksNew;
    // map the labels to the new labels
#pragma omp parallel for
    for(uint32_t i=0; i<z_.size(); ++i)
      z_(i) = labelMap(z_(i)); //+ z_(i)%2;
    K_ = Knew;
  }

#ifndef NDEBUG
  cout<<"after merges sticks = "<<sticks_.array().exp().matrix().transpose()
    <<" ||="<<sticks_.array().exp().matrix().sum()<<endl;
#endif
};

template <class B, typename T>
void DpSubclusterMM<B,T>::proposeRandomSplits()
{
  // sample new LR labels for all datapoints completely at random
  Matrix<T,Dynamic,1> alpha(2); alpha.setOnes(2);
  alpha *= alpha_/2.;
  Dir<Cat<T>, T> dirLr(alpha,pRndGen_);
  Cat<T> piLr = dirLr.sample();
  VectorXu lr(N_);
  piLr.sample(lr);
  //
  for(uint32_t k=0; k<K_; ++k)
  {
    LrCluster<B,T>* thetaNew(theta0_->copy());
    // TODO!
  }
};

template <class B, typename T>
void DpSubclusterMM<B,T>::proposeSplits()
{

  for(uint32_t k=0; k<K_; ++k) if(thetas_[k]->splittable()) cout<<" cluster "<<k<<" splittable"<<endl; else cout<<" cluster "<<k<<" NOT splittable"<<endl;

  VectorXi doReset(K_); doReset.setOnes(K_); doReset *= -1;
  VectorXi doSplit(K_); doSplit.setOnes(K_); doSplit *= -1;
  for(uint32_t k=0; k<K_; ++k)
    if(thetas_[k]->splittable())
    {
      cout<<" testing split for "<<k<<endl;

      uint32_t Nk = thetas_[k]->count();
      uint32_t Nk_l = thetas_[k]->countL();
      uint32_t Nk_r = thetas_[k]->countR();
      assert(Nk == Nk_l+Nk_r);
      if(Nk_l == 0 || Nk_r == 0)
      {
        doReset(k) = 1;
        continue;
      }
//      double HR = log(alpha_) + boost::math::lgamma(Nk_l);
//      HR += boost::math::lgamma(Nk_r) - boost::math::lgamma(Nk);
//      HR += thetas_[k]->getL()->logPdfUnderPriorMarginalized();
//      HR += thetas_[k]->getR()->logPdfUnderPriorMarginalized();
//      HR -= thetas_[k]->getUpper()->logPdfUnderPriorMarginalized();
//      //TODO
////      HR -= thetas_[k]->logLikelihoodOfSubclusterSplit();

      T logJointAfter = log(alpha_) + boost::math::lgamma(Nk_l)
        + boost::math::lgamma(Nk_r)
        + thetas_[k]->getL()->logPdfUnderPriorMarginalized()
        + thetas_[k]->getR()->logPdfUnderPriorMarginalized();
      T logJointBefore = boost::math::lgamma(Nk)
        + thetas_[k]->getUpper()->logPdfUnderPriorMarginalized();
      T qBefore = thetas_[k]->qRandomParamProposal();  //TODO
      T qAfter = 0.0;
// local split/merges
//      T qBefore = 0.0;
//      T qAfter = thetas_[k]->logLikelihoodOfSubclusterSplit();

      T HR = logJointAfter - logJointBefore + qBefore - qAfter;
#ifndef NDEBUG
      cout<<"\x1b[32m p after  = "<< logJointAfter <<"\x1b[0m"<<endl;
      cout<<"\x1b[32m p before = "<< logJointBefore <<"\x1b[0m"<<endl;
      cout<<"\x1b[32m q before = "<< qBefore <<"\x1b[0m"<<endl;
      cout<<"\x1b[32m q after  = "<< qAfter <<"\x1b[0m"<<endl;

      cout<<"\x1b[32m log(alpha) = "<<log(alpha_)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(Nkl)) = "<<boost::math::lgamma(Nk_l)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(Nkr)) = "<<boost::math::lgamma(Nk_r)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m log(Gamma(Nk)) = "<<boost::math::lgamma(Nk)<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike Left:  "
        <<thetas_[k]->getL()->logPdfUnderPriorMarginalized()<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike Right: "
        <<thetas_[k]->getR()->logPdfUnderPriorMarginalized()<<"\x1b[0m"<<endl;
      cout<<"\x1b[32m logLike Upper: "
        <<thetas_[k]->getUpper()->logPdfUnderPriorMarginalized()<<"\x1b[0m"<<endl;
#endif

      boost::uniform_01<> unif_;
      if( HR > 0 || unif_(*pRndGen_) < exp(HR))
      { // accept split
        doSplit(k) = 1;
        cout<<"\x1b[31m splitting "<<k<<" HR = "<<HR<<"\x1b[0m"<<endl;
#ifndef NDEBUG
        thetas_[k]->getUpper()->print();
        thetas_[k]->getL()->print();
        thetas_[k]->getR()->print();
#endif
      }else{
        cout<<"NOT splitting "<<k<<" HR = "<<HR<<endl;
#ifndef NDEBUG
        thetas_[k]->getUpper()->print();
        thetas_[k]->getL()->print();
        thetas_[k]->getR()->print();
#endif
      }
    }
  if(doReset.sum() + K_ > 0)
  {
    // update labels so we can compute posteriors
#pragma omp parallel for
    for(uint32_t i=0; i<z_.size(); ++i)
      if(doReset(z_(i)/2) >= 0)
      {
        boost::uniform_01<> unif;
        //TODO: should i assign the sublcusters randomly??
        z_(i) += -(z_(i)%2) + (unif(*pRndGen_)<0.5?0:1); //+ z_(i)%2;
        //cout<<(-(z_(i)%2))<<" "<< z_(i)<<endl;
      }
    //TODO: idealy only relinearize the clusters that need to be relinearized
//    relinearize();
    // update posteriors
    for(uint32_t k=0; k<K_; ++k)
      if(doReset(k) > 0)
      {
        cout<<"\x1b[31m resetting "<<k<<"\x1b[0m"<<endl;
        // TODO: posterior also samples the stick lengths whereas posteriorLR and posterior Upper do not do that - what is right?
        // TODO: I dont think I need to compute posteriors here, since this is
        // next anyways after splits and merges
//        thetas_[k]->posterior(*spx_,z_,2*k);
//        thetas_[k]->posteriorLR(*spx_,z_,2*k);
//        thetas_[k]->posteriorUpper();
        relinearize(k);
        // compute posteriors and sample parameters
        thetas_[k]->posterior(*spx_,z_,2*k);
        thetas_[k]->resetSplittable();// dont consider cluster until splittable
        // to decide internally about splitability
//        thetas_[k]->updateAvgLogLikeData();

#ifndef NDEBUG
        cout<<"reseting "<<k<<endl;
        thetas_[k]->getUpper()->print();
        thetas_[k]->getL()->print();
        thetas_[k]->getR()->print();
#endif
      }
  }
  if(doSplit.sum() + K_ > 0)
  {
    uint32_t Knew = K_ + int32_t(doSplit.sum() + K_)/2;
#ifndef NDEBUG
    cout<<"doSplit: "<<doSplit.transpose()<<endl;
    cout<<"Knew:    "<<Knew<<endl;
    cout<<"before splits sticks = "<<sticks_.array().exp().matrix().transpose()
      <<" ||="<<sticks_.array().exp().matrix().sum()<<endl;
#endif
    Matrix<T,Dynamic,1> sticksNew(Knew);
//    cout<<sticksNew.transpose()<<endl;
//    cout<<K_<<endl;
    sticksNew.topRows(K_) = sticks_;
    sticks_.resize(Knew);
    sticks_ = sticksNew; //TODO make sure resizing leaves the old contents intact
    uint32_t nSplit = 0;
    VectorXi labelMap(2*Knew);
    for(uint32_t k=0; k<2*Knew; ++k)
      labelMap(k)=k;
    for(uint32_t k=0; k<K_; ++k)
      if(doSplit(k) > 0)
      {
        uint32_t j = K_ + nSplit++;
        labelMap(2*k) = 2*k; // left cluster
        labelMap(2*k+1) = 2*j; // right cluster

        labelMap(2*j) = 2*k+1; // left cluster
        labelMap(2*j+1) = 2*j+1; // left cluster

        assert(fabs(logsumexp(thetas_[k]->stickR(),thetas_[k]->stickL())) < 1e-6);

        sticks_(j) = sticks_(k) + thetas_[k]->stickR();
        sticks_(k) = sticks_(k) + thetas_[k]->stickL();

        // create new clusters from scratch with 50% 50% sticks
        thetas_[k].reset(
          new LrCluster<B,T>(thetas_[k]->getL(),thetas_[k]->alpha(),pRndGen_));
        thetas_.push_back(shared_ptr<LrCluster<B,T> >(
          new LrCluster<B,T>(thetas_[k]->getR(),thetas_[k]->alpha(),pRndGen_)));

//#ifndef NDEBUG
//        cout<<"split up "<<k<<endl;
//        thetas_[k]->getUpper()->print();
//        thetas_[k]->getL()->print();
//        thetas_[k]->getR()->print();
//        thetas_[j]->getUpper()->print();
//        thetas_[j]->getL()->print();
//        thetas_[j]->getR()->print();
//#endif
      }
#ifndef NDEBUG
    cout<<"after splits sticks = "<<sticks_.array().exp().matrix().transpose()
      <<" ||="<<sticks_.array().exp().matrix().sum()<<endl;

    assert(checkLabelMap(labelMap,2*Knew));
#endif
    // update labels so we can compute posteriors
#pragma omp parallel for
    for(uint32_t i=0; i<z_.size(); ++i)
//      if(labelMap(z_(i)) >= 0)
      if(doSplit(z_(i)/2) > 0)
      {
        boost::uniform_01<> unif;
        //TODO: should i assign the sublcusters randomly??
        z_(i) = labelMap(z_(i)) + (unif(*pRndGen_)<0.5?0:1); //+ z_(i)%2;
      }
    K_ = Knew;
#ifndef NDEBUG
    for(uint32_t k=0; k<K_; ++k)
    {
      cout<<"Checking cluster pointer "<<k<<endl;
      assert(thetas_[k].get());
    }
#endif
    //TODO: idealy only relinearize the clusters that need to be relinearized
//    relinearize();
    // update posteriors
    nSplit = 0;
    for(uint32_t k=0; k<doSplit.size(); ++k)
      if(doSplit(k) > 0)
      {
        uint32_t j = doSplit.size() + nSplit++;
        // relinearize k and j
        cout<<"relinearizing k and j after split"<<endl;
        relinearize(k);
        relinearize(j);
        cout<<"posterior of k and j after split"<<endl;
        // compute posteriors and sample parameters
        thetas_[k]->posterior(*spx_,z_,2*k);
        thetas_[j]->posterior(*spx_,z_,2*j);
        // to decide internally about splitability
//        thetas_[k]->updateAvgLogLikeData();
      }
    // TODO: I dont think I need to compute posteriors here - will done
    // jointly by sampleParameters after correctly relinearizing
//    nSplit = 0;
//    for(uint32_t k=0; k<K_; ++k)
//      if(doSplit(k) > 0)
//      {
//        uint32_t j = K_+nSplit++;
//        thetas_[k]->posterior(*spx_,z_,2*k);
//        thetas_[j]->posterior(*spx_,z_,2*j);
//
////        thetas_[k]->posteriorLR(*spx_,z_,2*k);
////        thetas_[k]->posteriorUpper();
////        thetas_[j]->posteriorLR(*spx_,z_,2*j);
////        thetas_[j]->posteriorUpper();
//
//#ifndef NDEBUG
//        cout<<"split up posterior "<<k<<endl;
//        thetas_[k]->getUpper()->print();
//        thetas_[k]->getL()->print();
//        thetas_[k]->getR()->print();
//        thetas_[j]->getUpper()->print();
//        thetas_[j]->getL()->print();
//        thetas_[j]->getR()->print();
//#endif
//      }
  }
//  if((doReset.sum() + K_ > 0)||(doSplit.sum() + K_ > 0))
//  {
//    //TODO: idealy only relinearize the clusters that need to be relinearized
//    relinearize();
//
//    for(uint32_t k=0; k<K_; ++k)
//      if(doReset(k) > 0 || doSplit(k) > 0)
//      {
//        // compute posteriors and sample parameters
//        thetas_[k]->posterior(*spx_,z_,2*k);
//        // to decide internally about splitability
//        thetas_[k]->updateAvgLogLikeData();
//      }
//  }
};

template <class B, typename T>
double DpSubclusterMM<B,T>::logJoint()
{
  // p(\bz; \alpha) p(\bx; \Delta, \nu)
  double logJoint = K_*log(alpha_) + boost::math::lgamma(alpha_)
    - boost::math::lgamma(alpha_+ z_.size());
  for(uint32_t k=0; k<K_; ++k)
    if (thetas_[k]->getUpper()->count() >0)
    {
      logJoint += boost::math::lgamma(thetas_[k]->getUpper()->count());
      logJoint += thetas_[k]->getUpper()->logPdfUnderPriorMarginalized();
    }
  return logJoint;
}

template<class B, typename T>
double DpSubclusterMM<B,T>::evalLogLikelihood(const Matrix<T,Dynamic,1>& x)
{
  Matrix<T,Dynamic,1> logLikes = sticks_;
//  cout<<"--: "<<logLikes.transpose()<<" | "<<logSumExp<T>(logLikes)<<endl;
  for(uint32_t k=0; k< K_; ++k)
  {
    logLikes[k] += thetas_[k]->getUpper()->logLikelihood(x);
//    logLikes[k] += thetas_[k]->logLikelihood(x);
  }
//  cout<<logLikes.transpose()<<endl;
  return logSumExp<T>(logLikes);
};

template<class B, typename T>
MatrixXu DpSubclusterMM<B,T>::mostLikelyInds(uint32_t n,
  Matrix<T,Dynamic,Dynamic>& logLikes)
{
  MatrixXu inds = MatrixXu::Zero(n,K_);
  logLikes = Matrix<T,Dynamic,Dynamic>::Ones(n,K_);
  logLikes *= -99999.0;

#pragma omp parallel for
  for (uint32_t k=0; k<K_; ++k)
    if (thetas_[k]->count() > 0)
  {
    for (uint32_t i=0; i<z_.size(); ++i)
      if(z_(i)/2 == k) // left right clusters are seen as one
      {
        T logLike = thetas_[z_[i]/2]->logLikelihood(spq_->col(i));
        for (uint32_t j=0; j<n; ++j)
          if(logLikes(j,k) < logLike)
          {
            for(uint32_t l=n-1; l>j; --l)
            {
              logLikes(l,k) = logLikes(l-1,k);
              inds(l,k) = inds(l-1,k);
            }
            logLikes(j,k) = logLike;
            inds(j,k) = i;
//            cout<<"after update "<<logLike<<endl;
//            Matrix<T,Dynamic,Dynamic> out(n,K_*2);
//            out<<logLikes.cast<T>(),inds.cast<T>();
//            cout<<out<<endl;
            break;
          }
      }
  }
  cout<<"::mostLikelyInds: logLikes"<<endl;
  cout<<logLikes<<endl;
  cout<<"::mostLikelyInds: inds"<<endl;
  cout<<inds<<endl;
  return inds;
};

template<class B, typename T>
bool DpSubclusterMM<B,T>::checkLabelMap(const VectorXi & labelMap, uint32_t Knew)
{
    cout<<"DpSubclusterMM<B,T>::checkLabelMap: Knew: "<<Knew<<endl;
    cout<<"DpSubclusterMM<B,T>::checkLabelMap: labelMap: "
      <<labelMap.transpose()<<endl;
    VectorXi labelCount(Knew); labelCount.setZero();
    for(uint32_t k=2; k<labelMap.size(); ++k)
      if(labelMap(k) == labelMap(k-1) && labelMap(k-1) == labelMap(k-2))
        return false;
    for(uint32_t k=0; k<labelMap.size(); ++k)
      labelCount(labelMap(k)) ++;
    cout<<"DpSubclusterMM<B,T>::checkLabelMap: labelCount: "
      <<labelCount.transpose()<<endl;
    cout<< !(labelCount.array() == 0).any() <<endl;
    cout<< !(labelCount.array() > 2).any() <<endl;
    uint32_t count2= 0;
    for(uint32_t k=0; k<labelCount.size(); ++k)
      if(labelCount(k) == 2) count2 ++;

    cout<< count2<<" vs "<<(labelMap.size()-Knew)<<endl;
    assert( count2 == (labelMap.size()-Knew));
    return !(labelCount.array() == 0).any() && !(labelCount.array() > 2).any();
};


/*
 * Copyright (c) 2016, Princeton University, Johannes M Dieterich, Emily A Carter
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may
 * be used to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Author: Johannes M Dieterich
 */

#ifndef MULTITERMKEDF_HPP
#define MULTITERMKEDF_HPP

#include <armadillo>
#include <memory>
#include <vector>
#include "KEDF.hpp"
using namespace std;
using namespace arma;

template<class GridType>
class MultiTermKEDF: public KEDF<GridType> {
public:
    MultiTermKEDF(GridType* example, unique_ptr<vector<shared_ptr<KEDF<GridType> > > > kedfs, unique_ptr<vector<double> > prefactors)
             : _kedfs(move(kedfs)), _prefacs(move(prefactors)){
    }

    ~MultiTermKEDF(){
    }

    string getMethodDescription() const override {

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const vector<double>::iterator facB = _prefacs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        const vector<double>::iterator facE = _prefacs->end();

        typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf;
        vector<double>::iterator fac;
        string descr = "";
        for(kedf = kedfB, fac = facB; kedf != kedfE && fac != facE; ++kedf, ++fac){
            const double myFac = *fac;
            const string s = to_string(myFac) + (*kedf)->getMethodDescription() + "\n";
            descr += s;
        }

        return descr;
    }

    vector<string> getCitations() const override {

        vector<string> totCitations(0);

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        for(typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf = kedfB; kedf != kedfE; ++kedf){

            const vector<string> myCitations = (*kedf)->getCitations();
            totCitations.insert(end(totCitations), begin(myCitations), end(myCitations));
        }

        return totCitations;
    }

    vector<string> getWorkingEquations() const override {

        vector<string> totEquations(0);

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        for(typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf = kedfB; kedf != kedfE; ++kedf){

            const vector<string> myEqs = (*kedf)->getWorkingEquations();
            totEquations.insert(end(totEquations), begin(myEqs), end(myEqs));
        }

        return totEquations;
    }

    double calcEnergy(const GridType& grid) const override {

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const vector<double>::iterator facB = _prefacs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        const vector<double>::iterator facE = _prefacs->end();

        typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf;
        vector<double>::iterator fac;
        double energy = 0.0;
        for(kedf = kedfB, fac = facB; kedf != kedfE && fac != facE; ++kedf, ++fac){
            const double myFac = *fac;
            const double myE = (*kedf)->calcEnergy(grid);
            energy += myFac*myE;
        }

        return energy;
    }

    double calcPotential(const GridType& grid, GridType& potential) const override  {

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const vector<double>::iterator facB = _prefacs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        const vector<double>::iterator facE = _prefacs->end();

        unique_ptr<GridType> potCopy(potential.emptyDuplicate());
        cube *potCube = potential.getRealGrid();
        // zero the potential
        potCube->fill(0.0);
        potential.complete(potCube);

        typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf;
        vector<double>::iterator fac;
        double energy = 0.0;
        for(kedf = kedfB, fac = facB; kedf != kedfE && fac != facE; ++kedf, ++fac){
            const double myFac = *fac;
            const double myE = (*kedf)->calcPotential(grid, *potCopy.get());

            potential.fmaGrid(myFac,potCopy.get());
            potential.finalize();
            energy += myFac*myE;
        }

        potCopy.reset();

        return energy;
    }

    unique_ptr<StressTensor> calcStress(const GridType& grid) const override {

        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfB = _kedfs->begin();
        const vector<double>::iterator facB = _prefacs->begin();
        const typename vector<shared_ptr<KEDF<GridType> > >::iterator kedfE = _kedfs->end();
        const vector<double>::iterator facE = _prefacs->end();

        unique_ptr<StressTensor> stress = make_unique<StressTensor>();
        mat* const tensor = stress->getTensor();

        typename vector<shared_ptr<KEDF<GridType> > >::iterator kedf;
        vector<double>::iterator fac;
        for(kedf = kedfB, fac = facB; kedf != kedfE && fac != facE; ++kedf, ++fac){
            const double myFac = *fac;
            const unique_ptr<StressTensor> myStress = (*kedf)->calcStress(grid);
            const mat * const myTensor = myStress->getTensor();

            tensor->at(0,0) = myFac*myTensor->at(0,0);
            tensor->at(1,0) = myFac*myTensor->at(1,0);
            tensor->at(2,0) = myFac*myTensor->at(2,0);
            tensor->at(0,1) = myFac*myTensor->at(0,1);
            tensor->at(1,1) = myFac*myTensor->at(1,1);
            tensor->at(2,1) = myFac*myTensor->at(2,1);
            tensor->at(0,2) = myFac*myTensor->at(0,2);
            tensor->at(1,2) = myFac*myTensor->at(1,2);
            tensor->at(2,2) = myFac*myTensor->at(2,2);
        }

        return stress;
    }

private:
    unique_ptr<vector<shared_ptr<KEDF<GridType> > > > _kedfs;
    unique_ptr<vector<double> > _prefacs;
};

#endif /* MULTITERMKEDF_HPP */


/*
@copyright Louis Dionne 2015
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/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>

#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {
    auto ints = make<Tuple>(
        mpl::int_<-2>{}, mpl::integral_c<int, 0>{}, mpl::integral_c<int, 3>{}
    );

    auto bools = make<Tuple>(
        mpl::true_{}, mpl::false_{},
        mpl::integral_c<bool, true>{}, mpl::integral_c<bool, false>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    {
        auto t = test::ct_eq<3>{};
        auto e = test::ct_eq<4>{};

        // eval_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(mpl::true_{}, always(t), always(e)),
                t
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(mpl::false_{}, always(t), always(e)),
                e
            ));
        }

        // not_
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(mpl::true_{}),
                mpl::false_{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(mpl::false_{}),
                mpl::true_{}
            ));
        }

        // laws
        test::TestLogical<ext::boost::mpl::IntegralC<int>>{ints};
        test::TestLogical<ext::boost::mpl::IntegralC<bool>>{bools};
    }
}


#include <iostream>

using ::std::string;

#include <vector>

using ::std::vector;

#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>

using ::pcl::PointCloud;
using ::pcl::PointXYZ;
using ::pcl::computeCentroid;
using ::pcl::transformPointCloud;
using ::pcl::io::loadPLYFile;
using ::pcl::visualization::CloudViewer;

#include <Eigen/Dense>

using ::Eigen::Matrix4d;

#include "my_point_cloud.hpp"

MyPCL::MyPointCloud::MyPointCloud(string path) {
    int pcl_load_status;

    cloud.reset(new PointCloud<PointXYZ>());
    pcl_load_status = loadPLYFile(path, *cloud);

    if (pcl_load_status == -1) {
        PCL_ERROR("Reading error!\n");
        exit(pcl_load_status);
    }
}

MyPCL::MyPointCloud::~MyPointCloud() {}

void MyPCL::MyPointCloud::view() {
    CloudViewer viewer("My PointCloud viewer");

    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {} // Busy waiting
}

PointXYZ MyPCL::MyPointCloud::centroid() {
    size_t centroid_dimension;
    PointXYZ centroid;

    centroid_dimension = computeCentroid(*cloud, centroid);

    if (centroid_dimension < 3)
        exit(-1);

    return centroid;
}

void MyPCL::MyPointCloud::apply_offset(PointXYZ offset) {
    Matrix4d m_translation;
    m_translation = Matrix4d::Identity();

    // Setting the translation matrix
    /*
     * 1 0 0 x
     * 0 1 0 y
     * 0 0 1 z
     * 0 0 0 1
     */
    m_translation(0, 3) = offset.x;
    m_translation(1, 3) = offset.y;
    m_translation(2, 3) = offset.z;

    PointCloud<PointXYZ>::Ptr translated;
    translated.reset(new PointCloud<PointXYZ>());

    transformPointCloud(*cloud, *translated, m_translation);
    cloud = translated;
}


#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <sstream>
#include <stdint.h>
#include <string>
#include <vector>

#include <boost/units/systems/si.hpp>
#include <boost/units/systems/angle/revolutions.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/io.hpp>

using namespace std;

int main(int argc, char **argv)
{
  boost::units::quantity<boost::units::si::plane_angle> theta = 3.14 * boost::units::si::radians;

  namespace units = boost::units;
  namespace si = boost::units::si;
  units::quantity<si::angular_velocity> nu = theta / (2.0 * si::seconds);

  std::cout << "theta " << theta << std::endl;
  std::cout << "nu " << nu << std::endl;

  units::quantity<units::degree::plane_angle> omega =
    static_cast<units::quantity<units::degree::plane_angle> >(theta);

  std::cout << "omega " << omega << std::endl;
  return 0;
}


#ifndef __BOOSTMATRIX_H__
#define __BOOSTMATRIX_H__

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
// #include <boost/numeric/ublas/io.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <chrono>

using namespace boost::numeric;

class BoostMatrix
{
private:
    unsigned long rsz;
    unsigned long csz;
    ublas::matrix<float> b;

public:
    BoostMatrix(const unsigned long rs, const unsigned long cs);
    BoostMatrix(const pybind11::array_t<float> &arr);
    BoostMatrix(const unsigned long rs, const unsigned long cs, const std::string &fname);

    // copy
    BoostMatrix(const BoostMatrix &mat);            // copy constructor
    BoostMatrix &operator=(const BoostMatrix &mat); //copy assignment

    // move
    BoostMatrix(BoostMatrix &&mat);            // move constructor
    BoostMatrix &operator=(BoostMatrix &&mat); // move assignment

    // // setters
    void set_value(const int i, const int j, const float &newval) { b(i, j) = newval; } // change single value

    // getters
    const unsigned long rows() const { return rsz; }                                                                        // number of rows
    const unsigned long cols() const { return csz; }                                                                        // number of columns
    const std::pair<unsigned long, unsigned long> dim() const { return std::pair<unsigned long, unsigned long>{rsz, csz}; } // dimension
    const float get_value(const int i, const int j) const { return b(i, j); }                                               // value at (i,j) coordinate

    // operations
    bool dim_equal(const BoostMatrix &mat); // check if dimensions are equal
    // float inner_prod(const BoostMatrix & bmat) { return ublas::inner_prod(b, bmat.b); }
    float norm() { return norm_frobenius(b); }                          //Frobenius norm
    // BoostMatrix matmul(const BoostMatrix &mat);                         //matrix multiplication
    std::pair<BoostMatrix, float> BoostMatrix::matmul(const BoostMatrix &mat, const bool &return_time);
    // const std::pair<float, float> norm_performance(const int max_iter); // evaluate performance through c++
    // utility functions
    void print_mat();                 // print BoostMatrix
    void print_row(const int rownum); // auxiliary function for print_mat() to print each row
    // BoostMatrix import_data(const int&, const int&, const std::string&);  // import data (knowing data dimensions)
    // destructor
    ~BoostMatrix() { b.clear(); }
};

#endif //__BOOSTMATRIX_H__


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi_grammar.hpp>

namespace svgpp
{

namespace qi = boost::spirit::qi;

template <class Iterator, class Number = unsigned int>
class urange_grammar:
  public qi::grammar<Iterator, std::pair<Number, Number>(), qi::locals<Number> >
{
public:
  urange_grammar()
    : urange_grammar::grammar(rule_)
  {
    using qi::_a;
    using qi::_1;
    using qi::_val;
    using qi::lit;
    using qi::hex;
    using qi::raw;
    namespace phx = boost::phoenix;

    rule_
        =   lit("U+") >> hex[_a = _1]
            >> ( raw[ + lit('?') ]
                    [_val = phx::bind(&urange_grammar::make_mask_range, _a, _1)]
               | ( lit('-')
                   >> hex [_val = phx::bind(&urange_grammar::make_range, _a, _1)]
                 )
               | qi::eps [_val = phx::bind(&urange_grammar::make_range, _a, _a)]
               );
  }

private:
  typename urange_grammar::start_type rule_;

  static std::pair<Number, Number> make_range(Number start, Number end)
  {
    return std::pair<Number, Number>(start, end);
  }

  static std::pair<Number, Number> make_mask_range(Number base, boost::iterator_range<Iterator> const & mask)
  {
    Number mask_mul = 1 << (boost::size(mask) * 4);

    return std::pair<Number, Number>(base * mask_mul, (base + 1) * mask_mul - 1);
  }
};

}


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <graphblas/graphblas.hpp>
#include <algorithms/k_truss.hpp>

using namespace grb;
using namespace algorithms;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE k_truss_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss_test_basic)
{
    using T = int32_t;
    IndexType num_nodes = 5;
    IndexType num_edges = 6;
    IndexArrayType edge_num = {0, 1, 2, 3, 4, 5,  0, 1, 2, 3, 4, 5};
    IndexArrayType node_num = {0, 1, 0, 2, 0, 1,  1, 2, 3, 3, 2, 4};
    std::vector<T> val(edge_num.size(), 1);

    // build the incidence matrix
    Matrix<T> E(num_edges, num_nodes);
    E.build(edge_num.begin(), node_num.begin(), val.begin(), val.size());
    //print_matrix(std::cout, E, "Incidence");

    auto Eout3 = k_truss(E, 3);
    BOOST_CHECK_EQUAL(Eout3.nrows(), 5); // only removed one edge
    BOOST_CHECK_EQUAL(Eout3.ncols(), 5);

    auto Eout4 = k_truss(Eout3, 4);
    BOOST_CHECK_EQUAL(Eout4.nrows(), 0); // removed all edges
    BOOST_CHECK_EQUAL(Eout4.ncols(), 5);

    auto Etmp4 = k_truss(E, 4);
    BOOST_CHECK_EQUAL(Etmp4.nrows(), 0); // removed all edges
    BOOST_CHECK_EQUAL(Etmp4.ncols(), 5);

    // TODO test for correct contents
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss_test2)
{
    grb::IndexArrayType i = {
        0, 0, 0, 0, 0,
        1, 1, 1, 1,
        2, 2, 2, 2,
        3, 3, 3, 3, 3,
        4, 4, 4, 4, 4,
        5, 5, 5,
        6, 6, 6, 6,
        7, 7, 7, 7, 7,
        8, 8, 8, 8, 8,
        9, 9, 9, 9,
        10,10,10,10,10,
        11,11,11};

    grb::IndexArrayType j = {
        0, 1, 5, 6, 9,
        0, 1, 2, 4,
        1, 2, 3, 4,
        2, 3, 7, 8, 10,
        1, 2, 4, 6, 7,
        0, 5, 9,
        0, 4, 6, 9,
        3, 4, 7, 8, 10,
        3, 7, 8, 10, 11,
        0, 5, 6, 9,
        3, 7, 8, 10, 11,
        8, 10, 11};

    // create an incidence matrix from the data
    IndexType num_edges = 0;
    IndexType num_nodes = 0;
    IndexArrayType edge_array, node_array;
    // count edges in upper triangle of A
    for (IndexType ix = 0; ix < i.size(); ++ix)
    {
        if (i[ix] < j[ix])
        {
            edge_array.push_back(num_edges);
            node_array.push_back(i[ix]);
            edge_array.push_back(num_edges);
            node_array.push_back(j[ix]);
            ++num_edges;

            num_nodes = std::max(num_nodes, i[ix]);
            num_nodes = std::max(num_nodes, j[ix]);
        }
    }
    ++num_nodes;
    std::vector<int> v(edge_array.size(), 1);
    Matrix<int> E(num_edges, num_nodes);
    E.build(edge_array.begin(), node_array.begin(), v.begin(), v.size());
    //print_matrix(std::cout, E, "Incidence(test 2)");

    auto E3out = algorithms::k_truss(E, 3);
    //print_matrix(std::cout, E3out, "3-truss edges");
    BOOST_CHECK_EQUAL(E3out.nrows(), 16);
    BOOST_CHECK_EQUAL(E3out.ncols(), num_nodes);

    auto E4out = algorithms::k_truss(E3out, 4);
    //print_matrix(std::cout, E4out, "4-truss edges");
    BOOST_CHECK_EQUAL(E4out.nrows(), 6);
    BOOST_CHECK_EQUAL(E4out.ncols(), num_nodes);

    auto E5out = algorithms::k_truss(E4out, 5);
    //print_matrix(std::cout, E5out, "5-truss edges");
    BOOST_CHECK_EQUAL(E5out.nrows(), 0);
    BOOST_CHECK_EQUAL(E5out.ncols(), num_nodes);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss_test_peer_pressure1)
{
    IndexType num_nodes(5);
    IndexType num_edges(5);
    grb::IndexArrayType i = {0, 0, 1, 1, 2, 2, 3, 3, 4, 4};
    grb::IndexArrayType j = {0, 1, 1, 2, 0, 2, 0, 3, 3, 4};
    std::vector<int> v(i.size(), 1);
    Matrix<double> E(num_edges, num_nodes);
    E.build(i.begin(), j.begin(), v.begin(), v.size());

    auto E3out = algorithms::k_truss(E, 3);
    //print_matrix(std::cout, E3out, "3-truss edges");
    BOOST_CHECK_EQUAL(E3out.nrows(), 3);
    BOOST_CHECK_EQUAL(E3out.ncols(), num_nodes);

    auto E4out = algorithms::k_truss(E3out, 4);
    //print_matrix(std::cout, E4out, "4-truss edges");
    BOOST_CHECK_EQUAL(E4out.nrows(), 0);
    BOOST_CHECK_EQUAL(E4out.ncols(), num_nodes);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss_test_peer_pressure2)
{
    grb::IndexArrayType i = {0, 0, 0, 0,
                             1, 1, 1, 1,
                             2, 2, 2, 2,
                             3, 3, 3, 3,
                             4, 4, 4, 4,
                             5, 5, 5, 5, 5,
                             6, 6, 6,
                             7, 7, 7, 7};
    grb::IndexArrayType j = {0, 2, 3, 6,
                             1, 2, 3, 7,
                             0, 2, 4, 6,
                             0, 1, 3, 5,
                             0, 2, 4, 6,
                             1, 3, 5, 6, 7,
                             0, 4, 6,
                             1, 3, 5, 7};

    // create an incidence matrix from the data
    IndexType num_edges = 0;
    IndexType num_nodes = 0;
    IndexArrayType edge_array, node_array;
    // count edges in upper triangle of A
    for (IndexType ix = 0; ix < i.size(); ++ix)
    {
        if (i[ix] < j[ix])
        {
            edge_array.push_back(num_edges);
            node_array.push_back(i[ix]);
            edge_array.push_back(num_edges);
            node_array.push_back(j[ix]);
            ++num_edges;

            num_nodes = std::max(num_nodes, i[ix]);
            num_nodes = std::max(num_nodes, j[ix]);
        }
    }
    ++num_nodes;
    std::vector<int> v(edge_array.size(), 1);

    Matrix<int> E(num_edges, num_nodes);
    E.build(edge_array.begin(), node_array.begin(), v.begin(), v.size());
    //print_matrix(std::cout, E, "Incidence");

    auto Eout3 = algorithms::k_truss(E, 3);
    //print_matrix(std::cout, Eout3, "3-truss edges");
    BOOST_CHECK_EQUAL(num_nodes, Eout3.ncols());
    BOOST_CHECK_EQUAL(5, Eout3.nrows());

    auto Eout4 = algorithms::k_truss(Eout3, 4);
    //print_matrix(std::cout, Eout4, "4-truss edges");
    BOOST_CHECK_EQUAL(num_nodes, Eout4.ncols());
    BOOST_CHECK_EQUAL(0, Eout4.nrows());
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss2_test_basic)
{
    using T = int32_t;
    IndexType num_nodes = 5;
    IndexArrayType i = {0, 1, 0, 2, 0, 1, 1, 2, 3, 3, 2, 4};
    IndexArrayType j = {1, 2, 3, 3, 2, 4, 0, 1, 0, 2, 0, 1};
    std::vector<T> val(i.size(), 1);

    // build the adjacency matrix
    Matrix<T> A(num_nodes, num_nodes);
    A.build(i.begin(), j.begin(), val.begin(), val.size());
    //print_matrix(std::cout, A, "Adjacency");

    auto Aout3 = k_truss2(A, 3);
    BOOST_CHECK_EQUAL(Aout3.nvals(), 10); // only removed one edge
    //print_matrix(std::cout, Aout3, "Adjacency (k=3)");

    auto Aout4 = k_truss2(Aout3, 4);
    BOOST_CHECK_EQUAL(Aout4.nvals(), 0); // removed all edges
    //print_matrix(std::cout, Aout4, "Adjacency (k=4)");

    auto Atmp4 = k_truss2(A, 4);
    BOOST_CHECK_EQUAL(Atmp4.nvals(), 0); // removed all edges

    // TODO test for correct contents
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss2_test2)
{
    grb::IndexArrayType i = {
        0, 0, 0, 0,
        1,    1, 1,
        2,    2, 2,
        3,    3, 3, 3,
        4, 4,    4, 4,
        5,    5,
        6, 6,    6,
        7, 7,    7, 7,
        8, 8,    8, 8,
        9, 9, 9,
        10,10,10,   10,
        11,11   };

    grb::IndexArrayType j = {
        1, 5, 6, 9,
        0,    2, 4,
        1,    3, 4,
        2,    7, 8, 10,
        1, 2,    6, 7,
        0,    9,
        0, 4,    9,
        3, 4,    8, 10,
        3, 7,    10, 11,
        0, 5, 6,
        3, 7, 8,     11,
        8, 10    };

    // create an incidence matrix from the data
    IndexType num_edges = i.size();
    IndexType num_nodes = 12;
    std::vector<int> val(num_edges, 1);
    Matrix<int> A(num_nodes, num_nodes);
    A.build(i.begin(), j.begin(), val.begin(), num_edges);

    auto A3out = algorithms::k_truss2(A, 3);
    //print_matrix(std::cout, A3out, "3-truss edges");
    BOOST_CHECK_EQUAL(A3out.nvals(), 32);

    auto A4out = algorithms::k_truss2(A3out, 4);
    //print_matrix(std::cout, A4out, "4-truss edges");
    BOOST_CHECK_EQUAL(A4out.nvals(), 12);

    auto A5out = algorithms::k_truss2(A4out, 5);
    //print_matrix(std::cout, A5out, "5-truss edges");
    BOOST_CHECK_EQUAL(A5out.nvals(), 0);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss2_test_peer_pressure1)
{
    IndexType num_nodes(5);

    grb::IndexArrayType i = {0, 1, 2, 1, 2, 0, 3, 0, 4, 3};
    grb::IndexArrayType j = {1, 0, 1, 2, 0, 2, 0, 3, 3, 4};
    std::vector<int> v(i.size(), 1);
    Matrix<double> A(num_nodes, num_nodes);
    A.build(i.begin(), j.begin(), v.begin(), v.size());

    auto A3out = algorithms::k_truss2(A, 3);
    //print_matrix(std::cout, A3out, "3-truss edges");
    BOOST_CHECK_EQUAL(A3out.nvals(), 6);

    auto A4out = algorithms::k_truss2(A3out, 4);
    //print_matrix(std::cout, A4out, "4-truss edges");
    BOOST_CHECK_EQUAL(A4out.nvals(), 0);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(k_truss2_test_peer_pressure2)
{

    grb::IndexArrayType i = {0, 0, 0, 0,
                             1, 1, 1, 1,
                             2, 2, 2, 2,
                             3, 3, 3, 3,
                             4, 4, 4, 4,
                             5, 5, 5, 5, 5,
                             6, 6, 6,
                             7, 7, 7, 7};
    grb::IndexArrayType j = {0, 2, 3, 6,
                             1, 2, 3, 7,
                             0, 2, 4, 6,
                             0, 1, 3, 5,
                             0, 2, 4, 6,
                             1, 3, 5, 6, 7,
                             0, 4, 6,
                             1, 3, 5, 7};

    // create an adjacency matrix equivalent to the incidence matrix produced in a previous test
    IndexType num_nodes = 0;
    IndexArrayType iv, jv;
    // count edges in upper triangle of A
    for (IndexType ix = 0; ix < i.size(); ++ix)
    {
        if (i[ix] < j[ix])
        {
            iv.push_back(i[ix]); jv.push_back(j[ix]);
            iv.push_back(j[ix]); jv.push_back(i[ix]);  //reverse link

            num_nodes = std::max(num_nodes, i[ix]);
            num_nodes = std::max(num_nodes, j[ix]);
        }
    }
    ++num_nodes;
    IndexType num_edges(iv.size());
    std::vector<int> v(num_edges, 1);

    Matrix<int> A(num_nodes, num_nodes);
    A.build(iv.begin(), jv.begin(), v.begin(), num_edges);
    //print_matrix(std::cout, A, "graph");

    auto Aout3 = algorithms::k_truss2(A, 3);
    //print_matrix(std::cout, Aout3, "3-truss edges");
    BOOST_CHECK_EQUAL(10, Aout3.nvals());

    auto Aout4 = algorithms::k_truss2(Aout3, 4);
    //print_matrix(std::cout, Aout4, "4-truss edges");
    BOOST_CHECK_EQUAL(0, Aout4.nvals());
}

BOOST_AUTO_TEST_SUITE_END()


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   export_geometry
 * @brief  wraps geometry classes
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 * @author Frank Dellaert
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include <gtsam/geometry/Unit3.h>

using namespace boost::python;
using namespace gtsam;
using namespace std;

void export_geometry() {
  class_<Unit3>("Unit3")
      .def(init<>())
      .def(init<double, double, double>())
      .def(init<const Vector3&>());
}


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4310) // truncation of constant
#endif

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pi2o_6 generic tag

     Represents the Pi2o_6 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Pi2o_6, double
                                , 2, 0x3fd28d33LL
                                , 0x3ffa51a6625307d3ULL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pi2o_6, Site> dispatching_Pi2o_6(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Pi2o_6, Site>();
   }
   template<class... Args>
   struct impl_Pi2o_6;
  }
  /*!
    Generates \f$ \pi^2/6\f$

    @par Semantic:

    @code
    T r = Pi2o_6<T>();
    @endcode

    is similar to:

    @code
    T r =  sqr(Pi<T>())/Six<T>();
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pi2o_6, Pi2o_6);
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/simd/constant/common.hpp>

#endif


//  ratio_fwd.hpp  ---------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.4 Compile-time rational arithmetic [ratio], of the C++ committee working
paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

// The way overflow is managed for ratio_less is taken from llvm/libcxx/include/ratio

#ifndef BOOST_RATIO_RATIO_FWD_HPP
#define BOOST_RATIO_RATIO_FWD_HPP

#include <boost/cstdint.hpp>
#ifdef INTMAX_C
#define BOOST_RATIO_INTMAX_C(a) INTMAX_C(a)
#else
#define BOOST_RATIO_INTMAX_C(a) a##LL
#endif

namespace boost
{

//----------------------------------------------------------------------------//
//                                                                            //
//              20.6 Compile-time rational arithmetic [ratio]                 //
//                                                                            //
//----------------------------------------------------------------------------//

// ratio
template <boost::intmax_t N, boost::intmax_t D = 1> class ratio;

// ratio arithmetic
template <class R1, class R2> struct ratio_add;
template <class R1, class R2> struct ratio_subtract;
template <class R1, class R2> struct ratio_multiply;
template <class R1, class R2> struct ratio_divide;

// ratio comparison
template <class R1, class R2> struct ratio_equal;
template <class R1, class R2> struct ratio_not_equal;
template <class R1, class R2> struct ratio_less;
template <class R1, class R2> struct ratio_less_equal;
template <class R1, class R2> struct ratio_greater;
template <class R1, class R2> struct ratio_greater_equal;

// convenience SI typedefs
typedef ratio<BOOST_RATIO_INTMAX_C(1), BOOST_RATIO_INTMAX_C(1000000000000000000)> atto;
typedef ratio<BOOST_RATIO_INTMAX_C(1),    BOOST_RATIO_INTMAX_C(1000000000000000)> femto;
typedef ratio<BOOST_RATIO_INTMAX_C(1),       BOOST_RATIO_INTMAX_C(1000000000000)> pico;
typedef ratio<BOOST_RATIO_INTMAX_C(1),          BOOST_RATIO_INTMAX_C(1000000000)> nano;
typedef ratio<BOOST_RATIO_INTMAX_C(1),             BOOST_RATIO_INTMAX_C(1000000)> micro;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                BOOST_RATIO_INTMAX_C(1000)> milli;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                 BOOST_RATIO_INTMAX_C(100)> centi;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                  BOOST_RATIO_INTMAX_C(10)> deci;
typedef ratio<                 BOOST_RATIO_INTMAX_C(10), BOOST_RATIO_INTMAX_C(1)> deca;
typedef ratio<                BOOST_RATIO_INTMAX_C(100), BOOST_RATIO_INTMAX_C(1)> hecto;
typedef ratio<               BOOST_RATIO_INTMAX_C(1000), BOOST_RATIO_INTMAX_C(1)> kilo;
typedef ratio<            BOOST_RATIO_INTMAX_C(1000000), BOOST_RATIO_INTMAX_C(1)> mega;
typedef ratio<         BOOST_RATIO_INTMAX_C(1000000000), BOOST_RATIO_INTMAX_C(1)> giga;
typedef ratio<      BOOST_RATIO_INTMAX_C(1000000000000), BOOST_RATIO_INTMAX_C(1)> tera;
typedef ratio<   BOOST_RATIO_INTMAX_C(1000000000000000), BOOST_RATIO_INTMAX_C(1)> peta;
typedef ratio<BOOST_RATIO_INTMAX_C(1000000000000000000), BOOST_RATIO_INTMAX_C(1)> exa;

}  // namespace boost

#endif  // BOOST_RATIO_HPP


#ifndef GRAPH_LAPLACIAN_HPP_
#define GRAPH_LAPLACIAN_HPP_

#include <type_traits>
#include <tuple>

#include <boost/mpl/not.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using Matrix = boost::numeric::ublas::matrix<int>;

template <typename VertexListGraph>
Matrix graph_laplacian(const VertexListGraph& g)
{
  using namespace boost;

  BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexListGraph> ));
  static_assert(
      is_undirected_graph<VertexListGraph>::value,
      "Directed graphs are not supported.");
  static_assert(
      boost::mpl::not_<is_multigraph<VertexListGraph>>::value,
      "Multigraphs are not supported.");

  using GraphTraits = graph_traits<VertexListGraph>;

  const auto nv = num_vertices(g);
  Matrix mat(nv, nv, 0);

  auto index = get(vertex_index, g);

  using vertex_iter = typename GraphTraits::vertex_iterator;
  vertex_iter vi, vi_end;

  using adj_iter = typename GraphTraits::adjacency_iterator;
  adj_iter ai, ai_end;

  for (std::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    auto v = *vi;
    auto v_index = index[v];
    mat(v_index, v_index) = static_cast<int>(degree(v, g));
    for (std::tie(ai, ai_end) = adjacent_vertices(v, g); ai != ai_end; ++ai) {
      mat(v_index, index[*ai]) = -1;
    }
  }

  return mat;
}

#endif  // GRAPH_LAPLACIAN_HPP_


#include <iostream>
#include <Eigen/Core>
#include <npe.h>

npe_function(mutate_matrix)
npe_arg(a, dense_double, dense_float)
npe_begin_code()

a(0, 0) = 2.0;

return npe::move(a);

npe_end_code()