#include "TestScheme.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/RR.h>
#include <NTL/ZZ.h>

#include "Common.h"
#include "Ciphertext.h"
#include "EvaluatorUtils.h"
#include "NumUtils.h"
#include "Scheme.h"
#include "SchemeAlgo.h"
#include "SecretKey.h"
#include "StringUtils.h"
#include "TimeUtils.h"
#include "Context.h"
#include "SerializationUtils.h"

using namespace std;
using namespace NTL;

//----------------------------------------------------------------------------------
//   STANDARD TESTS
//----------------------------------------------------------------------------------

void TestScheme::testEncodeBatch(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST ENCODE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);

	timeutils.start("Encrypt batch");
	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);
	timeutils.stop("Encrypt batch");

	timeutils.start("Decrypt batch");
	complex<double>* dvec = scheme.decrypt(secretKey, cipher);
	timeutils.stop("Decrypt batch");

	StringUtils::showcompare(mvec, dvec, slots, "val");

	cout << "!!! END TEST ENCODE BATCH !!!" << endl;
}

void TestScheme::testEncodeSingle(long logN, long logQ, long logp, bool isComplex) {
	cout << "!!! START TEST ENCODE SINGLE !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	complex<double> mval = EvaluatorUtils::randomComplex();

	timeutils.start("Encrypt Single");
	Ciphertext cipher = scheme.encryptSingle(mval, logp, logQ);
	timeutils.stop("Encrypt Single");

	timeutils.start("Decrypt Single");
	complex<double> dval = scheme.decryptSingle(secretKey, cipher);
	timeutils.stop("Decrypt Single");

	StringUtils::showcompare(mval, dval, "val");

	cout << "!!! END TEST ENCODE SINGLE !!!" << endl;
}

void TestScheme::testBasic(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST BASIC !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec1 = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* mvec2 = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* mvecAdd = new complex<double>[slots];
	complex<double>* mvecMult = new complex<double>[slots];
	for(long i = 0; i < slots; i++) {
		mvecAdd[i] = mvec1[i] + mvec2[i];
		mvecMult[i] = mvec1[i] * mvec2[i];
	}

	timeutils.start("Encrypt two batch");
	Ciphertext cipher1 = scheme.encrypt(mvec1, slots, logp, logQ);
	Ciphertext cipher2 = scheme.encrypt(mvec2, slots, logp, logQ);
	timeutils.stop("Encrypt two batch");

	timeutils.start("Homomorphic Addition");
	Ciphertext addCipher = scheme.add(cipher1, cipher2);
	timeutils.stop("Homomorphic Addition");

	timeutils.start("Homomorphic Multiplication");
	Ciphertext multCipher = scheme.mult(cipher1, cipher2);
	timeutils.stop("Homomorphic Multiplication");

	timeutils.start("Decrypt batch");
	complex<double>* dvecAdd = scheme.decrypt(secretKey, addCipher);
	complex<double>* dvecMult = scheme.decrypt(secretKey, multCipher);
	timeutils.stop("Decrypt batch");

	StringUtils::showcompare(mvecAdd, dvecAdd, slots, "add");
	StringUtils::showcompare(mvecMult, dvecMult, slots, "mult");
}

//-----------------------------------------

void TestScheme::testConjugateBatch(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST CONJUGATE BATCH !!!" << endl;
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	scheme.addConjKey(secretKey);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* mvecconj = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		mvecconj[i] = conj(mvec[i]);
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("Conjugate batch");
	Ciphertext cconj = scheme.conjugate(cipher);
	timeutils.stop("Conjugate batch");

	complex<double>* dvecconj = scheme.decrypt(secretKey, cconj);

	StringUtils::showcompare(mvecconj, dvecconj, slots, "conj");

	cout << "!!! END TEST CONJUGATE BATCH !!!" << endl;
}

void TestScheme::testimultBatch(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST i MULTIPLICATION BATCH !!!" << endl;
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* imvec = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		imvec[i].real(-mvec[i].imag());
		imvec[i].imag(mvec[i].real());
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("Multiplication by i batch");
	Ciphertext icipher = scheme.imult(cipher);
	timeutils.stop("Multiplication by i batch");

	complex<double>* idvec = scheme.decrypt(secretKey, icipher);

	StringUtils::showcompare(imvec, idvec, slots, "imult");

	cout << "!!! END TEST i MULTIPLICATION BATCH !!!" << endl;
}

void TestScheme::testRotateByPo2Batch(long logN, long logQ, long logp, long logRotSlots, long logSlots, bool isLeft) {
	cout << "!!! START TEST ROTATE BY POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	scheme.addLeftRotKeys(secretKey);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	long rotSlots = (1 << logRotSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	if(isLeft) {
		timeutils.start("Left Rotate by power of 2 batch");
		scheme.leftRotateByPo2AndEqual(cipher, logRotSlots);
		timeutils.stop("Left Rotate by power of 2 batch");
	} else {
		timeutils.start("Right Rotate by power of 2 batch");
		scheme.rightRotateByPo2AndEqual(cipher, logRotSlots);
		timeutils.stop("Right Rotate by power of 2 batch");
	}

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);

	if(isLeft) {
		EvaluatorUtils::leftRotateAndEqual(mvec, slots, rotSlots);
	} else {
		EvaluatorUtils::rightRotateAndEqual(mvec, slots, rotSlots);
	}

	StringUtils::showcompare(mvec, dvec, slots, "rot");
	//-----------------------------------------
	cout << "!!! END TEST ROTATE BY POWER OF 2 BATCH !!!" << endl;
}

void TestScheme::testRotateBatch(long logN, long logQ, long logp, long rotSlots, long logSlots, bool isLeft) {
	cout << "!!! START TEST ROTATE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	scheme.addLeftRotKeys(secretKey);
	scheme.addRightRotKeys(secretKey);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	if(isLeft) {
		timeutils.start("Left rotate batch");
		scheme.leftRotateAndEqual(cipher, rotSlots);
		timeutils.stop("Left rotate batch");
	} else {
		timeutils.start("Right rotate batch");
		scheme.rightRotateAndEqual(cipher, rotSlots);
		timeutils.stop("Right rotate batch");
	}

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);

	if(isLeft) {
		EvaluatorUtils::leftRotateAndEqual(mvec, slots, rotSlots);
	} else {
		EvaluatorUtils::rightRotateAndEqual(mvec, slots, rotSlots);
	}

	StringUtils::showcompare(mvec, dvec, slots, "rot");

	cout << "!!! END TEST ROTATE BATCH !!!" << endl;
}

void TestScheme::testSlotsSum(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST SLOTS SUM !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	scheme.addLeftRotKeys(secretKey);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("slots sum");
	algo.partialSlotsSumAndEqual(cipher, slots);
	timeutils.stop("slots sum");

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);

	complex<double> msum;
	for (long i = 0; i < slots; ++i) {
		msum += mvec[i];
	}

	StringUtils::showcompare(msum, dvec, slots, "slotsum");

	cout << "!!! END TEST SLOTS SUM !!!" << endl;
}

//----------------------------------------------------------------------------------
//   POWER & PRODUCT TESTS
//----------------------------------------------------------------------------------

void TestScheme::testPowerOf2Batch(long logN, long logQ, long logp, long logDegree, long logSlots) {
	cout << "!!! START TEST POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	long degree = 1 << logDegree;
	complex<double>* mvec = new complex<double>[slots];
	complex<double>* mpow = new complex<double>[slots];

	for (long i = 0; i < slots; ++i) {
		mvec[i] = EvaluatorUtils::randomCircle();
		mpow[i] = pow(mvec[i], degree);
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("Power of 2 batch");
	Ciphertext cpow = algo.powerOf2(cipher, logp, logDegree);
	timeutils.stop("Power of 2 batch");

	complex<double>* dpow = scheme.decrypt(secretKey, cpow);

	StringUtils::showcompare(mpow, dpow, slots, "pow2");

	cout << "!!! END TEST POWER OF 2 BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testPowerBatch(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST POWER BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomCircleArray(slots);
	complex<double>* mpow = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		mpow[i] = pow(mvec[i], degree);
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("Power batch");
	Ciphertext cpow = algo.power(cipher, logp, degree);
	timeutils.stop("Power batch");

	complex<double>* dpow = scheme.decrypt(secretKey, cpow);

	StringUtils::showcompare(mpow, dpow, slots, "pow");

	cout << "!!! END TEST POWER BATCH !!!" << endl;
}

//-----------------------------------------

void TestScheme::testProdOfPo2Batch(long logN, long logQ, long logp, long logDegree, long logSlots) {
	cout << "!!! START TEST PROD OF POWER OF 2 BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(4);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	long degree = 1 << logDegree;

	complex<double>** mvec = new complex<double>*[degree];
	for (long i = 0; i < degree; ++i) {
		mvec[i] = EvaluatorUtils::randomCircleArray(slots);
	}

	complex<double>* pvec = new complex<double>[slots];
	for (long j = 0; j < slots; ++j) {
		pvec[j] = mvec[0][j];
		for (long i = 1; i < degree; ++i) {
			pvec[j] *= mvec[i][j];
		}
	}

	Ciphertext* cvec = new Ciphertext[degree];
	for (long i = 0; i < degree; ++i) {
		cvec[i] = scheme.encrypt(mvec[i], slots, logp, logQ);
	}

	timeutils.start("Product of power of 2 batch");
	Ciphertext cprod = algo.prodOfPo2(cvec, logp, logDegree);
	timeutils.stop("Product of power of 2 batch");

	complex<double>* dvec = scheme.decrypt(secretKey, cprod);

	StringUtils::showcompare(pvec, dvec, slots, "prod");

	cout << "!!! END TEST PROD OF POWER OF 2 BATCH !!!" << endl;
}

void TestScheme::testProdBatch(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST PROD BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(4);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>** mvec = new complex<double>*[degree];
	for (long i = 0; i < degree; ++i) {
		mvec[i] = EvaluatorUtils::randomCircleArray(slots);
	}

	complex<double>* pvec = new complex<double>[slots];
	for (long j = 0; j < slots; ++j) {
		pvec[j] = mvec[0][j];
		for (long i = 1; i < degree; ++i) {
			pvec[j] *= mvec[i][j];
		}
	}

	Ciphertext* cvec = new Ciphertext[degree];
	for (long i = 0; i < degree; ++i) {
		cvec[i] = scheme.encrypt(mvec[i], slots, logp, logQ);
	}

	timeutils.start("Product batch");
	Ciphertext cprod = algo.prod(cvec, logp, degree);
	timeutils.stop("Product batch");

	complex<double>* dvec = scheme.decrypt(secretKey, cprod);

	StringUtils::showcompare(pvec, dvec, slots, "prod");

	cout << "!!! END TEST PROD BATCH !!!" << endl;
}

//----------------------------------------------------------------------------------
//   FUNCTION TESTS
//----------------------------------------------------------------------------------

void TestScheme::testInverseBatch(long logN, long logQ, long logp, long invSteps, long logSlots) {
	cout << "!!! START TEST INVERSE BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomCircleArray(slots, 0.1);
	complex<double>* minv = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		minv[i] = 1. / mvec[i];
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start("Inverse batch");
	Ciphertext cinv = algo.inverse(cipher, logp, invSteps);
	timeutils.stop("Inverse batch");

	complex<double>* dinv = scheme.decrypt(secretKey, cinv);

	StringUtils::showcompare(minv, dinv, slots, "inv");

	cout << "!!! END TEST INVERSE BATCH !!!" << endl;
}

void TestScheme::testLogarithmBatch(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST LOGARITHM BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots, 0.1);
	complex<double>* mlog = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		mlog[i] = log(mvec[i] + 1.);
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start(LOGARITHM + " batch");
	Ciphertext clog = algo.function(cipher, LOGARITHM, logp, degree);
	timeutils.stop(LOGARITHM + " batch");

	complex<double>* dlog = scheme.decrypt(secretKey, clog);

	StringUtils::showcompare(mlog, dlog, slots, LOGARITHM);

	cout << "!!! END TEST LOGARITHM BATCH !!!" << endl;
}

void TestScheme::testExponentBatch(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST EXPONENT BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* mexp = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		mexp[i] = exp(mvec[i]);
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start(EXPONENT + " batch");
	Ciphertext cexp = algo.function(cipher, EXPONENT, logp, degree);
	timeutils.stop(EXPONENT + " batch");

	complex<double>* dexp = scheme.decrypt(secretKey, cexp);

	StringUtils::showcompare(mexp, dexp, slots, EXPONENT);

	cout << "!!! END TEST EXPONENT BATCH !!!" << endl;
}

void TestScheme::testExponentBatchLazy(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST EXPONENT LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* mexp = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		mexp[i] = exp(mvec[i]);
	}
	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start(EXPONENT + " lazy");
	Ciphertext cexp = algo.functionLazy(cipher, EXPONENT, logp, degree);
	timeutils.stop(EXPONENT + " lazy");

	complex<double>* dexp = scheme.decrypt(secretKey, cexp);

	StringUtils::showcompare(mexp, dexp, slots, EXPONENT);

	cout << "!!! END TEST EXPONENT LAZY !!!" << endl;
}

//-----------------------------------------

void TestScheme::testSigmoidBatch(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST SIGMOID BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;

	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* msig = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		msig[i] = exp(mvec[i]) / (1. + exp(mvec[i]));
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start(SIGMOID + " batch");
	Ciphertext csig = algo.function(cipher, SIGMOID, logp, degree);
	timeutils.stop(SIGMOID + " batch");

	complex<double>* dsig = scheme.decrypt(secretKey, csig);

	StringUtils::showcompare(msig, dsig, slots, SIGMOID);

	cout << "!!! END TEST SIGMOID BATCH !!!" << endl;
}

void TestScheme::testSigmoidBatchLazy(long logN, long logQ, long logp, long degree, long logSlots) {
	cout << "!!! START TEST SIGMOID LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = 1 << logSlots;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);
	complex<double>* msig = new complex<double>[slots];
	for (long i = 0; i < slots; ++i) {
		msig[i] = exp(mvec[i]) / (1. + exp(mvec[i]));
	}

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	timeutils.start(SIGMOID + " lazy");
	Ciphertext csig = algo.functionLazy(cipher, SIGMOID, logp, degree);
	timeutils.stop(SIGMOID + " lazy");

	complex<double>* dsig = scheme.decrypt(secretKey, csig);

	StringUtils::showcompare(msig, dsig, slots, SIGMOID);

	cout << "!!! END TEST SIGMOID LAZY !!!" << endl;
}

//----------------------------------------------------------------------------------
//   FFT TESTS
//----------------------------------------------------------------------------------

void TestScheme::testFFTBatch(long logN, long logQ, long logp, long logSlots, long logfftdim) {
	cout << "!!! START TEST FFT BATCH !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long slots = 1 << logSlots;
	complex<double>** mvec1 = new complex<double>*[slots];
	complex<double>** mvec2 = new complex<double>*[slots];
	for (long i = 0; i < slots; ++i) {
		mvec1[i] = EvaluatorUtils::randomComplexArray(fftdim);
		mvec2[i] = EvaluatorUtils::randomComplexArray(fftdim);
	}

	Ciphertext* cvec1 = new Ciphertext[fftdim];
	Ciphertext* cvec2 = new Ciphertext[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		complex<double>* mvals1 = new complex<double>[slots];
		complex<double>* mvals2	= new complex<double>[slots];
		for (long i = 0; i < slots; ++i) {
			mvals1[i] = mvec1[i][j];
			mvals2[i] = mvec2[i][j];
		}
		cvec1[j] = scheme.encrypt(mvals1, slots, logp, logQ);
		cvec2[j] = scheme.encrypt(mvals2, slots, logp, logQ);
		delete[] mvals1;
		delete[] mvals2;
	}

	for (long i = 0; i < slots; ++i) {
		context.fft(mvec1[i], fftdim);
		context.fft(mvec2[i], fftdim);
		for (long j = 0; j < fftdim; ++j) {
			mvec1[i][j] *= mvec2[i][j];
		}
		context.fftInv(mvec1[i], fftdim);
	}

	timeutils.start("ciphers fft 1 batch");
	algo.fft(cvec1, fftdim);
	timeutils.stop("ciphers fft 1 batch");

	timeutils.start("ciphers fft 2 batch");
	algo.fft(cvec2, fftdim);
	timeutils.stop("ciphers fft 2 batch");

	timeutils.start("ciphers hadamard mult batch");
	algo.multModSwitchAndEqualVec(cvec1, cvec2, logp, fftdim);
	timeutils.stop("ciphers hadamard mult batch");

	delete[] cvec2;

	timeutils.start("ciphers fft inverse batch");
	algo.fftInv(cvec1, fftdim);
	timeutils.stop("ciphers fft inverse batch");

	complex<double>** dvec1 = new complex<double>*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec1[j] = scheme.decrypt(secretKey, cvec1[j]);
	}

	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvec1[i][j], dvec1[j][i], "fft");
		}
	}

	cout << "!!! END TEST FFT BATCH !!!" << endl;
}

void TestScheme::testFFTBatchLazy(long logN, long logQ, long logp, long logSlots, long logfftdim) {
	cout << "!!! START TEST FFT BATCH LAZY !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long slots = 1 << logSlots;
	complex<double>** mvec1 = new complex<double>*[slots];
	complex<double>** mvec2 = new complex<double>*[slots];
	for (long i = 0; i < slots; ++i) {
		mvec1[i] = EvaluatorUtils::randomComplexArray(fftdim);
		mvec2[i] = EvaluatorUtils::randomComplexArray(fftdim);
	}

	Ciphertext* cvec1 = new Ciphertext[fftdim];
	Ciphertext* cvec2 = new Ciphertext[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		complex<double>* mvals1 = new complex<double>[slots];
		complex<double>* mvals2	= new complex<double>[slots];
		for (long i = 0; i < slots; ++i) {
			mvals1[i] = mvec1[i][j];
			mvals2[i] = mvec2[i][j];
		}
		cvec1[j] = scheme.encrypt(mvals1, slots, logp, logQ);
		cvec2[j] = scheme.encrypt(mvals2, slots, logp, logQ);
		delete[] mvals1;
		delete[] mvals2;
	}

	for (long i = 0; i < slots; ++i) {
		context.fft(mvec1[i], fftdim);
		context.fft(mvec2[i], fftdim);
		for (long j = 0; j < fftdim; ++j) {
			mvec1[i][j] *= mvec2[i][j];
		}
		context.fftInvLazy(mvec1[i], fftdim);
	}

	timeutils.start("ciphers fft 1");
	algo.fft(cvec1, fftdim);
	timeutils.stop("ciphers fft 1");

	timeutils.start("ciphers fft 2");
	algo.fft(cvec2, fftdim);
	timeutils.stop("ciphers fft 2");

	timeutils.start("ciphers hadamard mult");
	algo.multModSwitchAndEqualVec(cvec1, cvec2, logp, fftdim);
	timeutils.stop("ciphers hadamard mult");

	delete[] cvec2;

	timeutils.start("ciphers fft inverse lazy");
	algo.fftInvLazy(cvec1, fftdim);
	timeutils.stop("ciphers fft inverse lazy");

	complex<double>** dvec1 = new complex<double>*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec1[j] = scheme.decrypt(secretKey, cvec1[j]);
	}

	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvec1[i][j], dvec1[j][i], "fft");
		}
	}

	cout << "!!! END TEST FFT BATCH LAZY !!!" << endl;
}

void TestScheme::testFFTBatchLazyMultipleHadamard(long logN, long logQ, long logp, long logSlots, long logfftdim, long logHdim) {
	cout << "!!! START TEST FFT BATCH LAZY MULTIPLE HADAMARD !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	SetNumThreads(8);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long fftdim = 1 << logfftdim;
	long hdim = 1 << logHdim;
	long slots = 1 << logSlots;
	complex<double>*** mvecs = new complex<double>**[hdim];
	Ciphertext** cvecs = new Ciphertext*[hdim];
	for (long h = 0; h < hdim; ++h) {
		mvecs[h] = new complex<double>*[slots];
		for (long i = 0; i < slots; ++i) {
			mvecs[h][i] = EvaluatorUtils::randomComplexArray(fftdim);
		}

		cvecs[h] = new Ciphertext[fftdim];
		for (long j = 0; j < fftdim; ++j) {
			complex<double>* mvals = new complex<double>[slots];
			for (long i = 0; i < slots; ++i) {
				mvals[i] = mvecs[h][i][j];
			}
			cvecs[h][j] = scheme.encrypt(mvals, slots, logp, logQ);
			delete[] mvals;
		}
		for (long i = 0; i < slots; ++i) {
			context.fft(mvecs[h][i], fftdim);
		}
	}

	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			for (long s = logHdim - 1; s >= 0; --s) {
				long spow = 1 << s;
				for (long h = 0; h < spow; ++h) {
					mvecs[h][i][j] *= mvecs[h+spow][i][j];
				}
			}
		}
	}

	for (long i = 0; i < slots; ++i) {
		context.fftInvLazy(mvecs[0][i], fftdim);
	}

	for (long h = 1; h < hdim; ++h) {
		for (long i = 0; i < slots; ++i) {
			delete[] mvecs[h][i];
		}
		delete[] mvecs[h];
	}

	for (long h = 0; h < hdim; ++h) {
		timeutils.start("fft");
		algo.fft(cvecs[h], fftdim);
		timeutils.stop("fft");
	}

	for (long s = logHdim - 1; s >= 0; --s) {
		long spow = 1 << s;
		for (long h = 0; h < spow; ++h) {
			timeutils.start("hadamard mult");
			algo.multModSwitchAndEqualVec(cvecs[h], cvecs[h+spow], logp, fftdim);
			timeutils.stop("hadamard mult");
			delete[] cvecs[h+spow];
		}
	}

	timeutils.start("fft inverse lazy");
	algo.fftInvLazy(cvecs[0], fftdim);
	timeutils.stop("fft inverse lazy");

	complex<double>** dvec = new complex<double>*[fftdim];
	for (long j = 0; j < fftdim; ++j) {
		dvec[j] = scheme.decrypt(secretKey, cvecs[0][j]);
	}

	for (long i = 0; i < slots; ++i) {
		for (long j = 0; j < fftdim; ++j) {
			StringUtils::showcompare(mvecs[0][i][j], dvec[j][i], "fft");
		}
	}

	cout << "!!! END TEST FFT BATCH LAZY MULTIPLE HADAMARD !!!" << endl;
}

void TestScheme::testWriteAndRead(long logN, long logQ, long logp, long logSlots) {
	cout << "!!! START TEST WRITE AND READ !!!" << endl;
	//-----------------------------------------
	TimeUtils timeutils;
	Context context(logN, logQ);
	SecretKey secretKey(logN);
	Scheme scheme(secretKey, context);
	SchemeAlgo algo(scheme);
	//-----------------------------------------
	srand(time(NULL));
	//-----------------------------------------
	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);

	Ciphertext cipher = scheme.encrypt(mvec, slots, logp, logQ);

	string cipherPath = "testCiphertext.txt";
	timeutils.start("Write Ciphertext");
	SerializationUtils::writeCiphertext(cipher, cipherPath);
	timeutils.stop("Write Ciphertext");

	timeutils.start("Read Ciphertext");
	Ciphertext newcipher = SerializationUtils::readCiphertext(cipherPath);
	timeutils.stop("Read Ciphertext");

	if(newcipher.ax != cipher.ax || newcipher.bx != cipher.bx || newcipher.logq != cipher.logq || newcipher.slots != cipher.slots || newcipher.logp != cipher.logp) {
		cerr << "Write and Read for ciphertext does not work" << endl;
		cout << "difference ax = " << newcipher.ax - cipher.ax << endl;
		cout << "difference bx = " << newcipher.ax - cipher.bx << endl;
	} else {
		cout << "Write and Read for ciphertext works well" << endl;
	}

	string secretKeyPath = "testSecretKey.txt";
	timeutils.start("Write SecretKey");
	SerializationUtils::writeSecretKey(secretKey, secretKeyPath);
	timeutils.stop("Write SecretKey");

	timeutils.start("Read SecretKey");
	SecretKey newsecretKey = SerializationUtils::readSecretKey(secretKeyPath);
	timeutils.stop("Read SecretKey");
	if(secretKey.sx != newsecretKey.sx) {
		cout << "Write and Read for sk does not work" << endl;
	} else {
		cout << "Write and Read for sk works well" << endl;
	}

//	scheme.addLeftRotKeys(secretKey);
	scheme.addConjKey(secretKey);

	string schemeKeysPath = "testSchemeKeys.txt";
	timeutils.start("Write Scheme");
	SerializationUtils::writeSchemeKeys(scheme, schemeKeysPath);
	timeutils.stop("Write Scheme");

	string contextPath = "testContext.txt";
	timeutils.start("Write Context");
	SerializationUtils::writeContext(context, contextPath);
	timeutils.stop("Write Context");

	timeutils.start("Read Context");
	Context newcontext = SerializationUtils::readContext(contextPath);
	timeutils.stop("Read Context");

	Scheme newscheme(newcontext);

	timeutils.start("Read Scheme");
	SerializationUtils::readSchemeKeys(newscheme, schemeKeysPath);
	timeutils.stop("Read Scheme");

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);
	complex<double>* newdvec = newscheme.decrypt(newsecretKey, newcipher);

	StringUtils::showcompare(dvec, newdvec, slots, "r&w");

	cout << "!!! END TEST WRITE AND READ !!!" << endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <iostream>
#include <iterator>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/extensions/algorithms/remove_holes_if.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/read.hpp>

#include <boost/geometry/strategies/strategies.hpp>

template <typename G, typename Predicate>
void test_remove_holes_if(std::string const& wkt, Predicate const& predicate,
            int expected_points)
{
    G g;
    bg::read_wkt(wkt, g);
    bg::remove_holes_if(g, predicate);
    BOOST_CHECK_EQUAL(bg::num_points(g), expected_points);
}

template <typename P>
void test_all()
{
    bg::elongated_hole<bg::model::ring<P> > elongated(0.05);

    // No holes
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0))", elongated, 5);

    // Square hole (ratio 1/4), kept
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0), (1 1,1 2,2 2,2 1,1 1))", elongated, 10);

    // Elongated hole
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0), (1 1,1 2,1.02 2,1.02 1,1 1))", elongated, 5);

    // Invalid hole - removed by "elongated" predicate as well
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0), (1 1,1 2))", elongated, 5);

    // Invalid hole
    bg::invalid_hole<bg::model::ring<P> > invalid;
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0), (1 1,1 2))", invalid, 5);

    // Valid hole
    test_remove_holes_if<bg::model::polygon<P> >("POLYGON((0 0,0 4,4 4,4 0,0 0), (1 1,1 2,1.02 2,1.02 1,1 1))", invalid, 10);

}

int test_main(int, char* [])
{
    //test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

Copyright (c) 1999-2014 and onwards, Imperial College London
All rights reserved.
See LICENSE for details

=========================================================================*/

#include <irtkImage.h>

#include <irtkNoise.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

template <class VoxelType> irtkGaussianNoise<VoxelType>::irtkGaussianNoise() : irtkNoise<VoxelType>()
{
  _Mean   = 0;
  _Sigma  = 1;
  _MinVal = VoxelType(MIN_GREY);
  _MaxVal = VoxelType(MAX_GREY);

  long temp = -1 * this->_Init;

  boost::mt19937 rng;
  rng.seed(temp);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);
  (void) var_nor();
}

template <class VoxelType> irtkGaussianNoise<VoxelType>::irtkGaussianNoise(double Mean, double Sigma, VoxelType MinVal, VoxelType MaxVal) : irtkNoise<VoxelType>()
{
  this->_Mean   = Mean;
  this->_Sigma  = Sigma;
  this->_MinVal = MinVal;
  this->_MaxVal = MaxVal;

  long temp = -1 * this->_Init;

  boost::mt19937 rng;
  rng.seed(temp);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);
  (void) var_nor();
}

template <class VoxelType> const char *irtkGaussianNoise<VoxelType>::NameOfClass()
{
  return "irtkGaussianNoise";
}

template <class VoxelType> double irtkGaussianNoise<VoxelType>::Run(int x, int y, int z, int t)
{
  boost::mt19937 rng;
  rng.seed(this->_Init);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);

  double tmp = this->_input->Get(x, y, z, t) + this->_Sigma * var_nor() + this->_Mean;
  if (tmp < this->_MinVal) return this->_MinVal;
  if (tmp > this->_MaxVal) return this->_MaxVal;
  return tmp;
}

template class irtkGaussianNoise<irtkBytePixel>;
template class irtkGaussianNoise<irtkGreyPixel>;
template class irtkGaussianNoise<irtkRealPixel>;


#include <boost/integer/common_factor.hpp>


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/test/trace.cpp
 *
 * \brief Test suite for the \c trace operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/trace.hpp>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

const double tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( real_square_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Square Matrix - Column-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 3;

    matrix_type A(n,n);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;
    A(2,0) = 7; A(2,1) = 8; A(2,2) = 9;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( real_square_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Square Matrix - Row-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 3;

    matrix_type A(n,n);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;
    A(2,0) = 7; A(2,1) = 8; A(2,2) = 9;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_square_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Square Matrix - Column-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 3;

    matrix_type A(n,n);
    A(0,0) = value_type(1,-5); A(0,1) = value_type(2, 3); A(0,2) = value_type(3,-9);
    A(1,0) = value_type(4, 4); A(1,1) = value_type(5,-2); A(1,2) = value_type(6, 8);
    A(2,0) = value_type(7,-3); A(2,1) = value_type(8, 1); A(2,2) = value_type(9,-7);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_square_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Square Matrix - Row-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 3;

    matrix_type A(n,n);
    A(0,0) = value_type(1,-5); A(0,1) = value_type(2, 3); A(0,2) = value_type(3,-9);
    A(1,0) = value_type(4, 4); A(1,1) = value_type(5,-2); A(1,2) = value_type(6, 8);
    A(2,0) = value_type(7,-3); A(2,1) = value_type(8, 1); A(2,2) = value_type(9,-7);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( real_recth_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Rectangular Horizontal Matrix - Column-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 3;
    const std::size_t m = 5;

    matrix_type A(n,m);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4; A(0,4) =  5;
    A(1,0) =  6; A(1,1) =  7; A(1,2) =  8; A(1,3) =  9; A(1,4) = 10;
    A(2,0) = 11; A(2,1) = 12; A(2,2) = 13; A(2,3) = 14; A(2,4) = 15;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( real_recth_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Rectangular Horizontal Matrix - Row-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 3;
    const std::size_t m = 5;

    matrix_type A(n,m);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4; A(0,4) =  5;
    A(1,0) =  6; A(1,1) =  7; A(1,2) =  8; A(1,3) =  9; A(1,4) = 10;
    A(2,0) = 11; A(2,1) = 12; A(2,2) = 13; A(2,3) = 14; A(2,4) = 15;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_recth_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Rectangular Horizontal Matrix - Column-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 3;
    const std::size_t m = 5;

    matrix_type A(n,m);
    A(0,0) = value_type( 1,-11); A(0,1) = value_type( 2, 12); A(0,2) = value_type( 3,-13); A(0,3) = value_type( 4, 14); A(0,4) = value_type( 5, -15);
    A(1,0) = value_type( 6,  6); A(1,1) = value_type( 7,- 7); A(1,2) = value_type( 8,  8); A(1,3) = value_type( 9,- 9); A(1,4) = value_type(10,  10);
    A(2,0) = value_type(11,- 1); A(2,1) = value_type(12,  2); A(2,2) = value_type(13,- 3); A(2,3) = value_type(14,  4); A(2,4) = value_type(15, - 5);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_recth_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Rectangular Horizontal Matrix - Row-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 3;
    const std::size_t m = 5;

    matrix_type A(n,m);
    A(0,0) = value_type( 1,-11); A(0,1) = value_type( 2, 12); A(0,2) = value_type( 3,-13); A(0,3) = value_type( 4, 14); A(0,4) = value_type( 5,-15);
    A(1,0) = value_type( 6,  6); A(1,1) = value_type( 7,- 7); A(1,2) = value_type( 8,  8); A(1,3) = value_type( 9,- 9); A(1,4) = value_type(10, 10);
    A(2,0) = value_type(11,- 1); A(2,1) = value_type(12,  2); A(2,2) = value_type(13,- 3); A(2,3) = value_type(14,  4); A(2,4) = value_type(15,- 5);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( real_rectv_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Rectangular Vertical Matrix - Column-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 5;
    const std::size_t m = 3;

    matrix_type A(n,m);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3;
    A(1,0) =  4; A(1,1) =  5; A(1,2) =  6;
    A(2,0) =  7; A(2,1) =  8; A(2,2) =  9;
    A(3,0) = 10; A(3,1) = 11; A(3,2) = 12;
    A(4,0) = 13; A(4,1) = 14; A(4,2) = 15;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( real_rectv_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Type - Rectangular Vertical Matrix - Row-Major");

    typedef double value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 5;
    const std::size_t m = 3;

    matrix_type A(n,m);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3;
    A(1,0) =  4; A(1,1) =  5; A(1,2) =  6;
    A(2,0) =  7; A(2,1) =  8; A(2,2) =  9;
    A(3,0) = 10; A(3,1) = 11; A(3,2) = 12;
    A(4,0) = 13; A(4,1) = 14; A(4,2) = 15;

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_rectv_col_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Rectangular Vertical Matrix - Column-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

    const std::size_t n = 5;
    const std::size_t m = 3;

    matrix_type A(n,m);
    A(0,0) = value_type( 1,-13); A(0,1) = value_type( 2, 14); A(0,2) = value_type( 3,-15);
    A(1,0) = value_type( 4, 10); A(1,1) = value_type( 5,-11); A(1,2) = value_type( 6, 12);
    A(2,0) = value_type( 7,- 7); A(2,1) = value_type( 8,  8); A(2,2) = value_type( 9,- 9);
    A(3,0) = value_type(10,  4); A(3,1) = value_type(11,- 5); A(3,2) = value_type(12,  6);
    A(4,0) = value_type(13,- 1); A(4,1) = value_type(14,  2); A(4,2) = value_type(15,  3);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

BOOST_UBLASX_TEST_DEF( complex_rectv_row_major )
{
    BOOST_UBLASX_DEBUG_TRACE("Test Case: Complex Type - Vertical Horizontal Matrix - Row-Major");

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

    const std::size_t n = 5;
    const std::size_t m = 3;

    matrix_type A(n,m);
    A(0,0) = value_type( 1,-13); A(0,1) = value_type( 2, 14); A(0,2) = value_type( 3,-15);
    A(1,0) = value_type( 4, 10); A(1,1) = value_type( 5,-11); A(1,2) = value_type( 6, 12);
    A(2,0) = value_type( 7,- 7); A(2,1) = value_type( 8,  8); A(2,2) = value_type( 9,- 9);
    A(3,0) = value_type(10,  4); A(3,1) = value_type(11,- 5); A(3,2) = value_type(12,  6);
    A(4,0) = value_type(13,- 1); A(4,1) = value_type(14,  2); A(4,2) = value_type(15,  3);

    value_type expect_tr = A(0,0)+A(1,1)+A(2,2);
    value_type tr = ublasx::trace(A);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "tr(A) = " << tr );
    BOOST_UBLASX_TEST_CHECK_CLOSE( tr, expect_tr, tol );
}

int main()
{
    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( real_square_col_major );
    BOOST_UBLASX_TEST_DO( real_square_row_major );
    BOOST_UBLASX_TEST_DO( complex_square_col_major );
    BOOST_UBLASX_TEST_DO( complex_square_row_major );
    BOOST_UBLASX_TEST_DO( real_recth_col_major );
    BOOST_UBLASX_TEST_DO( real_recth_row_major );
    BOOST_UBLASX_TEST_DO( complex_recth_col_major );
    BOOST_UBLASX_TEST_DO( complex_recth_row_major );
    BOOST_UBLASX_TEST_DO( real_rectv_col_major );
    BOOST_UBLASX_TEST_DO( real_rectv_row_major );
    BOOST_UBLASX_TEST_DO( complex_rectv_col_major );
    BOOST_UBLASX_TEST_DO( complex_rectv_row_major );

    BOOST_UBLASX_TEST_END();
}


#include <boost/config.hpp>
#include <iostream>
#include <string>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/read_dimacs.hpp>
#include "../src/stopwatch/stopwatch.h"

int main()
{

    Stopwatch S;
    S.set_mode(REAL_TIME);

    S.start("TOTAL");
    S.start("READ");
    using namespace boost;

    typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
    typedef adjacency_list<
        vecS, vecS, directedS, property<vertex_name_t, std::string>,
        property<edge_capacity_t, long, property<edge_residual_capacity_t, long, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph;

    Graph g;
    long flow;

    property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
    property_map<Graph, edge_reverse_t>::type rev = get(edge_reverse, g);
    property_map<Graph, edge_residual_capacity_t>::type residual_capacity = get(edge_residual_capacity, g);

    Traits::vertex_descriptor s, t;
    read_dimacs_max_flow(g, capacity, rev, s, t);
    S.stop("READ");

    S.start("BST");
    flow = push_relabel_max_flow(g, s, t);
    S.stop("BST");
    S.stop("TOTAL");

    std::cout << "Vertices   : " << t + 1 << std::endl;
    std::cout << "Edges      : " << -1 << std::endl;
    // std::cout << "\n";
    std::cout << "BST Flow   : " << flow << std::endl;
    // std::cout << "Sta Flow : " << flowS << std::endl;
    // std::cout << "FF  Flow : " << FF.E.getFlow() << std::endl;
    // std::cout << "\n";
    std::cout << "BST Time   : " << S.get_total_time("BST") << std::endl;
    std::cout << "READ Time  : " << S.get_total_time("READ") << std::endl;
    std::cout << "TOTAL Time : " << S.get_total_time("TOTAL") << std::endl;
    // std::cout << "Sta Time : " << S.get_total_time("S") << endl;
    // std::cout << "FF  Time : " << S.get_total_time("FF") << endl;
    // std::cout << "\n";
    // std::cout << "FF Paths : " << FF.IterationsCount << endl;
    // std::cout << "\n";
    // std::cout << "Push     : " << RTF.PushCount << endl;
    // std::cout << "Relabel  : " << RTF.RelabelCount << endl;
    // std::cout << "Discharge: " << RTF.DischargeCount << endl;

    // std::cout << "c  The total flow:" << std::endl;
    // std::cout << "s " << flow << std::endl << std::endl;

    // std::cout << "c flow values:" << std::endl;
    // graph_traits<Graph>::vertex_iterator u_iter, u_end;
    // graph_traits<Graph>::out_edge_iterator ei, e_end;
    // for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
    //     for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
    //         if (capacity[*ei] > 0)
    //             std::cout << "f " << *u_iter << " " << target(*ei, g) << " " << (capacity[*ei] - residual_capacity[*ei]) << std::endl;
    return 0;
}


/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#ifndef EIGEN_OPERATOR_UTILITIES_H
    #define EIGEN_OPERTAOR_UTILITIES_h

#include <Eigen/Dense>

namespace Eigen
{

template<typename Derived>
Eigen::MatrixBase<Derived>& operator-=(Eigen::MatrixBase<Derived> &X, const typename Derived::Scalar &a)
{
    // Acceptably inefficient
    X -= a * Matrix<typename Derived::Scalar, -1, -1>::Ones(X.rows(), X.cols());
    return X;
}

template<typename Derived>
Eigen::MatrixBase<Derived>& operator+=(Eigen::MatrixBase<Derived> &X, const typename Derived::Scalar &a)
{
    X += a * Matrix<typename Derived::Scalar, -1, -1>::Ones(X.rows(), X.cols());
    return X;
}

}

#endif // EIGEN_OPERTAOR_UTILITIES_h


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from libasrl
#include <sm/eigen/gtest.hpp>

#include <sparse_block_matrix/linear_solver_cholmod.h>
#include <sparse_block_matrix/linear_solver_dense.h>
#include <sparse_block_matrix/linear_solver_spqr.h>

template <typename SOLVER_T>
void randomSparseBlockMatrix(sparse_block_matrix::SparseBlockMatrix<typename SOLVER_T::matrix_t>* A,
                             Eigen::MatrixXd& Adense) {
    typedef typename SOLVER_T::matrix_t SparseMatrixBlock;

    // Fill in like this:
    //   0  1   2
    // 0 a
    // 1    b   d
    // 2        c
    //
    // where a,b,c are symmetric
    // d is 3x5

    SparseMatrixBlock* a = A->block(0, 0, true);
    ASSERT_TRUE(a != NULL);
    ASSERT_EQ(a->rows(), 3);
    ASSERT_EQ(a->cols(), 3);
    a->setRandom();
    *a = (*a * a->transpose()) + Eigen::Matrix3d::Identity();
    Adense.block(0, 0, 3, 3) = *a;

    SparseMatrixBlock* b = A->block(1, 1, true);
    ASSERT_TRUE(b != NULL);
    ASSERT_EQ(b->rows(), 3);
    ASSERT_EQ(b->cols(), 3);
    b->setRandom();
    *b = (*b * b->transpose()) + Eigen::Matrix3d::Identity();
    Adense.block(3, 3, 3, 3) = *b;

    SparseMatrixBlock* c = A->block(2, 2, true);
    ASSERT_TRUE(c != NULL);
    ASSERT_EQ(c->rows(), 5);
    ASSERT_EQ(c->cols(), 5);
    c->setRandom();
    *c = (*c * c->transpose()).eval() + Eigen::MatrixXd::Identity(5, 5);
    // std::cout << "c:\n" << *c << std::endl;
    Adense.block(6, 6, 5, 5) = *c;

    SparseMatrixBlock* d = A->block(1, 2, true);
    ASSERT_TRUE(d != NULL);
    ASSERT_EQ(d->rows(), 3);
    ASSERT_EQ(d->cols(), 5);
    d->setRandom();
    Adense.block(3, 6, 3, 5) = *d;
}

template <typename SOLVER_T>
void testSolver(const std::string& solver_name) {
    // Build up a sparse matrix
    // 3x3 3x3 3x5
    // 3x3 3x3 3x5
    // 5x3 5x3 5x5
    int rows[] = {3, 6, 11};
    int cols[] = {3, 6, 11};
    sparse_block_matrix::SparseBlockMatrix<typename SOLVER_T::matrix_t> A(rows, cols, 3, 3);

    Eigen::MatrixXd Adense(11, 11);
    Adense.setZero();

    ASSERT_EQ(A.rows(), 11);
    ASSERT_EQ(A.cols(), 11);

    randomSparseBlockMatrix<SOLVER_T>(&A, Adense);

    Eigen::VectorXd xx(A.rows());
    xx.setZero();
    Eigen::VectorXd xx2(A.rows());
    xx2.setZero();

    SOLVER_T solver;
    ASSERT_TRUE(solver.init());

    Eigen::VectorXd bb(A.rows());
    bb.setRandom();

    // virtual bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b) = 0;
    ASSERT_TRUE(solver.solve(A, &xx[0], &bb[0]));
    // Solve dense
    Eigen::VectorXd dx = Adense.selfadjointView<Eigen::Upper>().ldlt().solve(bb);

    //  always solve twice to make sure the value only copying is working for symbolic factorizations
    randomSparseBlockMatrix<SOLVER_T>(&A, Adense);
    ASSERT_TRUE(solver.solve(A, &xx2[0], &bb[0]));

    Eigen::VectorXd dx2 = Adense.selfadjointView<Eigen::Upper>().ldlt().solve(bb);

    // Solve dense

    // Make sure the solutions match.
    sm::eigen::assertNear(dx, xx, 1e-10, SM_SOURCE_FILE_POS, "A: dense solution, B: solution from " + solver_name);
    sm::eigen::assertNear(dx2, xx2, 1e-10, SM_SOURCE_FILE_POS, "A: dense solution, B: solution from " + solver_name);
}

// Check that the setup as a whole is correct
TEST(g2oTestSuite, testCholmod) {
    //
    testSolver<sparse_block_matrix::LinearSolverQr<Eigen::MatrixXd> >("sparseQR");

    testSolver<sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> >("cholmod");

    testSolver<sparse_block_matrix::LinearSolverDense<Eigen::MatrixXd> >("dense");
}


// Copyright 2006. Peter Gottschling, Matthias Troyer, Rolf Bonderer
// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MATH_INVERSE_INCLUDE
#define MATH_INVERSE_INCLUDE

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

namespace math {

template <typename Operation, typename Element>
struct inverse_t {} ;

template <typename Element>
struct inverse_t< add<Element>, Element >
  : public binary_function<add<Element>, Element, Element>
{
    Element operator()(const add<Element>&, const Element& v) const
    {
	return -v;
    }
};

template <typename Element>
struct inverse_t< mult<Element>, Element >
  : public binary_function<mult<Element>, Element, Element>
{
    Element operator()(const mult<Element>&, const Element& v) const
    {
	return one(v) / v ;
    }
};

// Function is shorter than typetrait-like functor
template <typename Operation, typename Element>
inline Element inverse(const Operation& op, const Element& v)
{
    return inverse_t<Operation, Element>() (op, v);
}

// Short-cut for multiplicative inverse
template <typename Element>
inline Element reciprocal(const Element& v)
{
    return inverse(math::mult<Element>(), v);
}

} // namespace math

#endif // MATH_INVERSE_INCLUDE


#ifndef _mpipyp_hh
#define _mpipyp_hh

#include <math.h>
#include <map>
#include <tr1/unordered_map>
//#include <google/sparse_hash_map>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/serialization/map.hpp>
#include <boost/mpi.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/operations.hpp>

#include "pyp.hh"

//
// Pitman-Yor process with customer and table tracking
//

template <typename Dish, typename Hash=std::tr1::hash<Dish> >
class MPIPYP : public PYP<Dish, Hash> {
public:
  typedef std::map<Dish, int> dish_delta_type;

  MPIPYP(double a, double b, Hash hash=Hash());

  template < typename Uniform01 >
    int increment(Dish d, double p0, Uniform01& rnd);
  template < typename Uniform01 >
    int decrement(Dish d, Uniform01& rnd);

  void clear();
  void reset_deltas();

  void synchronise(dish_delta_type* result);

private:
  typedef std::map<Dish, typename PYP<Dish,Hash>::TableCounter> table_delta_type;

  dish_delta_type m_count_delta;
  table_delta_type m_table_delta;
};

template <typename Dish, typename Hash>
MPIPYP<Dish,Hash>::MPIPYP(double a, double b, Hash h)
: PYP<Dish,Hash>(a, b, 0, h) {}

template <typename Dish, typename Hash>
  template <typename Uniform01>
int
MPIPYP<Dish,Hash>::increment(Dish dish, double p0, Uniform01& rnd) {
  //std::cerr << "-----INCREMENT DISH " << dish << std::endl;
  int delta = 0;
  int table_joined=-1;
  typename PYP<Dish,Hash>::TableCounter &tc = PYP<Dish,Hash>::_dish_tables[dish];

  // seated on a new or existing table?
  int c = PYP<Dish,Hash>::count(dish);
  int t = PYP<Dish,Hash>::num_tables(dish);
  int T = PYP<Dish,Hash>::num_tables();
  double& a = PYP<Dish,Hash>::_a;
  double& b = PYP<Dish,Hash>::_b;
  double pshare = (c > 0) ? (c - a*t) : 0.0;
  double pnew = (b + a*T) * p0;
  if (pshare < 0.0) {
    std::cerr << pshare << " " << c << " " << a << " " << t << std::endl;
    assert(false);
  }

  if (rnd() < pnew / (pshare + pnew)) {
    // assign to a new table
    tc.tables += 1;
    tc.table_histogram[1] += 1;
    PYP<Dish,Hash>::_total_tables += 1;
    delta = 1;
    table_joined = 1;
  }
  else {
    // randomly assign to an existing table
    // remove constant denominator from inner loop
    double r = rnd() * (c - a*t);
    for (std::map<int,int>::iterator
         hit = tc.table_histogram.begin();
         hit != tc.table_histogram.end(); ++hit) {
      r -= ((hit->first - a) * hit->second);
      if (r <= 0) {
        tc.table_histogram[hit->first+1] += 1;
        hit->second -= 1;
        table_joined = hit->first+1;
        if (hit->second == 0)
          tc.table_histogram.erase(hit);
        break;
      }
    }
    if (r > 0) {
      std::cerr << r << " " << c << " " << a << " " << t << std::endl;
      assert(false);
    }
    delta = 0;
  }

  std::tr1::unordered_map<Dish,int,Hash>::operator[](dish) += 1;
  //google::sparse_hash_map<Dish,int,Hash>::operator[](dish) += 1;
  PYP<Dish,Hash>::_total_customers += 1;

  // MPI Delta handling
  // track the customer entering
  typename dish_delta_type::iterator customer_it;
  bool customer_insert_result;
  boost::tie(customer_it, customer_insert_result)
    = m_count_delta.insert(std::make_pair(dish,0));

  customer_it->second += 1;
  if (customer_it->second == 0)
    m_count_delta.erase(customer_it);

  // increment the histogram bar for the table joined
  /*
  typename PYP<Dish,Hash>::TableCounter &delta_tc = m_table_delta[dish];

  std::map<int,int> &histogram = delta_tc.table_histogram;
  assert (table_joined > 0);

  typename std::map<int,int>::iterator table_it; bool table_insert_result;
  boost::tie(table_it, table_insert_result) = histogram.insert(std::make_pair(table_joined,0));
  table_it->second += 1;
  if (delta == 0) {
    // decrement the histogram bar for the table left
    typename std::map<int,int>::iterator left_table_it;
    boost::tie(left_table_it, table_insert_result)
      = histogram.insert(std::make_pair(table_joined-1,0));
    left_table_it->second -= 1;
    if (left_table_it->second == 0) histogram.erase(left_table_it);
  }
  else delta_tc.tables += 1;

  if (table_it->second == 0) histogram.erase(table_it);

    //std::cerr << "Added (" << delta << ") " << dish << " to table " << table_joined << "\n";
    //std::cerr << "Dish " << dish << " has " << count(dish) << " customers, and is sitting at " << PYP<Dish,Hash>::num_tables(dish) << " tables.\n";
    //for (std::map<int,int>::const_iterator
    //     hit = delta_tc.table_histogram.begin();
    //     hit != delta_tc.table_histogram.end(); ++hit) {
    //  std::cerr << "    " << hit->second << " tables with " << hit->first << " customers." << std::endl;
    //}
    //std::cerr << "Added (" << delta << ") " << dish << " to table " << table_joined << "\n";
    //std::cerr << "Dish " << dish << " has " << count(dish) << " customers, and is sitting at " << PYP<Dish,Hash>::num_tables(dish) << " tables.\n";
    int x_num_customers=0, x_num_table=0;
    for (std::map<int,int>::const_iterator
         hit = delta_tc.table_histogram.begin();
         hit != delta_tc.table_histogram.end(); ++hit) {
      x_num_table += hit->second;
      x_num_customers += (hit->second*hit->first);
    }
    int tmp_c = PYP<Dish,Hash>::count(dish);
    int tmp_t = PYP<Dish,Hash>::num_tables(dish);
    assert (x_num_customers <= tmp_c);
    assert (x_num_table <= tmp_t);

  if (delta_tc.table_histogram.empty()) {
    assert (delta_tc.tables == 0);
    m_table_delta.erase(dish);
  }
  */

  //PYP<Dish,Hash>::debug_info(std::cerr);
  //std::cerr << "   Dish " << dish << " has count " << PYP<Dish,Hash>::count(dish) << " tables " << PYP<Dish,Hash>::num_tables(dish) << std::endl;

  return delta;
}

template <typename Dish, typename Hash>
  template <typename Uniform01>
int
MPIPYP<Dish,Hash>::decrement(Dish dish, Uniform01& rnd)
{
  //std::cerr << "-----DECREMENT DISH " << dish << std::endl;
  typename std::tr1::unordered_map<Dish, int>::iterator dcit = find(dish);
  //typename google::sparse_hash_map<Dish, int>::iterator dcit = find(dish);
  if (dcit == PYP<Dish,Hash>::end()) {
    std::cerr << dish << std::endl;
    assert(false);
  }

  int delta = 0, table_left=-1;

  typename std::tr1::unordered_map<Dish, typename PYP<Dish,Hash>::TableCounter>::iterator dtit
    = PYP<Dish,Hash>::_dish_tables.find(dish);
  //typename google::sparse_hash_map<Dish, TableCounter>::iterator dtit = _dish_tables.find(dish);
  if (dtit == PYP<Dish,Hash>::_dish_tables.end()) {
    std::cerr << dish << std::endl;
    assert(false);
  }
  typename PYP<Dish,Hash>::TableCounter &tc = dtit->second;

  double r = rnd() * PYP<Dish,Hash>::count(dish);
  for (std::map<int,int>::iterator hit = tc.table_histogram.begin();
       hit != tc.table_histogram.end(); ++hit) {
    r -= (hit->first * hit->second);
    if (r <= 0) {
      table_left = hit->first;
      if (hit->first > 1) {
        tc.table_histogram[hit->first-1] += 1;
      }
      else {
        delta = -1;
        tc.tables -= 1;
        PYP<Dish,Hash>::_total_tables -= 1;
      }

      hit->second -= 1;
      if (hit->second == 0) tc.table_histogram.erase(hit);
      break;
    }
  }
  if (r > 0) {
    std::cerr << r << " " << PYP<Dish,Hash>::count(dish) << " " << PYP<Dish,Hash>::_a << " "
      << PYP<Dish,Hash>::num_tables(dish) << std::endl;
    assert(false);
  }

  // remove the customer
  dcit->second -= 1;
  PYP<Dish,Hash>::_total_customers -= 1;
  assert(dcit->second >= 0);
  if (dcit->second == 0) {
    PYP<Dish,Hash>::erase(dcit);
    PYP<Dish,Hash>::_dish_tables.erase(dtit);
  }

  // MPI Delta processing
  typename dish_delta_type::iterator it;
  bool insert_result;
  boost::tie(it, insert_result) = m_count_delta.insert(std::make_pair(dish,0));
  it->second -= 1;
  if (it->second == 0) m_count_delta.erase(it);

  assert (table_left > 0);
  typename PYP<Dish,Hash>::TableCounter& delta_tc = m_table_delta[dish];
  if (table_left > 1) {
    std::map<int,int>::iterator tit;
    boost::tie(tit, insert_result) = delta_tc.table_histogram.insert(std::make_pair(table_left-1,0));
    tit->second += 1;
    if (tit->second == 0) delta_tc.table_histogram.erase(tit);
  }
  else delta_tc.tables -= 1;

  std::map<int,int>::iterator tit;
  boost::tie(tit, insert_result) = delta_tc.table_histogram.insert(std::make_pair(table_left,0));
  tit->second -= 1;
  if (tit->second == 0) delta_tc.table_histogram.erase(tit);

  //  std::cerr << "Dish " << dish << " has " << count(dish) << " customers, and is sitting at " << PYP<Dish,Hash>::num_tables(dish) << " tables.\n";
  //  for (std::map<int,int>::const_iterator
  //       hit = delta_tc.table_histogram.begin();
  //       hit != delta_tc.table_histogram.end(); ++hit) {
  //    std::cerr << "    " << hit->second << " tables with " << hit->first << " customers." << std::endl;
  //  }
    int x_num_customers=0, x_num_table=0;
    for (std::map<int,int>::const_iterator
         hit = delta_tc.table_histogram.begin();
         hit != delta_tc.table_histogram.end(); ++hit) {
      x_num_table += hit->second;
      x_num_customers += (hit->second*hit->first);
    }
    int tmp_c = PYP<Dish,Hash>::count(dish);
    int tmp_t = PYP<Dish,Hash>::num_tables(dish);
    assert (x_num_customers <= tmp_c);
    assert (x_num_table <= tmp_t);

  if (delta_tc.table_histogram.empty()) {
  //  std::cerr << "   DELETING " << dish << std::endl;
    assert (delta_tc.tables == 0);
    m_table_delta.erase(dish);
  }

  //PYP<Dish,Hash>::debug_info(std::cerr);
  //std::cerr << "   Dish " << dish << " has count " << PYP<Dish,Hash>::count(dish) << " tables " << PYP<Dish,Hash>::num_tables(dish) << std::endl;
  return delta;
}

template <typename Dish, typename Hash>
void
MPIPYP<Dish,Hash>::clear() {
  PYP<Dish,Hash>::clear();
  reset_deltas();
}

template <typename Dish, typename Hash>
void
MPIPYP<Dish,Hash>::reset_deltas() {
  m_count_delta.clear();
  m_table_delta.clear();
}

template <typename Dish>
struct sum_maps {
  typedef std::map<Dish,int> map_type;
  map_type& operator() (map_type& l, map_type const & r) const {
    for (typename map_type::const_iterator it=r.begin(); it != r.end(); it++)
      l[it->first] += it->second;
    return l;
  }
};

template <typename Dish>
struct subtract_maps {
  typedef std::map<Dish,int> map_type;
  map_type& operator() (map_type& l, map_type const & r) const {
    for (typename map_type::const_iterator it=r.begin(); it != r.end(); it++)
      l[it->first] -= it->second;
    return l;
  }
};

// Needed Boost definitions
namespace boost {
  namespace mpi {
    template <>
    struct is_commutative< sum_maps<int>, std::map<int,int> > : mpl::true_ {};
  }

  namespace serialization {
    template<class Archive>
    void serialize(Archive & ar, PYP<int>::TableCounter& t, const unsigned int version) {
      ar & t.table_histogram;
      ar & t.tables;
    }

  } // namespace serialization
} // namespace boost

template <typename A, typename B, typename C>
struct triple {
  triple() {}
  triple(const A& a, const B& b, const C& c) : first(a), second(b), third(c) {}
  A first;
  B second;
  C third;

  template<class Archive>
  void serialize(Archive &ar, const unsigned int version){
      ar & first;
      ar & second;
      ar & third;
  }
};

BOOST_IS_BITWISE_SERIALIZABLE(MPIPYP<int>::dish_delta_type)
BOOST_CLASS_TRACKING(MPIPYP<int>::dish_delta_type,track_never)

template <typename Dish, typename Hash>
void
MPIPYP<Dish,Hash>::synchronise(dish_delta_type* result) {
  boost::mpi::communicator world;
  //int rank = world.rank(), size = world.size();

  boost::mpi::all_reduce(world, m_count_delta, *result, sum_maps<Dish>());
  subtract_maps<Dish>()(*result, m_count_delta);

/*
  // communicate the customer count deltas
  dish_delta_type global_dish_delta;
  boost::mpi::all_reduce(world, m_count_delta, global_dish_delta, sum_maps<Dish>());

  // update this restaurant
  for (typename dish_delta_type::const_iterator it=global_dish_delta.begin();
       it != global_dish_delta.end(); ++it) {
    int global_delta = it->second - m_count_delta[it->first];
    if (global_delta == 0) continue;
    typename std::tr1::unordered_map<Dish,int,Hash>::iterator dit; bool inserted;
    boost::tie(dit, inserted)
      = std::tr1::unordered_map<Dish,int,Hash>::insert(std::make_pair(it->first, 0));
    dit->second += global_delta;
    assert(dit->second >= 0);
    if (dit->second == 0) {
      std::tr1::unordered_map<Dish,int,Hash>::erase(dit);
    }

    PYP<Dish,Hash>::_total_customers += (it->second - m_count_delta[it->first]);
    int tmp = PYP<Dish,Hash>::_total_customers;
    assert(tmp >= 0);
    //std::cerr << "Process " << rank << " adding " <<  (it->second - m_count_delta[it->first]) << " of customer " << it->first << std::endl;
  }
*/
/*
  // communicate the table count deltas
  for (int process = 0; process < size; ++process) {
    typename std::vector< triple<Dish, int, int> > message;
    if (rank == process) {
      // broadcast deltas
      for (typename table_delta_type::const_iterator dish_it=m_table_delta.begin();
           dish_it != m_table_delta.end(); ++dish_it) {
        //assert (dish_it->second.tables > 0);
        for (std::map<int,int>::const_iterator it=dish_it->second.table_histogram.begin();
             it != dish_it->second.table_histogram.end(); ++it) {
          triple<Dish, int, int> m(dish_it->first, it->first, it->second);
          message.push_back(m);
        }
        // append a special message with the total table delta for this dish
        triple<Dish, int, int> m(dish_it->first, -1, dish_it->second.tables);
        message.push_back(m);
      }
      boost::mpi::broadcast(world, message, process);
    }
    else {
      // receive deltas
      boost::mpi::broadcast(world, message, process);
      for (typename std::vector< triple<Dish, int, int> >::const_iterator it=message.begin(); it != message.end(); ++it) {
        typename PYP<Dish,Hash>::TableCounter& tc = PYP<Dish,Hash>::_dish_tables[it->first];
        if (it->second >= 0) {
          std::map<int,int>::iterator tit; bool inserted;
          boost::tie(tit, inserted) = tc.table_histogram.insert(std::make_pair(it->second, 0));
          tit->second += it->third;
          if (tit->second < 0) {
            std::cerr << tit->first << " " << tit->second << " " << it->first << " " << it->second << " " << it->third << std::endl;
            assert(tit->second >= 0);
          }
          if (tit->second == 0) {
            tc.table_histogram.erase(tit);
          }
        }
        else {
          tc.tables += it->third;
          PYP<Dish,Hash>::_total_tables += it->third;
          assert(tc.tables >= 0);
          if (tc.tables == 0) assert(tc.table_histogram.empty());
          if (tc.table_histogram.empty()) {
            assert (tc.tables == 0);
            PYP<Dish,Hash>::_dish_tables.erase(it->first);
          }
        }
      }
    }
  }
*/

//  reset_deltas();
}

#endif


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <iostream>
#include "vision-precomp.h"  // Precompiled headers

//#include <mrpt/math/types_math.h>  // Eigen must be included first via MRPT to
// enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/StdVector>

#include "lhm.h"
using namespace mrpt::vision::pnp;

lhm::lhm(
	Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_,
	int n0)
	: F(n0)
{
	obj_pts = obj_pts_;
	img_pts = img_pts_;
	cam_intrinsic = cam_;
	n = n0;

	// Store obj_pts as 3XN and img_projections as 2XN matrices
	P = obj_pts.transpose();
	Q = Eigen::MatrixXd::Ones(3, n);

	Q = img_pts.transpose();

	t.setZero();
}

void lhm::estimate_t()
{
	Eigen::Vector3d sum_;
	sum_.setZero();
	for (int i = 0; i < n; i++) sum_ += F[i] * R * P.col(i);
	t = G * sum_;
}

void lhm::xform()
{
	for (int i = 0; i < n; i++) Q.col(i) = R * P.col(i) + t;
}

Eigen::Matrix4d lhm::qMatQ(Eigen::VectorXd q)
{
	Eigen::Matrix4d Q_(4, 4);

	Q_ << q(0), -q(1), -q(2), -q(3), q(1), q(0), -q(3), q(2), q(2), q(3), q(0),
		-q(1), q(3), -q(2), q(1), q(0);

	return Q_;
}

Eigen::Matrix4d lhm::qMatW(Eigen::VectorXd q)
{
	Eigen::Matrix4d Q_(4, 4);

	Q_ << q(0), -q(1), -q(2), -q(3), q(1), q(0), q(3), -q(2), q(2), -q(3), q(0),
		q(1), q(3), q(2), -q(1), q(0);

	return Q_;
}

void lhm::absKernel()
{
	for (int i = 0; i < n; i++) Q.col(i) = F[i] * Q.col(i);

	Eigen::Vector3d P_bar, Q_bar;
	P_bar = P.rowwise().mean();
	Q_bar = Q.rowwise().mean();

	for (int i = 0; i < n; i++)
	{
		P.col(i) = P.col(i) - P_bar;
		Q.col(i) = Q.col(i) - Q_bar;
	}

	//<------------------- Use SVD Solution ------------------->//
	/*
	Eigen::Matrix3d M;
	M.setZero();

	for (i = 0; i < n; i++)
		M += P.col(i)*Q.col(i).transpose();

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU |
	Eigen::ComputeThinV);

	R = svd.matrixV()*svd.matrixU().transpose();

	Eigen::Matrix3d dummy;
	dummy.col(0) = -svd.matrixV().col(0);
	dummy.col(1) = -svd.matrixV().col(1);
	dummy.col(2) = svd.matrixV().col(2);

	if (R.determinant() == 1)
	{
		estimate_t();
		if (t(2) < 0)
		{
			R = dummy*svd.matrixU().transpose();
			estimate_t();
		}
	}
	else
	{
		R = -dummy*svd.matrixU().transpose();
		estimate_t();
		if (t(2) < 0)
		{
			R = -svd.matrixV()*svd.matrixU().transpose();
			estimate_t();
		}
	}

	err2 = 0;
	xform();

	Eigen::Vector3d vec;
	Eigen::Matrix3d I3 = Eigen::MatrixXd::Identity(3, 3);

	for (i = 0; i < n; i++)
	{
		vec = (I3 - F[i])*Q.col(i);
		err2 += vec.squaredNorm();
	}
	*/
	//<------------------- Use QTN Solution ------------------->//

	Eigen::Matrix4d A;
	A.setZero();

	for (int i = 0; i < n; i++)
	{
		Eigen::Vector4d q1, q2;
		q1 << 1, Q.col(i);
		q2 << 1, P.col(i);
		A += qMatQ(q1).transpose() * qMatW(q2);
	}

	Eigen::EigenSolver<Eigen::Matrix4d> es(A);

	const Eigen::Matrix4d Ae = es.pseudoEigenvalueMatrix();
	Eigen::Vector4d D;  // Ae.diagonal(); for some reason this leads to an
	// internal compiler error in MSVC11... (sigh)
	for (int i = 0; i < 4; i++) D[i] = Ae(i, i);

	Eigen::Matrix4d V_mat = es.pseudoEigenvectors();

	Eigen::Vector4d::Index max_index;

	D.maxCoeff(&max_index);

	Eigen::Vector4d V;

	V = V_mat.col(max_index);

	Eigen::Quaterniond q(V(0), V(1), V(2), V(3));

	R = q.toRotationMatrix();

	estimate_t();

	err2 = 0;
	xform();

	Eigen::Vector3d vec;
	Eigen::Matrix3d I3 = Eigen::MatrixXd::Identity(3, 3);

	for (int i = 0; i < n; i++)
	{
		vec = (I3 - F[i]) * Q.col(i);
		err2 += vec.squaredNorm();
	}
}

bool lhm::compute_pose(
	Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
	int i, j = 0;

	Eigen::VectorXd p_bar;
	Eigen::Matrix3d sum_F, I3;
	I3 = Eigen::MatrixXd::Identity(3, 3);
	sum_F.setZero();

	p_bar = P.rowwise().mean();

	for (i = 0; i < n; i++)
	{
		P.col(i) -= p_bar;
		F[i] = Q.col(i) * Q.col(i).transpose() / Q.col(i).squaredNorm();
		sum_F = sum_F + F[i];
	}

	G = (I3 - sum_F / n).inverse() / n;

	err = 0;
	err2 = 1000;
	absKernel();

	while (std::abs(err2 - err) > TOL_LHM && err2 > EPSILON_LHM)
	{
		err = err2;

		absKernel();

		j += 1;
		if (j > 100) break;
	}

	R_ = R;
	t_ = t - R * p_bar;

	return true;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2005, 2006 Theo Boafo
 Copyright (C) 2006, 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

// the only header you need to use QuantLib
#include <ql/quantlib.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>

#define LENGTH(a) (sizeof(a)/sizeof(a[0]))

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        Option::Type type(Option::Put);
        Real underlying = 36.0;
        Real spreadRate = 0.005;

        Spread dividendYield = 0.02;
        Rate riskFreeRate = 0.06;
        Volatility volatility = 0.20;

        Integer settlementDays = 3;
        Integer length = 5;
        Real redemption = 100.0;
        Real conversionRatio = redemption/underlying; // at the money

        // set up dates/schedules
        Calendar calendar = TARGET();
        Date today = calendar.adjust(Date::todaysDate());

        Settings::instance().evaluationDate() = today;
        Date settlementDate = calendar.advance(today, settlementDays, Days);
        Date exerciseDate = calendar.advance(settlementDate, length, Years);
        Date issueDate = calendar.advance(exerciseDate, -length, Years);

        BusinessDayConvention convention = ModifiedFollowing;

        Frequency frequency = Annual;

        Schedule schedule(issueDate, exerciseDate,
                          Period(frequency), calendar,
                          convention, convention,
                          DateGeneration::Backward, false);

        DividendSchedule dividends;
        CallabilitySchedule callability;

        std::vector<Real> coupons(1, 0.05);

        DayCounter bondDayCount = Thirty360();

        Integer callLength[] = { 2, 4 };  // Call dates, years 2, 4.
        Integer putLength[] = { 3 }; // Put dates year 3

        Real callPrices[] = { 101.5, 100.85 };
        Real putPrices[]= { 105.0 };

        // Load call schedules
        for (Size i=0; i<LENGTH(callLength); i++) {
            callability.push_back(
                   boost::shared_ptr<Callability>(
                       new SoftCallability(Callability::Price(
                                                   callPrices[i],
                                                   Callability::Price::Clean),
                                           schedule.date(callLength[i]),
                                           1.20)));
        }

        for (Size j=0; j<LENGTH(putLength); j++) {
            callability.push_back(
                   boost::shared_ptr<Callability>(
                           new Callability(Callability::Price(
                                                   putPrices[j],
                                                   Callability::Price::Clean),
                                           Callability::Put,
                                           schedule.date(putLength[j]))));
        }

        // Assume dividends are paid every 6 months.
        for (Date d = today + 6*Months; d < exerciseDate; d += 6*Months) {
            dividends.push_back(
                      boost::shared_ptr<Dividend>(new FixedDividend(1.0, d)));
        }

        DayCounter dayCounter = Actual365Fixed();
        Time maturity = dayCounter.yearFraction(settlementDate,
                                                exerciseDate);

        std::cout << "option type = "  << type << std::endl;
        std::cout << "Time to maturity = "        << maturity
                  << std::endl;
        std::cout << "Underlying price = "        << underlying
                  << std::endl;
        std::cout << "Risk-free interest rate = " << io::rate(riskFreeRate)
                  << std::endl;
        std::cout << "Dividend yield = " << io::rate(dividendYield)
                  << std::endl;
        std::cout << "Volatility = " << io::volatility(volatility)
                  << std::endl;
        std::cout << std::endl;

        std::string method;
        std::cout << std::endl ;

        // write column headings
        Size widths[] = { 35, 14, 14 };
        Size totalWidth = widths[0] + widths[1] + widths[2];
        std::string rule(totalWidth, '-'), dblrule(totalWidth, '=');

        std::cout << dblrule << std::endl;
        std::cout << "Tsiveriotis-Fernandes method" << std::endl;
        std::cout << dblrule << std::endl;
        std::cout << std::setw(widths[0]) << std::left << "Tree type"
                  << std::setw(widths[1]) << std::left << "European"
                  << std::setw(widths[1]) << std::left << "American"
                  << std::endl;
        std::cout << rule << std::endl;

        boost::shared_ptr<Exercise> exercise(
                                          new EuropeanExercise(exerciseDate));
        boost::shared_ptr<Exercise> amExercise(
                                          new AmericanExercise(settlementDate,
                                                               exerciseDate));

        Handle<Quote> underlyingH(
            boost::shared_ptr<Quote>(new SimpleQuote(underlying)));

        Handle<YieldTermStructure> flatTermStructure(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, riskFreeRate, dayCounter)));

        Handle<YieldTermStructure> flatDividendTS(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, dividendYield, dayCounter)));

        Handle<BlackVolTermStructure> flatVolTS(
            boost::shared_ptr<BlackVolTermStructure>(
                new BlackConstantVol(settlementDate, calendar,
                                     volatility, dayCounter)));

        boost::shared_ptr<BlackScholesMertonProcess> stochasticProcess(
                              new BlackScholesMertonProcess(underlyingH,
                                                            flatDividendTS,
                                                            flatTermStructure,
                                                            flatVolTS));

        Size timeSteps = 801;

        Handle<Quote> creditSpread(
                       boost::shared_ptr<Quote>(new SimpleQuote(spreadRate)));

        boost::shared_ptr<Quote> rate(new SimpleQuote(riskFreeRate));

        Handle<YieldTermStructure> discountCurve(
                boost::shared_ptr<YieldTermStructure>(
                    new FlatForward(today, Handle<Quote>(rate), dayCounter)));

        boost::shared_ptr<PricingEngine> engine(
                  new BinomialConvertibleEngine<JarrowRudd>(stochasticProcess,
                                                            timeSteps));

        ConvertibleFixedCouponBond europeanBond(
                            exercise, conversionRatio, dividends, callability,
                            creditSpread, issueDate, settlementDays,
                            coupons, bondDayCount, schedule, redemption);
        europeanBond.setPricingEngine(engine);

        ConvertibleFixedCouponBond americanBond(
                          amExercise, conversionRatio, dividends, callability,
                          creditSpread, issueDate, settlementDays,
                          coupons, bondDayCount, schedule, redemption);
        americanBond.setPricingEngine(engine);

        method = "Jarrow-Rudd";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                  new BinomialConvertibleEngine<JarrowRudd>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                  new BinomialConvertibleEngine<JarrowRudd>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Cox-Ross-Rubinstein";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
           new BinomialConvertibleEngine<CoxRossRubinstein>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
           new BinomialConvertibleEngine<CoxRossRubinstein>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Additive equiprobabilities";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                   new BinomialConvertibleEngine<AdditiveEQPBinomialTree>(
                                                            stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                   new BinomialConvertibleEngine<AdditiveEQPBinomialTree>(
                                                            stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Trigeorgis";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                  new BinomialConvertibleEngine<Trigeorgis>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                  new BinomialConvertibleEngine<Trigeorgis>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Tian";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                        new BinomialConvertibleEngine<Tian>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                        new BinomialConvertibleEngine<Tian>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Leisen-Reimer";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialConvertibleEngine<LeisenReimer>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialConvertibleEngine<LeisenReimer>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        method = "Joshi";
        europeanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialConvertibleEngine<Joshi4>(stochasticProcess,
                                                            timeSteps)));
        americanBond.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialConvertibleEngine<Joshi4>(stochasticProcess,
                                                            timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanBond.NPV()
                  << std::setw(widths[2]) << std::left << americanBond.NPV()
                  << std::endl;

        std::cout << dblrule << std::endl;

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        std::cout << " \nRun completed in ";
        if (hours > 0)
            std::cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            std::cout << minutes << " m ";
        std::cout << std::fixed << std::setprecision(0)
                  << seconds << " s\n" << std::endl;

        return 0;
    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }

}