#include "Scheme.h"

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

#include "EvaluatorUtils.h"
#include "NumUtils.h"
#include "Ring2Utils.h"
#include "StringUtils.h"

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

Scheme::Scheme(Context& context) : context(context) {
}

Scheme::Scheme(SecretKey& secretKey, Context& context) : context(context) {
	addEncKey(secretKey);
	addMultKey(secretKey);
};

//----------------------------------------------------------------------------------
//   KEYS GENERATION
//----------------------------------------------------------------------------------

void Scheme::addEncKey(SecretKey& secretKey) {
	ZZX ex, ax, bx;

	NumUtils::sampleUniform2(ax, context.N, context.logQQ);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::mult(bx, secretKey.sx, ax, context.QQ, context.N);
	Ring2Utils::sub(bx, ex, bx, context.QQ, context.N);

	keyMap.insert(pair<long, Key>(ENCRYPTION, Key(ax, bx)));
}

void Scheme::addMultKey(SecretKey& secretKey) {
	ZZX ex, ax, bx, sxsx;

	Ring2Utils::mult(sxsx, secretKey.sx, secretKey.sx, context.Q, context.N);
	Ring2Utils::leftShiftAndEqual(sxsx, context.logQ, context.QQ, context.N);
	NumUtils::sampleUniform2(ax, context.N, context.logQQ);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::addAndEqual(ex, sxsx, context.QQ, context.N);
	Ring2Utils::mult(bx, secretKey.sx, ax, context.QQ, context.N);
	Ring2Utils::sub(bx, ex, bx, context.QQ, context.N);

	keyMap.insert(pair<long, Key>(MULTIPLICATION, Key(ax, bx)));
}

void Scheme::addConjKey(SecretKey& secretKey) {
	ZZX ex, ax, bx, sxconj;

	Ring2Utils::conjugate(sxconj, secretKey.sx, context.N);
	Ring2Utils::leftShiftAndEqual(sxconj, context.logQ, context.QQ, context.N);
	NumUtils::sampleUniform2(ax, context.N, context.logQQ);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::addAndEqual(ex, sxconj, context.QQ, context.N);
	Ring2Utils::mult(bx, secretKey.sx, ax, context.QQ, context.N);
	Ring2Utils::sub(bx, ex, bx, context.QQ, context.N);

	keyMap.insert(pair<long, Key>(CONJUGATION, Key(ax, bx)));
}

void Scheme::addLeftRotKey(SecretKey& secretKey, long rot) {
	ZZX ex, ax, bx, sxrot;

	Ring2Utils::inpower(sxrot, secretKey.sx, context.rotGroup[rot], context.Q, context.N);
	Ring2Utils::leftShiftAndEqual(sxrot, context.logQ, context.QQ, context.N);
	NumUtils::sampleUniform2(ax, context.N, context.logQQ);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::addAndEqual(ex, sxrot, context.QQ, context.N);
	Ring2Utils::mult(bx, secretKey.sx, ax, context.QQ, context.N);
	Ring2Utils::sub(bx, ex, bx, context.QQ, context.N);

	leftRotKeyMap.insert(pair<long, Key>(rot, Key(ax, bx)));
}

void Scheme::addLeftRotKeys(SecretKey& secretKey) {
	for (long i = 0; i < context.logNh; ++i) {
		long idx = 1 << i;
		if(leftRotKeyMap.find(idx) == leftRotKeyMap.end()) {
			addLeftRotKey(secretKey, idx);
		}
	}
}

void Scheme::addRightRotKeys(SecretKey& secretKey) {
	for (long i = 0; i < context.logNh; ++i) {
		long idx = context.N/2 - (1 << i);
		if(leftRotKeyMap.find(idx) == leftRotKeyMap.end()) {
			addLeftRotKey(secretKey, idx);
		}
	}
}

void Scheme::addSortKeys(SecretKey& secretKey, long size) {
	for (long i = 1; i < size; ++i) {
		if(leftRotKeyMap.find(i) == leftRotKeyMap.end()) {
			addLeftRotKey(secretKey, i);
		}
	}
}

//----------------------------------------------------------------------------------
//   ENCODING & DECODING
//----------------------------------------------------------------------------------

Plaintext Scheme::encode(double* vals, long slots, long logp, long logq) {
	ZZX mx = context.encode(vals, slots, logp + context.logQ);
	return Plaintext(mx, logp, logq, slots, false);
}

Plaintext Scheme::encode(complex<double>* vals, long slots, long logp, long logq) {
	ZZX mx = context.encode(vals, slots, logp + context.logQ);
	return Plaintext(mx, logp, logq, slots, true);
}

complex<double>* Scheme::decode(Plaintext& msg) {
	ZZ q = context.qpowvec[msg.logq];
	long slots = msg.slots;
	long gap = context.Nh / slots;
	complex<double>* res = new complex<double>[slots];
	ZZ tmp;

	for (long i = 0, idx = 0; i < slots; ++i, idx += gap) {
		rem(tmp, msg.mx[idx], q);
		if(NumBits(tmp) == msg.logq) tmp -= q;
		res[i].real(EvaluatorUtils::scaleDownToReal(tmp, msg.logp));

		rem(tmp, msg.mx[idx + context.Nh], q);
		if(NumBits(tmp) == msg.logq) tmp -= q;
		res[i].imag(EvaluatorUtils::scaleDownToReal(tmp, msg.logp));
	}
	context.fftSpecial(res, slots);
	return res;
}

Plaintext Scheme::encodeSingle(complex<double> val, long logp, long logq) {
	ZZX mx;
	mx.SetLength(context.N);
	mx.rep[0] = EvaluatorUtils::scaleUpToZZ(val.real(), logp + context.logQ);
	mx.rep[context.Nh] = EvaluatorUtils::scaleUpToZZ(val.imag(), logp + context.logQ);
	return Plaintext(mx, logp, logq, 1, true);
}

Plaintext Scheme::encodeSingle(double val, long logp, long logq) {
	ZZX mx;
	mx.SetLength(context.N);
	mx.rep[0] = EvaluatorUtils::scaleUpToZZ(val, logp + context.logQ);
	return Plaintext(mx, logp, logq, 1, false);
}

complex<double> Scheme::decodeSingle(Plaintext& msg) {
	ZZ q = context.qpowvec[msg.logq];

	complex<double> res;
	ZZ tmp = msg.mx.rep[0] % q;
	if(NumBits(tmp) == msg.logq) tmp -= q;
	res.real(EvaluatorUtils::scaleDownToReal(tmp, msg.logp));

	if(msg.isComplex) {
		tmp = msg.mx.rep[context.Nh] % q;
		if(NumBits(tmp) == msg.logq) tmp -= q;
		res.imag(EvaluatorUtils::scaleDownToReal(tmp, msg.logp));
	}

	return res;
}

//----------------------------------------------------------------------------------
//   ENCRYPTION & DECRYPTION
//----------------------------------------------------------------------------------

Ciphertext Scheme::encryptMsg(Plaintext& msg) {
	ZZX ax, bx, vx, ex;
	Key key = keyMap.at(ENCRYPTION);
	ZZ qQ = context.qpowvec[msg.logq + context.logQ];

	NumUtils::sampleZO(vx, context.N);
	Ring2Utils::mult(ax, vx, key.ax, qQ, context.N);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::addAndEqual(ax, ex, qQ, context.N);

	Ring2Utils::mult(bx, vx, key.bx, qQ, context.N);
	NumUtils::sampleGauss(ex, context.N, context.sigma);
	Ring2Utils::addAndEqual(bx, ex, qQ, context.N);

	Ring2Utils::addAndEqual(bx, msg.mx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(bx, context.logQ, context.N);

	return Ciphertext(ax, bx, msg.logp, msg.logq, msg.slots, msg.isComplex);
}

Plaintext Scheme::decryptMsg(SecretKey& secretKey, Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZX mx;
	Ring2Utils::mult(mx, cipher.ax, secretKey.sx, q, context.N);
	Ring2Utils::addAndEqual(mx, cipher.bx, q, context.N);

	return Plaintext(mx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::encrypt(double* vals, long slots, long logp, long logq) {
	Plaintext msg = encode(vals, slots, logp, logq);
	return encryptMsg(msg);
}

Ciphertext Scheme::encrypt(complex<double>* vals, long slots, long logp, long logq) {
	Plaintext msg = encode(vals, slots, logp, logq);
	return encryptMsg(msg);
}

Ciphertext Scheme::encryptZeros(long slots, long logp, long logq) {
	Ciphertext czeros = encryptSingle(0.0, logp, logq);
	czeros.isComplex = true;
	czeros.slots = slots;
	return czeros;
}
complex<double>* Scheme::decrypt(SecretKey& secretKey, Ciphertext& cipher) {
	Plaintext msg = decryptMsg(secretKey, cipher);
	return decode(msg);
}

Ciphertext Scheme::encryptSingle(double val, long logp, long logq) {
	Plaintext msg = encodeSingle(val, logp,  logq);
	return encryptMsg(msg);
}

Ciphertext Scheme::encryptSingle(complex<double> val, long logp, long logq) {
	Plaintext msg = encodeSingle(val, logp,  logq);
	return encryptMsg(msg);
}

complex<double> Scheme::decryptSingle(SecretKey& secretKey, Ciphertext& cipher) {
	Plaintext msg = decryptMsg(secretKey, cipher);
	return decodeSingle(msg);
}

//----------------------------------------------------------------------------------
//   HOMOMORPHIC OPERATIONS
//----------------------------------------------------------------------------------

Ciphertext Scheme::negate(Ciphertext& cipher) {
	return Ciphertext(-cipher.ax, -cipher.bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::negateAndEqual(Ciphertext& cipher) {
	cipher.ax = -cipher.ax;
	cipher.bx = -cipher.bx;
}

Ciphertext Scheme::add(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];
	ZZX ax, bx;

	Ring2Utils::add(ax, cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::add(bx, cipher1.bx, cipher2.bx, q, context.N);

	return Ciphertext(ax, bx, cipher1.logp, cipher1.logq, cipher1.slots, cipher1.isComplex);
}

void Scheme::addAndEqual(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];

	Ring2Utils::addAndEqual(cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::addAndEqual(cipher1.bx, cipher2.bx, q, context.N);
}

Ciphertext Scheme::addConst(Ciphertext& cipher, double cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZX ax = cipher.ax;
	ZZX bx = cipher.bx;

	ZZ cnstZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst, cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst, logp);

	AddMod(bx.rep[0], cipher.bx.rep[0], cnstZZ, q);
	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::addConst(Ciphertext& cipher, RR& cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZX ax = cipher.ax;
	ZZX bx = cipher.bx;

	ZZ cnstZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst, cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst, logp);

	AddMod(bx.rep[0], cipher.bx.rep[0], cnstZZ, q);
	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::addConst(Ciphertext& cipher, complex<double> cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZX ax = cipher.ax;
	ZZX bx = cipher.bx;

	ZZ cnstrZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst.real(), cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst.real(), logp);
	ZZ cnstiZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst.imag(), cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst.imag(), logp);

	AddMod(bx.rep[0], cipher.bx.rep[0], cnstrZZ, q);
	AddMod(bx.rep[context.Nh], cipher.bx.rep[context.Nh], cnstiZZ, q);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::addConst(Ciphertext& cipher, ZZX& poly, long logp) {
    ZZ q = context.qpowvec[cipher.logq];
    ZZX ax = cipher.ax;
    ZZX bx = cipher.bx;

    Ring2Utils::add(bx, cipher.bx, poly, q, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::addConstAndEqual(Ciphertext& cipher, double cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ cnstZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst, cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst, logp);
	AddMod(cipher.bx.rep[0], cipher.bx.rep[0], cnstZZ, q);
}

void Scheme::addConstAndEqual(Ciphertext& cipher, RR& cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ cnstZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst, cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst, logp);
	AddMod(cipher.bx.rep[0], cipher.bx.rep[0], cnstZZ, q);
}

void Scheme::addConstAndEqual(Ciphertext& cipher, complex<double> cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZ cnstrZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst.real(), cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst.real(), logp);
	ZZ cnstiZZ = logp < 0 ? EvaluatorUtils::scaleUpToZZ(cnst.imag(), cipher.logp) : EvaluatorUtils::scaleUpToZZ(cnst.imag(), logp);

	AddMod(cipher.bx.rep[0], cipher.bx.rep[0], cnstrZZ, q);
	AddMod(cipher.bx.rep[context.Nh], cipher.bx.rep[context.Nh], cnstiZZ, q);
}

void Scheme::addConstAndEqual(Ciphertext& cipher, ZZX& poly, long logp) {
    ZZ q = context.qpowvec[cipher.logq];

    Ring2Utils::add(cipher.bx, cipher.bx, poly, q, context.N);
}

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

Ciphertext Scheme::sub(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];
	ZZX ax, bx;

	Ring2Utils::sub(ax, cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::sub(bx, cipher1.bx, cipher2.bx, q, context.N);

	return Ciphertext(ax, bx, cipher1.logp, cipher1.logq, cipher1.slots, cipher1.isComplex);
}

void Scheme::subAndEqual(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];

	Ring2Utils::subAndEqual(cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::subAndEqual(cipher1.bx, cipher2.bx, q, context.N);
}

void Scheme::subAndEqual2(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];

	Ring2Utils::subAndEqual2(cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::subAndEqual2(cipher1.bx, cipher2.bx, q, context.N);
}

Ciphertext Scheme::subConst2(Ciphertext& cipher, ZZX& poly, long logp) {
    ZZ q = context.qpowvec[cipher.logq];

    ZZX ax = - cipher.ax;

    ZZX bx = cipher.bx;
    Ring2Utils::sub(bx, poly, cipher.bx, q, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::subConstAndEqual2(Ciphertext& cipher, ZZX& poly, long logp) {
    ZZ q = context.qpowvec[cipher.logq];

    cipher.ax -= cipher.ax;   // -ax
    Ring2Utils::subAndEqual(cipher.bx, poly, q, context.N);   // poly - bx
}

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

Ciphertext Scheme::imult(Ciphertext& cipher) {
	ZZX ax, bx;

	Ring2Utils::multByMonomial(ax, cipher.ax, context.Nh, context.N);
	Ring2Utils::multByMonomial(bx, cipher.bx, context.Nh, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::idiv(Ciphertext& cipher) {
	ZZX ax, bx;

	Ring2Utils::multByMonomial(ax, cipher.ax, 3 * context.Nh, context.N);
	Ring2Utils::multByMonomial(bx, cipher.bx, 3 * context.Nh, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::imultAndEqual(Ciphertext& cipher) {
	Ring2Utils::multByMonomialAndEqual(cipher.ax, context.Nh, context.N);
	Ring2Utils::multByMonomialAndEqual(cipher.bx, context.Nh, context.N);
}

void Scheme::idivAndEqual(Ciphertext& cipher) {
	Ring2Utils::multByMonomialAndEqual(cipher.ax, 3 * context.Nh, context.N);
	Ring2Utils::multByMonomialAndEqual(cipher.bx, 3 * context.Nh, context.N);
}

Ciphertext Scheme::mult(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];
	ZZ qQ = context.qpowvec[cipher1.logq + context.logQ];

	ZZX axbx1, axbx2, axax, bxbx, axmult, bxmult;
	Key key = keyMap.at(MULTIPLICATION);

	Ring2Utils::add(axbx1, cipher1.ax, cipher1.bx, q, context.N);
	Ring2Utils::add(axbx2, cipher2.ax, cipher2.bx, q, context.N);
	Ring2Utils::multAndEqual(axbx1, axbx2, q, context.N);

	Ring2Utils::mult(axax, cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::mult(bxbx, cipher1.bx, cipher2.bx, q, context.N);

	Ring2Utils::mult(axmult, axax, key.ax, qQ, context.N);
	Ring2Utils::mult(bxmult, axax, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(axmult, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(bxmult, context.logQ, context.N);

	Ring2Utils::addAndEqual(axmult, axbx1, q, context.N);
	Ring2Utils::subAndEqual(axmult, bxbx, q, context.N);
	Ring2Utils::subAndEqual(axmult, axax, q, context.N);
	Ring2Utils::addAndEqual(bxmult, bxbx, q, context.N);

    return Ciphertext(axmult, bxmult, cipher1.logp, cipher1.logq, cipher1.slots, cipher1.isComplex);
	//return Ciphertext(axmult, bxmult, cipher1.logp + cipher2.logp, cipher1.logq, cipher1.slots, cipher1.isComplex);
}

void Scheme::multAndEqual(Ciphertext& cipher1, Ciphertext& cipher2) {
	ZZ q = context.qpowvec[cipher1.logq];
	ZZ qQ = context.qpowvec[cipher1.logq + context.logQ];
	ZZX axbx1, axbx2, axax, bxbx;
	Key key = keyMap.at(MULTIPLICATION);

	Ring2Utils::add(axbx1, cipher1.ax, cipher1.bx, q, context.N);
	Ring2Utils::add(axbx2, cipher2.ax, cipher2.bx, q, context.N);
	Ring2Utils::multAndEqual(axbx1, axbx2, q, context.N);

	Ring2Utils::mult(axax, cipher1.ax, cipher2.ax, q, context.N);
	Ring2Utils::mult(bxbx, cipher1.bx, cipher2.bx, q, context.N);

	Ring2Utils::mult(cipher1.ax, axax, key.ax, qQ, context.N);
	Ring2Utils::mult(cipher1.bx, axax, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(cipher1.ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(cipher1.bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(cipher1.ax, axbx1, q, context.N);
	Ring2Utils::subAndEqual(cipher1.ax, bxbx, q, context.N);
	Ring2Utils::subAndEqual(cipher1.ax, axax, q, context.N);
	Ring2Utils::addAndEqual(cipher1.bx, bxbx, q, context.N);

	//cipher1.logp += cipher2.logp;
}

Ciphertext Scheme::square(Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];
	ZZX axax, axbx, bxbx, bxmult, axmult;
	Key key = keyMap.at(MULTIPLICATION);

	Ring2Utils::square(bxbx, cipher.bx, q, context.N);
	Ring2Utils::mult(axbx, cipher.ax, cipher.bx, q, context.N);
	Ring2Utils::addAndEqual(axbx, axbx, q, context.N);
	Ring2Utils::square(axax, cipher.ax, q, context.N);

	Ring2Utils::mult(axmult, axax, key.ax, qQ, context.N);
	Ring2Utils::mult(bxmult, axax, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(axmult, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(bxmult, context.logQ, context.N);

	Ring2Utils::addAndEqual(axmult, axbx, q, context.N);
	Ring2Utils::addAndEqual(bxmult, bxbx, q, context.N);

    return Ciphertext(axmult, bxmult, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
	//return Ciphertext(axmult, bxmult, cipher.logp * 2, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::squareAndEqual(Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];
	ZZX bxbx, axbx, axax;
	Key key = keyMap.at(MULTIPLICATION);

	Ring2Utils::square(bxbx, cipher.bx, q, context.N);
	Ring2Utils::mult(axbx, cipher.bx, cipher.ax, q, context.N);
	Ring2Utils::addAndEqual(axbx, axbx, q, context.N);
	Ring2Utils::square(axax, cipher.ax, q, context.N);

	Ring2Utils::mult(cipher.ax, axax, key.ax, qQ, context.N);
	Ring2Utils::mult(cipher.bx, axax, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(cipher.ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(cipher.ax, axbx, q, context.N);
	Ring2Utils::addAndEqual(cipher.bx, bxbx, q, context.N);
	//cipher.logp *= 2;
}

Ciphertext Scheme::multByConst(Ciphertext& cipher, double cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZX ax, bx;

	ZZ cnstZZ = EvaluatorUtils::scaleUpToZZ(cnst, logp);

	Ring2Utils::multByConst(ax, cipher.ax, cnstZZ, q, context.N);
	Ring2Utils::multByConst(bx, cipher.bx, cnstZZ, q, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
	//return Ciphertext(ax, bx, cipher.logp + logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::multByConst(Ciphertext& cipher, RR& cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZX ax, bx;

	ZZ cnstZZ = EvaluatorUtils::scaleUpToZZ(cnst, logp);

	Ring2Utils::multByConst(ax, cipher.ax, cnstZZ, q, context.N);
	Ring2Utils::multByConst(bx, cipher.bx, cnstZZ, q, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
	//return Ciphertext(ax, bx, cipher.logp + logp, cipher.logq, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::multByConst(Ciphertext& cipher, complex<double> cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZX axr, bxr, axi, bxi;

	ZZ cnstrZZ = EvaluatorUtils::scaleUpToZZ(cnst.real(), logp);
	ZZ cnstiZZ = EvaluatorUtils::scaleUpToZZ(cnst.imag(), logp);

	Ring2Utils::multByMonomial(axi, cipher.ax, context.Nh, context.N);
	Ring2Utils::multByMonomial(bxi, cipher.bx, context.Nh, context.N);

	Ring2Utils::multByConst(axr, cipher.ax, cnstrZZ, q, context.N);
	Ring2Utils::multByConst(bxr, cipher.bx, cnstrZZ, q, context.N);

	Ring2Utils::multByConstAndEqual(axi, cnstiZZ, q, context.N);
	Ring2Utils::multByConstAndEqual(bxi, cnstiZZ, q, context.N);

	Ring2Utils::addAndEqual(axr, axi, q, context.N);
	Ring2Utils::addAndEqual(bxr, bxi, q, context.N);

    return Ciphertext(axr, bxr, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
	//return Ciphertext(axr, bxr, cipher.logp + logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::multByConstAndEqual(Ciphertext& cipher, double cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ cnstZZ = EvaluatorUtils::scaleUpToZZ(cnst, logp);

	Ring2Utils::multByConstAndEqual(cipher.ax, cnstZZ, q, context.N);
	Ring2Utils::multByConstAndEqual(cipher.bx, cnstZZ, q, context.N);
	//cipher.logp += logp;
}

void Scheme::multByConstAndEqual(Ciphertext& cipher, RR& cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ cnstZZ = EvaluatorUtils::scaleUpToZZ(cnst, logp);

	Ring2Utils::multByConstAndEqual(cipher.ax, cnstZZ, q, context.N);
	Ring2Utils::multByConstAndEqual(cipher.bx, cnstZZ, q, context.N);
	//cipher.logp += logp;
}

void Scheme::multByConstAndEqual(Ciphertext& cipher, complex<double> cnst, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZX axi, bxi;

	ZZ cnstrZZ = EvaluatorUtils::scaleUpToZZ(cnst.real(), logp);
	ZZ cnstiZZ = EvaluatorUtils::scaleUpToZZ(cnst.imag(), logp);

	Ring2Utils::multByMonomial(axi, cipher.ax, context.Nh, context.N);
	Ring2Utils::multByMonomial(bxi, cipher.bx, context.Nh, context.N);

	Ring2Utils::multByConstAndEqual(cipher.ax, cnstrZZ, q, context.N);
	Ring2Utils::multByConstAndEqual(cipher.bx, cnstrZZ, q, context.N);

	Ring2Utils::multByConstAndEqual(axi, cnstiZZ, q, context.N);
	Ring2Utils::multByConstAndEqual(bxi, cnstiZZ, q, context.N);

	Ring2Utils::addAndEqual(cipher.ax, axi, q, context.N);
	Ring2Utils::addAndEqual(cipher.bx, bxi, q, context.N);
	//cipher.logp += logp;
}

Ciphertext Scheme::multByPoly(Ciphertext& cipher, ZZX& poly, long logp) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZX ax, bx;

	Ring2Utils::mult(ax, cipher.ax, poly, q, context.N);
	Ring2Utils::mult(bx, cipher.bx, poly, q, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
	//return Ciphertext(ax, bx, cipher.logp + logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::multByPolyAndEqual(Ciphertext& cipher, ZZX& poly, long logp) {
	ZZ q = context.qpowvec[cipher.logq];

	Ring2Utils::multAndEqual(cipher.ax, poly, q, context.N);
	Ring2Utils::multAndEqual(cipher.bx, poly, q, context.N);
	//cipher.logp += logp;
}

Ciphertext Scheme::multByMonomial(Ciphertext& cipher, const long degree) {
	ZZX ax, bx;

	Ring2Utils::multByMonomial(ax, cipher.ax, degree, context.N);
	Ring2Utils::multByMonomial(bx, cipher.bx, degree, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::multByMonomialAndEqual(Ciphertext& cipher, const long degree) {
	Ring2Utils::multByMonomialAndEqual(cipher.ax, degree, context.N);
	Ring2Utils::multByMonomialAndEqual(cipher.bx, degree, context.N);
}

Ciphertext Scheme::multByPo2(Ciphertext& cipher, long deg) {
	ZZ q = context.qpowvec[cipher.logq];

	ZZX ax, bx;

	Ring2Utils::leftShift(ax, cipher.ax, deg, q, context.N);
	Ring2Utils::leftShift(bx, cipher.bx, deg, q, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::multByPo2AndEqual(Ciphertext& cipher, long deg) {
	ZZ q = context.qpowvec[cipher.logq];

	Ring2Utils::leftShiftAndEqual(cipher.ax, deg, q, context.N);
	Ring2Utils::leftShiftAndEqual(cipher.bx, deg, q, context.N);
}

void Scheme::multBy2AndEqual(Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];

	Ring2Utils::doubleAndEqual(cipher.ax, q, context.N);
	Ring2Utils::doubleAndEqual(cipher.bx, q, context.N);
}

Ciphertext Scheme::divByPo2(Ciphertext& cipher, long degree) {
	ZZX ax, bx;

	Ring2Utils::rightShift(ax, cipher.ax, degree, context.N);
	Ring2Utils::rightShift(bx, cipher.bx, degree, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq - degree, cipher.slots, cipher.isComplex);
}

void Scheme::divByPo2AndEqual(Ciphertext& cipher, long degree) {
	Ring2Utils::rightShiftAndEqual(cipher.ax, degree, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, degree, context.N);

	cipher.logq -= degree;
}

//----------------------------------------------------------------------------------
//   RESCALING & MODULUS DOWN
//----------------------------------------------------------------------------------

Ciphertext Scheme::reScaleBy(Ciphertext& cipher, long bitsDown) {
	ZZX ax, bx;

	Ring2Utils::rightShift(ax, cipher.ax, bitsDown, context.N);
	Ring2Utils::rightShift(bx, cipher.bx, bitsDown, context.N);

    return Ciphertext(ax, bx, cipher.logp, cipher.logq - bitsDown, cipher.slots, cipher.isComplex);
	//return Ciphertext(ax, bx, cipher.logp - bitsDown, cipher.logq - bitsDown, cipher.slots, cipher.isComplex);
}

Ciphertext Scheme::reScaleTo(Ciphertext& cipher, long newlogq) {
	ZZX ax, bx;
	long bitsDown = cipher.logq - newlogq;

	Ring2Utils::rightShift(ax, cipher.ax, bitsDown, context.N);
	Ring2Utils::rightShift(bx, cipher.bx, bitsDown, context.N);

    return Ciphertext(ax, bx, cipher.logp, newlogq, cipher.slots, cipher.isComplex);
	//return Ciphertext(ax, bx, cipher.logp - bitsDown, newlogq, cipher.slots, cipher.isComplex);
}

void Scheme::reScaleByAndEqual(Ciphertext& cipher, long bitsDown) {
	Ring2Utils::rightShiftAndEqual(cipher.ax, bitsDown, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, bitsDown, context.N);

	cipher.logq -= bitsDown;
	//cipher.logp -= bitsDown;
}

void Scheme::reScaleToAndEqual(Ciphertext& cipher, long logq) {
	long bitsDown = cipher.logq - logq;
	cipher.logq = logq;
	//cipher.logp -= bitsDown;

	Ring2Utils::rightShiftAndEqual(cipher.ax, bitsDown, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, bitsDown, context.N);
}

Ciphertext Scheme::modDownBy(Ciphertext& cipher, long bitsDown) {
	ZZX bx, ax;
	long newlogq = cipher.logq - bitsDown;
	ZZ q = context.qpowvec[newlogq];

	Ring2Utils::mod(ax, cipher.ax, q, context.N);
	Ring2Utils::mod(bx, cipher.bx, q, context.N);

	return Ciphertext(ax, bx, cipher.logp, newlogq, cipher.slots, cipher.isComplex);
}

void Scheme::modDownByAndEqual(Ciphertext& cipher, long bitsDown) {
	cipher.logq -= bitsDown;
	ZZ q = context.qpowvec[cipher.logq];

	Ring2Utils::modAndEqual(cipher.ax, q, context.N);
	Ring2Utils::modAndEqual(cipher.bx, q, context.N);
}

Ciphertext Scheme::modDownTo(Ciphertext& cipher, long logq) {
	ZZX bx, ax;
	ZZ q = context.qpowvec[logq];

	Ring2Utils::mod(ax, cipher.ax, q, context.N);
	Ring2Utils::mod(bx, cipher.bx, q, context.N);
	return Ciphertext(ax, bx, cipher.logp, logq, cipher.slots);
}

void Scheme::modDownToAndEqual(Ciphertext& cipher, long logq) {
	cipher.logq = logq;
	ZZ q = context.qpowvec[logq];

	Ring2Utils::modAndEqual(cipher.ax, q, context.N);
	Ring2Utils::modAndEqual(cipher.bx, q, context.N);
}

//----------------------------------------------------------------------------------
//   ROTATIONS & CONJUGATIONS
//----------------------------------------------------------------------------------

Ciphertext Scheme::leftRotateFast(Ciphertext& cipher, long rotSlots) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];

	ZZX bxrot, ax, bx;
	Key key = leftRotKeyMap.at(rotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, context.rotGroup[rotSlots], context.Q, context.N);
	Ring2Utils::inpower(bx, cipher.ax, context.rotGroup[rotSlots], context.Q, context.N);

	Ring2Utils::mult(ax, bx, key.ax, qQ, context.N);
	Ring2Utils::multAndEqual(bx, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(bx, bxrot, q, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::leftRotateAndEqualFast(Ciphertext& cipher, long rotSlots) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];
	ZZX bxrot;
	Key key = leftRotKeyMap.at(rotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, context.rotGroup[rotSlots], context.Q, context.N);
	Ring2Utils::inpower(cipher.bx, cipher.ax, context.rotGroup[rotSlots], context.Q, context.N);

	Ring2Utils::mult(cipher.ax, cipher.bx, key.ax, qQ, context.N);
	Ring2Utils::multAndEqual(cipher.bx, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(cipher.ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(cipher.bx, bxrot, q, context.N);
}

Ciphertext Scheme::leftRotateByPo2(Ciphertext& cipher, long logrotSlots) {
	long rotSlots = (1 << logrotSlots);
	return leftRotateFast(cipher, rotSlots);
}

void Scheme::leftRotateByPo2AndEqual(Ciphertext& cipher, long logrotSlots) {
	long rotSlots = (1 << logrotSlots);
	leftRotateAndEqualFast(cipher, rotSlots);
}

Ciphertext Scheme::rightRotateByPo2(Ciphertext& cipher, long logrotSlots) {
	long rotSlots = context.Nh - (1 << logrotSlots);
	return leftRotateFast(cipher, rotSlots);
}

void Scheme::rightRotateByPo2AndEqual(Ciphertext& cipher, long logrotSlots) {
	long rotSlots = context.Nh - (1 << logrotSlots);
	leftRotateAndEqualFast(cipher, rotSlots);
}

Ciphertext Scheme::leftRotate(Ciphertext& cipher, long rotSlots) {
	Ciphertext res = cipher;
	leftRotateAndEqual(res, rotSlots);
	return res;
}

void Scheme::leftRotateAndEqual(Ciphertext& cipher, long rotSlots) {
	long remrotSlots = rotSlots % cipher.slots;
	long logrotSlots = log2((double)remrotSlots) + 1;
	for (long i = 0; i < logrotSlots; ++i) {
		if(bit(remrotSlots, i)) {
			leftRotateByPo2AndEqual(cipher, i);
		}
	}
}

Ciphertext Scheme::rightRotate(Ciphertext& cipher, long rotSlots) {
	Ciphertext res = cipher;
	rightRotateAndEqual(res, rotSlots);
	return res;
}

void Scheme::rightRotateAndEqual(Ciphertext& cipher, long rotSlots) {
	long remrotSlots = rotSlots % cipher.slots;
	long logrotSlots = log2((double)remrotSlots) + 1;
	for (long i = 0; i < logrotSlots; ++i) {
		if(bit(remrotSlots, i)) {
			rightRotateByPo2AndEqual(cipher, i);
		}
	}
}

Ciphertext Scheme::conjugate(Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];

	ZZX bxconj, ax, bx;
	Key key = keyMap.at(CONJUGATION);

	Ring2Utils::conjugate(bxconj, cipher.bx, context.N);
	Ring2Utils::conjugate(bx, cipher.ax, context.N);

	Ring2Utils::mult(ax, bx, key.ax, qQ, context.N);
	Ring2Utils::multAndEqual(bx, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(bx, bxconj, q, context.N);

	return Ciphertext(ax, bx, cipher.logp, cipher.logq, cipher.slots, cipher.isComplex);
}

void Scheme::conjugateAndEqual(Ciphertext& cipher) {
	ZZ q = context.qpowvec[cipher.logq];
	ZZ qQ = context.qpowvec[cipher.logq + context.logQ];
	ZZX bxconj;
	Key key = keyMap.at(CONJUGATION);

	Ring2Utils::conjugate(bxconj, cipher.bx, context.N);
	Ring2Utils::conjugate(cipher.bx, cipher.ax, context.N);

	Ring2Utils::mult(cipher.ax, cipher.bx, key.ax, qQ, context.N);
	Ring2Utils::multAndEqual(cipher.bx, key.bx, qQ, context.N);

	Ring2Utils::rightShiftAndEqual(cipher.ax, context.logQ, context.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, context.logQ, context.N);

	Ring2Utils::addAndEqual(cipher.bx, bxconj, q, context.N);
}


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "stiffness_matrix.hpp"

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//----------------AssembleMatrixBegin----------------
//! Assemble the stiffness matrix
//! for the linear system
//!
//! @param[out] A will at the end contain the Galerkin matrix
//! @param[in] vertices a list of triangle vertices
//! @param[in] triangles a list of triangles
template<class Matrix>
void assembleStiffnessMatrix(Matrix& A, const Eigen::MatrixXd& vertices,
                            const Eigen::MatrixXi& triangles)
{

    const int numberOfElements = triangles.rows();
    A.resize(vertices.rows(), vertices.rows());

    std::vector<Triplet> triplets;

    triplets.reserve(numberOfElements * 3 * 3);
// (write your solution here)
    A.setFromTriplets(triplets.begin(), triplets.end());
}
//----------------AssembleMatrixEnd----------------


/*! @file gridsearch.cpp
    @brief Implementation of GridSearch class
*/
#include "gridsearch.hpp"
#include "util.hpp"

#include <wtl/exception.hpp>
#include <wtl/debug.hpp>
#include <wtl/iostr.hpp>
#include <wtl/zlib.hpp>
#include <wtl/numeric.hpp>
#include <wtl/math.hpp>
#include <wtl/concurrent.hpp>
#include <wtl/itertools.hpp>

#include <boost/math/distributions/chi_squared.hpp>

#include <chrono>

namespace likeligrid {

void GridSearch::init(const std::pair<size_t, size_t>& epistasis_pair, const bool pleiotropy) {HERE;
    model_.set_epistasis(epistasis_pair, pleiotropy);
    mle_params_.resize(model_.names().size());
    mle_params_ = 1.0;
}

void GridSearch::run(const bool writing) {HERE;
    while (stage_ < STEPS.size()) {
        if (writing) {run_fout();} else {run_cout();}
    }
    --stage_;
    search_limits();
}

void GridSearch::run_fout() {HERE;
    const std::string outfile = init_meta();
    std::cerr << "mle_params_: " << mle_params_ << std::endl;
    if (outfile.empty()) return;
    const auto axes = make_vicinity(mle_params_, BREAKS.at(stage_), radius(stage_));
    for (size_t j=0u; j<model_.names().size(); ++j) {
        std::cerr << model_.names()[j] << ": " << axes[j] << std::endl;
    }
    {
        wtl::zlib::ofstream fout(outfile, std::ios_base::out | std::ios_base::app);
        std::cerr << "Writing: " << outfile << std::endl;
        run_impl(fout, wtl::itertools::product(axes));
    }
}

void GridSearch::run_cout() {HERE;
    const auto axes = make_vicinity(mle_params_, BREAKS.at(stage_), radius(stage_));
    for (size_t j=0u; j<model_.names().size(); ++j) {
        std::cerr << model_.names()[j] << ": " << axes[j] << std::endl;
    }
    {
        std::stringstream sst;
        run_impl(sst, wtl::itertools::product(axes));
        std::cout << sst.str();
        read_results(sst);
    }
    std::cerr << "mle_params_: " << mle_params_ << std::endl;
    ++stage_;
}

void GridSearch::search_limits() {HERE;
    namespace bmath = boost::math;
    bmath::chi_squared_distribution<> chisq(1.0);
    const double diff95 = 0.5 * bmath::quantile(bmath::complement(chisq, 0.05));
    auto axis = wtl::round(wtl::lin_spaced(200, 2.0, 0.01), 100);
    axis = (axis * 100.0).apply(std::round) / 100.0;
    std::map<std::string, std::valarray<double>> intersections;
    for (size_t i=0u; i<model_.names().size(); ++i) {
        const std::string outfile = "uniaxis-" + model_.names()[i] + ".tsv.gz";
        std::cerr << outfile << std::endl;
        std::stringstream sst;
        run_impl(sst, wtl::itertools::uniaxis(axis, mle_params_, i));
        wtl::zlib::ofstream(outfile) << sst.str();
        const auto logliks = read_loglik(sst, axis.size());
        const double threshold = logliks.max() - diff95;
        const std::valarray<double> range = axis[logliks > threshold];
        auto bound_params = mle_params_;
        bound_params[i] = std::max(range.min() - 0.01, 0.01);
        intersections.emplace(model_.names()[i] + "_L", bound_params);
        bound_params[i] = std::min(range.max() + 0.01, 2.00);
        intersections.emplace(model_.names()[i] + "_U", bound_params);
    }
    for (const auto& p: intersections) {
        const std::string outfile = "limit-" + p.first + ".tsv.gz";
        std::cerr << outfile << ": " << p.second << std::endl;
        const auto axes = make_vicinity(p.second, 5u, 0.02);
        wtl::zlib::ofstream fout(outfile);
        //TODO: if exists
        run_impl(fout, wtl::itertools::product(axes));
    }
}

void GridSearch::run_impl(std::ostream& ost, wtl::itertools::Generator<std::valarray<double>>&& gen) {HERE;
    std::cerr << skip_ << " to " << gen.max_count() << std::endl;
    if (skip_ == 0u) {
        write_header(ost, gen.max_count());
    }

    auto task = [this](const std::valarray<double> th_path) {
        // argument and model are copied for each thread
        auto buffer = wtl::make_oss();
        auto model_copy = this->model_;
        buffer << model_copy.calc_loglik(th_path) << "\t";
        wtl::join(th_path, buffer, "\t") << "\n";
        return buffer.str();
    };

    static wtl::ThreadPool pool(concurrency_);
    std::vector<std::future<std::string>> futures;
    futures.reserve(gen.max_count() - skip_);
    for (const auto& th_path: gen(skip_)) {
        futures.push_back(pool.submit(task, th_path));
    }

    auto buffer = wtl::make_oss();
    size_t stars = 0u;
    size_t i = skip_;
    const auto min_interval = std::chrono::seconds(1);
    auto next_time = std::chrono::system_clock::now();
    for (auto& ftr: futures) {
        buffer << ftr.get();
        ++i;
        auto now = std::chrono::system_clock::now();
        if (now > next_time || &ftr == &futures.back()) {
            next_time = now + min_interval;
            ost << buffer.str();
            buffer.str("");
            buffer.clear();
            for (size_t n= static_cast<size_t>(20.0 * i / gen.max_count()); stars<n; ++stars) {
                std::cerr << "*";
            }
        }
        if (wtl::SIGINT_RAISED()) {throw wtl::KeyboardInterrupt();}
    }
    std::cerr << "\n";
}

std::string GridSearch::init_meta() {HERE;
    if (stage_ >= STEPS.size()) return "";
    auto oss = wtl::make_oss(2u, std::ios_base::fixed);
    oss << "grid-" << STEPS.at(stage_) << ".tsv.gz";
    std::string outfile = oss.str();
    try {
        wtl::zlib::ifstream ist(outfile);
        std::cerr << "Reading: " << outfile << std::endl;
        read_results(ist);
        if (skip_ == 0u) {
            ++stage_;
            outfile = init_meta();
        }
    } catch (std::ios_base::failure& e) {
        if (errno != ENOENT) throw e;
    }
    return outfile;
}

void GridSearch::read_results(std::istream& ist) {HERE;
    size_t max_count;
    double step;
    std::tie(std::ignore, std::ignore, max_count, step) = read_metadata(ist);
    stage_ = guess_stage(step);
    std::vector<std::string> colnames;
    std::valarray<double> mle_params;
    std::tie(skip_, colnames, mle_params) = read_body(ist);
    if (skip_ == max_count) {  // is complete file
        skip_ = 0u;
        mle_params_.swap(mle_params);
    }
}

void GridSearch::read_results(const std::string& infile) {
    wtl::zlib::ifstream ist(infile);
    read_results(ist);
}

void GridSearch::write_header(std::ostream& ost, const size_t max_count) const {
    ost << "##genotype_file=" << model_.filename() << "\n";
    ost << "##max_sites=" << model_.max_sites() << "\n";
    ost << "##max_count=" << max_count << "\n";
    ost << "##step=" << STEPS.at(stage_) << "\n";
    ost << "loglik\t";
    wtl::join(model_.names(), ost, "\t") << "\n";
}

} // namespace likeligrid


#include <ros/ros.h>
#include <iostream>
#include <Eigen/Dense>

#include "sensor_msgs/Imu.h"

namespace ImuTransform {

static inline Eigen::Matrix<double, 3, 3> CrossOperator(Eigen::Matrix<double, 3, 1> u)
{
	Eigen::Matrix<double, 3, 3> m;

	m << 0, -u[2], u[1], u[2], 0, -u[0], -u[1], u[0], 0;

	return m;
}

class ImuMeasurementModel {

public:

    //! Measurement type shortcut definition
    typedef Eigen::Matrix<double, 3, 1> Vector3;
    typedef Eigen::Matrix<double, 3, 3> Matrix3;
    typedef Eigen::Matrix<double, 4, 4> Matrix4;

    ImuMeasurementModel(ros::NodeHandle& nh) : _nh(nh) {

    	_sub = _nh.subscribe("/imu/data", 100, &ImuMeasurementModel::ImuCallback, this);
    	_pub = _nh.advertise<sensor_msgs::Imu>("/imu/transformed_data", 1000);

    	Matrix4 tf_base_to_imu;
    	tf_base_to_imu << 0.7067584,  0.7074550,  0.0000000, 0.192,
    	                 -0.7074550,  0.7067584,  0.0000000, -0.086,
    	                  0.0000000,  0.0000000,  1.0000000, 0.185,
    	                  0.0, 0.0, 0.0, 1.0;

    	_tf_imu_to_base = tf_base_to_imu.inverse();
    	_rot_imu_to_base = tf_base_to_imu.block<3,3>(0,0);
    	_t_imu_to_base = _tf_imu_to_base.block<3,1>(0,3);

    	std::cout << "_tf_base_to_imu: " << std::endl << tf_base_to_imu << std::endl;
    	std::cout << "_tf_imu_to_base: " << std::endl << _tf_imu_to_base << std::endl;
    	std::cout << "_rot_imu_to_base: " << std::endl << _rot_imu_to_base << std::endl;
    	std::cout << "_t_imu_to_base: " << std::endl << _t_imu_to_base << std::endl;

	// _tf_mast_imu_to_base_imu = tf_mast_imu_to_link + tf_link_to_base_imu;
	// _rot_mast_imu_to_base_imu = _tf_mast_imu_to_base_imu.getRotationMatrix().cast<T>().transpose();
	// _t_mast_imu_to_base_imu = _tf_mast_imu_to_base_imu.translation_src().cast<T>();
    }

    sensor_msgs::Imu transform(const sensor_msgs::Imu::ConstPtr& msg) const
    {
    	sensor_msgs::Imu transformed;
    	transformed.header = msg->header;
    	transformed.orientation = msg->orientation;
    	transformed.orientation_covariance = msg->orientation_covariance;
    	transformed.angular_velocity = msg->angular_velocity;
        transformed.angular_velocity_covariance = msg->angular_velocity_covariance;
    	transformed.linear_acceleration = msg->linear_acceleration;
    	transformed.linear_acceleration_covariance = msg->linear_acceleration_covariance;

        Vector3 a_imu = {msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z};
        Vector3 w_imu = {msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z};

        Matrix3 wx = CrossOperator(w_imu);

        Vector3 a_base = _rot_imu_to_base * (a_imu) + _rot_imu_to_base * wx * wx * _t_imu_to_base;
        Vector3 w_base = _rot_imu_to_base * w_imu;

        transformed.header.frame_id = "base_link";

        transformed.angular_velocity.x = w_base[0];
        transformed.angular_velocity.y = w_base[1];
        transformed.angular_velocity.z = w_base[2];

        transformed.linear_acceleration.x = a_base[0];
        transformed.linear_acceleration.y = a_base[1];
        transformed.linear_acceleration.z = a_base[2];

        return transformed;
    }

    void spin() {
    	while (ros::ok()) {
    		ros::spinOnce();
    	}
    }

    void test() {
    	sensor_msgs::Imu::Ptr msg_ptr(new sensor_msgs::Imu);
    	msg_ptr->linear_acceleration.x = 0.5;
    	msg_ptr->linear_acceleration.y = 0.5;
    	std::cout << "old ax: " << msg_ptr->linear_acceleration.x << std::endl;
    	std::cout << "old ay: " << msg_ptr->linear_acceleration.y << std::endl;

    	sensor_msgs::Imu ret = transform(msg_ptr);
    	std::cout << "new ax: " << ret.linear_acceleration.x << std::endl;
    	std::cout << "new ay: " << ret.linear_acceleration.y << std::endl;

    	msg_ptr->linear_acceleration.x = 1;
    	msg_ptr->linear_acceleration.y = 0;
    	std::cout << "old ax: " << msg_ptr->linear_acceleration.x << std::endl;
    	std::cout << "old ay: " << msg_ptr->linear_acceleration.y << std::endl;

    	ret = transform(msg_ptr);
    	std::cout << "new ax: " << ret.linear_acceleration.x << std::endl;
    	std::cout << "new ay: " << ret.linear_acceleration.y << std::endl;

    	msg_ptr->linear_acceleration.x = 0;
    	msg_ptr->linear_acceleration.y = 1;
    	std::cout << "old ax: " << msg_ptr->linear_acceleration.x << std::endl;
    	std::cout << "old ay: " << msg_ptr->linear_acceleration.y << std::endl;

    	ret = transform(msg_ptr);
    	std::cout << "new ax: " << ret.linear_acceleration.x << std::endl;
    	std::cout << "new ay: " << ret.linear_acceleration.y << std::endl;

    	msg_ptr->linear_acceleration.x = -0.2;
    	msg_ptr->linear_acceleration.y = 0;
    	msg_ptr->angular_velocity.z = 1;
    	std::cout << "old ax: " << msg_ptr->linear_acceleration.x << std::endl;
    	std::cout << "old ay: " << msg_ptr->linear_acceleration.y << std::endl;

    	ret = transform(msg_ptr);
    	std::cout << "new ax: " << ret.linear_acceleration.x << std::endl;
    	std::cout << "new ay: " << ret.linear_acceleration.y << std::endl;
    }

private:
    void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    	sensor_msgs::Imu new_msg = transform(msg);
    	_pub.publish(new_msg);
    }

    Matrix4 _tf_imu_to_base;
    Matrix3 _rot_imu_to_base;
    Vector3 _t_imu_to_base;

    ros::NodeHandle _nh;
    ros::Publisher _pub;
    ros::Subscriber _sub;
};

}

int main(int argc, char* argv[]) {
	ros::init(argc, argv, "imu_transform");
	ros::NodeHandle n;
	ImuTransform::ImuMeasurementModel transformer(n);

	//transformer.test();

	transformer.spin();

	return 0;
}


#include <ctime>
#include <string>
#include <fstream>
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include "keys.hpp"
#include "utils.hpp"
#include "cipher.hpp"
#include "stream_handler.hpp"

namespace mp = boost::multiprecision;
namespace po = boost::program_options;

template<size_t E, size_t D>
void encrypt_file(const std::string& in_file, const std::string& out_file, const rsa::keys<D * 8> keys) {
    std::ifstream inp(in_file, std::ios::binary);
    rsa::ifstream_handler<rsa::read_number_padding_operation, rsa::eof_operation> read_handler(inp);
    std::ofstream out(out_file, std::ios::binary);
    rsa::ofstream_handler<rsa::write_number_operation, rsa::write_bytes_operation> write_handler(out);
    write_handler.write_number<D * 8>(keys.get_d());
    write_handler.write_number<D * 8>(keys.get_n());
    unsigned int n = 0;
    for (const auto& it : rsa::cipher<E, D>::encrypt(read_handler, keys, n)) {
        write_handler.write_number<D * 8>(it);
    }
    if (n == E) {
        std::array<char, E> padding{};
        padding.fill(static_cast<const char&>(E));
        write_handler.write_number<D * 8>(mp::powm(
                static_cast<typename rsa::num_utils<D * 8>::number>(
                        rsa::num_utils<E * 8>::bytes_to_number(padding.begin(), padding.end())),
                keys.get_e(), keys.get_n()));
    }
}

template<size_t E, size_t D>
void decrypt_file(const std::string& in_file, const std::string& out_file, const bool print_keys) {
    std::ifstream inp(in_file, std::ios::binary);
    rsa::ifstream_handler<rsa::read_number_operation, rsa::eof_operation> read_handler(inp);
    std::ofstream out(out_file, std::ios::binary);
    rsa::ofstream_handler<rsa::write_number_operation> write_handler(out);
    const auto d = read_handler.read_number<D * 8>();
    const auto n = read_handler.read_number<D * 8>();
    if (print_keys)
        std::cout << "Decrypt keys:\n    d  : " << d << "\n    n  : " << n << std::endl;
    const auto numbers = rsa::cipher<E, D>::decrypt(read_handler, rsa::keys<D * 8>(n, 0, d, 0));
    for (size_t i = 0; i < numbers.size() - 1; ++i)
        write_handler.write_number<E * 8>(numbers[i]);
    const auto bytes = rsa::num_utils<E * 8>::number_to_bytes(numbers.back());
    const auto p = bytes.back();
    bool ok = true;
    for (size_t i = 1; i < p; ++i)
        ok &= bytes[E - i - 1] == p;
    if (!ok)
        throw std::runtime_error("Could not decrypt file");
    out.write(bytes.data(), E - p);
}

using number = rsa::num_utils<256>::number;

int main(int argc, char* argv[]) {
    rsa::random::default_random().init_generator(static_cast<const uint64_t>(std::time(nullptr)));
    std::string in_file, out_file, enc_file;
    number n, e, d;
    po::options_description desc("Allowed options");
    desc.add_options()
            ("help,h", "Print this message")
            ("n", po::value(&n), "n key")
            ("e", po::value(&e), "e key")
            ("d", po::value(&d), "d key")
            ("input_file,i", po::value(&in_file)->default_value("input.txt"), "input filename")
            ("output_file,o", po::value(&out_file)->default_value("output.txt"), "output filename")
            ("encfile,c", po::value(&enc_file)->default_value("encrypted.txt"), "encrypted filename")
            ("random_keys", "use random keys")
            ("print_keys", "print keys")
            ("encrypt", "encrypt file")
            ("decrypt", "decrypt file");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }
    if (vm.count("encrypt")) {
        const auto keys = vm.count("random_keys") ? rsa::keys<256>() : rsa::keys<256>(n, e, d, 0);
        if (vm.count("print_keys")) {
            std::cout << "Encrypt keys:\n";
            std::cout << "    n  : " << keys.get_n() << '\n';
            std::cout << "    e  : " << keys.get_e() << '\n';
            std::cout << "    d  : " << keys.get_d() << '\n';
            std::cout << "    phi: " << keys.get_phi() << std::endl;
        }
        encrypt_file<8, 32>(in_file, enc_file, keys);
    }
    if (vm.count("decrypt"))
        decrypt_file<8, 32>(enc_file, out_file, vm.count("print_keys"));
}


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// 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 "aml/set.hpp"

#include <type_traits>

#include <iostream>
#include <boost/core/demangle.hpp>

namespace aml
{
    template< typename... X>
    struct list
    {
        template<template<typename...> class F>
        using apply  =  F<X...>;
    };
}

namespace test::set
{
    using aml::_;

    using s0   =  aml::set<>;
    using s1   =  aml::set< _<2> >;
    using s2a  =  aml::set< _<1>, _<2> >;
    using s2b  =  aml::set<_<2>, _<1> >;
    using s3   =  aml::set< _<1>, _<3>, _<2> >;
    using s5   =  aml::set< _<1>, _<2>, _<3>, _<4>, _<5> >;

    void test_empty_set()
    {
        static_assert( s0() == s0() );
        static_assert( s0()  <= s0() );
        static_assert( s0() >= s0() );
        static_assert( ! (s0() != s0()) );
        static_assert( ! (s0() < s0())  );
        static_assert( ! (s0() > s0() ) );

        static_assert( std::is_same< s0::add_elements<>, s0 >::value );
        static_assert( std::is_same< s0::add_elements< _<2>, _<3> >, aml::set< _<2>, _<3> > >::value );
    }

    void test_size()
    {
        static_assert( s0::size() == 0 );
        static_assert( s1::size() == 1 );
        static_assert( s2a::size() == 2 );
        static_assert( s3::size() == 3 );
        static_assert( s5::size() == 5 );
    }

    void test_add_elements()
    {
        static_assert( std::is_same<s0,  s0::add_elements<> >::value );
        static_assert( std::is_same<s1,  s0::add_elements<_<2> > >::value );
        static_assert( std::is_same<s2a, s0::add_elements< _<1>, _<2> > >::value );
        static_assert( std::is_same<s5,  aml::set< _<1>, _<2> >::add_elements< _<3> >::add_elements< _<4>, _<5> > >::value );

        static_assert( std::is_same< aml::set<_<2> >::add_elements< _<2>, _<2> >, aml::set<_<2> > >::value );
    }

    void test_remove_elements()
    {
        static_assert( std::is_same< s0::remove_elements<>, s0 >::value );
        static_assert( std::is_same< s1::remove_elements< _<2>, _<3>, _<2> >, s0 >::value );
        static_assert( std::is_same< s3::remove_elements< _<2>, _<1>, _<4> >, aml::set< _<3> > >::value );
    }

    void test_contains()
    {
        static_assert( s0::contains<>::eval() );
        static_assert( s0::contains<_<1> >::eval() == false );
        static_assert( s3::contains< _<2>, _<1> >::eval() );
        static_assert( s3::contains_any_of< _<2>, _<8> >::eval() );
        static_assert( s3::contains_any_of< _<8>, _<2> >::eval() );
        static_assert( s3::contains_none_of< _<8>, _<5>, _<-1> >::eval() );
        static_assert( s3::contains_none_of< _<1>, _<8> >::eval() == false );
    }

    template<typename X>
    using is_even = aml::bool_< X::eval() % 2 == 0 >;

    void test_subset_by_predicate()
    {
        using p0  =  s0::subset_by_predicate< is_even >;
        using p1  =  s1::subset_by_predicate< is_even >;
        using p2a =  s2a::subset_by_predicate< is_even >;
        using p2b =  s2b::subset_by_predicate< is_even >;
        using p3  =  s3::subset_by_predicate< is_even >;
        using p5  =  s5::subset_by_predicate< is_even >;

        static_assert( std::is_same < p0, aml::set<> >::value );
        static_assert( std::is_same < p1, aml::set< _<2> > >::value );
        static_assert( std::is_same < p2a, aml::set<_<2> > >::value );
        static_assert( std::is_same < p2b, aml::set< _<2> > >::value );
        static_assert( std::is_same < p3, aml::set< _<2> > >::value );
        static_assert( std::is_same < p5, aml::set< _<2>, _<4> > >::value );
    }

    void test_set_operations_and_relations()
    {
        using s1  =  aml::set< _<1>, _<2>, _<3>             >;
        using s2  =  aml::set<       _<2>, _<3>, _<4>       >;
        using s3  =  aml::set<             _<3>, _<4>, _<5> >;
        using s4  =  aml::set< _<1>, _<2>, _<3>, _<4>       >;

        using s2a = aml::set< _<3>, _<2>, _<4> >;

        static_assert( std::is_same< decltype(s1() & s2() & s3()), aml::set<_<3>> >::value );

        using s_13 = decltype(s1() | s3());

        static_assert( std::is_same< s_13, aml::set< _<1>, _<2>, _<3>, _<4>, _<5> > >::value );

        static_assert( std::is_same< decltype( s1() - s2() ), aml::set< _<1> > >::value );
        static_assert( std::is_same< decltype( aml::set<>() - aml::set<>() ), aml::set<> >::value );

        static_assert(  s2a() == s2()  );
        static_assert(   s1() < s4()   );
        static_assert(   s4() > s1()   );
        static_assert(   s1()  <= s4() );
        static_assert(   s4() >= s1()  );
        static_assert(   s3() != s4()  );

        static_assert( !( s3() < s3() ) );
        static_assert( !( s3() > s3() ) );
        static_assert( s3() <= s3() );
        static_assert( s3() >= s3() );
        static_assert( s3() == s3() );
        static_assert( ! (s3() != s3() ) );
    }
}

#include <iostream>
#include <string>

int main()
{
    void (*test_set[])() =
    {
        test::set::test_empty_set,
        test::set::test_size,
        test::set::test_add_elements,
        test::set::test_remove_elements,
        test::set::test_contains,
        test::set::test_subset_by_predicate,
        test::set::test_set_operations_and_relations,
    };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;

}


//==============================================================================
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/nthroot.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/half.hpp>

NT2_TEST_CASE_TPL ( nthroot,  NT2_REAL_TYPES)
{
  using nt2::nthroot;
  using nt2::tag::nthroot_;
  typedef typename nt2::meta::as_integer<T>::type          iT;
  typedef typename nt2::meta::call<nthroot_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<T>(),iT(3)), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<T>(),iT(4)), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<T>(),iT(3)), nt2::Minf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<T>(),iT(4)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<T>(),iT(3)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<T>(),iT(4)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<T>(),iT(4)), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<T>(),iT(0)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One <T>(),iT(0)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Half<T>(),iT(0)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Two <T>(),iT(0)), nt2::Inf <r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<T>(),iT(0)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Two <T>(),iT(0)), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Half<T>(),iT(0)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<T>(),iT(3)), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<T>(),iT(3)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<T>(),iT(4)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<T>(),iT(3)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<T>(),iT(4)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(T(-8),iT(3)), r_t(-2), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(T(256),iT(4)), r_t(4), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(T(8),iT(3)), r_t(2), 0.5);
}


#pragma once

#include <cstddef>
#include <cstdint>
#include <type_traits>
#include <array>
#include <boost/multiprecision/cpp_int.hpp>

namespace cryptb
{
	class sha512
	{
		// The hash size in bits
		static constexpr int hash_digest_size_in_bits{ 512 };
		// The hash size in bytes
		static constexpr int hash_digest_size_in_bytes{ hash_digest_size_in_bits / 8 };

		// The size of each message block in bits
		static constexpr int message_block_size_bits{ sha512::hash_digest_size_in_bits * 2 };

		// The size of each message block in bytes
		static constexpr int message_block_size_bytes{ message_block_size_bits / 8 };

		// Current hash values, of the concatenation of all of the message
		// blocks that we went through until now not including the current message block.
		std::array<std::uint64_t, 8> m_hash_values{ {
		0x6a09e667f3bcc908ULL, 0xbb67ae8584caa73bULL, 0x3c6ef372fe94f82bULL, 0xa54ff53a5f1d36f1ULL,
		0x510e527fade682d1ULL, 0x9b05688c2b3e6c1fULL, 0x1f83d9abfb41bd6bULL, 0x5be0cd19137e2179ULL } };

		using message_block_t = std::array<std::uint64_t, message_block_size_bits / 64>;

		// The partial message block that hasn't yet been accounted for in
		// the summation m_hash_values.
		// The assumption is that m_message_block will be "emptied" when full
		// The variable "m_num_bytes_filled" keeps track of how full m_message_block is.
		message_block_t m_message_block{ {0} };

		// The message block contains 64-bit integers but we're measuring the fullness level in
		// bytes. The message block is filled in big endian order inside of each 64-bit integer.
		// Meaning if "m_num_bytes_filled" is set to 3 then the 3 most significant bytes are set
		// in the first element of "current_message_block".
		int m_num_bytes_filled = 0;

		// Entire message length in bits counter
		boost::multiprecision::uint128_t m_bits_counter{ 0 };

	public:
		sha512() = default;
		sha512(const sha512&) = default;
		sha512(sha512&&) = default;
		sha512& operator=(const sha512&) = default;
		sha512& operator=(sha512&&) = default;

		sha512(const std::uint8_t* const data, const std::size_t len) { this->update(data, len); }

		// Appends another part of the message to be concatenated.
		// Even though that sounds expensive, the memory usage is constant.
		void update(const std::uint8_t* const data, const std::size_t len);

		using digest_t = std::array<std::uint8_t, hash_digest_size_in_bytes>;

		// At any point you can ask for the hash of the concatenated data so far
		digest_t digest() const;

		~sha512() = default;
	private:
		template <typename x_T, int amount>
		static x_T rotater(const x_T& x)
		{
			static_assert(std::is_unsigned<x_T>::value, "Rotates unsigned integer by a specific number of bits");
			static_assert(std::numeric_limits<x_T>::digits > amount, "Amount to shift needs to be smaller than number of bits in unsigned integer."
				" Otherwise it\'s undefined behavior.");
			return (x >> amount) | (x << (64 - amount));
		}

		template <typename x_T>
		static x_T lowercase_sigma0(const x_T& x)
		{
			static_assert(std::is_unsigned<x_T>::value, "Must be unsigned integer");
			return sha512::rotater<x_T, 1>(x) ^ sha512::rotater<x_T, 8>(x) ^ (x >> 7);
		}

		template <typename x_T>
		static x_T lowercase_sigma1(const x_T& x)
		{
			static_assert(std::is_unsigned<x_T>::value, "Must be unsigned integer");
			return sha512::rotater<x_T, 19>(x) ^ sha512::rotater<x_T, 61>(x) ^ (x >> 6);
		}

		template <typename x_T>
		static x_T uppercase_sigma0(const x_T& x)
		{
			static_assert(std::is_unsigned<x_T>::value, "Must be unsigned integer");
			return sha512::rotater<x_T, 28>(x) ^ sha512::rotater<x_T, 34>(x) ^ sha512::rotater<x_T, 39>(x);
		}

		template <typename x_T>
		static x_T uppercase_sigma1(const x_T& x)
		{
			static_assert(std::is_unsigned<x_T>::value, "Must be unsigned integer");
			return sha512::rotater<x_T, 14>(x) ^ sha512::rotater<x_T, 18>(x) ^ sha512::rotater<x_T, 41>(x);
		}

		template <typename xyz_T>
		static xyz_T choice(const xyz_T& x, const xyz_T& y, const xyz_T& z)
		{
			static_assert(std::is_unsigned<xyz_T>::value, "Must be unsigned integer");
			return (x & y) ^ ((~x) & z);
		}

		template <typename xyz_T>
		static xyz_T majority(const xyz_T& x, const xyz_T& y, const xyz_T& z)
		{
			static_assert(std::is_unsigned<xyz_T>::value, "Must be unsigned integer");
			return (x & y) ^ (x & z) ^ (y & z);
		}

		// Helper function to copy elements from an array of bytes
		// into an array of 64 bit unsigned integers in big-endian byte loading.
		// That means that the first byte copies into the most significant
		// byte of the first element of the 64 bit integer array etc.
		//
		// All of the bytes in the 64-bit integer array that aren't direct
		// targets to be overridden, are unaffected by this function's operation.
		static void copy_arr_bytes_into_message_block(
			// Source array of bytes
			const std::uint8_t* const bytes,
			// Length of "bytes" array
			const int num_bytes,
			// Destination
			message_block_t& messsage_block,
			// How many bytes are already used inside of the entire message block
			// The first used byte is the most significant one byte of the first
			// element in the array, in big-endian style.
			const int num_bytes_already_used_in_message_block);

		static void compress(const message_block_t& message_block, std::array<std::uint64_t, 8>& hash_values);

		// Index of byte in array of uint64_t based on big-endian byte order.
		static void zero_bytes(message_block_t& messsage_block, int index_byte_to_start_zeroing);
	};
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::q::lemma2.hpp                        	        //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA2_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA2_HPP_ER_2010
#include <string>
#include <boost/mpl/bool.hpp>
#include <boost/format.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{
namespace lemma2{

    template<typename T,typename Int,typename IntT>
    inline
    T lhs(const T& q, const Int& i_mean,const Int& i_y, const IntT& converter){
        return q + IntT::convert( i_y * ( i_y + 1 ) )
            / IntT::convert( 2 * i_mean );
    }

    template<typename T,typename Int,typename IntT>
    inline
    T upper_bound(const Int& i_mean,const Int& i_y, const IntT& converter){
        return IntT::convert( i_y * ( i_y + 1 ) * (2 * i_y + 1) )
         / IntT::convert( 12 * i_mean * i_mean );
    }

namespace impl{

    template<typename T,typename Int,typename IntT>
    inline
    T lower_bound_common(
        const Int& i_mean,const Int& i_y,
        const IntT& converter, const T& den
    ){
        T ub = upper_bound<T>(i_mean,i_y,converter);
        T num = IntT::convert( (i_y * i_y) * ( (i_y + 1) * (i_y + 1) ) );
        return ub - num / den;
    }

    template<typename T,typename Int,typename IntT>
    inline
    T lower_bound(
        const Int& i_mean,const Int& i_y,
        const IntT& converter, boost::mpl::bool_<true>/*y>=0*/
    ){
        T den = IntT::convert(12 * i_mean * i_mean * i_mean);
        return lower_bound_common<T>(i_mean,i_y,converter,den);
    }

    template<typename T,typename Int,typename IntT>
    inline
    T lower_bound(
        const Int& i_mean,const Int& i_y,
        const IntT& converter, boost::mpl::bool_<false>/*y<0*/
    ){
        T den = IntT::convert(12 * i_mean * i_mean * (i_mean + i_y + 1) );
        return lower_bound_common<T>(i_mean,i_y,converter,den);
    }

}//impl

    template<typename T,typename Int,typename IntT>
    inline
    T lower_bound(const Int& i_mean,const Int& i_y,const IntT& converter){
        if(i_y >= 0){
           typedef boost::mpl::bool_<true> true_;
           return lemma2::impl::lower_bound<T>(i_mean,i_y,converter,true_());
        }else{
           typedef boost::mpl::bool_<false> false_;
           return lemma2::impl::lower_bound<T>(i_mean,i_y,converter,false_());
        }
    }

namespace impl{

    template<typename T,typename Int,typename IntT>
    bool do_raise_error1(
        const T& lhs,
        const T& tol,
        std::string& str,
        const Int& i_mean,
        const Int& i_y,
        const IntT& converter,
        boost::mpl::bool_<true> /*y>=0*/
    ){
        BOOST_ASSERT(i_y>=0);
        if(lhs <= -tol){
            const std::string msg = " [ lemma2 : lhs = %1% >=0 has failed ]";
            typedef boost::format f_;
            f_ f( msg );
            f % lhs;
            str += f.str();
            return true;
        }
        return false;
    }

    template<typename T,typename Int,typename IntT>
    bool do_raise_error1(
        const T& lhs,
        const T& tol,
        std::string& str,
        const Int& i_mean,
        const Int& i_y,
        const IntT& converter,
        boost::mpl::bool_<false> /*y>=0*/
    ){
        BOOST_ASSERT(i_y<0);
        if(lhs >= tol){
            const std::string msg = " [ lemma2.1 : lhs = %1% <=0 has failed ]";
            typedef boost::format f_;
            f_ f( msg ); f % lhs;
            str += f.str();
            return true;
        }
        return false;
    }

}

    template<typename T,typename Int,typename IntT>
    bool do_raise_error1(
        const T& q,
        const T& tol,
        std::string& str,
        const Int& i_mean,
        const Int& i_y,
        const IntT& converter
    ){
        T lhs = lemma2::lhs(q, i_mean, i_y, converter);
        if(i_y >= 0){
           typedef boost::mpl::bool_<true> true_;
           return lemma2::impl::do_raise_error1(
               lhs,tol,str,i_mean,i_y,converter,true_());
        }else{
           typedef boost::mpl::bool_<false> false_;
           return lemma2::impl::do_raise_error1(
               lhs,tol,str,i_mean,i_y,converter,false_());
        }
    }

    template<typename T,typename Int,typename IntT>
    bool do_raise_error2(
        const T& q,
        const T& tol,
        std::string& str,
        const Int& i_mean,
        const Int& i_y,
        const IntT& converter
    ){
        T lhs = lemma2::lhs(q, i_mean, i_y, converter);
        T ub = lemma2::upper_bound<T>(i_mean,i_y, converter);
        if(lhs > ub + tol){
            const std::string msg
                = " [ lemma2.2 : lhs = %1% < %2% has failed ]";
            typedef boost::format f_;
            f_ f( msg ); f % lhs % ub;
            str += f.str();
            return true;
        }
        return false;
    }

    template<typename T,typename Int,typename IntT>
    bool do_raise_error3(
        const T& q,
        const T& tol,
        std::string& str,
        const Int& i_mean,
        const Int& i_y,
        const IntT& converter
    ){
        T lhs = lemma2::lhs(q, i_mean, i_y, converter);
        T lb = lemma2::lower_bound<T>(i_mean,i_y,converter);
        if(lhs + tol < lb){
            const std::string msg
               = " [ lemma2.3 : lhs = %1% > %2% has failed ]";
            typedef boost::format f_;
            f_ f( msg ); f % lhs % lb;
            str += f.str();
            return true;
        }
        return false;
    }

}// lemma2
}// q
}// devroye
}// poisson
}// random
}// boost

#endif


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/20/problem20.hpp"

BOOST_AUTO_TEST_SUITE( Problem20 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem20::solve(10);
        BOOST_CHECK_EQUAL(res, 27);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem20::solve();
        BOOST_CHECK_EQUAL(res, 648);
    }

BOOST_AUTO_TEST_SUITE_END()


//
// Created by devi on 1/9/20.
//
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/graph/graphviz.hpp>
#include <iostream>
#include <fstream>
#include "Node.h"
#include <boost/graph/astar_search.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>

namespace boost {
    enum vertex_Node_t { vertex_Node = 0 };
    BOOST_INSTALL_PROPERTY(vertex, Node);
}

int main() {
    typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS,
            boost::property<boost::vertex_Node_t, std::shared_ptr<Node> > > Graph;

    typedef boost::graph_traits <Graph>::edge_descriptor Edge;
    typedef boost::graph_traits <Graph>::vertex_descriptor Vertex;
    typedef std::pair<int, int> E;

    std::shared_ptr<Node> object_one = std::make_shared<Node>(123, "obj1");
    std::shared_ptr<Node> object_two = std::make_shared<Node>(456, "obj2");
    std::shared_ptr<Node> object_three = std::make_shared<Node>(789, "obj3");

    std::shared_ptr<Node> object_four = std::make_shared<Node>(101, "obj4");
    std::shared_ptr<Node> object_five = std::make_shared<Node>(121, "obj5");
    std::shared_ptr<Node> object_six = std::make_shared<Node>(145, "obj6");

    const int num_nodes = 5;
    E edge_array[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
                       E(3, 4), E(4, 0), E(4, 1)
    };
    int weights[] = { 1, 1, 2, 7, 3, 1, 1, 1 };
    std::size_t num_edges = sizeof(edge_array) / sizeof(E);

    Graph g;

    Vertex vertex_one = boost::add_vertex(object_one, g);
    Vertex vertex_two = boost::add_vertex(object_two, g);
    Vertex vertex_three = boost::add_vertex(object_three, g);
    Vertex vertex_four = boost::add_vertex(object_four, g);
    Vertex vertex_five = boost::add_vertex(object_five, g);
    Vertex vertex_six = boost::add_vertex(object_six, g);

    boost::add_edge(vertex_one, vertex_two, g);
    boost::add_edge(vertex_one, vertex_three, g);
    boost::add_edge(vertex_two, vertex_six, g);
    boost::add_edge(vertex_three, vertex_five, g);
    boost::add_edge(vertex_five, vertex_two, g);
    boost::add_edge(vertex_six, vertex_one, g);
/*
    boost::property_map<Graph, boost::edge_weight_t >::type weight = get(boost::edge_weight, g);
    std::vector < Edge > spanning_tree;

    boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

    std::cout << "Print the edges in the MST:" << std::endl;

    for (std::vector < Edge >::iterator ei = spanning_tree.begin();
         ei != spanning_tree.end(); ++ei)
    {
        std::cout << source(*ei, g)
                  << " <--> "
                  << target(*ei, g)
                  << " with weight of "
                  << weight[*ei]
                  << std::endl;
    }*/

    {
        boost::dynamic_properties dp;
        boost::property_map<Graph , boost::vertex_Node_t>::type custom = get(boost::vertex_Node, g);
        dp.property("node_id", boost::make_transform_value_property_map(std::mem_fn(&Node::get_id), custom));
        dp.property("label", boost::make_transform_value_property_map(std::mem_fn(&Node::get_name), custom));
        boost::write_graphviz_dp(std::cout, g, dp);
    }

    std::vector < Edge > spanning_tree;
    boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
    std::cout << "Print the edges in the MST:" << std::endl;

    for (std::vector < Edge >::iterator ei = spanning_tree.begin();
         ei != spanning_tree.end(); ++ei)
    {
        std::cout << source(*ei, g)
                  << " <--> "
                  << target(*ei, g)
                  << std::endl;
    }

    return 0;
}


#include "stiffness_checker/SharedConst.h"
#include "stiffness_checker/StiffnessSolver.h"
#include <Eigen/LU>
#include <Eigen/SparseCholesky>
#include <iostream>

namespace conmech
{
namespace stiffness_checker
{

bool StiffnessSolver::solveSparseSimplicialLDLT(
    const Eigen::SparseMatrix<double>& A, const Eigen::VectorXd& b, Eigen::VectorXd& x, const bool& verbose)
{
  if(timing_)
  {
    solve_timer_.Start();
  }

  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
  solver.compute(A);

  if (solver.info() == Eigen::NumericalIssue)
  {
    if (verbose)
    {
      std::cerr << "SolverSystem(LDLT): Error in Decomposition!: " << solver.info() << std::endl;
    }
    int info = 0;
    auto Diag = solver.vectorD();
    for (int i = 0; i < Diag.size(); i++)
    {
      if (std::abs(Diag[i]) < conmech::DOUBLE_EPS)
      {
        if (verbose)
        {
          std::cerr << " SolveSystem(LDLT): zero found on diagonal ..." << std::endl;
          std::cerr << " d[" << i << "] = " << Diag[i] << std::endl;
        }
      }
      if (Diag[i] < - conmech::DOUBLE_EPS)
      {
        if (verbose)
        {
          std::cerr << " SolveSystem(LDLT): negative number found on diagonal ..." << std::endl;
          std::cerr << " d[" << i << "] = " << Diag[i] << std::endl;
        }
        info--;
      }
    }
    if (info < 0)
    {
      if (verbose)
      {
        std::cerr << "Stiffness Matrix is not positive definite: " << info
          << " negative elements found on decomp diagonal of K." << std::endl;
        std::cerr << "Matrix size:" << A.rows() << ", " << A.cols() << std::endl;
        std::cerr << "The stucture may have mechanism and thus not stable in general," << std::endl;
        std::cerr << "please Make sure that all six rigid body translations are restrained!" << std::endl;
      }
    }
    return false;
  }

  x = solver.solve(b);
  if (solver.info() != Eigen::Success)
  {
    if (verbose)
    {
      std::cerr << "SolverSystem(LDLT): Error in Solving!" << std::endl;
    }
    return false;
  }

  if(timing_)
  {
    solve_timer_.Stop();
  }
  return true;
}

bool StiffnessSolver::solveSystemLU(
    const Eigen::MatrixXd& A, const Eigen::VectorXd& b, Eigen::VectorXd& x)
{
  if(timing_)
  {
    solve_timer_.Start();
  }

  x = A.fullPivLu().solve(b);

  if(timing_)
  {
    solve_timer_.Stop();
  }

  if ((A*x).isApprox(b))
  {
    return true;
  }
  else
  {
//    std::cout << "A is invertible? - " << A.fullPivLu().isInvertible() << std::endl;
    return false;
  }
}

} // namespace stiffness_checker
} // namespace conmech


#include <iostream>
#include <fstream>
#include <utility>
#include <sstream>

#include <boost/numeric/mtl/mtl.hpp>

typedef double value_type;
typedef std::size_t size_type;
typedef mtl::dense_vector<value_type>  vector_type;
typedef mtl::compressed2D<value_type>  matrix_type;
const double pi(3.14159265358979323846);

template <typename Vector, typename Matrix>
class grad_f_ftor
{
  public:
    grad_f_ftor(const Matrix& M, const Matrix& K, const Matrix& G, const value_type& beta, const value_type& w)
      : M(M), K(K), G(G), beta(beta), w(w)
      {
	q.change_dim(num_cols(G));
	x.change_dim(num_cols(G));
	x=0.0; q=0.0;
	for(size_type i=1; i< size(x); i++)
	  x(i)= x(i-1) + 0.01;
      }

    //get timedependent input q for the heating
    Vector get_input(const value_type& time)
    {
      value_type center(0.5*sin(2*pi*time)+0.5);
      for(size_type i=0; i< size(x); i++)
	  q(i)=beta*cos(std::min(std::abs(pi*(x(i)-center)/(2*w)),pi/2));
      return q;
    }
    template <typename VectorIn>
    Vector operator()(const VectorIn& u, const value_type& time)
    {
	q=get_input(time);
	Vector x(K*u + G*q);
	return x;
    }

  private:
    Vector      q, x;
    Matrix      M, K, G;
    value_type  beta, w;

};

template <typename grad_f_ftor, typename Vector, typename Matrix>
Vector ode23s(grad_f_ftor func, value_type start_time, value_type end_time, Vector start_value, Matrix M, Matrix K){

  value_type time(start_time), time_step(0.005), h= 0.005, gamma(1-1/sqrt(2));
  mtl::dense2D<value_type> LU(M-h*gamma*K);
  lu(LU);
  Vector x(start_value);
  size_type time_counter(0);
  Vector k1, k2, step1, step2;
  while (time < end_time){
      k1= lu_solve_apply(LU, func(x,time));
      step1= x + h * k1;
      step2= func(step1, time + time_step) - 2*M*k1;
      k2= lu_solve_apply(LU, step2);
      save_data(x, time_counter);
      x+= 3/2*h*k1 + 1/2*h*k2;
      time_counter++;
      time+= time_step;
  }
  return x;
}

//save current state of solution x(time)
template<typename Vector>
void save_data(const Vector& x, const size_type& time)
{
    std::stringstream name;
    name  <<"plot_data_" << time <<".dat";
#if 1
    std::cout<< "#!/usr/bin/gnuplot\n";
    std::cout<< "set term png\n";
   // std::cout<< "set zrange[-4:14]\n";
    std::cout<< "set output \""<<time<<".png\"\n";
    std::cout<< "splot './plot_data_"<< time<<".dat' using 1:2:3 with pm3d\n";
#endif
    std::fstream f;
    f.open(name.str().c_str(), std::ios::out);
    size_type n(1111), n1(101);

    size_type row(0), col(0);
    for(size_type i=0; i < n; i++){
      if(i%(n/n1)==0){
	row=0;col+=1;
	f << "\n";
      }
      f <<  col << " " << row << " " << x(i) << "\n";
      row+=1;
    }
    f.close();
}

int main( int  , char ** )
{
     //read jörgs matrices
    matrix_type M(mtl::io::matrix_market( "M.mtx")),
		K(mtl::io::matrix_market( "K.mtx")),
		G(mtl::io::matrix_market( "G.mtx"));

    size_type n(num_rows(K));
    value_type  start_time(0.0), end_time(1.0);
    vector_type  x(n,0.0), x0(n,0.0);

    grad_f_ftor< vector_type, matrix_type >  grad_f(M, K, G, 0.2, 0.1);

    x= ode23s(grad_f, start_time, end_time, x0, M, K);

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::distributions::gamma::include.hpp         //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/gamma.hpp>

#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/is_log_concave.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/os.hpp>

#endif


#include "Generic/common/leak_detection.h"

#include "PriorLoss.h"
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "Generic/common/SessionLogger.h"
#include "Generic/common/UnrecoverableException.h"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/SmoothedL1.h"

using Eigen::VectorXd;
using Eigen::SparseVector;

PriorLoss::PriorLoss(InferenceDataView_ptr data, double prior, double lossWeight,
					 Loss loss)
: _data(data), _prior(prior), _lossWeight(lossWeight), _lossFunc(loss) {}

double PriorLoss::operator()(VectorXd& gradient, size_t thread_idx,
		size_t n_threads) const
{
	double loss = 0.0;

	for (int i=static_cast<int>(thread_idx); i<static_cast<int>(_data->nInstances()); i+=static_cast<int>(n_threads)) {
		double pos = _data->prediction(i);

		if (_lossFunc == KL) {
			double neg = 1.0 - pos;

			loss -= _lossWeight * (_prior * log (_prior/pos) +
				(1.0-_prior)*log((1.0-_prior)/neg));

			double factor = _lossWeight * (pos - _prior);

			SparseVector<double>::InnerIterator it(_data->features(i));
			for (; it; ++it) {
				int inst = it.index();
				gradient(it.index()) -= factor * it.value();
			}
		} else { // _lossFunc == L1
			loss += SmoothedL1::SmoothedL1ProbConstant(pos, _prior,
				_data->features(i), gradient, _lossWeight);
		}
	}
	_debug_loss = loss;
	return loss;
}

double PriorLoss::forInstance(int inst, SparseVector<double>& gradient) const {
	double ret = 0.0;

	double pos = _data->prediction(inst);

	if (_lossFunc == KL) {
		double neg = 1.0 - pos;

		ret -= _lossWeight * (_prior * log (_prior/pos) +
			(1.0-_prior)*log((1.0-_prior)/neg));

		double factor = _lossWeight * (pos - _prior);

		SparseVector<double>::InnerIterator it(_data->features(inst));
		for (; it; ++it) {
			int inst = it.index();
			gradient.coeffRef(it.index()) -= factor * it.value();
		}
	} else { // _lossFunc == L1
		throw UnrecoverableException("PriorLoss::forInstance",
			"Instance-wise loss not yet implemented for L1 (but it's not "
			"hard to do)");
/*		ret += SmoothedL1::SmoothedL1ProbConstant(pos, _prior,
			_data->features(inst), gradient, _lossWeight);*/
	}

	return ret;
}

std::wstring PriorLoss::status() const {
	return L"prior_loss = " + boost::lexical_cast<std::wstring>(_debug_loss);
}

void PriorLoss::snapshot() {
}


// Boost.Geometry

// Copyright (c) 2017-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DENSIFY_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DENSIFY_HPP

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/signed_size_type.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace densify
{

/*!
\brief Densification of geographic segment.
\ingroup strategies
\tparam FormulaPolicy The geodesic formulas used internally.
\tparam Spheroid The spheroid model.
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.densify.densify_4_with_strategy densify (with strategy)]
\* [link geometry.reference.srs.srs_spheroid srs::spheroid]
}
 */
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
public:
    geographic()
        : m_spheroid()
    {}

    explicit geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point, typename AssignPolicy, typename T>
    inline void apply(Point const& p0, Point const& p1, AssignPolicy & policy, T const& length_threshold) const
    {
        typedef typename AssignPolicy::point_type out_point_t;
        typedef typename select_most_precise
            <
                typename coordinate_type<Point>::type,
                typename coordinate_type<out_point_t>::type,
                CalculationType
            >::type calc_t;

        typedef typename FormulaPolicy::template direct<calc_t, true, false, false, false> direct_t;
        typedef typename FormulaPolicy::template inverse<calc_t, true, true, false, false, false> inverse_t;

        typename inverse_t::result_type
            inv_r = inverse_t::apply(get_as_radian<0>(p0), get_as_radian<1>(p0),
                                     get_as_radian<0>(p1), get_as_radian<1>(p1),
                                     m_spheroid);

        BOOST_GEOMETRY_ASSERT(length_threshold > T(0));

        signed_size_type n = signed_size_type(inv_r.distance / length_threshold);
        if (n <= 0)
            return;

        calc_t step = inv_r.distance / (n + 1);

        calc_t current = step;
        for (signed_size_type i = 0 ; i < n ; ++i, current += step)
        {
            typename direct_t::result_type
                dir_r = direct_t::apply(get_as_radian<0>(p0), get_as_radian<1>(p0),
                                        current, inv_r.azimuth,
                                        m_spheroid);

            out_point_t p;
            set_from_radian<0>(p, dir_r.lon2);
            set_from_radian<1>(p, dir_r.lat2);
            geometry::detail::conversion::point_to_point
                <
                    Point, out_point_t,
                    2, dimension<out_point_t>::value
                >::apply(p0, p);

            policy.apply(p);
        }
    }

private:
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<geographic_tag>
{
    typedef strategy::densify::geographic<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DENSIFY_HPP