/**************************************************************************\
|
|    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 <string>
#include <stdexcept>
#include <cmath>

#include <boost/lexical_cast.hpp>
#include <boost/filesystem/operations.hpp>

#include <hashclash/saveload_gz.hpp>
#include <hashclash/md5detail.hpp>
#include <hashclash/booleanfunction.hpp>
#include <hashclash/rng.hpp>

#include "main.hpp"

#define HASHCLASH_MD5COMPRESS_STEP(f, a, b, c, d, m, ac, rc) \
        a += f(b, c, d) + m + ac; a = rotate_left(a,rc); a += b;

double testnearcollprob(uint32 dm11, uint32 diffcd, uint32 diffb) {
        uint32 count = 0;
        for (unsigned k = 0; k < (1<<23); ++k)
        {
                uint32 a = xrng64(), b = xrng64()+xrng64()*11, c = xrng64(), d = xrng64()+xrng64()*11;
                uint32 a2 = a, b2 = b, c2 = c, d2 = d;
                uint32 m11 = xrng64(), m2 = xrng64(), m9 = xrng64();
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, m11, 0xbd3af235, 10);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, m2, 0x2ad7d2bb, 15);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, m9, 0xeb86d391, 21);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d2, a2, b2, c2, m11+dm11, 0xbd3af235, 10);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c2, d2, a2, b2, m2, 0x2ad7d2bb, 15);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b2, c2, d2, a2, m9, 0xeb86d391, 21);
                if (d2-d==diffcd && c2-c==diffcd && b2-b==diffb)
                        ++count;
        }
        return double(count)/double(1<<23);
}

void constructupperpath_sbcpc(differentialpath& path, uint32 dm[16], uint32 diffihv[4])
{
	uint32 a,b,c,d,a2,b2,c2,d2,oa,ob,oc,od,m11,m2,m9,m4;
	a = xrng64(); b = xrng64()+xrng64()*11; c = xrng64(); d = xrng64()+xrng64()*11;
	while (true) {
                oa = a2 = a; ob = b2 = b; oc = c2 = c; od = d2 = d;
                m4 = xrng64();
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, m4, 0xf7537e82,  6);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a2, b2, c2, d2, m4+dm[4], 0xf7537e82,  6);
		if (a2-a != diffihv[0]) {
			b = xrng64(); c = xrng64(); d = xrng64();
			continue;
		}
		m11 = xrng64();
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, m11, 0xbd3af235, 10);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d2, a2, b2, c2, m11+dm[11], 0xbd3af235, 10);
		if (d2-d != diffihv[3]) {
			b = xrng64(); c = xrng64();
			continue;
		}
		m2 = xrng64();
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, m2, 0x2ad7d2bb, 15);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c2, d2, a2, b2, m2+dm[2], 0x2ad7d2bb, 15);
		if (c2-c != diffihv[2]) {
			b = xrng64();
			continue;
		}
		m9 = xrng64();
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, m9, 0xeb86d391, 21);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b2, c2, d2, a2, m9+dm[9], 0xeb86d391, 21);
		if (b2-b == diffihv[1]) break;
        }
	for (unsigned i = 55; i <= 64; ++i)
		path[i].clear();
	path[61] = sdr(a,a2);
	path[62] = sdr(d,d2);
	path[63] = sdr(c,c2);
	path[64] = sdr(b,b2);

	a2 = oa; b2 = ob; c2 = oc; d2 = od;
	a = oa; b = ob; c = oc; d = od;
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, m4, 0xf7537e82,  6);
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, a2, b2, c2, d2, m4+dm[4], 0xf7537e82,  6);
	sdr dF61 = sdr(md5_ii(a,b,c),md5_ii(a2,b2,c2));
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, m11, 0xbd3af235, 10);
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, d2, a2, b2, c2, m11+dm[11], 0xbd3af235, 10);
	sdr dF62 = sdr(md5_ii(d,a,b),md5_ii(d2,a2,b2));
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, m2, 0x2ad7d2bb, 15);
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, c2, d2, a2, b2, m2+dm[2], 0x2ad7d2bb, 15);
	sdr dF63 = sdr(md5_ii(c,d,a),md5_ii(c2,d2,a2));

	for (unsigned b = 0; b < 32; ++b) {
		bitcondition f61 = bc_constant, f62 = bc_constant, f63 = bc_constant;
		if (dF61.get(b) == +1) f61 = bc_plus;
		if (dF61.get(b) == -1) f61 = bc_minus;
		if (dF62.get(b) == +1) f62 = bc_plus;
		if (dF62.get(b) == -1) f62 = bc_minus;
		if (dF63.get(b) == +1) f63 = bc_plus;
		if (dF63.get(b) == -1) f63 = bc_minus;
		bf_conditions bc0 = MD5_I_data.forwardconditions(path[61][b], bc_constant, bc_constant, f61);
		bf_conditions bc1 = MD5_I_data.forwardconditions(path[62][b], bc0.first, bc0.second, f62);
		bf_conditions bc2 = MD5_I_data.forwardconditions(path[63][b], bc1.first, bc1.second, f63);
		path.setbitcondition(59, b, bc0.third);
		path.setbitcondition(60, b, bc1.third);
		path.setbitcondition(61, b, bc2.third);
		path.setbitcondition(62, b, bc2.second);
		path.setbitcondition(63, b, bc2.first);
	}
}

void constructupperpath(differentialpath& path, uint32 dm11, uint32 diffcd, uint32 diffb)
{
	uint32 a,b,c,d,a2,b2,c2,d2,oa,ob,oc,od,m11,m2,m9;
	while (true) {
                a = xrng64(); b = xrng64()+xrng64()*11; c = xrng64(); d = xrng64()+xrng64()*11;
                a2 = a; b2 = b; c2 = c; d2 = d;
                oa = a; ob = b; oc = c; od = d;
                m11 = xrng64(); m2 = xrng64(); m9 = xrng64();
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, m11, 0xbd3af235, 10);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, m2, 0x2ad7d2bb, 15);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, m9, 0xeb86d391, 21);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, d2, a2, b2, c2, m11+dm11, 0xbd3af235, 10);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, c2, d2, a2, b2, m2, 0x2ad7d2bb, 15);
                HASHCLASH_MD5COMPRESS_STEP(md5_ii, b2, c2, d2, a2, m9, 0xeb86d391, 21);
                if (d2-d==diffcd && c2-c==diffcd && b2-b==diffb)
                	break;
        }
	for (unsigned i = 58; i <= 64; ++i)
		path[i].clear();
	path[62] = sdr(d,d2);
	path[63] = sdr(c,c2);
	path[64] = sdr(b,b2);

	a2 = oa; b2 = ob; c2 = oc; d2 = od;
	a = oa; b = ob; c = oc; d = od;
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, m11, 0xbd3af235, 10);
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, d2, a2, b2, c2, m11+dm11, 0xbd3af235, 10);
	sdr dF62 = sdr(md5_ii(d,a,b),md5_ii(d2,a2,b2));
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, m2, 0x2ad7d2bb, 15);
	HASHCLASH_MD5COMPRESS_STEP(md5_ii, c2, d2, a2, b2, m2, 0x2ad7d2bb, 15);
	sdr dF63 = sdr(md5_ii(c,d,a),md5_ii(c2,d2,a2));

	for (unsigned b = 0; b < 32; ++b) {
		bitcondition f62 = bc_constant, f63 = bc_constant;
		if (dF62.get(b) == +1) f62 = bc_plus;
		if (dF62.get(b) == -1) f62 = bc_minus;
		if (dF63.get(b) == +1) f63 = bc_plus;
		if (dF63.get(b) == -1) f63 = bc_minus;
		bf_conditions bc1 = MD5_I_data.forwardconditions(path[62][b], bc_constant, bc_constant, f62);
		bf_conditions bc2 = MD5_I_data.forwardconditions(path[63][b], bc1.first, bc1.second, f63);
		path.setbitcondition(60, b, bc1.third);
		path.setbitcondition(61, b, bc2.third);
		path.setbitcondition(62, b, bc2.second);
		path.setbitcondition(63, b, bc2.first);
	}
}

int startnearcollision(parameters_type& parameters)
{
	uint32 ihv1[4];
	uint32 ihv2[4];
	uint32 msg1[16];
	uint32 msg2[16];
	{
		ifstream if1(parameters.infile1.c_str(), ios::binary);
		if (!if1) {
			cerr << "Error: cannot open inputfile 1 '" << parameters.infile1 << "'!" << endl;
			return 1;
		}
		ifstream if2(parameters.infile2.c_str(), ios::binary);
		if (!if2) {
			cerr << "Error: cannot open inputfile 2 '" << parameters.infile2 << "'!" << endl;
			return 1;
		}

		for (unsigned k = 0; k < 4; ++k)
			ihv1[k] = ihv2[k] = md5_iv[k];

		// load, md5 and save inputfile1
		unsigned file1blocks = 0;
		while (load_block(if1, msg1) > 0) {
			md5compress(ihv1, msg1);
			++file1blocks;
		}
		// load, md5 and save inputfile2
		unsigned file2blocks = 0;
		while (load_block(if2, msg2) > 0) {
			md5compress(ihv2, msg2);
			++file2blocks;
		}
		if (file1blocks != file2blocks) {
			cerr << "Error: inputfile 1 and 2 are not of equal size" << endl;
			return 2;
		}
	}

	cout << "IHV1   = {" << ihv1[0] << "," << ihv1[1] << "," << ihv1[2] << "," << ihv1[3] << "}" << endl;
	cout << "IHV1   = " << hex;
	for (unsigned k = 0; k < 4; ++k)
		for (unsigned c = 0; c < 4; ++c)
		{
			cout.width(2); cout.fill('0');
			cout << ((ihv1[k]>>(c*8))&0xFF);
		}
	cout << dec << endl << endl;

	cout << "IHV2   = {" << ihv2[0] << "," << ihv2[1] << "," << ihv2[2] << "," << ihv2[3] << "}" << endl;
	cout << "IHV2   = " << hex;
	for (unsigned k = 0; k < 4; ++k)
		for (unsigned c = 0; c < 4; ++c)
		{
			cout.width(2); cout.fill('0');
			cout << ((ihv2[k]>>(c*8))&0xFF);
		}
	cout << dec << endl << endl;

	uint32 dihv[4] = { ihv2[0] - ihv1[0], ihv2[1] - ihv1[1], ihv2[2] - ihv1[2], ihv2[3] - ihv1[3] };
	cout << "dIHV   = {" << dihv[0] << "," << dihv[1] << "," << dihv[2] << "," << dihv[3] << "}" << endl;

	bool sbcpc = false;
	if (dihv[0]==0 && dihv[1]==0 && dihv[2]==0 && dihv[3]==0)
	{
		cerr << "dIHV is zero!" << endl;
		return 4;
	}
	if (dihv[0] == +(1<<5) && dihv[3] == +(1<<5) -(1<<25))
		sbcpc = true;
	else if (dihv[0] != 0 || dihv[2] != dihv[3])
	{
		cerr << "Error: dIHV is not of the required form dIHV[0]=0, dIHV[2]=dIHV[3]" << endl;
		return 3;
	}
	uint32 m_diff[16] = { 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
	vector<uint32> dm11;
	vector< pair<uint32,uint32> > ncdiff;
	unsigned j = 0;
	if (sbcpc) {
		m_diff[11] = 1<<15;
		m_diff[4] = 1<<31;
		m_diff[14] = 1<<31;
		m_diff[2] = 1<<8;
	} else {
		sdr diffcd = naf(0-dihv[3]);
		sdr diffb  = naf(dihv[3] - dihv[1]);
		unsigned bb = 0;
		while (bb < 32) {
			if (diffcd.get(bb) != 0) {
				unsigned bend=bb+1;
				while (bend < 32 && bend<=bb+parameters.pathtyperange && diffcd.get(bend)==0)
					++bend;
				uint32 diff1 = 1<<bb;
				if (diffcd.get(bb) == -1)
					diff1 = -(1<<bb);
				uint32 diff2 = diff1;
				for (unsigned b2 = bb; b2 < bend; ++b2)
					if (diffb.get((b2+21)%32) == +1)
						diff2 += 1<<((b2+21)%32);
					else if (diffb.get((b2+21)%32) == -1)
						diff2 -= 1<<((b2+21)%32);
				ncdiff.push_back( pair<uint32,uint32>(diff1,diff2) );
				bb = bend;
			} else if (diffb.get((bb+21)%32) != 0) {
				uint32 diff1 = 0, diff2 = 0;
				unsigned bend=bb+1;
				unsigned tempinc=0;
				if (bb+1 < 32 && diffcd.get(bb+1) != 0) {
					if (diffcd.get(bb+1) == +1)
						diff1 += 1<<bb;
					else
						diff1 -= 1<<bb;
					ncdiff.push_back( pair<uint32,uint32>(diff1,diff1) );
					bend=bb+2;
					tempinc=1;
				} else {
					diff1 = 1<<bb;
					if (diffb.get((bb+21)%32) == +1)
						diff1 = 0-(1<<bb);
					ncdiff.push_back( pair<uint32,uint32>(0-diff1,0-diff1) );
				}
				diff2 = diff1;
				while (bend < 32 && bend<=bb+parameters.pathtyperange+tempinc && diffcd.get(bend)==0)
					++bend;
				for (unsigned b2 = bb; b2 < bend; ++b2)
					if (diffb.get((b2+21)%32) == +1)
						diff2 += 1<<((b2+21)%32);
					else if (diffb.get((b2+21)%32) == -1)
						diff2 -= 1<<((b2+21)%32);
				ncdiff.push_back( pair<uint32,uint32>(diff1,diff2) );
				bb = bend;
			} else ++bb;
		}
		if (ncdiff.size()> 1 && 0!=naf(ncdiff[ncdiff.size()-1].first).get(31) && 0!=(ncdiff[0].first&1)) {
			ncdiff[0].first += ncdiff[ncdiff.size()-1].first;
			ncdiff[0].second += ncdiff[ncdiff.size()-1].second;
			ncdiff.pop_back();
		}
		uint32 temp1 = 0, temp2 = 0;
		for (unsigned i = 0; i < ncdiff.size(); ++i)
		{
			cout << "NC" << i << ": delta c = delta d = " << naf(ncdiff[i].first) << endl;
			cout << "NC" << i << ": delta b           = " << naf(ncdiff[i].second) << endl;
			temp1 += ncdiff[i].first;
			temp2 += ncdiff[i].second;
		}
		if (dihv[3] != uint32(0-temp1) || dihv[1] != uint32(0-temp2)) {
			cerr << "Internal error in determining near-collision blocks:" << endl;
			cerr << dihv[3] << "=?" << uint32(0-temp1) << endl;
			cerr << dihv[1] << "=?" << uint32(0-temp2) << endl;
		}
		dm11.resize(ncdiff.size(),0);
		vector<double> prob(ncdiff.size(),0);
		for (unsigned i = 0; i < ncdiff.size(); ++i)
		{
			sdr fnaf = naf(ncdiff[i].first);
			for (unsigned b = 0; b < 32; ++b)
				if (fnaf.get(b) == +1) {
					dm11[i] = uint32(1)<<((b-10)%32);
					break;
				} else if (fnaf.get(b) == -1) {
					dm11[i] = 0-(uint32(1)<<((b-10)%32));
					break;
				}
			if (fnaf.get(0) != 0 && fnaf.get(31) != 0) {
				if (fnaf.get(0) == +1)
					dm11[i] = uint32(1)<<21;
				else
					dm11[i] = 0-(uint32(1)<<21);
			}
			prob[i] = testnearcollprob(dm11[i], ncdiff[i].first, ncdiff[i].second);
			cout << "NC" << i << ": prob=" << log(prob[i])/log(double(2)) << endl;
		}
		j = 0;
		for (unsigned i = 1; i < prob.size(); ++i)
			if (prob[i] > prob[j]) j = i;
		unsigned skipnum = parameters.skipnc % ncdiff.size();
		for (unsigned skip = 0; skip < skipnum; ++skip) {
			swap(dm11[j],dm11[dm11.size()-1]); dm11.pop_back();
			swap(ncdiff[j],ncdiff[ncdiff.size()-1]); ncdiff.pop_back();
			swap(prob[j],prob[prob.size()-1]); prob.pop_back();
			j = 0;
			for (unsigned i = 1; i < prob.size(); ++i)
				if (prob[i] > prob[j]) j = i;
		}
		unsigned b = 0;
		int m11sign = +1;
		for (bb = 0; bb < 32; ++bb)
			if (naf(dm11[j]).get(bb) != 0) {
				b = bb;
				if (naf(dm11[j]).get(bb) == -1)
					m11sign = -1;
				break;
			}

		cout << "delta m_11 = " << naf(dm11[j]) << endl;
		m_diff[11] = dm11[j];
	}

	/*** Construct lower diff. path ***/
	differentialpath lowerpath;
	uint32 Q1[4] = { ihv1[0], ihv1[3], ihv1[2], ihv1[1] };
	uint32 Q2[4] = { ihv2[0], ihv2[3], ihv2[2], ihv2[1] };
	for (int i = 0; i < 4; ++i)
		for (unsigned k = 0; k < 32; ++k)
		{
			if ((Q1[i]>>k)&1) {
				if ((Q2[i]>>k)&1)
					lowerpath.setbitcondition(i-3,k,bc_one);
				else
					lowerpath.setbitcondition(i-3,k,bc_minus);
			} else {
				if ((Q2[i]>>k)&1)
					lowerpath.setbitcondition(i-3,k,bc_plus);
				else
					lowerpath.setbitcondition(i-3,k,bc_zero);
			}
		}
	uint32 dF = 0;
	for (unsigned k = 0; k < 32; ++k)
	{
		bf_outcome outcome = MD5_F_data.outcome(lowerpath(0,k), lowerpath(-1,k), lowerpath(-2,k));
		if (outcome.size()) {
			if (outcome[0] == bc_plus) 			dF += 1<<k;
			else if (outcome[0] == bc_minus)	dF -= 1<<k;
		}
	}
	uint32 dQtm3 = lowerpath[-3].diff();
	uint32 dT = dQtm3 + dF + m_diff[0];
	std::vector<std::pair<uint32,double> > rotateddiff;
	rotate_difference(dT, md5_rc[0], rotateddiff);
	double bestrot = 0;
	for (unsigned i = 0; i < rotateddiff.size(); ++i)
	{
		uint32 dR = rotateddiff[i].first;
		wordconditions Q1 = naf(lowerpath[0].diff() + dR);
		rotateddiff[i].second = check_rotation(dR, dT, md5_rc[0], lowerpath[0], Q1, 1<<12);
		if (rotateddiff[i].second > bestrot) {
			lowerpath[1] = Q1;
			bestrot = rotateddiff[i].second;
		}
	}

	/*** Show and store lower diff. path ***/
	show_path(lowerpath, m_diff);
	try {
		vector<differentialpath> temp;
		temp.push_back(lowerpath);
		save_gz(temp, workdir +  "/lowerpath", binary_archive);
		cout << "Saved lower diff. path to '" << workdir + "/lowerpath.bin.gz'." << endl;
	} catch (...) {
		cerr << "Error: could not write '" << workdir + "/lowerpath.bin.gz'!" << endl;
	}
	cout << endl;

	/*** Construct upper diff. path ***/
	differentialpath upperpath, bestpath;
	unsigned bestcond = 1<<31;
	for (unsigned i = 0; i < 1<<10; ++i) {
		if (sbcpc) {
			uint32 dihvmin[4] = { -dihv[0], -dihv[1], -dihv[2], -dihv[3] };
			constructupperpath_sbcpc(upperpath, m_diff, dihvmin);
		} else {
			upperpath[32].clear();
			constructupperpath(upperpath, dm11[j], ncdiff[j].first, ncdiff[j].second);
		}
		if (upperpath.nrcond() < bestcond) {
			bestcond = upperpath.nrcond();
			bestpath = upperpath;
		}
	}
	upperpath = bestpath;

	/*** Show and store upper diff. path ***/
	show_path(upperpath, m_diff);
	try {
		vector<differentialpath> temp;
		temp.push_back(upperpath);
		save_gz(temp, workdir +  "/upperpath", binary_archive);
		cout << "Saved upper diff. path to '" << workdir + "/upperpath.bin.gz'." << endl;
	} catch (...) {
		cerr << "Error: could not write '" << workdir + "/upperpath.bin.gz'!" << endl;
	}

	/*** Write md5diffpath_forward.cfg ***/
	{
		ofstream off("md5diffpathforward.cfg");
		if (!off)
			cerr << "Error: could not write md5diffpathforward.cfg!" << endl;
		else {
			/* write IHV info */
			off << "# IHV" << endl;
			off << "ihv0a = " << ihv1[0] << endl;
			off << "ihv1a = " << ihv1[1] << endl;
			off << "ihv2a = " << ihv1[2] << endl;
			off << "ihv3a = " << ihv1[3] << endl << endl;
			off << "# IHV'" << endl;
			off << "ihv0b = " << ihv2[0] << endl;
			off << "ihv1b = " << ihv2[1] << endl;
			off << "ihv2b = " << ihv2[2] << endl;
			off << "ihv3b = " << ihv2[3] << endl << endl;
			/* write delta m11 */
			off << "# message block difference" << endl;
			for (unsigned i = 0; i < 16; ++i)
				for (unsigned b = 0; b < 32; ++b)
					if (naf(m_diff[i]).get(b) != 0)
						off << "diffm" << i << " = " << int(b+1)*naf(m_diff[i]).get(b) << endl;
			/* other settings */
			off << "# parameters" << endl;

			/* copy template file if any */
			ifstream iff("md5diffpathforward.cfg.template");
			if (!iff)
				cerr << "Warning: could not read md5diffpathforward.cfg.template" << endl;
			else
				off << iff.rdbuf() << endl;
			cout << "Saved 'md5diffpathforward.cfg'." << endl;
		}
	}

	/*** Write md5diffpath_backward.cfg ***/
	{
		ofstream ofb("md5diffpathbackward.cfg");
		if (!ofb)
			cerr << "Error: could not write md5diffpathbackward.cfg!" << endl;
		else {
			/* write delta m11 */
			ofb << "# message block difference" << endl;
			for (unsigned i = 0; i < 16; ++i)
				for (unsigned b = 0; b < 32; ++b)
					if (naf(m_diff[i]).get(b) != 0)
						ofb << "diffm" << i << " = " << int(b+1)*naf(m_diff[i]).get(b) << endl;
			/* other settings */
			ofb << "# parameters" << endl;

			/* copy template file if any */
			ifstream ifb("md5diffpathbackward.cfg.template");
			if (!ifb)
				cerr << "Warning: could not read md5diffpathbackward.cfg.template" << endl;
			else
				ofb << ifb.rdbuf() << endl;
			cout << "Saved 'md5diffpathbackward.cfg'." << endl;
		}
	}

	/*** Write md5diffpath_connect.cfg ***/
	{
		ofstream ofc("md5diffpathconnect.cfg");
		if (!ofc)
			cerr << "Error: could not write md5diffpathconnect.cfg!" << endl;
		else {
			/* write delta m11 */
			ofc << "# message block difference" << endl;
			for (unsigned i = 0; i < 16; ++i)
				for (unsigned b = 0; b < 32; ++b)
					if (naf(m_diff[i]).get(b) != 0)
						ofc << "diffm" << i << " = " << int(b+1)*naf(m_diff[i]).get(b) << endl;
			/* other settings */
			ofc << "# parameters" << endl;

			/* copy template file if any */
			ifstream ifc("md5diffpathconnect.cfg.template");
			if (!ifc)
				cerr << "Warning: could not read md5diffpathconnect.cfg.template" << endl;
			else
				ofc << ifc.rdbuf() << endl;
			cout << "Saved 'md5diffpathconnect.cfg'." << endl;
		}
	}

	/*** Write md5diffpath_helper.cfg ***/
	{
		ofstream ofc("md5diffpathhelper.cfg");
		if (!ofc)
			cerr << "Error: could not write md5diffpathhelper.cfg!" << endl;
		else {
			/* write delta m11 */
			ofc << "# message block difference" << endl;
			for (unsigned i = 0; i < 16; ++i)
				for (unsigned b = 0; b < 32; ++b)
					if (naf(m_diff[i]).get(b) != 0)
						ofc << "diffm" << i << " = " << int(b+1)*naf(m_diff[i]).get(b) << endl;

			cout << "Saved 'md5diffpathhelper.cfg'." << endl;
		}
	}

	return 0;
}

void writeupperpath(std::string dir, parameters_type& parameters, unsigned b, int m11sign)
{
	try {
		boost::filesystem::create_directory(dir);
	} catch( std::exception& e) {
		std::cerr << e.what() << endl;
		std::cerr << "failed to create directory: " << dir << endl;
		throw;
	} catch(...) {
		std::cerr << "failed to create directory: " << dir << endl;
		throw;
	}
	bitcondition plus = bc_plus, minus = bc_minus;
	if (m11sign == -1)
		swap(plus, minus);
	uint32 m_diff[16] = { 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
	m_diff[11] = m11sign * (1<<b);

	/*** Construct upper diff. path ***/
	differentialpath upperpath;
	upperpath[32].clear();
	upperpath.setbitcondition(60, (b+10)&31, bc_zero);
	upperpath.setbitcondition(61, (b+10)&31, bc_one);
	upperpath.setbitcondition(62, (b+10)&31, plus);
	upperpath.setbitcondition(63, (b+10)&31, plus);
	upperpath.setbitcondition(64, (b+10)&31, plus);

	/*** Show and store upper diff. path ***/
	try {
		vector<differentialpath> temp;
		temp.push_back(upperpath);
		save_gz(temp, dir +  "/upperpath", binary_archive);
		//cout << "Saved upper diff. path to '" << dir + "/upperpath.bin'." << endl;
	} catch (...) {
		cerr << "Error: could not write '" << dir << "/upperpath.bin'!" << endl;
	}

	/*** Write md5diffpath_backward.cfg ***/
	{
		ofstream ofb((dir+"/md5diffpathbackward.cfg").c_str());
		if (!ofb)
			cerr << "Error: could not write " << dir << "md5diffpathbackward.cfg!" << endl;
		else {
			/* write delta m11 */
			ofb << "# message block difference" << endl;
			ofb << "diffm11 = " << m11sign*int(b+1) << endl << endl;
			/* other settings */
			ofb << "# parameters" << endl;

			/* copy template file if any */
			ifstream ifb("md5diffpathbackward.cfg.template");
			if (!ifb)
				cerr << "Error: could not read md5diffpathbackward.cfg.template" << endl;
			else {
				ofb << ifb.rdbuf() << endl;
			}
			cout << "Saved '" << dir << "md5diffpathbackward.cfg'." << endl;
		}
	}
}

int upperpaths(parameters_type& parameters)
{
	for (unsigned b = 0; b < 32; ++b)
	{
		string bitstr = boost::lexical_cast<string>(b);
		writeupperpath(workdir + "plus2power" + bitstr, parameters, b, 1);
		writeupperpath(workdir + "minus2power" + bitstr, parameters, b, -1);
	}
	return 0;
}


/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University Osnabrück 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 University Osnabrück 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.
 */

/**
 * @file MatrixTypes.hpp
 * @author Thomas Wiemann (twiemann@uos.de)
 * @date 2019-08-14
 *
 * @copyright Copyright (c) 2019
 *
 */

#ifndef LVR2_TYPES_MATRIXTYPES_HPP
#define LVR2_TYPES_MATRIXTYPES_HPP

#include <Eigen/Dense>

namespace lvr2
{
/// General alias for row major 4x4 matrices
template<typename T>
using Matrix4RM = Eigen::Matrix<T, 4, 4, Eigen::RowMajor>;

/// 4x4 row major matrix with float scalars
using Matrix4fRM = Matrix4RM<float>;

/// 4x4 row major matrix with double scalars
using Matrix4dRM = Matrix4RM<double>;

/// General alias for row major 3x3 matrices
template<typename T>
using Matrix3RM = Eigen::Matrix<T, 3, 3, Eigen::RowMajor>;

/// 3x3 row major matrix with float scalars
using Matrix3fRM = Matrix3RM<float>;
/// 3x3 row major matrix with double scalars
using Matrix3dRM = Matrix3RM<double>;

/// General 4x4 transformation matrix (4x4)
template<typename T>
using Transform = Eigen::Matrix<T, 4, 4>;

/// 4x4 single precision transformation matrix
using Transformf = Transform<float>;

/// 4x4 double precision transformation matrix
using Transformd = Transform<double>;

/// General 3x3 rotation matrix
template<typename T>
using Rotation = Eigen::Matrix<T, 3, 3>;

/// Single precision 3x3 rotation matrix
using Rotationf = Rotation<float>;

/// Double precision 3x3 rotation matrix
using Rotationd = Rotation<double>;

/// 4x4 extrinsic calibration
template<typename T>
using Extrinsics = Eigen::Matrix<T, 4, 4>;

/// 4x4 extrinsic calibration (single precision)
using Extrinsicsf = Extrinsics<float>;

/// 4x4 extrinsic calibration (double precision)
using Extrinsicsd = Extrinsics<double>;

/// 4x4 extrinsic calibration
template<typename T>
using Intrinsics = Eigen::Matrix<T, 3, 3>;

/// 4x4 intrinsic calibration (single precision)
using Intrinsicsf = Intrinsics<float>;

/// 4x4 extrinsic calibration (double precision)
using Intrinsicsd = Intrinsics<double>;

/// Distortion Parameters
template<typename T>
using Distortion = Eigen::Matrix<T, 6, 1>;

/// Distortion Parameters (double precision)
using Distortiond = Distortion<double>;

/// Distortion Parameters (single precision)
using Distortionf = Distortion<float>;

/// Eigen 3D vector
template<typename T>
using Vector3 = Eigen::Matrix<T, 3, 1>;

/// Eigen 3D vector, single precision
using Vector3f = Eigen::Vector3f;

/// Eigen 3D vector, double precision
using Vector3d = Eigen::Vector3d;

/// Eigen 3D vector, integer
using Vector3i = Eigen::Vector3i;

/// Eigen 4D vector
template<typename T>
using Vector4 = Eigen::Matrix<T, 4, 1>;

/// Eigen 4D vector, single precision
using Vector4f = Eigen::Vector4f;

/// Eigen 4D vector, double precision
using Vector4d = Eigen::Vector4d;

/// Eigen 2D vector
template<typename T>
using Vector2 = Eigen::Matrix<T, 2, 1>;

/// Eigen 2D vector, single precision
using Vector2f = Eigen::Vector2f;

/// Eigen 2D vector, double precision
using Vector2d = Eigen::Vector2d;

/// Eigen 4x4 matrix, single precision
using Matrix4f = Eigen::Matrix4f;

/// Eigen 4x4 matrix, double precision
using Matrix4d = Eigen::Matrix4d;

/// 6D Matrix, single precision
using Matrix6f = Eigen::Matrix<float, 6, 6>;

/// 6D vector, single precision
using Vector6f = Eigen::Matrix<float, 6, 1>;

/// 6D matrix double precision
using Matrix6d = Eigen::Matrix<double, 6, 6>;

/// 6D vector double precision
using Vector6d = Eigen::Matrix<double, 6, 1>;

template<typename T>
Vector3<T> multiply(const Transform<T>& transform, const Vector3<T>& p)
{
    Vector4<T> ret(p.coeff(0), p.coeff(1), p.coeff(2), 1.0);
    ret = transform * ret;
    return Vector3<T>(ret.coeff(0), ret.coeff(1), ret.coeff(2));
}

} // namespace lvr2

// additional operators

template<typename T>
lvr2::Vector3<T> operator*(const lvr2::Transform<T>& transform, const lvr2::Vector3<T>& p)
{
    return lvr2::multiply(transform, p);
}

#endif // LVR2_TYPES_MATRIXTYPES_HPP


#pragma once

#include "ga/ga.h"
#include "tamm/tamm.hpp"
#include <Eigen/Dense>

namespace tamm {

template<typename T>
inline void jacobi(ExecutionContext& ec, Tensor<T>& d_r, Tensor<T>& d_t,
                   T shift, bool transpose, std::vector<double>& evl_sorted,
                   const TAMM_SIZE& n_occ_alpha, const TAMM_SIZE& n_occ_beta) {
    // EXPECTS(transpose == false);
    block_for(ec, d_r(), [&](IndexVector blockid) {
        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);

        TAMM_SIZE noab = n_occ_alpha+n_occ_beta;
        std::vector<double> p_evl_sorted_occ(noab);
        std::vector<double> p_evl_sorted_virt(evl_sorted.size()-noab);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noab,
                  p_evl_sorted_occ.begin());
        std::copy(evl_sorted.begin() + noab, evl_sorted.end(),
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 2) {
            auto ioff  = rtiss[0].tile_offset(blockid[0]);
            auto joff  = rtiss[1].tile_offset(blockid[1]);
            auto isize = rblock_dims[0];
            auto jsize = rblock_dims[1];

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff + i] +
                                     p_evl_sorted_occ[joff + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff + i] -
                                     p_evl_sorted_virt[joff + j] + shift);
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 4) {

            auto rblock_offset = d_r.block_offsets(blockid);

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

template<typename T>
inline void jacobi_cs(ExecutionContext& ec, Tensor<T>& d_r, Tensor<T>& d_t,
                   T shift, bool transpose, std::vector<double>& evl_sorted,
                   const TAMM_SIZE& n_occ_alpha, const TAMM_SIZE& n_vir_alpha,
                   const bool not_spin_orbital=false) {
    // EXPECTS(transpose == false);
    block_for(ec, d_r(), [&](IndexVector blockid) {
        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);

        TAMM_SIZE noa  = n_occ_alpha;
        TAMM_SIZE noab = n_occ_alpha + n_occ_alpha;
        TAMM_SIZE nva  = n_vir_alpha;
        std::vector<double> p_evl_sorted_occ(noa);
        std::vector<double> p_evl_sorted_virt(nva);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noa,
                  p_evl_sorted_occ.begin());
        if(not_spin_orbital)
            std::copy(evl_sorted.begin() + noa, evl_sorted.begin() + noa + nva,
                        p_evl_sorted_virt.begin());
        else
            std::copy(evl_sorted.begin() + noab, evl_sorted.begin() + noab + nva,
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 2) {
            auto ioff  = rtiss[0].tile_offset(blockid[0]);
            auto joff  = rtiss[1].tile_offset(blockid[1]);
            auto isize = rblock_dims[0];
            auto jsize = rblock_dims[1];

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff + i] +
                                     p_evl_sorted_occ[joff + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff + i] -
                                     p_evl_sorted_virt[joff + j] + shift);
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 4) {

            auto rblock_offset = d_r.block_offsets(blockid);

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

template<typename T>
inline void jacobi_eom(ExecutionContext& ec, LabeledTensor<T> d_r_lt, LabeledTensor<T> d_t_lt,
                      T shift, bool transpose, std::vector<double>& evl_sorted, const TAMM_SIZE n_occ_alpha, const TAMM_SIZE n_occ_beta) {
    // EXPECTS(transpose == false);
     Tensor<T> d_r = d_r_lt.tensor();
     Tensor<T> d_t = d_t_lt.tensor();

    block_for(ec, d_r_lt, [&](IndexVector bid) {
        IndexVector blockid   = internal::translate_blockid(bid, d_r_lt);

        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        // auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);
        auto rblock_offset = d_r.block_offsets(blockid);

        TAMM_SIZE noab = n_occ_alpha+n_occ_beta;
        std::vector<double> p_evl_sorted_occ(noab);
        std::vector<double> p_evl_sorted_virt(evl_sorted.size()-noab);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noab,
                  p_evl_sorted_occ.begin());
        std::copy(evl_sorted.begin() + noab, evl_sorted.end(),
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 3) {

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize[0]; i++) {
                    for(auto j = 0U; j < isize[1]; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff[0] + i] +
                                     p_evl_sorted_occ[ioff[1] + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize[0]; i++) {
                    for(auto j = 0U; j < isize[1]; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff[0] + i] -
                                     p_evl_sorted_virt[ioff[1] + j] + shift);
                    }
                }
            }
            // auto last_id = rtiss[2].translate(blockid[2], d_t.tiled_index_spaces()[2]);
            // blockid[2] = last_id;
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 5) {

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            // auto last_id = rtiss[4].translate(blockid[4], d_t.tiled_index_spaces()[4]);
            // blockid[4] = last_id;
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

/**
 * @brief dot product between data held in two labeled tensors. Corresponding
 * elements are multiplied.
 *
 * This routine ignores permutation symmetry, and associated symmetrizatin
 * factors
 *
 * @tparam T Type of elements in both tensors
 * @param ec Execution context in which this function is invoked
 * @param lta Labeled tensor A
 * @param ltb labeled Tensor B
 * @return dot product A . B
 */
template<typename T>
inline T ddot(ExecutionContext& ec, LabeledTensor<T> lta,
              LabeledTensor<T> ltb) {
    T ret = 0;
    block_for(ec.pg(), lta, [&](IndexVector blockid) {
        Tensor<T> atensor     = lta.tensor();
        const TAMM_SIZE asize = atensor.block_size(blockid);
        std::vector<T> abuf(asize);

        Tensor<T> btensor     = ltb.tensor();
        const TAMM_SIZE bsize = btensor.block_size(blockid);
        std::vector<T> bbuf(bsize);

        const size_t sz = asize;
        for(size_t i = 0; i < sz; i++) { ret += abuf[i] * bbuf[i]; }
    });
    return ret;
}

/**
 * @brief DIIS routine
 * @tparam T Type of element in each tensor
 * @param ec Execution context in which this function invoked
 * @param[in] d_rs Vector of R tensors
 * @param[in] d_ts Vector of T tensors
 * @param[out] d_t Vector of T tensors produced by DIIS
 * @pre d_rs.size() == d_ts.size()
 * @pre 0<=i<d_rs.size(): d_rs[i].size() == d_t.size()
 * @pre 0<=i<d_ts.size(): d_ts[i].size() == d_t.size()
 */
template<typename T>
inline void diis(ExecutionContext& ec,
                 std::vector<std::vector<Tensor<T>>>& d_rs,
                 std::vector<std::vector<Tensor<T>>>& d_ts,
                 std::vector<Tensor<T>> d_t) {

    EXPECTS(d_t.size() == d_rs.size());
    size_t ntensors = d_t.size();
    EXPECTS(ntensors > 0);
    size_t ndiis = d_rs[0].size();
    EXPECTS(ndiis > 0);
    for(auto i = 0U; i < ntensors; i++) { EXPECTS(d_rs[i].size() == ndiis); }

    using Matrix =
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using Vector =
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    Matrix A = Matrix::Zero(ndiis + 1, ndiis + 1);
    Vector b = Vector::Zero(ndiis + 1, 1);
    for(auto k = 0U; k < ntensors; k++) {
        for(auto i = 0U; i < ndiis; i++) {
            for(auto j = i; j < ndiis; j++) {
                Tensor<T> d_r1{};
                Tensor<T>::allocate(&ec,d_r1);
                Tensor<T>& t1 = d_rs[k].at(i);
                Tensor<T>& t2 = d_rs[k].at(j);
                //A(i, j) += ddot(ec, (*d_rs[k]->at(i))(), (*d_rs[k]->at(j))());
                Scheduler{ec}(d_r1() = t1() * t2()).execute();

                A(i,j) += get_scalar(d_r1);
                Tensor<T>::deallocate(d_r1);
            }
        }
    }

    for(auto i = 0U; i < ndiis; i++) {
        for(auto j = i; j < ndiis; j++) { A(j, i) = A(i, j); }
    }
    for(auto i = 0U; i < ndiis; i++) {
        A(i, ndiis) = -1.0;
        A(ndiis, i) = -1.0;
    }

    b(ndiis, 0) = -1;

    // Solve AX = B
    // call dgesv(diis+1,1,a,maxdiis+1,iwork,b,maxdiis+1,info)
    // Vector x = A.colPivHouseholderQr().solve(b);
    Vector x = A.lu().solve(b);

    auto sch = Scheduler{ec};
    for(auto k = 0U; k < ntensors; k++) {
        Tensor<T>& dt = d_t[k];
        sch(dt() = 0);
        for(auto j = 0U; j < ndiis; j++) {
            auto& tb = d_ts[k].at(j);
            sch(dt() += x(j, 0) * tb());
        }
    }
    // GA_Sync();
    sch.execute();
    // GA_Sync();
}

} // namespace tamm


///////////////////////////////////////////////////////////////////////////////
// weighted_tail_mean.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_MEAN_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_MEAN_HPP_DE_01_01_2006

#include <numeric>
#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_mean.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{

    ///////////////////////////////////////////////////////////////////////////////
    // coherent_weighted_tail_mean_impl
    //
    // TODO

    ///////////////////////////////////////////////////////////////////////////////
    // non_coherent_weighted_tail_mean_impl
    //
    /**
        @brief Estimation of the (non-coherent) weighted tail mean based on order statistics (for both left and right tails)

        An estimation of the non-coherent, weighted tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$ is given by the weighted mean
        of the

        \f[
            \lambda = \inf\left\{ l \left| \frac{1}{\bar{w}_n}\sum_{i=1}^{l} w_i \geq \alpha \right. \right\}
        \f]

        smallest samples (left tail) or the weighted mean of the

        \f[
            n + 1 - \rho = n + 1 - \sup\left\{ r \left| \frac{1}{\bar{w}_n}\sum_{i=r}^{n} w_i \geq (1 - \alpha) \right. \right\}
        \f]

        largest samples (right tail) above a quantile \f$\hat{q}_{\alpha}\f$ of level \f$\alpha\f$, \f$n\f$ being the total number of sample
        and \f$\bar{w}_n\f$ the sum of all \f$n\f$ weights:

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X) = \frac{\sum_{i=1}^{\lambda} w_i X_{i:n}}{\sum_{i=1}^{\lambda} w_i},
        \f]

        \f[
            \widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X) = \frac{\sum_{i=\rho}^n w_i X_{i:n}}{\sum_{i=\rho}^n w_i}.
        \f]

        @param quantile_probability
    */
    template<typename Sample, typename Weight, typename LeftRight>
    struct non_coherent_weighted_tail_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        typedef typename numeric::functional::fdiv<Weight, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef typename numeric::functional::fdiv<weighted_sample, std::size_t>::result_type result_type;

        non_coherent_weighted_tail_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            float_type threshold = sum_of_weights(args)
                             * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] );

            std::size_t n = 0;
            Weight sum = Weight(0);

            while (sum < threshold)
            {
                if (n < static_cast<std::size_t>(tail_weights(args).size()))
                {
                    sum += *(tail_weights(args).begin() + n);
                    n++;
                }
                else
                {
                    if (std::numeric_limits<result_type>::has_quiet_NaN)
                    {
                        return std::numeric_limits<result_type>::quiet_NaN();
                    }
                    else
                    {
                        std::ostringstream msg;
                        msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                        boost::throw_exception(std::runtime_error(msg.str()));
                        return result_type(0);
                    }
                }
            }

            return numeric::fdiv(
                std::inner_product(
                    tail(args).begin()
                  , tail(args).begin() + n
                  , tail_weights(args).begin()
                  , weighted_sample(0)
                )
              , sum
            );
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::non_coherent_weighted_tail_mean<>
//
namespace tag
{
    template<typename LeftRight>
    struct non_coherent_weighted_tail_mean
      : depends_on<sum_of_weights, tail_weights<LeftRight> >
    {
        typedef accumulators::impl::non_coherent_weighted_tail_mean_impl<mpl::_1, mpl::_2, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::non_coherent_weighted_tail_mean;
//
namespace extract
{
    extractor<tag::abstract_non_coherent_tail_mean> const non_coherent_weighted_tail_mean = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(non_coherent_weighted_tail_mean)
}

using extract::non_coherent_weighted_tail_mean;

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


#ifndef SODES_BOOSTODEINT_HPP
#define SODES_BOOSTODEINT_HPP

#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_resize.hpp>

#include <tuple>
#include <functional>
#include <vector>
#include <utility>
#include <stdexcept>

#include "math_types.hpp"

namespace sodes::detail::odeint {

using namespace boost::numeric::odeint;
using namespace Eigen;
using namespace std;

using namespace sodes::math_types;

typedef eigen_array_1d state_type;

struct Observer
{
    vector<state_type>& m_states;
    vector<double>& m_times;

    Observer(vector<state_type>& states, vector<double>& times)
        : m_states(states), m_times(times) {}

    void operator()(const state_type& x, const double& t)
    {
        m_states.push_back(x);
        m_times.push_back(t);
    }
};

template<typename Stepper>
pair<vector<double>, vector<state_type>> _integrate_const_stepper(
    const function<state_type(eigen_array_1d_constref, eigen_array_1d_ref, const double&)>& f,
    const tuple<double, double>& t_span,
    const double& dt,
    state_type& y0,
    Stepper stepper)
{
    // Containers to store solution
    vector<state_type> states;
    vector<double> times;

    // Integrate over time
    auto[t_init, t_final] = t_span;
    auto observer = Observer(states, times);
    integrate_const(
        stepper,
        f,
        y0,
        t_init,
        t_final,
        dt,
        observer);

    auto solution_pair = make_pair(times, states);

    return solution_pair;
}
}  // namespace sodes::detail::odeint

namespace sodes::odeint {

using namespace std;
using namespace Eigen;
using namespace sodes::math_types;

pair<vector<double>, vector<ArrayXd>> solve_ivp_const(
    const function<ArrayXd(eigen_array_1d_constref, eigen_array_1d_ref, const double&)>& f,
    const tuple<double, double>& t_span,
    const double& dt,
    ArrayXd& y0,
    const string& method = "runge_kutta4")
{
    using namespace sodes::detail::odeint;

    if (method == "runge_kutta4") {
        typedef runge_kutta4<state_type, double, state_type, double, vector_space_algebra> stepper;
        return _integrate_const_stepper<stepper>(f, t_span, dt, y0, stepper());
    }
//    else if (method == "runge_kutta_dopri5") {
//        typedef runge_kutta_dopri5<state_type, double, state_type, double, vector_space_algebra> stepper;
//        auto dense_output = make_dense_output<stepper>(1e-6, 1e-6);
//        return _integrate_const_stepper<dense_output>(f, t_span, dt, y0, dense_output);
//    }
    else if (method == "runge_kutta_cash_karp54") {
        typedef runge_kutta_cash_karp54<state_type, double, state_type, double, vector_space_algebra> stepper;
        return _integrate_const_stepper<stepper>(f, t_span, dt, y0, stepper());
    }
    else if (method == "runge_kutta_fehlberg78") {
        typedef runge_kutta_fehlberg78<state_type, double, state_type, double, vector_space_algebra> stepper;
        return _integrate_const_stepper<stepper>(f, t_span, dt, y0, stepper());
    }
    else if (method == "modified_midpoint") {
        typedef modified_midpoint<state_type, double, state_type, double, vector_space_algebra> stepper;
        return _integrate_const_stepper<stepper>(f, t_span, dt, y0, stepper());
    }
    else if (method == "euler") {
        typedef euler<state_type, double, state_type, double, vector_space_algebra> stepper;
        return _integrate_const_stepper<stepper>(f, t_span, dt, y0, stepper());
    }
    else {
        throw invalid_argument("Unavailable integration method from Boost::odeint");
    }
}
}  // namespace sodes::odeint

#endif //SODES_BOOSTODEINT_HPP


/*
 * ThreephaseEngine.cpp
 *
 * Copyright (c) 2014, Alessandro Pezzato
 */

#include "ThreephaseEngine.h"
#include "../../common/Logger.h"
#include "../../common/Config.h"
#include "../../projector/Projector.h"
#include "../../input/ImageInput.h"

#include <format.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <glm/gtx/transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>

/*
 * http://public.vrac.iastate.edu/~song/publications/papers/2010-ole-review.pdf
 */

namespace threescanner {

namespace /* anonymous */{

static const int WRAP_V_SIZE = 511; /* i1 - i3, -255 to 255, |511| */
static const int WRAP_U_SIZE = 1021; /* 2 * i2 - i1 - i3, -510 to 510, |1021| */
static const int WRAP_V_OFFSET = 255; /* min value is -255 */
static const int WRAP_U_OFFSET = 510; /* min value is -510 */
static unsigned char PHASE_GAMMA_LUT[WRAP_V_SIZE][WRAP_U_SIZE][2];

/*
 * initPhaseGammaLut() is automatically called once at startup
 */
void initPhaseGammaLut() __attribute__ ((constructor)); /* FIXME: only gcc is compatible */
void initPhaseGammaLut() {
	float sqrt3 = sqrtf(3.0);
	float ipi = float(128. / CV_PI);
	for (int vo = 0; vo < WRAP_V_SIZE; vo++) {
		for (int uo = 0; uo < WRAP_U_SIZE; uo++) {
			float v = float(vo - WRAP_V_OFFSET);
			float u = float(uo - WRAP_U_OFFSET);
			float phase = atan2f(sqrt3 * v, u) * ipi;
			float modulation = sqrtf(3 * v * v + u * u);
			PHASE_GAMMA_LUT[vo][uo][0] = 128 + (unsigned char) phase;
			PHASE_GAMMA_LUT[vo][uo][1] = modulation > 255 ? 255 : (unsigned char) modulation;
		}
	}
}

bool isVertical(const std::string& orientation) {
	return orientation[0] == 'v' || orientation[0] == 'V';
}

bool isHorizontal(const std::string& orientation) {
	return orientation[0] == 'h' || orientation[0] == 'H';
}

void traceMatrix(const std::string& name, const glm::mat4& mat) {
	const float* a = (const float*) glm::value_ptr(mat);
	logDebug("%16s | %8.3f %8.3f %8.3f %8.3f", name.c_str(), a[0], a[4], a[8], a[12]);
	logDebug("                 | %8.3f %8.3f %8.3f %8.3f", a[1], a[5], a[9], a[13]);
	logDebug("                 | %8.3f %8.3f %8.3f %8.3f", a[2], a[6], a[10], a[14]);
	logDebug("                 | %8.3f %8.3f %8.3f %8.3f", a[3], a[7], a[11], a[15]);
}

} /* namespace anonymous */

ThreephaseEngine::ThreephaseEngine(const Config& cfg) :
				Engine(cfg),
				wrapMethod_(cfg.get<int>("wrapMethod")),
				options_(),
				toProcess_(),
				hImages_ { },
				vImages_ { },
				phases_(hImages_),
				process_(),
				unwrapped_(),
				mask_(),
				depth_() {
	/* tuning */
	Config tuning = cfg.getChild("tuning");

	options_["mscale"] = tuning.get<float>("mscale", 200.0);
	options_["zscale"] = tuning.get<float>("zscale", 151.0);
	options_["zskew"] = tuning.get<float>("zskew", 180.0);
	options_["znoise"] = tuning.get<float>("znoise", 0.720);
	options_["zblur"] = tuning.get<float>("zblur", 16);
	options_["cloudScale"] = tuning.get<float>("cloudScale", 1.0);

}

ThreephaseEngine::~ThreephaseEngine() {

}

void ThreephaseEngine::scanSync() {
	/* TODO:
	 * should get images from ImageInput (Camera)
	 */
	if (projector_.get() == nullptr) {
		throw std::runtime_error("Cannot scan without a valid projector");
	}
	if (input_.get() == nullptr) {
		throw std::runtime_error("Cannot scan without a valid image input");
	}
	logDebug("Wait until ready");
	projector_->waitUntilReady();
	logDebug("ready");
	std::string orientation = "h"; /* TODO: from Config */
	projector_->setParameter("orientation", orientation);
	for (int phase = 1; phase <= 3; ++phase) {
		logDebug("Scan phase %i", phase);
		this->projector_->setParameter("phase", fmt::sprintf("%i", phase));
		std::this_thread::sleep_for(std::chrono::milliseconds(250));
		ImagePtr image = input_->grabImage();
		this->setImage(fmt::sprintf("%s:%i", orientation, phase), *image);
	}
	this->process(orientation);
}

void ThreephaseEngine::setImage(const std::string& orientation, const size_t& phase, const cv::Mat& image) {
	if (phase > 3) {
		throw std::runtime_error(fmt::sprintf("Threephase invalid phase: %u", phase));
	}
	cv::Mat* phases = isVertical(orientation) ? vImages_ : hImages_;
	cv::Mat& dst = phases[phase - 1];
	cv::cvtColor(image, dst, CV_RGB2GRAY);
	logDebug("Threephase set image %u", phase);
}

void ThreephaseEngine::process(const std::string& orientation) {
	if (isVertical(orientation)) {
		phases_ = vImages_;
	} else if (isHorizontal(orientation)) {
		phases_ = hImages_;
	} else if (orientation == "last" && phases_ == nullptr) {
		throw std::runtime_error("Last orientation is undefined");
	} else {
		throw std::runtime_error("Invalid orientation");
	}
	if (!phases_[0].rows || !phases_[1].rows || !phases_[2].rows) {
		logWarning("Cannot process empty images");
		return;
	}
	const int znoise = options_["znoise"];
	const float zscale = options_["zscale"];
	const float mscale = options_["mscale"];
	const float zskew = options_["zskew"];
	this->setup();
	this->wrap(znoise);
	this->unwrap();
	this->blurMask();
	this->computeDepth(zscale - mscale, zskew - mscale);
	this->createCloud();
}

void ThreephaseEngine::blurMask() {
	const int zblur = options_["zblur"];
	if (zblur) {
		cv::blur(mask_, mask_, cv::Size(zblur, zblur));
	}
}

void ThreephaseEngine::unwrap() {
	int width = phases_[0].cols;
	int height = phases_[0].rows;
	int startX = width / 2;
	int startY = height / 2;

	toProcess_.clear();

	toProcess_.push_back(cv::Point(startX, startY));
	process_.at<uchar>(startY, startX) = false;

	while (!toProcess_.empty()) {
		cv::Point p = toProcess_.back();
		toProcess_.pop_back();

		int x = p.x;
		int y = p.y;
		float r = unwrapped_.at<float>(y, x);
		if (y > 0) {
			this->unwrap(r, x, y - 1);
		}
		if (y < height - 2) {
			this->unwrap(r, x, y + 1);
		}
		if (x > 0) {
			this->unwrap(r, x - 1, y);
		}
		if (x < width - 2) {
			this->unwrap(r, x + 1, y);
		}
	}
	toProcess_.clear();
}

void ThreephaseEngine::unwrap(float basePhase, int x, int y) {
	const int idx = x + y * process_.cols;
	float* unwrappedBuffer = reinterpret_cast<float*>(unwrapped_.data);
	const float bfquo = basePhase - round(basePhase);
	if (process_.data[idx]) {
		float& p0 = unwrappedBuffer[idx];
		float diff = p0 - bfquo;
		if (diff > 0.5f) {
			diff -= 1.0;
		} else if (diff < -0.5f) {
			diff += 1.0;
		}
		p0 = basePhase + diff;
		process_.data[idx] = 0;
		toProcess_.push_back(cv::Point(x, y));
	}
}

void ThreephaseEngine::wrap(float noiseThreshold) {
	int width = process_.cols;
	int height = process_.rows;

	for (int y = 0; y < height; y++) {
		for (int x = 0; x < width; x++) {
			const int i = x + y * width;
			const int lum0 = phases_[0].data[i];
			const int lum1 = phases_[1].data[i];
			const int lum2 = phases_[2].data[i];
			const int v = lum0 - lum2;
			const int u = 2 * lum1 - lum0 - lum2;
			const unsigned char* const cur = PHASE_GAMMA_LUT[v + WRAP_V_OFFSET][u + WRAP_U_OFFSET];
			mask_.data[i] = (cur[1] < noiseThreshold * 64);

			process_.data[i] = !mask_.data[i];
			static const float SQRT3 = sqrtf(3.0);
			static const float TWO_PI = 6.283185;

			float p2;
			switch (wrapMethod_) {
			case 0:
				p2 = atan2(SQRT3 * (lum0 - lum2), 2.0f * lum1 - lum0 - lum2) / TWO_PI;
				break;
			case 1:
				p2 = (cur[0] - 128.0f) / 255.0f;
				break;
			default:
				logWarning("calibration.json engine.wrapMethod can be 0 or 1");
				break;
			}
			float* unwrappedBuffer = reinterpret_cast<float*>(unwrapped_.data);
			unwrappedBuffer[i] = p2;
		}
	}
}

void ThreephaseEngine::computeDepth(float zscale, float zskew) {
	float* unwrappedBuffer = reinterpret_cast<float*>(unwrapped_.data);
	float* depthBuffer = reinterpret_cast<float*>(depth_.data);

	for (int i = 0; i < unwrapped_.rows; ++i) {
		float planephase = float(i - unwrapped_.rows / 2);
		if (zskew) {
			planephase /= zskew;
		}
		planephase += 0.5f;
		for (int j = 0; j < unwrapped_.cols; ++j) {
			int ii = i * unwrapped_.cols + j;
			depthBuffer[ii] = mask_.data[ii] ? 0.0f : ((unwrappedBuffer[ii] - planephase) * zscale);
		}
	}
}

void ThreephaseEngine::setup() {
	int w = phases_[0].cols;
	int h = phases_[0].rows;
	logDebug("Threephase setup %ux%u", w, h);
	unwrapped_ = cv::Mat::zeros(h, w, CV_32F);
	depth_ = cv::Mat::zeros(h, w, CV_32F);
	mask_ = cv::Mat::zeros(h, w, CV_8U);
	process_ = cv::Mat::zeros(h, w, CV_8U);
}

void ThreephaseEngine::createCloud() {
	logDebug("Save to cloud");
	/*
	 * TODO: controlla che il risultato sia corretto prima di salvare la point cloud.
	 * A volte (causa delay telecamera) le immagini catturate non sono valide.
	 * Un'opzione per controllare se e` valido e` vedere se il numero di punti
	 * rispetta una soglia minima aspettata.
	 */
	const float zscale = options_["zscale"];
	const float mscale = options_["mscale"];
	const float zskew = options_["zskew"];
	const float scale = zscale - mscale;
	float skew = zskew - mscale;
	if (skew == 0.0) {
		skew = 1.0;
	}
	int width = depth_.cols;
	int height = depth_.rows;

	struct RawPoint {
		float x, y, z;
	};

	PointCloud& cloud = *cloud_;
	/*
	 * TODO: need optimization. cloud_ memory can be reserved in
	 * initialization, then only size change. But how? std::vector
	 * do not permit to change size without deallocating.
	 */
	cloud.clear();

	for (int y = 0; y < height; ++y) {
		float planephase = 0.5 + float(y - (height / 2)) / skew;
		for (int x = 0; x < width; ++x) {
			if (!mask_.at<uchar>(y, x)) {
				float z = (unwrapped_.at<float>(y, x) - planephase) * scale;
				Point p;
				p.x = x;
				p.y = y;
				p.z = z;
				p.r = 0xff;
				p.g = 0xff;
				p.b = 0xff;
				cloud.push_back(p);
			}
		}
	}

	logDebug("Points: %u", cloud.size());
}

void ThreephaseEngine::setOption(const std::string& key, const float& value) {
	if (!options_.count(key)) {
		throw std::invalid_argument("Threephase option does not exist: " + key);
	}
	options_[key] = value;
}

void ThreephaseEngine::setParameter(const std::string& key, const std::string& value) {
	std::vector<std::string> k;
	boost::split(k, key, boost::is_any_of(":"));
	if (k[0] == "tuning" && k.size() == 2) {
		return this->setOption(k[1], boost::lexical_cast<float>(value));
	}
	throw std::invalid_argument("Threephase invalid parameter: " + key);
}

void ThreephaseEngine::setImage(const std::string& id, const cv::Mat& image) {
	std::vector<std::string> params;
	boost::split(params, id, boost::is_any_of(":"));
	auto orientation = params[0];
	auto phase = boost::lexical_cast<size_t>(params[1]);
	this->setImage(orientation, phase, image);
}

} /* namespace threescanner */


#include "types.hh"
#include "tcrdist.hh"
#include "io.hh"
#include <random>
#include <boost/math/distributions/hypergeometric.hpp>

Size const MIN_FG_COUNT(5);
Real const MIN_CORE_ENRICH(1.05);
Real const MIN_PAIR_ENRICH(1.5);
Real const MIN_TRIPLE_ENRICH(2.0);
Real const MAX_CORE_PVAL_ADJ(100.);
Real const MAX_PAIR_PVAL_ADJ(1.);
Real const MAX_TRIPLE_PVAL_ADJ(1.);
Real const MAX_FINAL_PVAL_ADJ(1.);
Real const MAX_JACCARD(0.7);
Size const NUM_CORE_AAS(26); // 'A'-'Z'
Size const NUM_CORE_GAPS(3); // 0,1,2

Size const NUM_CORES(
	NUM_CORE_AAS*NUM_CORE_AAS*NUM_CORE_AAS*NUM_CORE_GAPS*NUM_CORE_GAPS);

Real const PSEUDOCOUNT(0.25);

typedef vector< char > chars;

inline
Size
get_core_index(
	char const aa1, // 'A' - 'Z'
	Size const gap1, // 0-2
	char const aa2, // 'A' - 'Z'
	Size const gap2, // 0-2
	char const aa3  // 'A' - 'Z'
)
{
	// 26 * 3 * 26 * 3 * 26
	return  (
		NUM_CORE_AAS*NUM_CORE_GAPS*NUM_CORE_AAS*NUM_CORE_GAPS*(aa1-'A')+
		NUM_CORE_AAS*NUM_CORE_GAPS*NUM_CORE_AAS*gap1+
		NUM_CORE_AAS*NUM_CORE_GAPS*(aa2-'A')+
		NUM_CORE_AAS*gap2+
		(aa3-'A'));
}

struct Motif {
	chars aas;
	Sizes gaps;
	//Size span;
	//Size core_index;

	Motif():
		aas(),
		gaps()
		//core_index(0)
	{}

	Motif(
		char const aa1,
		Size const gap1,
		char const aa2,
		Size const gap2,
		char const aa3
	){
		aas.resize(3);
		aas[0] = aa1;
		aas[1] = aa2;
		aas[2] = aa3;
		gaps.resize(2);
		gaps[0] = gap1;
		gaps[1] = gap2;
		//core_index = get_core_index(aa1,gap1,aa2,gap2,aa3);
		//span = gaps[0] + gaps[1] + 1; // gap between first and last position
	}

	// total gap between aa_i and aa_j
	Size
	gap(Size const i, Size const j) const
	{
		Size total(0);
		for ( Size k=i; k<j; ++k)
			total += gaps[k];
		return total+(j-i)-1;
	}

	string
	to_string() const
	{
		runtime_assert(aas.size() == gaps.size()+1);
		ostringstream out;
		for (Size k=0; k<aas.size(); ++k ){
			out << aas[k];
			if (k<gaps.size()) {
				for (Size g=0; g<gaps[k]; ++g) out << '.';
			}
		}
		return out.str();
	}
};

inline
Real
compute_overlap_pvalue(
	Size const overlap,
	Size const count1,
	Size const count2,
	Size const total
)
{
	using namespace boost::math;
	// whats the smallest possible overlap? max( 0, count1+count2-total )
	if ( overlap==0 || count1==total || count2==total ||
		count1+count2 >= total+overlap ) return 1.0;
	hypergeometric_distribution<> hgd( count1, count2, total );
	// need overlap-1 to be a valid overlap value:
	return cdf( complement( hgd, overlap-1 ) );
}

inline
void
unpack_core_index(
	Size ind,
	char & aa1,
	Size & gap1,
	char & aa2,
	Size & gap2,
	char & aa3
)
{
	Size aa3_i = ind%NUM_CORE_AAS;
	ind = (ind-aa3_i)/NUM_CORE_AAS;
	gap2 = ind%NUM_CORE_GAPS;
	ind = (ind-gap2)/NUM_CORE_GAPS;
	Size aa2_i = ind%NUM_CORE_AAS;
	ind = (ind-aa2_i)/NUM_CORE_AAS;
	gap1 = ind%NUM_CORE_GAPS;
	ind = (ind-gap1)/NUM_CORE_GAPS;
	Size aa1_i = ind%NUM_CORE_AAS;

	aa1 = char('A'+aa1_i);
	aa2 = char('A'+aa2_i);
	aa3 = char('A'+aa3_i);
}

inline
string
core_index_to_string(
	Size ind
)
{
	char aa1, aa2, aa3;
	Size gap1, gap2;

	unpack_core_index(ind, aa1, gap1, aa2, gap2, aa3);
	ostringstream out;
	out << aa1;
	for (Size g=0; g<gap1; ++g) out << '.';
	out << aa2;
	for (Size g=0; g<gap2; ++g) out << '.';
	out << aa3;
	return out.str();
}

inline
Motif
core_index_to_motif(
	Size ind
)
{
	char aa1, aa2, aa3;
	Size gap1, gap2;

	unpack_core_index(ind, aa1, gap1, aa2, gap2, aa3);
	return Motif(aa1, gap1, aa2, gap2, aa3);
}

inline
bool
motifs_overlap(
	Motif const & a,
	Motif const & b,
	Size & a1,
	Size & a2,
	Size & b1,
	Size & b2
)
{
	// NOTE -- there might be more than one match? if dup aas?
	//
	// a.aas[a1] == b.aas[b1]
	// a.aas[a2] == b.aas[b2]
	//

	for ( a1=0; a1<1; ++a1 ){
		for ( b1=0; b1<1; ++b1 ){
			if (a.aas[a1] == b.aas[b1] ) {
				for ( a2=a1+1; a2<2; ++a2 ){
					for ( b2=b1+1; b2<2; ++b2 ){
						if (a.aas[a2] == b.aas[b2] &&
							a.gap(a1,a2) == b.gap(b1,b2)){
							// actually they could be in conflict
							if (a1==b1 && a2==b2 && a.gaps[0]==b.gaps[0] &&
								a.gaps[1]==b.gaps[1]) return false; // unless they are identical
							return true; // and a1,a2,b1,b2 are set
						}
					}
				}
			}
		}
	}
	return false;
}

inline
bool
motifs_overlap(
	Motif const & a,
	Motif const & b
)
{
	static Size a1, a2, b1, b2;
	return motifs_overlap(a,b,a1,a2,b1,b2);
}

void
get_core_counts_for_cdr3s(
	strings const & cdr3s,
	Sizes & counts
)
{
	counts.clear();
	counts.resize(NUM_CORES);

	cout << "get_core_counts_for_cdr3s: num cdr3s= " << cdr3s.size() << endl;

	for (string const & cdr3 : cdr3s ) {
		for ( Size i=0; i<cdr3.size()-2; ++i ) {
			for ( Size j=i+1; j<cdr3.size()-1 && j<=i+NUM_CORE_GAPS; ++j ) {
				for ( Size k=j+1; k<cdr3.size() && k<=j+NUM_CORE_GAPS; ++k ) {
					++counts[get_core_index(cdr3[i], j-i-1, cdr3[j], k-j-1, cdr3[k])];
				}
			}
		}
	}

}

//typedef map<Size, set<Size>> AllOccs;
typedef map<Size, Sizes> AllOccs;

void
get_core_occurrences_for_cdr3s(
	strings const & cdr3s,
	AllOccs & all_occs
)
{
	cout << "get_core_occurrences_for_cdr3s: num cdr3s= " << cdr3s.size() << endl;

	AllOccs::iterator it;

	Size ind(0);
	for (Size icdr3=0; icdr3< cdr3s.size(); ++icdr3) {
		string const & cdr3(cdr3s[icdr3]);
		for ( Size i=0; i<cdr3.size()-2; ++i ) {
			for ( Size j=i+1; j<cdr3.size()-1 && j<=i+NUM_CORE_GAPS; ++j ) {
				for ( Size k=j+1; k<cdr3.size() && k<=j+NUM_CORE_GAPS; ++k ) {
					ind = get_core_index(cdr3[i], j-i-1, cdr3[j], k-j-1, cdr3[k]);
					it = all_occs.find(ind);
					if (it != all_occs.end()) it->second.push_back(icdr3);
				}
			}
		}
	}

}

// assumes that a and b are sorted!!!
Real
compute_jaccard(
	Sizes const & a,
	Sizes const & b
)
{
	Sizes overlap(max(a.size(),b.size()));

	Sizes::iterator const ite(
		set_intersection(a.begin(), a.end(), b.begin(), b.end(), overlap.begin()));
	Size overlap_size(ite - overlap.begin());
	return Real(overlap_size)/(a.size() + b.size() - overlap_size);
}

int main(int argc, char** argv)
{
	try { // to catch tclap exceptions

		TCLAP::CmdLine cmd( "tcrdist_distributions",' ', "0.1" );

		// path to database files
 		TCLAP::ValueArg<std::string> db_filename_arg("d","db_filename",
			"Database file with info for tcrdist calculation", true,
			"", "string",cmd);

 		TCLAP::ValueArg<string> fg_tcrs_file_arg("f","fg_tcrs_file","TSV (tab separated values) "
			"file containing TCRs for neighbor calculation. Should contain the 4 columns "
			"'va_gene' 'cdr3a' 'vb_gene' 'cdr3b' (or alt fieldnames: 'va' and 'vb')", true,
			"unk", "string", cmd);

 		TCLAP::ValueArg<string> bg_tcrs_file_arg("b","bg_tcrs_file","TSV (tab separated values) "
			"file containing TCRs for neighbor calculation. Should contain the 4 columns "
			"'va_gene' 'cdr3a' 'vb_gene' 'cdr3b' (or alt fieldnames: 'va' and 'vb')", true,
			"unk", "string", cmd);

 		TCLAP::ValueArg<std::string> outfile_prefix_arg("o","outfile_prefix",
			"Prefix for the knn_indices and knn_distances output files",true,
			"", "string",cmd);

 		TCLAP::ValueArg<string> chain_arg("c", "chain",
			"TCR chain (A or B)", true, "", "string", cmd);

		cmd.parse( argc, argv );

		string const db_filename( db_filename_arg.getValue() );
		string const fg_tcrs_file( fg_tcrs_file_arg.getValue() );
		string const bg_tcrs_file( bg_tcrs_file_arg.getValue() );
		char const chain(chain_arg.getValue()[0]);
		runtime_assert(chain == 'A' || chain == 'B');
		string const outfile_prefix( outfile_prefix_arg.getValue());

		TCRdistCalculator const tcrdist(chain, db_filename);

		// tmp hacking-- we really only need the CDR3, right?
		vector< DistanceTCR_g > fg_tcrs, bg_tcrs;

		read_single_chain_tcrs_from_tsv_file(fg_tcrs_file, chain, tcrdist, fg_tcrs);
		read_single_chain_tcrs_from_tsv_file(bg_tcrs_file, chain, tcrdist, bg_tcrs);

		strings fg_cdr3s, bg_cdr3s;
		for (auto t : fg_tcrs) {
			fg_cdr3s.push_back("B"+t.cdr3.substr(3,t.cdr3.size()-5)+"Z");
		}
		//cout <<fg_cdr3s.front() << ' ' << fg_cdr3s.back() << endl;

		for (auto t : bg_tcrs) {
			bg_cdr3s.push_back("B"+t.cdr3.substr(3,t.cdr3.size()-5)+"Z");
		}

		Real const fg_bg_ratio(Real(fg_cdr3s.size())/bg_cdr3s.size());

		//
		Sizes fg_counts, bg_counts;

		get_core_counts_for_cdr3s(fg_cdr3s, fg_counts);
		get_core_counts_for_cdr3s(bg_cdr3s, bg_counts);

		Size total_core_pvals(0);
		for ( Size ind=0; ind<NUM_CORES; ++ind ) {
			if ( fg_counts[ind] >= MIN_FG_COUNT ) ++total_core_pvals;
		}

		vector<pair<Real, Size> > sortl;
		//vector<Motif> motifs;
		map<Size,Motif> motifs;
		AllOccs all_fg_occs, all_bg_occs;
		Reals all_pvals_adj(NUM_CORES, Real(NUM_CORES));

		for ( Size ind=0; ind<NUM_CORES; ++ind ) {
			if ( fg_counts[ind] < MIN_FG_COUNT ) continue;
			Size const fg_count(fg_counts[ind]), bg_count(bg_counts[ind]);
			Real const expected(max(PSEUDOCOUNT,Real(bg_count))*fg_bg_ratio),
				enrich(fg_count/expected);
			if (enrich>=MIN_CORE_ENRICH) {
				Real const pval(compute_overlap_pvalue(fg_count, fg_count+bg_count,
						fg_cdr3s.size(), fg_cdr3s.size() + bg_cdr3s.size())),
					pval_adj(pval*total_core_pvals);
				//Real chisq((fg_count-expected)*(fg_count-expected)/expected);
				//if (chisq>5){ // tmp hack
				if (pval_adj <= MAX_CORE_PVAL_ADJ) {
					cout << "core_pval: " << core_index_to_string(ind) <<
						//" pval: " << pval <<
						" pval_adj: " << pval_adj <<
						" enrich: " << enrich <<
						" counts: " << fg_count << ' ' << bg_count <<
						" total_pvals: " << total_core_pvals << endl;
					sortl.push_back(make_pair(pval*total_core_pvals, ind));
					motifs[ind] = core_index_to_motif(ind);
					all_fg_occs[ind]; // create empty list
					all_bg_occs[ind]; // create empty list
					all_pvals_adj[ind] = pval_adj;
				}
			}
		}

		// get occurrence list for all good cores
		get_core_occurrences_for_cdr3s(fg_cdr3s, all_fg_occs);
		get_core_occurrences_for_cdr3s(bg_cdr3s, all_bg_occs);

		sort(sortl.begin(), sortl.end());

		//Size a,b,c,d;
		Sizes fg_overlap(fg_cdr3s.size()), bg_overlap(bg_cdr3s.size());

		Size total_pair_pvals(0);
		vector<pair<Real, Size> > pair_sortl;
		map<Size, Sizes> all_combos; // indexed by "index"

		for (Size jj=1; jj<sortl.size(); ++jj) { // jj is the lower-scoring motif
			Size const ind2(sortl[jj].second);
			Motif const & m2(motifs[ind2]);
			Sizes const & m2_fg_occs(all_fg_occs.find(ind2)->second);
			Sizes const & m2_bg_occs(all_bg_occs.find(ind2)->second);
			for (Size ii=0; ii<jj; ++ii) { // ii is the higher-scoring motif
				Size const ind1(sortl[ii].second);
				Motif const & m1(motifs[ind1]);
				if (motifs_overlap(m1,m2)) {
					// look at intersection
					Sizes const & m1_fg_occs(all_fg_occs.find(ind1)->second);
					Sizes::iterator const fg_ite(
						set_intersection(m1_fg_occs.begin(), m1_fg_occs.end(),
							m2_fg_occs.begin(), m2_fg_occs.end(), fg_overlap.begin()));
					Size const fg_overlap_size(fg_ite - fg_overlap.begin());
					if ( fg_overlap_size >= MIN_FG_COUNT ){
						Sizes const & m1_bg_occs(all_bg_occs.find(ind1)->second);
						Sizes::iterator const bg_ite(
							set_intersection(m1_bg_occs.begin(), m1_bg_occs.end(),
								m2_bg_occs.begin(), m2_bg_occs.end(), bg_overlap.begin()));
						Size const bg_overlap_size(bg_ite - bg_overlap.begin());
						++total_pair_pvals;

						Real const
							expected(max(PSEUDOCOUNT,Real(bg_overlap_size))*fg_bg_ratio),
							enrich(fg_overlap_size/expected);
						if (enrich>=MIN_PAIR_ENRICH) {
							Real const
								pval(compute_overlap_pvalue(fg_overlap_size, fg_overlap_size+
										bg_overlap_size, fg_cdr3s.size(), fg_cdr3s.size() +
										bg_cdr3s.size())),
								pval_adj(pval*(total_core_pvals+total_pair_pvals));
							if (pval_adj <= MAX_PAIR_PVAL_ADJ) {
								cout << "pair_pval: " << core_index_to_string(ind1) << '+' <<
									core_index_to_string(ind2) << ' ' <<
									//" pval: " << pval <<
									" pval_adj: " << pval_adj <<
									" enrich: " << enrich <<
									" counts: " << fg_overlap_size << ' ' << bg_overlap_size <<
									" total_pvals: " << total_core_pvals + total_pair_pvals <<
									endl;
								// create a new index
								Size const index(fg_counts.size());
								// extend the arrays
								runtime_assert(index == bg_counts.size());
								fg_counts.push_back(fg_overlap_size);
								bg_counts.push_back(bg_overlap_size);
								all_pvals_adj.push_back(pval_adj);
								// add index to the dictionaries
								all_fg_occs[index].resize(fg_overlap_size);
								copy(fg_overlap.begin(), fg_ite,
									all_fg_occs.find(index)->second.begin());
								all_bg_occs[index].resize(bg_overlap_size);
								copy(bg_overlap.begin(), bg_ite,
									all_bg_occs.find(index)->second.begin());
								all_combos[index] = Sizes({ind1, ind2});
								pair_sortl.push_back(make_pair(pval_adj, index));
							}
						}
					}
				}
			}
		}

		sort(pair_sortl.begin(), pair_sortl.end());
		// now go through and look for pair+single combos
		//
		// what data structures do we have?
		// all_fg_occs, all_bg_occs  keys are "index"
		//
		// all_combos  keys are "index"
		//
		// motifs  keys are "index" only valid for core indices
		//
		Size total_triple_pvals(0);
		for ( Size ij_sum=0; ij_sum <pair_sortl.size()+sortl.size(); ++ij_sum) {
			for ( Size ii=0; ii<ij_sum && ii<pair_sortl.size(); ++ii) {
				Size const jj(ij_sum-ii);
				if (jj>=sortl.size()) continue;
				Size const ii_index(pair_sortl[ii].second);
				Size const jj_index(sortl[jj].second);
				Sizes const & ii_combos(all_combos[ii_index]);
				Motif const & m1(motifs[ii_combos[0]]);
				Motif const & m2(motifs[ii_combos[1]]);
				Motif const & m3(motifs[jj_index]);
				runtime_assert(motifs_overlap(m1,m2)); // HACKING REMOVE LATER
				if (motifs_overlap(m1,m3) || motifs_overlap(m2,m3)) {
					Sizes const & m1m2_fg_occs(all_fg_occs[ii_index]),
						& m1m2_bg_occs(all_bg_occs[ii_index]),
						& m3_fg_occs(all_fg_occs[jj_index]),
						& m3_bg_occs(all_bg_occs[jj_index]);

					Sizes::iterator const fg_ite(
						set_intersection(m1m2_fg_occs.begin(), m1m2_fg_occs.end(),
							m3_fg_occs.begin(), m3_fg_occs.end(), fg_overlap.begin()));
					Size const fg_overlap_size(fg_ite - fg_overlap.begin());
					if ( fg_overlap_size >= MIN_FG_COUNT ){
						Sizes::iterator const bg_ite(
							set_intersection(m1m2_bg_occs.begin(), m1m2_bg_occs.end(),
								m3_bg_occs.begin(), m3_bg_occs.end(), bg_overlap.begin()));
						Size const bg_overlap_size(bg_ite - bg_overlap.begin());
						++total_triple_pvals;

						Real const
							expected(max(PSEUDOCOUNT,Real(bg_overlap_size))*fg_bg_ratio),
							enrich(fg_overlap_size/expected);
						if (enrich>=MIN_TRIPLE_ENRICH) {
							Real const
								pval(compute_overlap_pvalue(fg_overlap_size, fg_overlap_size+
										bg_overlap_size, fg_cdr3s.size(), fg_cdr3s.size() +
										bg_cdr3s.size())),
								pval_adj(pval*(total_core_pvals+total_pair_pvals+
										total_triple_pvals));
							if (pval_adj <= MAX_TRIPLE_PVAL_ADJ) {
								cout << "triple_pval: " <<
									core_index_to_string(ii_combos[0]) << '+' <<
									core_index_to_string(ii_combos[1]) << '+' <<
									core_index_to_string(jj_index) << ' ' <<
									//" pval: " << pval <<
									" pval_adj: " << pval_adj <<
									" enrich: " << enrich <<
									" counts: " << fg_overlap_size << ' ' << bg_overlap_size <<
									" total_pvals: " << total_core_pvals + total_pair_pvals +
									total_triple_pvals << endl;

								// new motif, create a new index
								Size const index(fg_counts.size());
								// extend the arrays
								runtime_assert(index == bg_counts.size());
								fg_counts.push_back(fg_overlap_size);
								bg_counts.push_back(bg_overlap_size);
								all_pvals_adj.push_back(pval_adj);
								// add index to the dictionaries
								all_fg_occs[index].resize(fg_overlap_size);
								copy(fg_overlap.begin(), fg_ite,
									all_fg_occs.find(index)->second.begin());
								all_bg_occs[index].resize(bg_overlap_size);
								copy(bg_overlap.begin(), bg_ite,
									all_bg_occs.find(index)->second.begin());
								all_combos[index] = Sizes(
									{ii_combos[0], ii_combos[1], jj_index}); // pair then single
							} // pval <
						} // enrich >
					} // fg_overlap_size >
				}
			} // ii
		} // ij_sum, actually ii+jj

		// now sort all the motifs by pval, look for overlap
		vector<pair<Real,Size> > final_sortl;
		for (Size i=0; i<all_pvals_adj.size(); ++i ) {
			if (all_pvals_adj[i] <= MAX_FINAL_PVAL_ADJ) {
				final_sortl.push_back(make_pair(all_pvals_adj[i], i));
			}
		}
		sort(final_sortl.begin(), final_sortl.end());

		bools is_good(final_sortl.size(), true); // indexed by rank in final_sortl

		ofstream out_info(outfile_prefix+"_motif_info.tsv");
		ofstream out_members(outfile_prefix+"_motif_members.txt");
		ofstream out_distances(outfile_prefix+"_motif_distances.txt");

		out_info << "motif_string\tpval_adj\tenrich\tmax_jaccard\tnum_positions\tfg_count\tbg_count\ttotal_pvals\n";

		vector<Reals> all_jdists;
		Size const total_pvals(
			total_core_pvals + total_pair_pvals + total_triple_pvals);

		for ( Size ii=0; ii<final_sortl.size(); ++ii ) {
			Size const ii_index(final_sortl[ii].second);
			// check if too close to previous motif
			Real max_jaccard(0);
			Reals jdists;
			for ( Size jj=0; jj<ii; ++jj ) {
				if (is_good[jj]) {
					Size const jj_index(final_sortl[jj].second);
					Real const jaccard(compute_jaccard(
							all_fg_occs[ii_index], all_fg_occs[jj_index]));
					jdists.push_back(1.0-jaccard);
					max_jaccard = max(max_jaccard, jaccard);
					if ( jaccard > MAX_JACCARD ) {
						//cout<< "too close: " << jaccard << ' ' << ii << ' ' << jj << endl;
						is_good[ii] = false;
						break;
					}
				}
			}
			if ( is_good[ii] ) {
				// update the distance matrix
				jdists.push_back(0.0); // self dist
				for ( Size jj=0; jj<all_jdists.size(); ++jj ) { // make it square
					all_jdists[jj].push_back(jdists[jj]);
					// runtime_assert(all_jdists[jj].size() == jdists.size());
				}
				all_jdists.push_back(jdists);

				// not redundant by jaccard
				cout << "final_pval: ";
				// show the motif
				if (ii_index < NUM_CORES) {
					cout << motifs[ii_index].to_string();
					out_info << motifs[ii_index].to_string() << '\t';
				} else {
					bool first(true);
					for ( Size kk : all_combos[ii_index] ) {
						if (!first) {
							cout << '+';
							out_info << '+';
						} else first=false;
						cout << motifs[kk].to_string();
						out_info << motifs[kk].to_string();
					}
					out_info << '\t';
				}
				// this is not quite right: triple could only specify 4 positions...
				Size const num_positions
					(ii_index<NUM_CORES ? 3 : 2+all_combos[ii_index].size());
				Real const
					expected(max(PSEUDOCOUNT,Real(bg_counts[ii_index]))*fg_bg_ratio),
					enrich(fg_counts[ii_index]/expected);
				cout << " pval_adj: " << all_pvals_adj[ii_index] <<
					" enrich: " << enrich <<
					" max_jaccard: " << max_jaccard <<
					" num_positions: " << num_positions <<
					" counts: " << fg_counts[ii_index] << ' ' << bg_counts[ii_index] <<
					" total_pvals: " << total_pvals << endl;

				out_info << all_pvals_adj[ii_index] << '\t' <<
					enrich << '\t' <<
					max_jaccard << '\t' <<
					num_positions << '\t' <<
					fg_counts[ii_index] << '\t' <<
					bg_counts[ii_index] << '\t' <<
					total_pvals << '\n';
				{ // write members
					runtime_assert(all_fg_occs[ii_index].size() == fg_counts[ii_index]);
					bool first(true);
					for ( Size m : all_fg_occs[ii_index] ) {
						if (first) first=false;
						else out_members << ' ';
						out_members << m;
					}
					out_members << '\n';
				}
			} // good!
		}
		out_info.close();
		out_members.close();

		// write jaccard distance matrix:
		for ( Reals const & jdists : all_jdists ) {
			runtime_assert(jdists.size() == all_jdists.size());
			bool first(true);
			for ( Real d : jdists ) {
				if ( first ) first=false;
				else out_distances << ' ';
				out_distances << d;
			}
			out_distances << '\n';
		}
		out_distances.close();

		cout << "DONE" << endl;

	} catch (TCLAP::ArgException &e)  // catch any exceptions
		{
			std::cerr << "error: " << e.error() << " for arg " << e.argId() <<
				std::endl;
		}

}