#include <gtest/gtest.h>

#include "scheme/objective/hash/XformHash.hh"
#include "scheme/objective/hash/XformHashNeighbors.hh"
#include "scheme/numeric/rand_xform.hh"

#include <Eigen/Geometry>
#include <random>
#include <sparsehash/dense_hash_set>

namespace scheme { namespace objective { namespace hash { namespace xhnbtest {

using std::cout;
using std::endl;

typedef float Float;
typedef Eigen::Transform<Float,3,Eigen::AffineCompact> Xform;
// typedef Eigen::Affine3d Xform;

template<class XH>
void get_neighbors_ref(
	XH const & xh,
	Xform const & x,
	double cart_bound, double ang_bound,
	std::set<typename XH::Key> & nbrs,
	int NSAMP=100000
){
	std::set<typename XH::Key> keys;
	double quat_bound = numeric::deg2quat(ang_bound);
	std::mt19937 rng((unsigned int)time(0) + 9384756);
	Xform xrand;
	for(int i = 0; i < NSAMP; ++i){
		numeric::rand_xform_quat(rng,xrand,cart_bound,quat_bound);
		nbrs.insert( xh.get_key( xrand*x ) );
	}
}

TEST( XformHashNeighbors, Quat_BCC7_Zorder_can_repr_identity ){
	double eps_tol = 0.0000001;
	if( sizeof(Float) == 4 ) eps_tol = 0.0001;
	typedef uint64_t Key;
	std::mt19937 rng((unsigned int)time(0) + 94586049);
	std::uniform_real_distribution<> runif;
	using namespace Eigen;
	for(int j = 0; j < 10000; ++j){
		for(int i = 2; i < 10; ++i){
			XformHash_Quat_BCC7_Zorder<Xform> xh( 0.1+runif(rng), i, runif(rng)*400.0+2.0 );
			Key k0 = xh.grid_[util::SimpleArray<7,double>(0.0)];
			ASSERT_EQ( k0%2, (i+1)%2 );
			util::SimpleArray<7,double> f7 = xh.grid_[k0];
			for(int k = 0; k < 7; ++k) ASSERT_NEAR( f7[k], 0.0, eps_tol );
			Xform x = Xform::Identity();
			// x.translation()[0] = 2.0*runif(rng)-1.0; // this doesn't work... even/odd cells
			// x.translation()[1] = 2.0*runif(rng)-1.0; //
			// x.translation()[2] = 2.0*runif(rng)-1.0; //
			Quaternion<Float> q( xh.get_center( xh.get_key( x ) ).rotation() );
			ASSERT_NEAR( q.w(), 1.0, eps_tol );
			ASSERT_NEAR( q.x(), 0.0, eps_tol );
			ASSERT_NEAR( q.y(), 0.0, eps_tol );
			ASSERT_NEAR( q.z(), 0.0, eps_tol );
		}
	}
}

TEST( XformHashNeighbors, DISABLED_Quat_BCC7_Zorder_quaternion_reflection_symmetry ){
	double eps_tol = 0.0000001;
	if( sizeof(Float) == 4 ) eps_tol = 0.0001;
	using namespace Eigen;
	typedef uint64_t Key;
	typedef util::SimpleArray<7,double> F7;
	typedef util::SimpleArray<7,Key> I7;
	std::mt19937 rng((unsigned int)time(0) + 293457);
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;

	for(int iter1 = 0; iter1 < 100; ++iter1){
		XformHash_Quat_BCC7_Zorder<Xform> xh( runif(rng)+0.1, 2+(int)(runif(rng)*60.0), runif(rng)*400+10.0 );
		for(int iter2 = 0; iter2 < 100; ++iter2){
			double a = runif(rng);
			double b = runif(rng);
			double c = runif(rng);
			double x = rnorm(rng);
			double y = rnorm(rng);
			double z = rnorm(rng);
			double w = rnorm(rng);
			double l = sqrt(w*w+x*x+y*y+z*z);
			x /= l;
			w /= l;
			y /= l;
			z /= l;
			assert( fabs(w*w+x*x+y*y+z*z - 1.0) < 0.00001 );

			std::vector<Key> i(16);
			i[ 0] = xh.grid_[ F7( a,b,c, +w,+x,+y,+z ) ];
			i[ 1] = xh.grid_[ F7( a,b,c, +w,+x,+y,-z ) ];
			i[ 2] = xh.grid_[ F7( a,b,c, +w,+x,-y,+z ) ];
			i[ 3] = xh.grid_[ F7( a,b,c, +w,+x,-y,-z ) ];
			i[ 4] = xh.grid_[ F7( a,b,c, +w,-x,+y,+z ) ];
			i[ 5] = xh.grid_[ F7( a,b,c, +w,-x,+y,-z ) ];
			i[ 6] = xh.grid_[ F7( a,b,c, +w,-x,-y,+z ) ];
			i[ 7] = xh.grid_[ F7( a,b,c, +w,-x,-y,-z ) ];
			i[ 8] = xh.grid_[ F7( a,b,c, -w,+x,+y,+z ) ];
			i[ 9] = xh.grid_[ F7( a,b,c, -w,+x,+y,-z ) ];
			i[10] = xh.grid_[ F7( a,b,c, -w,+x,-y,+z ) ];
			i[11] = xh.grid_[ F7( a,b,c, -w,+x,-y,-z ) ];
			i[12] = xh.grid_[ F7( a,b,c, -w,-x,+y,+z ) ];
			i[13] = xh.grid_[ F7( a,b,c, -w,-x,+y,-z ) ];
			i[14] = xh.grid_[ F7( a,b,c, -w,-x,-y,+z ) ];
			i[15] = xh.grid_[ F7( a,b,c, -w,-x,-y,-z ) ];

			// cout << i[0] <<" "<< i[1] <<" "<< i[2] <<" "<< i[3] <<" "<< i[4] <<" "<< i[5] <<" "<< i[6] <<" "<< i[7] << endl;
			std::vector<F7> f(16);
			f[ 0] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,+x,+y,+z ) ] ];
			f[ 1] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,+x,+y,-z ) ] ];
			f[ 2] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,+x,-y,+z ) ] ];
			f[ 3] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,+x,-y,-z ) ] ];
			f[ 4] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,-x,+y,+z ) ] ];
			f[ 5] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,-x,+y,-z ) ] ];
			f[ 6] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,-x,-y,+z ) ] ];
			f[ 7] = xh.grid_[ xh.grid_[ F7( a,b,c, +w,-x,-y,-z ) ] ];
			f[ 8] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,+x,+y,+z ) ] ];
			f[ 9] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,+x,+y,-z ) ] ];
			f[10] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,+x,-y,+z ) ] ];
			f[11] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,+x,-y,-z ) ] ];
			f[12] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,-x,+y,+z ) ] ];
			f[13] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,-x,+y,-z ) ] ];
			f[14] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,-x,-y,+z ) ] ];
			f[15] = xh.grid_[ xh.grid_[ F7( a,b,c, -w,-x,-y,-z ) ] ];
			for(int j = 1; j < 16; ++j){
				ASSERT_EQ  (      i[0]%2  ,      i[j]%2    ); // all even or all odd
				ASSERT_NEAR( fabs(f[0][3]), fabs(f[j][3]) , eps_tol );
				ASSERT_NEAR( fabs(f[0][4]), fabs(f[j][4]) , eps_tol );
				ASSERT_NEAR( fabs(f[0][5]), fabs(f[j][5]) , eps_tol );
				ASSERT_NEAR( fabs(f[0][6]), fabs(f[j][6]) , eps_tol );
			}

			// if( iter1+iter2 == 0 ){
			// 	for(int j=0; j<8; ++j){
			// 		Xform xf = Xform( numeric::to_half_cell( Quaternion<Float>(f[j][3],f[j][4],f[j][5],f[j][6]) ).matrix());
			// 		for(int k=0;k<3;++k) xf.translation()[k] = f[j][k];
			// 		Key k = xh.get_key( xf );
			// 		Key o = k & 1;
			// 		Key w =  util::undilate<7>( k>>4 ) & 63;
			// 		Key x =  util::undilate<7>( k>>5 ) & 63;
			// 		Key y =  util::undilate<7>( k>>6 ) & 63;
			// 		Key z =  util::undilate<7>( k>>7 ) & 63;
			// 		std::cout << xh.grid_.nside_[3]-o <<" " << o << "    " << w << "\t" << x << "\t" << y << "\t" << z << std::endl;
			// 	}
			// }

		}
	}

}

TEST( XformHashNeighbors, Quat_BCC7_Zorder_key_symmetry ){
	int NSAMP = 100;
	#ifdef SCHEME_BENCHMARK
	NSAMP = 1000;
	#endif

	double eps_tol = 0.0000001;
	if( sizeof(Float) == 4 ) eps_tol = 0.0001;

	using namespace Eigen;
	typedef uint64_t Key;
	typedef util::SimpleArray<7,double> F7;
	typedef util::SimpleArray<7,Key> I7;
	std::mt19937 rng((unsigned int)time(0) + 293457);
	std::uniform_real_distribution<> runif;

	XformHash_Quat_BCC7_Zorder<Xform> xh( runif(rng)+0.1, 2+(int)(runif(rng)*60.0), runif(rng)*400+10.0 );

	for(int j = 0; j < NSAMP; ++j){
		XformHash_Quat_BCC7_Zorder<Xform> xh( runif(rng)+0.1, 2+(int)(runif(rng)*20.0), runif(rng)*400+10.0 );
		for(int i = 0; i < NSAMP; ++i){
			Xform x; numeric::rand_xform(rng,x);
			// while( fabs(Quaternion<Float>(x.rotation()).w()) > 0.01 ) numeric::rand_xform(rng,x);
			Key key = xh.get_key(x);
			Key akey,ksym;
			akey = xh.asym_key(key,ksym);
			bool fx = (ksym>>2)&1;
			bool fy = (ksym>>1)&1;
			bool fz = (ksym>>0)&1;
			// cout << "TEST FX " << fx << endl;
			// cout << "TEST FY " << fy << endl;
			// cout << "TEST FZ " << fz << endl;
			// 	{
			// 		Key k = key;
			// 		Key o = k & 1;
			// 		Key w =  util::undilate<7>( k>>4 ) & 63;
			// 		Key x =  util::undilate<7>( k>>5 ) & 63;
			// 		Key y =  util::undilate<7>( k>>6 ) & 63;
			// 		Key z =  util::undilate<7>( k>>7 ) & 63;
			// 		std::cout << "KEY  " << ksym << "\t" << xh.grid_.nside_[3]-o <<"\t" << o << "    "
			// 		<< w << "\t" << x << "\t" << y << "\t" << z << std::endl;
			// 	}
			// 	{
			// 		Key k = akey;
			// 		Key o = k & 1;
			// 		Key w =  util::undilate<7>( k>>4 ) & 63;
			// 		Key x =  util::undilate<7>( k>>5 ) & 63;
			// 		Key y =  util::undilate<7>( k>>6 ) & 63;
			// 		Key z =  util::undilate<7>( k>>7 ) & 63;
			// 		std::cout << "ASYM " << ksym << "\t" << xh.grid_.nside_[3]-o <<"\t" << o << "    "
			// 		<< w << "\t" << x << "\t" << y << "\t" << z << std::endl;
			// 	}

			ASSERT_LT( ksym, xh.num_key_symmetries() );
			ASSERT_EQ( key, xh.sym_key(akey,ksym) );
			ASSERT_EQ( key%2, akey%2 ); // preserves parity
			if( ksym==0 ) ASSERT_EQ( key, akey );
			else          ASSERT_NE( key, akey );

			Xform xc = xh.get_center(  key );
			Xform xa = xh.get_center( akey );
			ASSERT_EQ( xc.translation(), xa.translation() );
			Quaternion<Float> qc( xc.rotation() );
			Quaternion<Float> qa( xa.rotation() );
			// cout << qc.coeffs().transpose() << endl;
			// cout << qa.coeffs().transpose() << endl << endl;
			qc = numeric::to_half_cell(qc);
			qa = numeric::to_half_cell(qa);
			// cout << qc.coeffs().transpose() << endl;
			// cout << qa.coeffs().transpose() << endl << endl;
			ASSERT_NEAR( qc.w(),              qa.w()  , eps_tol );
			if( fabs( qc.w() ) > eps_tol  ){
				ASSERT_NEAR( qc.x(), (fx?-1.0:1.0)*qa.x() , eps_tol );
				ASSERT_NEAR( qc.y(), (fy?-1.0:1.0)*qa.y() , eps_tol );
				ASSERT_NEAR( qc.z(), (fz?-1.0:1.0)*qa.z() , eps_tol );
			}
			// cout << std::showpos;

		}
	}
}

// // this test is fucked.... can't get 0-registration correct...
// // thinking ori + cart_shift should still work correctly.. will test that
// TEST( XformHashNeighbors, Quat_BCC7_Zorder_check_cart_neighbors ){
// 	typedef uint64_t Key;
// 	int NSAMP = 5;
// 	double THRESH = 0.01;
// 	#ifdef SCHEME_BENCHMARK
// 	NSAMP = 20;
// 	THRESH = 0.001;
// 	#endif
// 	std::mt19937 rng((unsigned int)time(0) + 4564631);
// 	std::uniform_real_distribution<> runif;

// 	// double cart_bound=1.0+runif(rng)*4.0, ang_bound=20.0;
// 	double cart_bound=2.0, ang_bound=20.0;

// 	for(int iter1 = 0; iter1 < NSAMP; ++iter1){
// 		// XformHash_Quat_BCC7_Zorder<Xform> xh( 1.0, 5.0+runif(rng)*10.0, 10+runif(rng)*200.0 );
// 		XformHash_Quat_BCC7_Zorder<Xform> xh( 1.0, 10.0 );
// 		XformHashNeighbors< XformHash_Quat_BCC7_Zorder<Xform> > nb( cart_bound, ang_bound, xh, 0 );

// 		for(int i = 0; i < NSAMP; ++i){
// 			// cout << "TEST ITER " << i << endl;
// 			Xform x0; numeric::rand_xform(rng,x0,10.0);
// 			Key key0 = xh.get_key(x0);
// 			Xform cen0 = xh.get_center(key0);
// 			ASSERT_EQ( xh.get_key(cen0), key0 );
// 			Xform x1(cen0);
// 			if( key0%2 ){
// 				x1.translation()[0] -= xh.cart_width()/2.0;
// 				x1.translation()[1] -= xh.cart_width()/2.0;
// 				x1.translation()[2] -= xh.cart_width()/2.0;
// 			} else {
// 				x1.translation()[0] += xh.cart_width()/2.0;
// 				x1.translation()[1] += xh.cart_width()/2.0;
// 				x1.translation()[2] += xh.cart_width()/2.0;
// 			}
// 			Key key1 = xh.get_key(x1);
// 			cout << x0.translation().transpose() << endl;
// 			cout << x1.translation().transpose() << endl;
// 			xh.print_key(key0);
// 			xh.print_key(key1);
// 			ASSERT_NE( key0%2, key1%2 );
// 			Xform cen1 = xh.get_center(key1);

// 			std::vector<Eigen::Vector3d> vnbrs,vsamp;
// 			std::set<Key> nbrs_set, nbrs_set_ori, nbrs_set_cart;
// 			Eigen::Vector3d nbrcen(0,0,0);
// 			std::vector< util::SimpleArray<3,int16_t> > shifts = nb.get_cart_shifts();
// 			for(int j =0; j < shifts.size(); ++j){
// 				Key nbkey0 = xh.cart_shift_key( key0, shifts[j][0], shifts[j][1], shifts[j][2], 0 );
// 				Key nbkey1 = xh.cart_shift_key( key1, shifts[j][0], shifts[j][1], shifts[j][2], 1 );
// 				nbrs_set.insert( nbkey0 );
// 				nbrs_set.insert( nbkey1 );
// 				vnbrs.push_back( xh.get_center(nbkey0).translation() );
// 				vnbrs.push_back( xh.get_center(nbkey1).translation() );
// 				nbrcen += xh.get_center(nbkey0).translation();
// 				nbrcen += xh.get_center(nbkey1).translation();

// 				nbrs_set_cart.insert( nbkey0 & xh.CART_MASK_NO0 );
// 				nbrs_set_cart.insert( nbkey1 & xh.CART_MASK_NO0 );
// 				nbrs_set_ori.insert( nbkey0 & xh.ORI_MASK_NO0 );
// 				nbrs_set_ori.insert( nbkey1 & xh.ORI_MASK_NO0 );
// 				// cout << nbkey%2 << " " <<  nbrs[j]%2 << endl;
// 			}

// 			cout << "NBRS SET ORI: " << endl;
// 			for(std::set<Key>::const_iterator i = nbrs_set_ori.begin(); i != nbrs_set_ori.end(); ++i){
// 				cout << "       ";
// 				cout << ( util::undilate<7>( (*i)>>4 ) & 63 ) << " ";
// 				cout << ( util::undilate<7>( (*i)>>5 ) & 63 ) << " ";
// 				cout << ( util::undilate<7>( (*i)>>6 ) & 63 ) << " ";
// 				cout << ( util::undilate<7>( (*i)>>7 ) & 63 ) << " ";
// 				cout << (*i)%2;
// 				cout << endl;
// 			}
// 			nbrcen /= shifts.size()*2;

// 			int nfail=0;
// 			for(int j = 0; j < 1000*NSAMP; ++j){
// 				Xform p; numeric::rand_xform_quat(rng,p,cart_bound,0.0);
// 				Key nk = xh.get_key( p*cen0 );
// 				vsamp.push_back( xh.get_center(nk).translation() );
// 				bool fail = nbrs_set.find(nk) == nbrs_set.end();
// 				nfail += fail;
// 				// if( fail ) cout << nk%2 << endl;
// 				ASSERT_TRUE( nbrs_set_cart.find( nk & xh.CART_MASK_NO0) != nbrs_set_cart.end() );
// 				if( fail ){
// 					cout << ( util::undilate<7>( nk>>4 ) & 63 ) << " ";
// 					cout << ( util::undilate<7>( nk>>5 ) & 63 ) << " ";
// 					cout << ( util::undilate<7>( nk>>6 ) & 63 ) << " ";
// 					cout << ( util::undilate<7>( nk>>7 ) & 63 ) << " ";
// 					cout << nk%2;
// 					cout << endl;

// 					// ASSERT_TRUE( nbrs_set_ori.find( nk & xh.ORI_MASK_NO0) == nbrs_set_ori.end() );
// 				}
// 			}

// 			std::ofstream out1("test_nbrs.pdb");
// 			for(int i = 0; i < vnbrs.size(); ++i) io::dump_pdb_atom( out1, "C" ,i, 10.0*vnbrs[i] );
// 			out1.close();
// 			std::ofstream out2("test_samp.pdb");
// 			for(int i = 0; i < vnbrs.size(); ++i) io::dump_pdb_atom( out2, "C" ,i, 10.0*vsamp[i] );
// 			out2.close();
// 			std::ofstream out3("test_cen.pdb");
// 			io::dump_pdb_atom( out3, "C" ,i, 10.0*cen0.translation() );
// 			io::dump_pdb_atom( out3, "C" ,i, 10.0*cen1.translation() );
// 			out3.close();

// 			ASSERT_LE( nfail/1000.0/NSAMP, THRESH );
// 			return ;
// 		}
// 	}
// }

TEST( XformHashNeighbors, Quat_BCC7_Zorder_check_ori_neighbors ){
	typedef uint64_t Key;
	int NSAMP = 3;
	double THRESH = 0.01;
	#ifdef SCHEME_BENCHMARK
	NSAMP = 8;
	THRESH = 0.001;
	#endif
	std::mt19937 rng((unsigned int)time(0) + 94586049);
	std::uniform_real_distribution<> runif;

	double cart_bound=2.0, ang_bound=10.0+runif(rng)*30.0;
	// double cart_bound=1.0, ang_bound=20.0;
	double quat_bound=numeric::deg2quat(ang_bound);

	for(int iter1 = 0; iter1 < NSAMP; ++iter1){
		XformHash_Quat_BCC7_Zorder<Xform> xh( Float(1.0), Float(5.0+runif(rng)*10.0), Float(10+runif(rng)*100.0) );
		// XformHash_Quat_BCC7_Zorder<Xform> xh( 1.0, 10.0 );
		XformHashNeighbors< XformHash_Quat_BCC7_Zorder<Xform> > nb( cart_bound, ang_bound, xh, NSAMP*20.0 );

		for(int i = 0; i < NSAMP; ++i){
			// cout << "TEST ITER " << i << endl;
			Xform x; numeric::rand_xform(rng,x);
			x.translation()[0] = x.translation()[1] = x.translation()[2] = 0.0;
			Key key = xh.get_key(x);
			Xform c = xh.get_center( key );

			std::vector<Key> const & nbrs = nb.get_ori_neighbors( key );
			google::dense_hash_set<Key> nbrs_set;
			nbrs_set.set_empty_key(9999999999);
			Eigen::Vector4d qpass(0,0,0,0), qfail(0,0,0,0);
			int opass=0,ofail=0;
			for(int j =0; j < nbrs.size(); ++j) nbrs_set.insert( nbrs[j] );
			int nfail = 0;
			std::vector<Key> fails,passes;
			for(int j = 0; j < 300*NSAMP; ++j){
				Xform p; numeric::rand_xform_quat(rng,p,cart_bound,quat_bound);
				p.translation()[0] = p.translation()[1] = p.translation()[2] = 0.0;
				Key nk = xh.get_key( p*c ) & xh.ORI_MASK;
				// nk = nk | (Key)1; // debug... this makes it correct much more of the time....
				bool fail = nbrs_set.find(nk) == nbrs_set.end();
				nfail += fail;
				Key k = nk;
				Key o = k & 1;
				Key w =  util::undilate<7>( k>>4 ) & 63;
				Key x =  util::undilate<7>( k>>5 ) & 63;
				Key y =  util::undilate<7>( k>>6 ) & 63;
				Key z =  util::undilate<7>( k>>7 ) & 63;

				if( fail ){ qfail += Eigen::Vector4d( w,x,y,z ); ofail += o; fails.push_back(nk); }
				else      { qpass += Eigen::Vector4d( w,x,y,z ); opass += o; passes.push_back(nk); }
			}

				// cout << fails.size() << " " << passes.size() << endl;
				// std::ofstream out("nbrs_sym_fails.pdb");
				// for(int i = 0; i < fails.size(); ++i){
				// 	Xform x = xh.get_center( fails[i] );
				// 	Eigen::Quaternion<Float> q(x.rotation());
				// 	io::dump_pdb_atom( out, "C" ,i, 50*q.coeffs() );
				// }
				// out.close();
				// std::ofstream out2("nbrs_sym_passes.pdb");
				// for(int i = 0; i < passes.size(); ++i){
				// 	Xform x = xh.get_center( passes[i] );
				// 	Eigen::Quaternion<Float> q(x.rotation());
				// 	io::dump_pdb_atom( out2, "C" ,i, 50*q.coeffs() );
				// }
				// out2.close();

			// cout << "PASS " << qpass.transpose() << " " << opass << endl;
			// cout << "FAIL " << qfail.transpose() << " " << ofail << endl;
			ASSERT_LE( nfail/300.0/NSAMP, THRESH );

			// return;
		}
	}
}

TEST( XformHashNeighbors, Quat_BCC7_Zorder_check_general_neighbors ){
	typedef uint64_t Key;
	int NSAMP = 4;
	double THRESH = 0.003;
	#ifdef SCHEME_BENCHMARK
	NSAMP = 8;
	THRESH = 0.002;
	#endif
	std::mt19937 rng((unsigned int)time(0) + 94586049);
	std::uniform_real_distribution<> runif;

	for(int iter1 = 0; iter1 < NSAMP; ++iter1){
		double cart_bound=1.0+runif(rng)*2.0, ang_bound=10.0+runif(rng)*20.0;
		// double cart_bound=2.0, ang_bound=20.0;
		double quat_bound=numeric::deg2quat(ang_bound);
		// XformHash_Quat_BCC7_Zorder<Xform> xh( 1.0, 5.0+runif(rng)*10.0, 10+runif(rng)*200.0 );
		XformHash_Quat_BCC7_Zorder<Xform> xh( Float(1.0), Float(10.0) );
		typedef XformHashNeighbors< XformHash_Quat_BCC7_Zorder<Xform>, true > XNB;
		XNB nb( cart_bound, ang_bound, xh, NSAMP*50.0 );

		for(int iter2 = 0; iter2 < NSAMP; ++iter2){
			Xform x; numeric::rand_xform(rng,x);
			Key key = xh.get_key(x);
			Xform c = xh.get_center( key );
			// xh.print_key(key);
			// int ix = (int)((util::undilate<7>( key>>1 ) & 63) | ((key>>57)&127)<<6);
			// int iy = (int)((util::undilate<7>( key>>2 ) & 63) | ((key>>50)&127)<<6);
			// int iz = (int)((util::undilate<7>( key>>3 ) & 63) | ((key>>43)&127)<<6);

			std::set<Key> nbrs_set; {

				XNB::crappy_iterator itr = nb.neighbors_begin(key);
				XNB::crappy_iterator end = nb.neighbors_end(key);
				int count = 0;
				for( ; itr != end; ++itr){
					nbrs_set.insert( *itr );
					++count;
				}
				// UNIQUE option has been commented out
				// ASSERT_EQ( nbrs_set.size(), count );

				// std::vector<Key> const & ori_nbrs = nb.get_ori_neighbors( key );
				// std::vector< util::SimpleArray<3,int16_t> > shifts = nb.get_cart_shifts();
				// for(int j =0; j < ori_nbrs.size(); ++j){
				// 	Key ori_key = ori_nbrs[j];
				// 	ori_key = xh.cart_shift_key( ori_key, ix, iy, iz );
				// 	// xh.print_key( ori_key );
				// 	for(int k=0; k < shifts.size(); ++k){
				// 		Key nbkey0 = xh.cart_shift_key( ori_key, shifts[k][0], shifts[k][1], shifts[k][2], 0 );
				// 		Key nbkey1 = xh.cart_shift_key( ori_key, shifts[k][0], shifts[k][1], shifts[k][2], 1 );
				// 		nbrs_set.insert( nbkey0 );
				// 		nbrs_set.insert( nbkey1 );
				// 	}
				// }
				// cout << nbrs_set.size() << " " << ori_nbrs.size() << " " << shifts.size() << endl;
			}

			int nfail = 0;
			for(int j = 0; j < 300*NSAMP; ++j){
				Xform p; numeric::rand_xform_quat(rng,p,cart_bound,quat_bound);
				Key nk = xh.get_key( c*p );
				// Xform ncen = xh.get_center( nk );
				bool fail = nbrs_set.find(nk) == nbrs_set.end();
				nfail += fail;
				// if( fail ){
				// 	xh.print_key(key);
				// 	xh.print_key(nk);
				// 	cout << endl;
				// }
			}
			double failfrac = nfail/300.0/NSAMP;
			cout << "TEST ITER " << iter1 << " " << iter2 << " " << cart_bound << " " << ang_bound << " " << failfrac << endl;
			ASSERT_LE( failfrac, THRESH );

		}

		std::ofstream out( "test.sxhn", std::ios::binary );
		nb.save( out );
		out.close();

		XNB nb2( cart_bound, ang_bound, xh, NSAMP*50.0 );
		std::ifstream in( "test.sxhn", std::ios::binary );
		ASSERT_TRUE( nb2.load( in ) );
		in.close();

		// XformHash const hasher_;
		// std::map< Key, std::vector<Key> > ori_cache_;
		// double cart_bound_, ang_bound_, quat_bound_;
		// int nsamp_;
		// std::vector< util::SimpleArray<3,int16_t> > cart_shifts_;
		ASSERT_EQ( nb.hasher_, nb2.hasher_ );
		ASSERT_EQ( nb.cart_bound_, nb2.cart_bound_ );
		ASSERT_EQ( nb.ang_bound_, nb2.ang_bound_ );
		ASSERT_EQ( nb.quat_bound_, nb2.quat_bound_ );
		ASSERT_EQ( nb.nsamp_, nb2.nsamp_ );
		ASSERT_EQ( nb.ori_cache_.size(), nb2.ori_cache_.size() );
		ASSERT_EQ( nb.cart_shifts_.size(), nb2.cart_shifts_.size() );
		ASSERT_EQ( nb.ori_cache_, nb2.ori_cache_ );
		ASSERT_EQ( nb.cart_shifts_, nb2.cart_shifts_ );

	}
}

}}}}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <vector>
#include <string>
#include <map>

#include <boost/lexical_cast.hpp>

#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1detail.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>

#include "main.hpp"

using namespace hashclash;
using namespace std;

void sha1_forward_differential_step(const sha1differentialpath& path, path_container_autobalance& outpaths)
{
	const int t = int(outpaths.t);
	const unsigned& maxcond = outpaths.maxcond;
	const unsigned& maxsdrs = outpaths.maxsdrs;
	const unsigned& maxweight = outpaths.maxweight;
	const unsigned& minweight = outpaths.minweight;

	booleanfunction* F = 0;
	if (t < 20) F = &SHA1_F1_data;
	else if (t < 40) F = &SHA1_F2_data;
	else if (t < 60) F = &SHA1_F3_data;
	else if (t < 80) F = &SHA1_F4_data;

	static sha1differentialpath newpath;
	static vector<sdr> sdrs;
	static bitcondition Qtm1b[32], Qtm2b[32], Qtm3b[32];
	static vector<unsigned> bval;
	static uint32 fdiv[32];
	static bf_outcome foutcomes[32];
	static vector<sdr> deltam;

	newpath = path;
	unsigned totprecond = 0;
	unsigned totcond = 0;
	for (int k = outpaths.tbegin; k < t-3; ++k)
		if (k >= newpath.tbegin())
			totprecond += newpath[k].hw();
	totcond = totprecond + newpath[t-2].hw() + newpath[t-3].hw();

	unsigned minextracond = 0;
	bf_outcome outconstant, outplus, outminus;
	for (unsigned b = 0; b < 32; ++b)
	{
		Qtm2b[b] = newpath(t-2,(b+2)&31);
		Qtm3b[b] = newpath(t-3,(b+2)&31);
		if (b < 31) {
			outconstant = F->outcome( bc_constant, Qtm2b[b], Qtm3b[b] );
			outplus = F->outcome( bc_plus, Qtm2b[b], Qtm3b[b] );
			outminus = F->outcome( bc_minus, Qtm2b[b], Qtm3b[b] );
		} else {
			outconstant = msb_bf_outcome(*F, bc_constant, Qtm2b[b], Qtm3b[b] );
			outplus = msb_bf_outcome(*F, bc_plus, Qtm2b[b], Qtm3b[b] );
			outminus = msb_bf_outcome(*F, bc_minus, Qtm2b[b], Qtm3b[b] );
		}
		if (outconstant.size() > 1 && outplus.size() > 1 && outminus.size() > 1)
			++minextracond;
	}
	unsigned Qtm1_hwnaf = hwnaf(newpath[t-1].diff());
	unsigned Qt_hwnaf = hwnaf(newpath[t].diff());
	if (totcond + Qt_hwnaf + Qtm1_hwnaf + minextracond > maxcond) return;

	/* table deltam */
	sdr mmask = outpaths.m_mask[t];
	deltam.clear();
	deltam.reserve(1<<mmask.hw());
	uint32 addmask = (~mmask.mask)+1;
	uint32 andmask = mmask.mask & ~0x80000000;
	mmask.sign = 0;
	do {
		mmask.sign += addmask; mmask.sign &= andmask;
		deltam.push_back(mmask);
	} while (mmask.sign != 0 && !outpaths.onemessagediff);

	/* table sdrs for delta Qtm1 */
	unsigned w = Qtm1_hwnaf+1;
	sdr sdrQtm1 = newpath[t-1].getsdr();
	if (w < minweight) w = minweight;
        unsigned mincount = 0;
        if (minweight > 0)
                mincount = count_sdrs(sdrQtm1,minweight-1,5);
	while (
			(w < 32)
			&& (w+1 <= maxweight)
			&& (totcond + w + 1 + minextracond <= maxcond)
			&& (count_sdrs(sdrQtm1, w+1,5)-mincount <= maxsdrs)
		)
		++w;
	table_sdrs(sdrs, sdrQtm1, w, 5);
	// we have to keep Q_t-1 intact if t=0 or 1
	if (t == 0 || t == 1) {
		sdrs.clear();
		sdrs.push_back(path[t-1].getsdr());
	}

	wordconditions& newpathQtp1 = newpath[t+1];
	wordconditions& newpathQt = newpath[t];
	wordconditions& newpathQtm1 = newpath[t-1];
	wordconditions& newpathQtm2 = newpath[t-2];
	wordconditions& newpathQtm3 = newpath[t-3];

	uint32 dQtp1pc = newpath[t].getsdr().rotate_left(5).adddiff() + newpath[t-4].getsdr().rotate_left(30).adddiff();
	vector<sdr>::const_iterator cit = sdrs.begin(), citend = sdrs.end();
	for (; cit != citend; ++cit)
	{
		sdrQtm1 = *cit;
		unsigned hwQtm1 = sdrQtm1.hw();
		if (hwQtm1 < minweight) continue;
		if (hwQtm1 > maxweight) continue;
		if (totcond + Qt_hwnaf + hwQtm1 + minextracond > maxcond)
			continue;

		// we have to keep Q_t-1 intact if t=0 or 1
		if (t == 0 || t == 1)
			newpathQtm1 = path[t-1];
		else
			newpathQtm1 = sdrQtm1;

		uint32 cnt = 1;
		uint32 dF_fixed = 0;
		unsigned maxextracond = 0;
		bval.clear();
		for (unsigned b = 0; b < 32; ++b)
		{
			Qtm1b[b] = newpathQtm1.get(b);
			if (b == 31)
				foutcomes[b] = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b]);
			else
				foutcomes[b] = F->outcome( Qtm1b[b], Qtm2b[b], Qtm3b[b] );
			unsigned fsize = foutcomes[b].size();
			if (fsize > 1)
			{
				fdiv[b] = cnt;
				if (fsize == 2) cnt <<= 1;
				else if (fsize == 3) { cnt += cnt<<1; ++ maxextracond; }
				bval.push_back(b);
			} else
				dF_fixed += foutcomes[b](0,b);
		}
		if (totcond + Qt_hwnaf + hwQtm1 + bval.size() > maxcond)
			continue;
		if (outpaths.estimatefactor != 0)
		{
			outpaths.estimate(totcond + Qt_hwnaf + hwQtm1 + bval.size() + ((maxextracond+1)>>1), cnt*deltam.size());
			continue;
		}

		newpathQtm2 = path[int(t)-2];
		newpathQtm3 = path[int(t)-3];

		std::reverse(bval.begin(), bval.end());
		bf_conditions newconditions;
		for (uint32 k = 0; k < cnt; ++k)
		{
			uint32 m = k;
			uint32 dF = dF_fixed;
			for (unsigned j = 0; j < bval.size(); ++j)
			{
				const unsigned b = bval[j];
				unsigned i = 0;
				while (m >= fdiv[b])
				{
					m -= fdiv[b];
					++i;
				}
				if (b == 31)
					newconditions = msb_bf_forwardconditions(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i]);
				else
					newconditions = F->forwardconditions( Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i] );
				newpathQtm1.set(b, newconditions.first);
				newpathQtm2.set((b+2)&31, newconditions.second);
				newpathQtm3.set((b+2)&31, newconditions.third);
				dF += foutcomes[b](i,b);
			}
			unsigned ncond = totprecond + newpathQtm3.hw() + newpathQtm2.hw() + newpathQtm1.hw() + Qt_hwnaf;
			if (ncond > maxcond) continue;
			for (unsigned i = 0; i < deltam.size(); ++i)
			{
				newpath.getme(t) = deltam[i];
				uint32 dQtp1 = dQtp1pc + dF + deltam[i].adddiff();
				newpathQtp1 = naf(dQtp1);
				unsigned ncond2 = ncond;
				if (outpaths.includenaf) {
					if (outpaths.halfnafweight)
						ncond2 += (newpathQtp1.hw()>>1);
					else
						ncond2 += newpathQtp1.hw();
				}
				if (ncond2 <= maxcond)
					outpaths.push_back(newpath, ncond2);
			}
		} // for cnt
	} // for sdrs

}


//
// Created by wei on 3/3/17.
//

#include "NeighborSelection.hpp"

#include <Eigen/Dense>
#include <iostream>
#include <map>

#ifdef BAZEL
#include "Models/LinearRegression.hpp"
#include "Models/Model.hpp"
#else
#include "../Models/LinearRegression.hpp"
#include "../Models/Model.hpp"
#endif

using namespace Eigen;
using namespace std;

NeighborSelection::NeighborSelection(const unordered_map<string, string>& opts) {
    try {
        tolerance = stod(opts.at("tolerance"));
    } catch(std::out_of_range& oor) {
        tolerance = default_tolerance;
    }
    try {
        learningRate = stod(opts.at("learning_rate"));
    } catch(std::out_of_range& oor) {
        learningRate = default_learning_rate;
    }
}

NeighborSelection::NeighborSelection() {
    learningRate = default_learning_rate;
    tolerance = default_tolerance;
}

/*void NeighborSelection::stop() {
    shouldStop = true;
}*/

void NeighborSelection::setTolerance(float tol) {
    tolerance = tol;
}

void NeighborSelection::assertReadyToRun() {
    return;    // there is no data that cannot be inferred
}

void NeighborSelection::setUpRun() {
    mtx.lock();
    isRunning = true;
    progress = 0.0;
    shouldStop = false;
}

void NeighborSelection::finishRun() {
    isRunning = false;
    progress = 1.0;
    mtx.unlock();
}

void NeighborSelection::setLearningRate(float lr) {
    learningRate = lr;
}

void NeighborSelection::run(shared_ptr<Model> model) {
    cerr << "The algorithm for this specific model is not implemented, runs on basic model"<<endl;
    int epoch = 0;
    float residue = model->cost();
    VectorXf grad;
    VectorXf in;
    while (!shouldStop && epoch < maxIteration && residue > tolerance) {
        epoch++;
        progress = float(epoch) / maxIteration;
        grad = model->proximal_derivative();
        in = model->getBeta() - learningRate * grad;
        model->updateBeta(model->proximal_operator(in, learningRate));
        residue = model->cost();
    }
}

void NeighborSelection::run(shared_ptr<LinearRegression> model) {
    int epoch = 0;
    MatrixXf X_origin = model->getX();
    MatrixXf X = model->getX();
    model->setL1_reg(model->getL1_reg()*10);
    long s = X_origin.cols();
    for (long i = 0; i < s; i++){
        if (shouldStop) {
            break;
        }
        X.col(i).setZero();
        model->setX(X);
        X.col(i) = X_origin.col(i);
        model->setY(X_origin.col(i));
        VectorXf beta = VectorXf::Random(X.cols());
        beta(i) = 0;
        model->updateBeta(beta);
        float residue = model->cost();
        VectorXf grad;
        VectorXf in;
        epoch = 0;
        while (!shouldStop && epoch < maxIteration && residue > tolerance && !shouldStop) {
            epoch++;
            progress = (float(epoch) + i*maxIteration )/(maxIteration*s);
            grad = model->proximal_derivative();
            in = model->getBeta() - learningRate * grad;
            model->updateBeta(model->proximal_operator(in, learningRate));
            residue = model->cost();
        }
        model->updateBetaAll(model->getBeta());
    }
    auto result = model->getBetaAll();
    for (long col = 0; col < result.cols(); col++) {
        for (long row = 0; row < result.rows(); row++) {
            result(row, col) = abs(result(row, col)) < 1e-5 ? 0 : 1;
        }
    }
    model->updateBeta(result);
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FEATURES_MATCHING_BRIEF_DESCRIPTOR_HPP
#define PIC_FEATURES_MATCHING_BRIEF_DESCRIPTOR_HPP

#include <random>
#include <chrono>

#include "../util/std_util.hpp"
#include "../util/math.hpp"
#include "../image.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief The BRIEFDescriptor class
 */
class BRIEFDescriptor
{
protected:
    int S;
    unsigned int n;
    float sigma_sq_2, sigma_sq;
    std::mt19937 *m;

    //samples coordinates
    int *x, *y;

    /**
     * @brief generateSample
     * @param sample
     */
    void generateSample(int *sample)
    {
        float theta = C_PI_2 * getRandom((*m)());

        float u = getRandom((*m)());
        float r = sqrtf(MAX(-logf(u) * sigma_sq_2, 0.0f));

        sample[0] = int(r * cosf(theta));
        sample[1] = int(r * sinf(theta));
    }

    /**
     * @brief generateSamples
     * @param n
     */
    void generateSamples(unsigned int n)
    {
        this->n = n;
        unsigned int n2 = n * 2;

        x = new int [n2];
        y = new int [n2];

        for(unsigned int i = 0; i < n2; i += 2) {
            generateSample(&x[i]);
            generateSample(&y[i]);
        }
    }

    /**
     * @brief countZeros
     * @param x
     * @return
     */
    static unsigned int countZeros(unsigned int x)
    {
        unsigned int n = sizeof(unsigned int) * 8;

        unsigned int ret = 0;

        for(unsigned int i = 0; i < n; i++) {
            if((x & (1 << i)) == 0) {
                ret++;
            }
        }

        return ret;
    }

    /**
     * @brief getAux computes a descriptor at position (x0,y0) with size n.
     * @param img
     * @param x0
     * @param y0
     * @param desc
     * @return
     */
    unsigned int *getAux(Image *img, int x0, int y0, int *x, int *y, unsigned int *desc = NULL)
    {
        unsigned int bits = sizeof(unsigned int) * 8;
        unsigned int subBlock = n / bits;

        if(desc == NULL) {
            desc = new unsigned int[subBlock];
        }

        int c = 0;

        for(unsigned int i = 0; i < subBlock; i++) {
            unsigned int value = 0;

            for(unsigned int j = 0; j < bits; j++) {
                int cShifted = c * 2;

                float *p_x_val = (*img)(x0 + x[cShifted], y0 + x[cShifted + 1]);
                float *p_y_val = (*img)(x0 + y[cShifted], y0 + y[cShifted + 1]);

                float p_x = 0.0f;
                float p_y = 0.0f;

                for(int k = 0; k < img->channels; k++) {
                    p_x	+= p_x_val[k];
                    p_y	+= p_y_val[k];
                }

                unsigned int ret = (p_x < p_y) ? 1 : 0;

                value += (ret << j);

                c++;
            }

            desc[i] = value;
        }

        return desc;
    }

public:

    /**
     * @brief BRIEFDescriptor
     * @param S
     * @param n
     */
    BRIEFDescriptor(int S = 32, int n = 256, int seed = 1)
    {
        if(seed >= 0) {
            m = new std::mt19937(seed);
        } else {
            auto seed_time = std::chrono::system_clock::now().time_since_epoch().count();
            m = new std::mt19937(int (seed_time));
        }

        this->S = S;
        this->sigma_sq = float(S * S) / 25.0f;
        this->sigma_sq_2 = 2.0f * this->sigma_sq;

        generateSamples(n);
    }

    ~BRIEFDescriptor()
    {
        release();
    }

    /**
     * @brief Release deallocates memory.
     */
    void release()
    {
        m = delete_s(m);
        x = delete_s(x);
        y = delete_s(y);
    }

    /**
     * @brief get computes a descriptor at position (x0,y0) with size n.
     * @param img
     * @param x0
     * @param y0
     * @param desc
     * @return
     */
    unsigned int *get(Image *img, int x0, int y0, unsigned int *desc = NULL)
    {
        if(img == NULL) {
            return NULL;
        }

        if(!img->checkCoordinates(x0, y0)) {
            return NULL;
        }

        return getAux(img, x0, y0, x, y, desc);
    }

    #ifndef PIC_DISABLE_EIGEN
    /**
     * @brief getAll
     * @param descs
     * @param corners
     * @param img
     */
    void getAll(Image *img,
                std::vector< Eigen::Vector2f > &corners,
                std::vector< unsigned int *> &descs)
    {
        descs.clear();

        for(unsigned int i = 0; i < corners.size(); i++) {
            int x0 = int(corners[i][0]);
            int y0 = int(corners[i][1]);
            descs.push_back(get(img, x0, y0, NULL));
        }
    }
    #endif

    /**
     * @brief getDescriptorSize returns the descriptor size.
     * @return the descriptor size.
     */
    int getDescriptorSize() {
        return n / (sizeof(unsigned int) * 8);
    }

    /**
     * @brief match matches two descriptors. Note: Higher scores means better matching.
     * @param fv0
     * @param fv1
     * @param nfv
     * @return
     */
    static unsigned int match(unsigned int *fv0, unsigned int *fv1, unsigned int nfv)
    {
        if((fv0 == NULL) || (fv1 == NULL)) {
            return 0;
        }

        unsigned int ret = 0;

        for(unsigned int i = 0; i < nfv; i++) {
            ret += countZeros(fv0[i] ^ fv1[i]);
        }

        return ret;
    }
};

#endif

} // end namespace pic

#endif /* PIC_FEATURES_MATCHING_BRIEF_DESCRIPTOR_HPP */


/* Copyright 2019 Fred Hutchinson Cancer Research Center
 * See the included LICENSE file for details on the license that is granted to the
 * user of this software.
 * transformation.hpp
 *
 *  Created on: Apr 24, 2012
 *      Author: wjiang2
 */

#ifndef TRANSFORMATION_HPP_
#define TRANSFORMATION_HPP_
#include <map>
#include <cmath>
#include <string>
#include <vector>
#include <stdexcept>
#include "calibrationTable.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/compare.hpp>

#include "in_polygon.hpp"

using namespace std;

namespace cytolib
{
#define CALTBL 0
#define LOG 1
#define LIN 2
#define FLIN 3
#define FASINH 4
#define BIEXP 5
#define LOGICLE 6
#define LOGGML2 7
#define SCALE 8
/* case insensitive compare predicate*/
struct ciLessBoost : std::binary_function<std::string, std::string, bool>
{
    bool operator() (const std::string & s1, const std::string & s2) const {
        return lexicographical_compare(s1, s2, boost::is_iless());
    }
};

typedef map<std::string, std::string, ciLessBoost> CHANNEL_MAP;

struct coordinate : public cytolib::CYTO_POINT
{
	coordinate(EVENT_DATA_TYPE _x,EVENT_DATA_TYPE _y):CYTO_POINT(_x, _y){};//{x=_x;y=_y;};
	coordinate(){};
	void convertToPb(pb::coordinate & coor_pb){
		coor_pb.set_x(x);
		coor_pb.set_y(y);
	};
	coordinate(const pb::coordinate & coor_pb):CYTO_POINT(coor_pb.x(),coor_pb.y()){};
};

class transformation;
typedef shared_ptr<transformation> TransPtr;
class transformation{
protected:
	calibrationTable calTbl; //no longer have to store calTbl since we now can compute it from biexp even for mac workspace
	bool isGateOnly;
	unsigned short type;//could have been avoided if it is not required by R API getTransformation that needs to extract concrete transformation
	string name;
	string channel;
	bool isComputed;//this flag allow lazy computCalTbl/interpolation
public:
	/*
		 * if it is pure transformation object,then assume calibration is directly read from ws
		 * so there is no need to compute calibration
		 */

	transformation();
	transformation(bool _isGate, unsigned short _type);
	virtual ~transformation(){};
	virtual void transforming(EVENT_DATA_TYPE * input, int nSize);

	virtual void computCalTbl();//dummy routine that does nothing
	virtual Spline_Coefs getSplineCoefs();
	virtual void setCalTbl(calibrationTable _tbl);

	virtual calibrationTable getCalTbl();
	virtual void interpolate();
	virtual bool isInterpolated();
	virtual bool gateOnly();
	virtual void setGateOnlyFlag(bool _flag);
	virtual bool computed();
	virtual void setComputeFlag(bool _flag);
	virtual string getName();
	virtual void setName(string _name);
	virtual string getChannel();
	virtual void setChannel(string _channel);
	virtual unsigned short getType();
	virtual unsigned short getType(string &ctype);
	virtual void setType(unsigned short _type);
	virtual TransPtr clone() const;
	transformation(const pb::transformation & trans_pb);
	virtual void convertToPb(pb::transformation & trans_pb);
	virtual TransPtr  getInverseTransformation();
	virtual void setTransformedScale(int scale);
	virtual int getTransformedScale();
	virtual int getRawScale();
};
typedef shared_ptr<transformation> TransPtr;

class biexpTrans:public transformation{
public:
	int channelRange;
	EVENT_DATA_TYPE pos, neg, widthBasis, maxValue;
public:

	biexpTrans();

	biexpTrans(int channelRange_,EVENT_DATA_TYPE pos_, EVENT_DATA_TYPE neg_, EVENT_DATA_TYPE widthBasis_, EVENT_DATA_TYPE maxValue_);
	/*
	 * directly translated from java routine from tree star
	 */
	EVENT_DATA_TYPE logRoot(EVENT_DATA_TYPE b, EVENT_DATA_TYPE w);

	 /*
	  * directly translated from java routine from tree star
	  * potential segfault risk: the inappropriate biexp parameters can cause
	  * the indexing of vector out of the boundary.
	  */

	void computCalTbl();
	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	biexpTrans(const pb::transformation & trans_pb);
	void setTransformedScale(int scale);
	int getTransformedScale();
	int getRawScale();

};

class fasinhTrans:public transformation{
public:
	EVENT_DATA_TYPE maxRange;
	EVENT_DATA_TYPE length;//unused at this moment
	EVENT_DATA_TYPE T, A, M;
public:
	fasinhTrans();

	fasinhTrans(EVENT_DATA_TYPE _maxRange, EVENT_DATA_TYPE _length, EVENT_DATA_TYPE _T, EVENT_DATA_TYPE _A, EVENT_DATA_TYPE _M);
	/*
	 * implementation copied from flowCore
	 */
	virtual void transforming(EVENT_DATA_TYPE * input, int nSize);

	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	fasinhTrans(const pb::transformation & trans_pb);
	TransPtr  getInverseTransformation();

	void setTransformedScale(int scale);
	int getTransformedScale();
	int getRawScale();
};
/*
 * inverse transformation of fasinhTrans
 */
class fsinhTrans:public fasinhTrans{

public:

	fsinhTrans();

	fsinhTrans(EVENT_DATA_TYPE _maxRange, EVENT_DATA_TYPE _length, EVENT_DATA_TYPE _T, EVENT_DATA_TYPE _A, EVENT_DATA_TYPE _M);

	void  transforming(EVENT_DATA_TYPE * input, int nSize);
	TransPtr getInverseTransformation();
};

/*
 * TODO:right now set two flags to TRUE in the contructor to avoid doing cal table stuff,
 * we should consider redesign the classes so that logTrans does not share this extra feature from parent class
 */
class logTrans:public transformation{
public:
		EVENT_DATA_TYPE offset;//i.e. min in flowjo
		EVENT_DATA_TYPE decade; //i.e. log(max) - log(min) in flowjo
		unsigned scale;
		unsigned T; //(deprecated)top value; derived from keyword $PnR for each channel
public:
	logTrans();

	logTrans(EVENT_DATA_TYPE _offset,EVENT_DATA_TYPE _decade, unsigned _scale, unsigned _T);

	void transforming(EVENT_DATA_TYPE * input, int nSize);
	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	logTrans(const pb::transformation & trans_pb);

	TransPtr  getInverseTransformation();

	void setTransformedScale(int _scale);
	int getTransformedScale();
	int getRawScale();
};

class logInverseTrans:public logTrans{
public:
	logInverseTrans(EVENT_DATA_TYPE _offset,EVENT_DATA_TYPE _decade, unsigned _scale, unsigned _T);
	void transforming(EVENT_DATA_TYPE * input, int nSize);
};

/*
 * Separate class created to faithfully represent GML 2.0 specification
 * for "parametrized logarithmic transformation -- flog" (Section 6.2.1)
 *
 * Names of data members intentionally mirror the specification
 */
class logGML2Trans:public transformation{
public:
		EVENT_DATA_TYPE T; // top of scale value
		EVENT_DATA_TYPE M; // number of logarithmic decades
public:
	logGML2Trans();

	logGML2Trans(EVENT_DATA_TYPE _T,EVENT_DATA_TYPE _M);

	void transforming(EVENT_DATA_TYPE * input, int nSize);
	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	logGML2Trans(const pb::transformation & trans_pb);

	TransPtr  getInverseTransformation();

//	void setTransformedScale(int _scale);
	int getTransformedScale();
	int getRawScale();
};

class logGML2InverseTrans:public logGML2Trans{
public:
	logGML2InverseTrans(EVENT_DATA_TYPE _T,EVENT_DATA_TYPE _M);
	void transforming(EVENT_DATA_TYPE * input, int nSize);
};

class linTrans:public transformation{
public:
	linTrans();
	void transforming(EVENT_DATA_TYPE * input, int nSize);

		TransPtr clone() const;
        void convertToPb(pb::transformation & trans_pb);
        linTrans(const pb::transformation & trans_pb);
        TransPtr getInverseTransformation();
        void setTransformedScale(int scale);
};

/*
 * This class is dedicated to scale the EllipsoidGate
 */
class scaleTrans:public linTrans{
public:
	int t_scale; //transformed scale
	int r_scale; // raw scale
	EVENT_DATA_TYPE scale_factor;

	scaleTrans();
	scaleTrans(int _t_scale, int _r_scale);
	scaleTrans(EVENT_DATA_TYPE _scale_factor);
	void transforming(EVENT_DATA_TYPE * input, int nSize);

	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	scaleTrans(const pb::transformation & trans_pb);

	TransPtr getInverseTransformation();

	void setTransformedScale(int _scale);

};

class flinTrans:public transformation{
	EVENT_DATA_TYPE min;
	EVENT_DATA_TYPE max;
public:
	flinTrans();
	flinTrans(EVENT_DATA_TYPE _minRange, EVENT_DATA_TYPE _maxRange);

	EVENT_DATA_TYPE flin(EVENT_DATA_TYPE x);

	void transforming(EVENT_DATA_TYPE * input, int nSize);

	TransPtr clone() const;

	void convertToPb(pb::transformation & trans_pb);
	flinTrans(const pb::transformation & trans_pb);
	TransPtr getInverseTransformation();
	void setTransformedScale(int scale);

};

/**
 * abridged from flowCore version
 */
struct logicle_params
{
	double T, W, M, A;

	double a, b, c, d, f;
	double w, x0, x1, x2;

	double xTaylor;
	vector<double> taylor;

	int bins;
	bool isInverse;
};

struct sfun_info{
	double b,w;
};
class logicleTrans:public transformation
{
//	const double DEFAULT_DECADES = 4.5;

	const double LN_10 = log(10.);
	const double EPSILON = std::numeric_limits<double>::epsilon();
	const double NaN = std::numeric_limits<double>::quiet_NaN();

	const int TAYLOR_LENGTH = 16;

	logicle_params p;
	bool isGml2;

public:
	logicle_params get_params();
	logicleTrans (double T, double W, double M, double A, bool _isGml2, int bins= 0, bool isInverse = false);
	void init();

	// f(w,b) = 2 * (ln(d) - ln(b)) + w * (b + d)
	static double logicle_fn(double x,void*info);

	/*
	 * root finder routines are copied from stats/src/zeroin.c
	 */
	double R_zeroin(			/* An estimate of the root */
	    double ax,				/* Left border | of the range	*/
	    double bx,				/* Right border| the root is seeked*/
	    double (*f)(double x, void *info),	/* Function under investigation	*/
	    void *info,				/* Add'l info passed on to f	*/
	    double *Tol,			/* Acceptable tolerance		*/
	    int *Maxit)				/* Max # of iterations */
	;

	/* R_zeroin2() is faster for "expensive" f(), in those typical cases where
	 *             f(ax) and f(bx) are available anyway : */

	double R_zeroin2(			/* An estimate of the root */
	    double ax,				/* Left border | of the range	*/
	    double bx,				/* Right border| the root is seeked*/
	    double fa, double fb,		/* f(a), f(b) */
	    double (*f)(double x, void *info),	/* Function under investigation	*/
	    void *info,				/* Add'l info passed on to f	*/
	    double *Tol,			/* Acceptable tolerance		*/
	    int *Maxit)				/* Max # of iterations */
	;
	/*
	 * use R built-in root finder API :R_zeroin
	 */
	double solve (double b, double w);

	double slope (double scale) const;

	double seriesBiexponential (double scale) const;

	double scale (double value) const;
	double inverse (double scale) const;

	virtual void transforming(EVENT_DATA_TYPE * input, int nSize);
	TransPtr clone() const;
	void convertToPb(pb::transformation & trans_pb);
	logicleTrans(const pb::transformation & trans_pb);
	TransPtr  getInverseTransformation();

	void setTransformedScale(int scale);
	int getTransformedScale();
	int getRawScale();
};

};

#endif /* TRANSFORMATION_HPP_ */


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval_map_mixed unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

#include "../test_interval_map_mixed.hpp"

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_add_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_add_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_add2_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_add2_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_subtract_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_subtract_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_erase_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_erase_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_erase2_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_erase2_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_insert_erase_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_insert_erase_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_insert_erase2_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_insert_erase2_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_basic_intersect_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_basic_intersect_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_basic_intersect2_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_basic_intersect2_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_intersect_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_intersect_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_intersect2_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_intersect2_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_disjoint_4_bicremental_types, T, bicremental_types)
{         interval_map_mixed_disjoint_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_icl_interval_map_mixed_erase_if_4_integral_types, T, integral_types)
{         interval_map_mixed_erase_if_4_integral_types<T, int>(); }


/**
 *  This test ensures that the sketch application (for mixed: CombBLAS ->
 *  Elemental matrices) is done correctly (on-the-fly matrix multiplication in
 *  the code is compared to true matrix multiplication).
 *  This test builds on the following assumptions:
 *
 *      - CombBLAS PSpGEMM returns the correct result, and
 *      - the random numbers in row_idx and row_value (see
 *        hash_transform_data_t) are drawn from the promised distributions.
 */

#include <vector>

#include <boost/mpi.hpp>
#include <boost/test/minimal.hpp>

//FIXME: ugly
#define SKYLARK_SKETCH_HPP 1

#include "../../utility/distributions.hpp"
#include "../../base/context.hpp"
#include "../../sketch/hash_transform.hpp"

#include "../../base/sparse_matrix.hpp"

#include "../../utility/external/combblas_comm_grid.hpp"

typedef FullyDistVec<size_t, double> mpi_vector_t;
typedef SpDCCols<size_t, double> col_t;
typedef SpParMat<size_t, double, col_t> DistMatrixType;
typedef PlusTimesSRing<double, double> PTDD;
typedef skylark::base::sparse_matrix_t<double> LocalMatrixType;

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct Dummy_t : public skylark::sketch::hash_transform_t<
    InputMatrixType, OutputMatrixType,
    boost::random::uniform_int_distribution,
    skylark::utility::rademacher_distribution_t > {

    typedef skylark::sketch::hash_transform_t<
        InputMatrixType, OutputMatrixType,
        boost::random::uniform_int_distribution,
        skylark::utility::rademacher_distribution_t >
            hash_t;

    Dummy_t(int N, int S, skylark::base::context_t& context)
        : skylark::sketch::hash_transform_t<InputMatrixType, OutputMatrixType,
          boost::random::uniform_int_distribution,
          skylark::utility::rademacher_distribution_t>(N, S, context)
    {}

    std::vector<size_t> getRowIdx() { return hash_t::row_idx; }
    std::vector<double> getRowValues() { return hash_t::row_value; }
};

template<typename sketch_t>
void compute_sketch_matrix(sketch_t sketch, const DistMatrixType &A,
                           DistMatrixType &result) {

    std::vector<size_t> row_idx = sketch.getRowIdx();
    std::vector<double> row_val = sketch.getRowValues();

    // PI generated by random number gen
    size_t sketch_size = row_val.size();
    mpi_vector_t cols(sketch_size);
    mpi_vector_t rows(sketch_size);
    mpi_vector_t vals(sketch_size);

    for(size_t i = 0; i < sketch_size; ++i) {
        cols.SetElement(i, i);
        rows.SetElement(i, row_idx[i]);
        vals.SetElement(i, row_val[i]);
    }

    result = DistMatrixType(result.getnrow(), result.getncol(),
                            rows, cols, vals);
}

void compare_result(size_t rank, DistMatrixType &expected_A,
                    elem::DistMatrix<double, elem::STAR, elem::STAR> &result) {

    col_t &data = expected_A.seq();

    const size_t my_row_offset = skylark::utility::cb_my_row_offset(expected_A);
    const size_t my_col_offset = skylark::utility::cb_my_col_offset(expected_A);

    for(typename col_t::SpColIter col = data.begcol();
        col != data.endcol(); col++) {
        for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
            nz != data.endnz(col); nz++) {

            const size_t rowid = nz.rowid()  + my_row_offset;
            const size_t colid = col.colid() + my_col_offset;
            const double value = nz.value();

            if(value != result.GetLocal(rowid, colid)) {
                std::ostringstream os;
                os << rank << ": " << rowid << ", " << colid << ": "
                   << value << " != "
                   << result.GetLocal(rowid, colid) << std::endl;
                std::cout << os.str() << std::flush;
                BOOST_FAIL("Result application not as expected");
            }
        }
    }
}

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

    //////////////////////////////////////////////////////////////////////////
    //[> Parameters <]

    //FIXME: use random sizes?
    const size_t n   = 100;
    const size_t m   = 50;
    const size_t n_s = 60;
    const size_t m_s = 30;

    //////////////////////////////////////////////////////////////////////////
    //[> Setup test <]
    namespace mpi = boost::mpi;

#ifdef SKYLARK_HAVE_OPENMP
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
#endif

    mpi::environment env(argc, argv);
    mpi::communicator world;
    const size_t rank = world.rank();

    elem::Initialize(argc, argv);
    MPI_Comm mpi_world(world);
    elem::Grid grid(mpi_world);

    skylark::base::context_t context (0);

    double count = 1.0;

    const size_t matrix_full = n * m;
    mpi_vector_t colsf(matrix_full);
    mpi_vector_t rowsf(matrix_full);
    mpi_vector_t valsf(matrix_full);

    for(size_t i = 0; i < matrix_full; ++i) {
        colsf.SetElement(i, i % m);
        rowsf.SetElement(i, i / m);
        valsf.SetElement(i, count);
        count++;
    }

    DistMatrixType A(n, m, rowsf, colsf, valsf);
    mpi_vector_t zero;

    count = 1.0;
    elem::Matrix<double> local_A(n, m);
    for( size_t j = 0; j < local_A.Height(); j++ )
        for( size_t i = 0; i < local_A.Width(); i++ )
            local_A.Set(j, i, count++);

    elem::DistMatrix<double, elem::STAR, elem::STAR> result;

    // columnwise application
    DistMatrixType expected_A;
    DistMatrixType pi_sketch(n_s, n, zero, zero, zero);

    // rowwise application
    DistMatrixType expected_AR;
    DistMatrixType pi_sketch_r(m_s, m, zero, zero, zero);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> DistMatrix[MC/MR] <]

    typedef elem::DistMatrix<double> mcmr_target_t;

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, mcmr_target_t> Sparse(n, n_s, context);

    //[> 2. Create space for the sketched matrix <]
    mcmr_target_t sketch_A(n_s, m, grid);
    elem::Zero(sketch_A);

    //[> 3. Apply the transform <]
    Sparse.apply(A, sketch_A, skylark::sketch::columnwise_tag());

    //[> 4. Build structure to compare <]
    // easier to check if all processors own result
    result = sketch_A;
    elem::Display(result);

    compute_sketch_matrix(Sparse, A, pi_sketch);
    expected_A = Mult_AnXBn_Synch<PTDD, double, col_t>(
            pi_sketch, A, false, false);

    compare_result(rank, expected_A, result);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> DistMatrix[VC/*] <]

    typedef elem::DistMatrix<double, elem::VC, elem::STAR> vcs_target_t;

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, vcs_target_t> SparseVC(n, n_s, context);

    //[> 2. Create space for the sketched matrix <]
    vcs_target_t sketch_A_vcs(n_s, m, grid);
    elem::Zero(sketch_A_vcs);

    //[> 3. Apply the transform <]
    SparseVC.apply(A, sketch_A_vcs, skylark::sketch::columnwise_tag());

    //[> 4. Build structure to compare <]
    // easier to check if all processors own result
    result = sketch_A_vcs;

    compute_sketch_matrix(SparseVC, A, pi_sketch);
    expected_A = Mult_AnXBn_Synch<PTDD, double, col_t>(
            pi_sketch, A, false, false);

    compare_result(rank, expected_A, result);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> DistMatrix[*/VR] <]

    typedef elem::DistMatrix<double, elem::STAR, elem::VR> svr_target_t;

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, svr_target_t> SparseVR(n, n_s, context);

    //[> 2. Create space for the sketched matrix <]
    svr_target_t sketch_A_svr(n_s, m, grid);
    elem::Zero(sketch_A_svr);

    //[> 3. Apply the transform <]
    SparseVR.apply(A, sketch_A_svr, skylark::sketch::columnwise_tag());

    //[> 4. Build structure to compare <]
    // easier to check if all processors own result
    result = sketch_A_svr;

    compute_sketch_matrix(SparseVR, A, pi_sketch);
    expected_A = Mult_AnXBn_Synch<PTDD, double, col_t>(
            pi_sketch, A, false, false);

    compare_result(rank, expected_A, result);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> DistMatrix[*/*] <]

    typedef elem::DistMatrix<double, elem::STAR, elem::STAR> st_target_t;

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, st_target_t> SparseST(n, n_s, context);

    //[> 2. Create space for the sketched matrix <]
    st_target_t sketch_A_st(n_s, m, grid);
    elem::Zero(sketch_A_st);

    //[> 3. Apply the transform <]
    SparseST.apply(A, sketch_A_st, skylark::sketch::columnwise_tag());

    //[> 4. Compare <]
    compute_sketch_matrix(SparseST, A, pi_sketch);
    expected_A = Mult_AnXBn_Synch<PTDD, double, col_t>(
            pi_sketch, A, false, false);

    compare_result(rank, expected_A, sketch_A_st);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> LocalDenseMatrix <]

    Dummy_t<DistMatrixType, elem::Matrix<double>> LocalSparse(n, n_s, context);
    elem::Matrix<double> local_sketch_A(n_s, m);
    elem::Zero(local_sketch_A);
    LocalSparse.apply(A, local_sketch_A, skylark::sketch::columnwise_tag());

    elem::Matrix<double> pi_sketch_l(n_s, n);
    elem::Zero(pi_sketch_l);
    elem::Matrix<double> expected_A_l(n_s, m);
    elem::Zero(expected_A_l);

    if(rank == 0) {
        // PI generated by random number gen
        std::vector<size_t> row_idx = LocalSparse.getRowIdx();
        std::vector<double> row_val = LocalSparse.getRowValues();

        int sketch_size = row_val.size();
        typename LocalMatrixType::coords_t coords;

        for(int i = 0; i < sketch_size; ++i)
            pi_sketch_l.Set(row_idx[i], i, row_val[i]);

        elem::Gemm(elem::NORMAL, elem::NORMAL, 1.0, pi_sketch_l, local_A,
                   0.0, expected_A_l);

        for(int col = 0; col < expected_A_l.Width(); col++) {
            for(int row = 0; row < expected_A_l.Height(); row++) {
                if(local_sketch_A.Get(row, col) !=
                        expected_A_l.Get(row, col))
                    BOOST_FAIL("Result of local colwise application not as expected");
            }
        }
    }

    //////////////////////////////////////////////////////////////////////////
    //[> Row wise application DistSparseMatrix -> DistMatrix[MC/MR] <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, mcmr_target_t> Sparse_r(m, m_s, context);

    //[> 2. Create space for the sketched matrix <]
    mcmr_target_t sketch_A_r(n, m_s, grid);
    elem::Zero(sketch_A_r);

    //[> 3. Apply the transform <]
    Sparse_r.apply(A, sketch_A_r, skylark::sketch::rowwise_tag());

    //[> 4. Build structure to compare <]
    // easier to check if all processors own result
    result = sketch_A_r;

    compute_sketch_matrix(Sparse_r, A, pi_sketch_r);
    pi_sketch_r.Transpose();
    expected_AR = Mult_AnXBn_Synch<PTDD, double, col_t>(
            A, pi_sketch_r, false, false);

    compare_result(rank, expected_AR, result);

    //////////////////////////////////////////////////////////////////////////
    //[> Row wise application DistSparseMatrix -> DistMatrix[VC/*] <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, vcs_target_t> Sparse_r_vcs(m, m_s, context);

    //[> 2. Create space for the sketched matrix <]
    vcs_target_t sketch_A_r_vcs(n, m_s, grid);
    elem::Zero(sketch_A_r_vcs);

    //[> 3. Apply the transform <]
    Sparse_r_vcs.apply(A, sketch_A_r_vcs, skylark::sketch::rowwise_tag());

    //[> 4. Build structure to compare <]
    // easier to check if all processors own result
    result = sketch_A_r_vcs;

    pi_sketch_r.Transpose();
    compute_sketch_matrix(Sparse_r_vcs, A, pi_sketch_r);
    pi_sketch_r.Transpose();
    expected_AR = Mult_AnXBn_Synch<PTDD, double, col_t>(
            A, pi_sketch_r, false, false);

    compare_result(rank, expected_AR, result);

    //////////////////////////////////////////////////////////////////////////
    //[> Row wise application DistSparseMatrix -> LocalDenseMatrix <]

    Dummy_t<DistMatrixType, elem::Matrix<double>> LocalSparse_r(m, m_s, context);
    elem::Matrix<double> local_sketch_A_r(n, m_s);
    elem::Zero(local_sketch_A_r);
    LocalSparse_r.apply(A, local_sketch_A_r, skylark::sketch::rowwise_tag());

    elem::Matrix<double> local_pi_sketch_r(m_s, m);
    elem::Zero(local_pi_sketch_r);
    elem::Matrix<double> expected_A_r(n, m_s);
    elem::Zero(expected_A_r);

    if(rank == 0) {
        // PI generated by random number gen
        std::vector<size_t> row_idx = LocalSparse_r.getRowIdx();
        std::vector<double> row_val = LocalSparse_r.getRowValues();

        int sketch_size = row_val.size();
        typename LocalMatrixType::coords_t coords;

        for(int i = 0; i < sketch_size; ++i)
            local_pi_sketch_r.Set(row_idx[i], i, row_val[i]);

        elem::Gemm(elem::NORMAL, elem::TRANSPOSE, 1.0, local_A, local_pi_sketch_r,
                   0.0, expected_A_r);

        for(int col = 0; col < expected_A_r.Width(); col++) {
            for(int row = 0; row < expected_A_r.Height(); row++) {
                if(local_sketch_A_r.Get(row, col) !=
                        expected_A_r.Get(row, col))
                    BOOST_FAIL("Result of local rowwise application not as expected");
            }
        }
    }

    return 0;
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#include <boost/qvm/mat_operations.hpp>
#include "test_qvm_matrix.hpp"
#include "test_qvm_vector.hpp"
#include "gold.hpp"

namespace
    {
    template <int D>
    void
    test_x()
        {
        using namespace boost::qvm;
        test_qvm::vector<V1,3> axis; axis.a[0]=1;
        for( float r=0; r<6.28f; r+=0.5f )
            {
            test_qvm::matrix<M1,D,D> const m1=rot_mat<D>(axis,r);
            test_qvm::rotation_x(m1.b,r);
            BOOST_QVM_TEST_EQ(m1.a,m1.b);
            test_qvm::matrix<M1,D,D> m2(42,1);
            set_rot(m2,axis,r);
            test_qvm::rotation_x(m2.b,r);
            BOOST_QVM_TEST_EQ(m2.a,m2.b);
            test_qvm::matrix<M1,D,D> m3(42,1);
            test_qvm::matrix<M1,D,D> m4(42,1);
            rotate(m3,axis,r);
            m3 = m3*m1;
            BOOST_QVM_TEST_EQ(m3.a,m3.a);
            }
        }

    template <int D>
    void
    test_y()
        {
        using namespace boost::qvm;
        test_qvm::vector<V1,3> axis; axis.a[1]=1;
        for( float r=0; r<6.28f; r+=0.5f )
            {
            test_qvm::matrix<M1,D,D> m1=rot_mat<D>(axis,r);
            test_qvm::rotation_y(m1.b,r);
            BOOST_QVM_TEST_EQ(m1.a,m1.b);
            test_qvm::matrix<M1,D,D> m2(42,1);
            set_rot(m2,axis,r);
            test_qvm::rotation_y(m2.b,r);
            BOOST_QVM_TEST_EQ(m2.a,m2.b);
            test_qvm::matrix<M1,D,D> m3(42,1);
            test_qvm::matrix<M1,D,D> m4(42,1);
            rotate(m3,axis,r);
            m3 = m3*m1;
            BOOST_QVM_TEST_EQ(m3.a,m3.a);
            }
        }

    template <int D>
    void
    test_z()
        {
        using namespace boost::qvm;
        test_qvm::vector<V1,3> axis; axis.a[2]=1;
        for( float r=0; r<6.28f; r+=0.5f )
            {
            test_qvm::matrix<M1,D,D> m1=rot_mat<D>(axis,r);
            test_qvm::rotation_z(m1.b,r);
            BOOST_QVM_TEST_EQ(m1.a,m1.b);
            test_qvm::matrix<M1,D,D> m2(42,1);
            set_rot(m2,axis,r);
            test_qvm::rotation_z(m2.b,r);
            BOOST_QVM_TEST_EQ(m2.a,m2.b);
            test_qvm::matrix<M1,D,D> m3(42,1);
            test_qvm::matrix<M1,D,D> m4(42,1);
            rotate(m3,axis,r);
            m3 = m3*m1;
            BOOST_QVM_TEST_EQ(m3.a,m3.a);
            }
        }
    }

int
main()
    {
    test_x<3>();
    test_y<3>();
    test_z<3>();
    test_x<4>();
    test_y<4>();
    test_z<4>();
    test_x<5>();
    test_y<5>();
    test_z<5>();
    return boost::report_errors();
    }


#include <aslam/backend/DenseQrLinearSystemSolver.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <Eigen/Dense> // householderQr.solve
#include <sm/PropertyTree.hpp>

namespace aslam {
  namespace backend {

    DenseQrLinearSystemSolver::DenseQrLinearSystemSolver(const DenseQRLinearSolverOptions& options) :
        _options(options) {
    }

  DenseQrLinearSystemSolver::DenseQrLinearSystemSolver(const sm::PropertyTree& /* config */) {
      // NO OPTIONS CURRENTLY IMPLEMENTED
      // USING C++11 would allow to do constructor delegation and more elegant code
    }

    DenseQrLinearSystemSolver::~DenseQrLinearSystemSolver()
    {
    }

    const Matrix* DenseQrLinearSystemSolver::Jacobian() const
    {
      return &_J;
    }

  void DenseQrLinearSystemSolver::initMatrixStructureImplementation(const std::vector<DesignVariable*>& /* dvs */, const std::vector<ErrorTerm*>& /* errors */, bool /* useDiagonalConditioner */)
    {
      // \todo Verify that this is similar to the "reserve()" feature in a standard vector.
      _J._M.resize(_JRows, _JCols);
    }

    void DenseQrLinearSystemSolver::buildSystem(size_t nThreads, bool useMEstimator)
    {
      _J._M.setZero();
      setupThreadedJob(boost::bind(&DenseQrLinearSystemSolver::evaluateJacobians, this, _1, _2, _3, _4), nThreads, useMEstimator);
      _rhs = _J._M.transpose() * _e;
    }

    bool DenseQrLinearSystemSolver::solveSystem(Eigen::VectorXd& outDx)
    {
      if (_useDiagonalConditioner) {
        // Append the diagonal. Thanks to the ceres developers for this trick.
        _J._M.conservativeResize(_JRows + _JCols, Eigen::NoChange);
        _J._M.bottomRows(_JCols) = _diagonalConditioner.asDiagonal();
        // Append zeros to make the sizes match.
        _e.conservativeResize(_JRows + _JCols);
        _e.tail(_JCols) = Eigen::VectorXd::Zero(_JCols);
      }
      outDx = _J._M.colPivHouseholderQr().solve(_e);
      if (_useDiagonalConditioner) {
        // Remove the diagonal
        _J._M.conservativeResize(_JRows, Eigen::NoChange);
        _e.conservativeResize(_JRows);
      }
      return true;
    }

  void DenseQrLinearSystemSolver::evaluateJacobians(size_t /* threadId */, size_t startIdx, size_t endIdx, bool useMEstimator)
    {
      for (size_t i = startIdx; i < endIdx; ++i) {
        JacobianContainerSparse<Eigen::Dynamic> jc(_errorTerms[i]->dimension());
        ErrorTerm* e = _errorTerms[i];
        e->getWeightedJacobians(jc, useMEstimator);
        auto it = jc.begin();
        for (; it != jc.end(); ++it) {
          _J._M.block(e->rowBase(), it->first->columnBase(), it->second.rows(), it->second.cols()) = it->second;
        }
      }
    }

    const DenseQRLinearSolverOptions&
    DenseQrLinearSystemSolver::getOptions() const {
      return _options;
    }

    DenseQRLinearSolverOptions&
    DenseQrLinearSystemSolver::getOptions() {
      return _options;
    }

    void DenseQrLinearSystemSolver::setOptions(
        const DenseQRLinearSolverOptions& options) {
      _options = options;
    }

    double DenseQrLinearSystemSolver::rhsJtJrhs() {
        Eigen::VectorXd Jrhs;
        _J.rightMultiply(_rhs, Jrhs);
        return Jrhs.squaredNorm();
    }

    const Eigen::MatrixXd& DenseQrLinearSystemSolver::getJacobian() const
    {
     return _J._M;
    }

  } // namespace backend
} // namespace aslam