/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file crypto.cpp
 * @author Alex Leverington <nessence@gmail.com>
 * @author Gav Wood <i@gavwood.com>
 * @date 2014
 * Crypto test functions.
 */

#include <libdevcore/Guards.h>
#include <secp256k1.h>
#include <cryptopp/keccak.h>
#include <cryptopp/pwdbased.h>
#include <cryptopp/sha.h>
#include <cryptopp/modes.h>
#include <cryptopp/eccrypto.h>
#include <cryptopp/osrng.h>
#include <cryptopp/oids.h>
#include <cryptopp/dsa.h>
#include <libdevcore/Common.h>
#include <libdevcore/RLP.h>
#include <libdevcore/Log.h>
#include <boost/test/unit_test.hpp>
#include <libdevcore/SHA3.h>
#include <libdevcrypto/ECDHE.h>
#include <libdevcrypto/CryptoPP.h>
#include <test/libtesteth/TestHelper.h>

using namespace std;
using namespace dev;
using namespace dev::test;
using namespace dev::crypto;
using namespace CryptoPP;

BOOST_AUTO_TEST_SUITE(Crypto)

struct DevcryptoTestFixture: public TestOutputHelper {
	DevcryptoTestFixture() : s_secp256k1(Secp256k1PP::get()) {}

	Secp256k1PP* s_secp256k1;
};
BOOST_FIXTURE_TEST_SUITE(devcrypto, DevcryptoTestFixture)

static CryptoPP::AutoSeededRandomPool& rng()
{
	static CryptoPP::AutoSeededRandomPool s_rng;
	return s_rng;
}

static CryptoPP::OID& curveOID()
{
	static CryptoPP::OID s_curveOID(CryptoPP::ASN1::secp256k1());
	return s_curveOID;
}

static CryptoPP::DL_GroupParameters_EC<CryptoPP::ECP>& params()
{
	static CryptoPP::DL_GroupParameters_EC<CryptoPP::ECP> s_params(curveOID());
	return s_params;
}

BOOST_AUTO_TEST_CASE(sha3general)
{
	BOOST_REQUIRE_EQUAL(sha3(""), h256("c5d2460186f7233c927e7db2dcc703c0e500b653ca82273b7bfad8045d85a470"));
	BOOST_REQUIRE_EQUAL(sha3("hello"), h256("1c8aff950685c2ed4bc3174f3472287b56d9517b9c948127319a09a7a36deac8"));
}

BOOST_AUTO_TEST_CASE(emptySHA3Types)
{
	h256 emptySHA3(fromHex("c5d2460186f7233c927e7db2dcc703c0e500b653ca82273b7bfad8045d85a470"));
	BOOST_REQUIRE_EQUAL(emptySHA3, EmptySHA3);

	h256 emptyListSHA3(fromHex("1dcc4de8dec75d7aab85b567b6ccd41ad312451b948a7413f0a142fd40d49347"));
	BOOST_REQUIRE_EQUAL(emptyListSHA3, EmptyListSHA3);
}

BOOST_AUTO_TEST_CASE(pubkeyOfZero)
{
	auto pub = toPublic({});
	BOOST_REQUIRE_EQUAL(pub, Public{});
}

BOOST_AUTO_TEST_CASE(KeyPairMix)
{
	KeyPair k = KeyPair::create();
	BOOST_REQUIRE(!!k.secret());
	BOOST_REQUIRE(!!k.pub());
	Public test = toPublic(k.secret());
	BOOST_CHECK_EQUAL(k.pub(), test);
}

BOOST_AUTO_TEST_CASE(keypairs)
{
	KeyPair p(Secret(fromHex("3ecb44df2159c26e0f995712d4f39b6f6e499b40749b1cf1246c37f9516cb6a4")));
	BOOST_REQUIRE(p.pub() == Public(fromHex("97466f2b32bc3bb76d4741ae51cd1d8578b48d3f1e68da206d47321aec267ce78549b514e4453d74ef11b0cd5e4e4c364effddac8b51bcfc8de80682f952896f")));
	BOOST_REQUIRE(p.address() == Address(fromHex("8a40bfaa73256b60764c1bf40675a99083efb075")));
	eth::Transaction t(1000, 0, 0, h160(fromHex("944400f4b88ac9589a0f17ed4671da26bddb668b")), {}, 0, p.secret());
	auto rlp = t.rlp(eth::WithoutSignature);
	auto expectedRlp = "dc80808094944400f4b88ac9589a0f17ed4671da26bddb668b8203e880";
	BOOST_CHECK_EQUAL(toHex(rlp), expectedRlp);
	rlp = t.rlp(eth::WithSignature);
	auto expectedRlp2 = "f85f80808094944400f4b88ac9589a0f17ed4671da26bddb668b8203e8801ca0bd2402a510c9c9afddf2a3f63c869573bd257475bea91d6f164638134a3386d6a0609ad9775fd2715e6a359c627e9338478e4adba65dd0dc6ef2bcbe6398378984";
	BOOST_CHECK_EQUAL(toHex(rlp), expectedRlp2);
	BOOST_CHECK_EQUAL(t.sender(), p.address());
}

BOOST_AUTO_TEST_CASE(KeyPairVerifySecret)
{
	auto keyPair = KeyPair::create();
	auto* ctx = secp256k1_context_create(SECP256K1_CONTEXT_NONE);
	BOOST_CHECK(secp256k1_ec_seckey_verify(ctx, keyPair.secret().data()));
	secp256k1_context_destroy(ctx);
}

BOOST_AUTO_TEST_CASE(SignAndRecover)
{
	// This basic test that compares **fixed** results. Useful to test new
	// implementations or changes to implementations.
	auto sec = Secret{sha3("sec")};
	auto msg = sha3("msg");
	auto sig = sign(sec, msg);
	auto expectedSig = "b826808a8c41e00b7c5d71f211f005a84a7b97949d5e765831e1da4e34c9b8295d2a622eee50f25af78241c1cb7cfff11bcf2a13fe65dee1e3b86fd79a4e3ed000";
	BOOST_CHECK_EQUAL(sig.hex(), expectedSig);

	auto pub = recover(sig, msg);
	auto expectedPub = "e40930c838d6cca526795596e368d16083f0672f4ab61788277abfa23c3740e1cc84453b0b24f49086feba0bd978bb4446bae8dff1e79fcc1e9cf482ec2d07c3";
	BOOST_CHECK_EQUAL(pub.hex(), expectedPub);
}

BOOST_AUTO_TEST_CASE(SignAndRecoverLoop)
{
	auto num = 13;
	auto msg = h256::random();
	while (--num)
	{
		msg = sha3(msg);
		auto kp = KeyPair::create();
		auto sig = sign(kp.secret(), msg);
		BOOST_CHECK(verify(kp.pub(), sig, msg));
		auto pub = recover(sig, msg);
		BOOST_CHECK_EQUAL(kp.pub(), pub);
	}
}

BOOST_AUTO_TEST_CASE(cryptopp_patch)
{
	KeyPair k = KeyPair::create();
	bytes io_text;
	s_secp256k1->decrypt(k.secret(), io_text);
	BOOST_REQUIRE_EQUAL(io_text.size(), 0);
}

BOOST_AUTO_TEST_CASE(verify_secert)
{
	Secret empty;
	KeyPair kNot(empty);
	BOOST_REQUIRE(!kNot.address());
	KeyPair k(sha3(empty));
	BOOST_REQUIRE(k.address());
}

BOOST_AUTO_TEST_CASE(common_encrypt_decrypt)
{
	string message("Now is the time for all good persons to come to the aid of humanity.");
	bytes m = asBytes(message);
	bytesConstRef bcr(&m);

	KeyPair k = KeyPair::create();
	bytes cipher;
	encrypt(k.pub(), bcr, cipher);
	BOOST_REQUIRE(cipher != asBytes(message) && cipher.size() > 0);

	bytes plain;
	decrypt(k.secret(), bytesConstRef(&cipher), plain);

	BOOST_REQUIRE(asString(plain) == message);
	BOOST_REQUIRE(plain == asBytes(message));
}

BOOST_AUTO_TEST_CASE(sha3_norestart)
{
	CryptoPP::Keccak_256 ctx;
	bytes input(asBytes("test"));
	ctx.Update(input.data(), 4);
	CryptoPP::Keccak_256 ctxCopy(ctx);
	bytes interimDigest(32);
	ctx.Final(interimDigest.data());
	ctx.Update(input.data(), 4);
	bytes firstDigest(32);
	ctx.Final(firstDigest.data());
	BOOST_REQUIRE(interimDigest == firstDigest);

	ctxCopy.Update(input.data(), 4);
	bytes finalDigest(32);
	ctxCopy.Final(interimDigest.data());
	BOOST_REQUIRE(interimDigest != finalDigest);

	// we can do this another way -- copy the context for final
	ctxCopy.Update(input.data(), 4);
	ctxCopy.Update(input.data(), 4);
	CryptoPP::Keccak_256 finalCtx(ctxCopy);
	bytes finalDigest2(32);
	finalCtx.Final(finalDigest2.data());
	BOOST_REQUIRE(finalDigest2 == interimDigest);
	ctxCopy.Update(input.data(), 4);
	bytes finalDigest3(32);
	finalCtx.Final(finalDigest3.data());
	BOOST_REQUIRE(finalDigest2 != finalDigest3);
}

BOOST_AUTO_TEST_CASE(ecies_kdf)
{
	KeyPair local = KeyPair::create();
	KeyPair remote = KeyPair::create();
	// nonce
	Secret z1;
	ecdh::agree(local.secret(), remote.pub(), z1);
	auto key1 = s_secp256k1->eciesKDF(z1, bytes(), 64);
	bytesConstRef eKey1 = bytesConstRef(&key1).cropped(0, 32);
	bytesRef mKey1 = bytesRef(&key1).cropped(32, 32);
	sha3(mKey1, mKey1);

	Secret z2;
	ecdh::agree(remote.secret(), local.pub(), z2);
	auto key2 = s_secp256k1->eciesKDF(z2, bytes(), 64);
	bytesConstRef eKey2 = bytesConstRef(&key2).cropped(0, 32);
	bytesRef mKey2 = bytesRef(&key2).cropped(32, 32);
	sha3(mKey2, mKey2);

	BOOST_REQUIRE(eKey1.toBytes() == eKey2.toBytes());
	BOOST_REQUIRE(mKey1.toBytes() == mKey2.toBytes());

	BOOST_REQUIRE(!!z1);
	BOOST_REQUIRE(z1 == z2);

	BOOST_REQUIRE(key1.size() > 0 && ((u512)h512(key1)) > 0);
	BOOST_REQUIRE(key1 == key2);
}

BOOST_AUTO_TEST_CASE(ecies_standard)
{
	KeyPair k = KeyPair::create();

	string message("Now is the time for all good persons to come to the aid of humanity.");
	string original = message;
	bytes b = asBytes(message);

	s_secp256k1->encryptECIES(k.pub(), b);
	BOOST_REQUIRE(b != asBytes(original));
	BOOST_REQUIRE(b.size() > 0 && b[0] == 0x04);

	s_secp256k1->decryptECIES(k.secret(), b);
	BOOST_REQUIRE(bytesConstRef(&b).cropped(0, original.size()).toBytes() == asBytes(original));
}

BOOST_AUTO_TEST_CASE(ecies_sharedMacData)
{
	KeyPair k = KeyPair::create();

	string message("Now is the time for all good persons to come to the aid of humanity.");
	string original = message;
	bytes b = asBytes(message);

	string shared("shared MAC data");
	string wrongShared("wrong shared MAC data");

	s_secp256k1->encryptECIES(k.pub(), shared, b);
	BOOST_REQUIRE(b != asBytes(original));
	BOOST_REQUIRE(b.size() > 0 && b[0] == 0x04);

	BOOST_REQUIRE(!s_secp256k1->decryptECIES(k.secret(), wrongShared, b));

	s_secp256k1->decryptECIES(k.secret(), shared, b);

	// Temporary disable this assertion, which is failing in TravisCI only for Ubuntu Trusty.
	// See https://travis-ci.org/bobsummerwill/cpp-ethereum/jobs/143250866.
	#if !defined(DISABLE_BROKEN_UNIT_TESTS_UNTIL_WE_FIX_THEM)
		BOOST_REQUIRE(bytesConstRef(&b).cropped(0, original.size()).toBytes() == asBytes(original));
	#endif // !defined(DISABLE_BROKEN_UNIT_TESTS_UNTIL_WE_FIX_THEM)
}

BOOST_AUTO_TEST_CASE(ecies_eckeypair)
{
	KeyPair k = KeyPair::create();

	string message("Now is the time for all good persons to come to the aid of humanity.");
	string original = message;

	bytes b = asBytes(message);
	s_secp256k1->encrypt(k.pub(), b);
	BOOST_REQUIRE(b != asBytes(original));

	s_secp256k1->decrypt(k.secret(), b);
	BOOST_REQUIRE(b == asBytes(original));
}

BOOST_AUTO_TEST_CASE(ecdhCryptopp)
{
	ECDH<ECP>::Domain dhLocal(curveOID());
	SecByteBlock privLocal(dhLocal.PrivateKeyLength());
	SecByteBlock pubLocal(dhLocal.PublicKeyLength());
	dhLocal.GenerateKeyPair(rng(), privLocal, pubLocal);

	ECDH<ECP>::Domain dhRemote(curveOID());
	SecByteBlock privRemote(dhRemote.PrivateKeyLength());
	SecByteBlock pubRemote(dhRemote.PublicKeyLength());
	dhRemote.GenerateKeyPair(rng(), privRemote, pubRemote);

	assert(dhLocal.AgreedValueLength() == dhRemote.AgreedValueLength());

	// local: send public to remote; remote: send public to local

	// Local
	SecByteBlock sharedLocal(dhLocal.AgreedValueLength());
	assert(dhLocal.Agree(sharedLocal, privLocal, pubRemote));

	// Remote
	SecByteBlock sharedRemote(dhRemote.AgreedValueLength());
	assert(dhRemote.Agree(sharedRemote, privRemote, pubLocal));

	// Test
	Integer ssLocal, ssRemote;
	ssLocal.Decode(sharedLocal.BytePtr(), sharedLocal.SizeInBytes());
	ssRemote.Decode(sharedRemote.BytePtr(), sharedRemote.SizeInBytes());

	assert(ssLocal != 0);
	assert(ssLocal == ssRemote);

	// Now use our keys
	KeyPair a = KeyPair::create();
	byte puba[65] = {0x04};
	memcpy(&puba[1], a.pub().data(), 64);

	KeyPair b = KeyPair::create();
	byte pubb[65] = {0x04};
	memcpy(&pubb[1], b.pub().data(), 64);

	ECDH<ECP>::Domain dhA(curveOID());
	Secret shared;
	BOOST_REQUIRE(dhA.Agree(shared.writable().data(), a.secret().data(), pubb));
	BOOST_REQUIRE(shared);
}

BOOST_AUTO_TEST_CASE(ecdhe)
{
	ECDHE local;
	ECDHE remote;
	BOOST_CHECK_NE(local.pubkey(), remote.pubkey());

	// local tx pubkey -> remote
	Secret sremote;
	remote.agree(local.pubkey(), sremote);

	// remote tx pbukey -> local
	Secret slocal;
	local.agree(remote.pubkey(), slocal);

	BOOST_REQUIRE(sremote);
	BOOST_REQUIRE(slocal);
	BOOST_CHECK_EQUAL(sremote, slocal);
}

BOOST_AUTO_TEST_CASE(ecdhAgree)
{
	auto sec = Secret{sha3("ecdhAgree")};
	auto pub = toPublic(sec);
	Secret sharedSec;
	ecdh::agree(sec, pub, sharedSec);
	BOOST_CHECK(sharedSec);
	auto expectedSharedSec = "8ac7e464348b85d9fdfc0a81f2fdc0bbbb8ee5fb3840de6ed60ad9372e718977";
	BOOST_CHECK_EQUAL(sharedSec.makeInsecure().hex(), expectedSharedSec);
}

BOOST_AUTO_TEST_CASE(handshakeNew)
{
	//	authInitiator -> E(remote-pubk, S(ecdhe-random, ecdh-shared-secret^nonce) || H(ecdhe-random-pubk) || pubk || nonce || 0x0)
	//	authRecipient -> E(remote-pubk, ecdhe-random-pubk || nonce || 0x0)

	h256 base(sha3("privacy"));
	sha3(base.ref(), base.ref());
	Secret nodeAsecret(base);
	KeyPair nodeA(nodeAsecret);
	BOOST_REQUIRE(nodeA.pub());

	sha3(base.ref(), base.ref());
	Secret nodeBsecret(base);
	KeyPair nodeB(nodeBsecret);
	BOOST_REQUIRE(nodeB.pub());

	BOOST_REQUIRE_NE(nodeA.secret(), nodeB.secret());

	// Initiator is Alice (nodeA)
	ECDHE eA;
	bytes nAbytes(fromHex("0xAAAA"));
	h256 nonceA(sha3(nAbytes));
	bytes auth(Signature::size + h256::size + Public::size + h256::size + 1);
	Secret ssA;
	{
		bytesRef sig(&auth[0], Signature::size);
		bytesRef hepubk(&auth[Signature::size], h256::size);
		bytesRef pubk(&auth[Signature::size + h256::size], Public::size);
		bytesRef nonce(&auth[Signature::size + h256::size + Public::size], h256::size);

		crypto::ecdh::agree(nodeA.secret(), nodeB.pub(), ssA);
		sign(eA.seckey(), (ssA ^ nonceA).makeInsecure()).ref().copyTo(sig);
		sha3(eA.pubkey().ref(), hepubk);
		nodeA.pub().ref().copyTo(pubk);
		nonceA.ref().copyTo(nonce);
		auth[auth.size() - 1] = 0x0;
	}
	bytes authcipher;
	encrypt(nodeB.pub(), &auth, authcipher);
	BOOST_REQUIRE_EQUAL(authcipher.size(), 279);

	// Receipient is Bob (nodeB)
	ECDHE eB;
	bytes nBbytes(fromHex("0xBBBB"));
	h256 nonceB(sha3(nAbytes));
	bytes ack(Public::size + h256::size + 1);
	{
		// todo: replace nodeA.pub() in encrypt()
		// decrypt public key from auth
		bytes authdecrypted;
		decrypt(nodeB.secret(), &authcipher, authdecrypted);
		Public node;
		bytesConstRef pubk(&authdecrypted[Signature::size + h256::size], Public::size);
		pubk.copyTo(node.ref());

		bytesRef epubk(&ack[0], Public::size);
		bytesRef nonce(&ack[Public::size], h256::size);

		eB.pubkey().ref().copyTo(epubk);
		nonceB.ref().copyTo(nonce);
		auth[auth.size() - 1] = 0x0;
	}
	bytes ackcipher;
	encrypt(nodeA.pub(), &ack, ackcipher);
	BOOST_REQUIRE_EQUAL(ackcipher.size(), 182);

	BOOST_REQUIRE(eA.pubkey());
	BOOST_REQUIRE(eB.pubkey());
	BOOST_REQUIRE_NE(eA.seckey(), eB.seckey());

	/// Alice (after receiving ack)
	Secret aEncryptK;
	Secret aMacK;
	Secret aEgressMac;
	Secret aIngressMac;
	{
		bytes ackdecrypted;
		decrypt(nodeA.secret(), &ackcipher, ackdecrypted);
		BOOST_REQUIRE(ackdecrypted.size());
		bytesConstRef ackRef(&ackdecrypted);
		Public eBAck;
		h256 nonceBAck;
		ackRef.cropped(0, Public::size).copyTo(bytesRef(eBAck.data(), Public::size));
		ackRef.cropped(Public::size, h256::size).copyTo(nonceBAck.ref());
		BOOST_REQUIRE_EQUAL(eBAck, eB.pubkey());
		BOOST_REQUIRE_EQUAL(nonceBAck, nonceB);

		// TODO: export ess and require equal to b

		bytes keyMaterialBytes(512);
		bytesRef keyMaterial(&keyMaterialBytes);

		Secret ess;
		// todo: ecdh-agree should be able to output bytes
		eA.agree(eBAck, ess);
		ess.ref().copyTo(keyMaterial.cropped(0, h256::size));
		ssA.ref().copyTo(keyMaterial.cropped(h256::size, h256::size));
//		auto token = sha3(ssA);
		aEncryptK = sha3Secure(keyMaterial);
		aEncryptK.ref().copyTo(keyMaterial.cropped(h256::size, h256::size));
		aMacK = sha3Secure(keyMaterial);

		keyMaterialBytes.resize(h256::size + authcipher.size());
		keyMaterial.retarget(keyMaterialBytes.data(), keyMaterialBytes.size());
		(aMacK ^ nonceBAck).ref().copyTo(keyMaterial);
		bytesConstRef(&authcipher).copyTo(keyMaterial.cropped(h256::size, authcipher.size()));
		aEgressMac = sha3Secure(keyMaterial);

		keyMaterialBytes.resize(h256::size + ackcipher.size());
		keyMaterial.retarget(keyMaterialBytes.data(), keyMaterialBytes.size());
		(aMacK ^ nonceA).ref().copyTo(keyMaterial);
		bytesConstRef(&ackcipher).copyTo(keyMaterial.cropped(h256::size, ackcipher.size()));
		aIngressMac = sha3Secure(keyMaterial);
	}

	/// Bob (after sending ack)
	Secret ssB;
	crypto::ecdh::agree(nodeB.secret(), nodeA.pub(), ssB);
	BOOST_REQUIRE_EQUAL(ssA, ssB);

	Secret bEncryptK;
	Secret bMacK;
	Secret bEgressMac;
	Secret bIngressMac;
	{
		bytes authdecrypted;
		decrypt(nodeB.secret(), &authcipher, authdecrypted);
		BOOST_REQUIRE(authdecrypted.size());
		bytesConstRef ackRef(&authdecrypted);
		Signature sigAuth;
		h256 heA;
		Public eAAuth;
		Public nodeAAuth;
		h256 nonceAAuth;
		bytesConstRef sig(&authdecrypted[0], Signature::size);
		bytesConstRef hepubk(&authdecrypted[Signature::size], h256::size);
		bytesConstRef pubk(&authdecrypted[Signature::size + h256::size], Public::size);
		bytesConstRef nonce(&authdecrypted[Signature::size + h256::size + Public::size], h256::size);

		nonce.copyTo(nonceAAuth.ref());
		pubk.copyTo(nodeAAuth.ref());
		BOOST_REQUIRE(nonceAAuth);
		BOOST_REQUIRE_EQUAL(nonceA, nonceAAuth);
		BOOST_REQUIRE(nodeAAuth);
		BOOST_REQUIRE_EQUAL(nodeA.pub(), nodeAAuth); // bad test, bad!!!
		hepubk.copyTo(heA.ref());
		sig.copyTo(sigAuth.ref());

		Secret ss;
		ecdh::agree(nodeB.secret(), nodeAAuth, ss);
		eAAuth = recover(sigAuth, (ss ^ nonceAAuth).makeInsecure());
		// todo: test when this fails; means remote is bad or packet bits were flipped
		BOOST_REQUIRE_EQUAL(heA, sha3(eAAuth));
		BOOST_REQUIRE_EQUAL(eAAuth, eA.pubkey());

		bytes keyMaterialBytes(512);
		bytesRef keyMaterial(&keyMaterialBytes);

		Secret ess;
		// todo: ecdh-agree should be able to output bytes
		eB.agree(eAAuth, ess);
//		s_secp256k1->agree(eB.seckey(), eAAuth, ess);
		ess.ref().copyTo(keyMaterial.cropped(0, h256::size));
		ssB.ref().copyTo(keyMaterial.cropped(h256::size, h256::size));
//		auto token = sha3(ssA);
		bEncryptK = sha3Secure(keyMaterial);
		bEncryptK.ref().copyTo(keyMaterial.cropped(h256::size, h256::size));
		bMacK = sha3Secure(keyMaterial);

		// todo: replace nonceB with decrypted nonceB
		keyMaterialBytes.resize(h256::size + ackcipher.size());
		keyMaterial.retarget(keyMaterialBytes.data(), keyMaterialBytes.size());
		(bMacK ^ nonceAAuth).ref().copyTo(keyMaterial);
		bytesConstRef(&ackcipher).copyTo(keyMaterial.cropped(h256::size, ackcipher.size()));
		bEgressMac = sha3Secure(keyMaterial);

		keyMaterialBytes.resize(h256::size + authcipher.size());
		keyMaterial.retarget(keyMaterialBytes.data(), keyMaterialBytes.size());
		(bMacK ^ nonceB).ref().copyTo(keyMaterial);
		bytesConstRef(&authcipher).copyTo(keyMaterial.cropped(h256::size, authcipher.size()));
		bIngressMac = sha3Secure(keyMaterial);
	}

	BOOST_REQUIRE_EQUAL(aEncryptK, bEncryptK);
	BOOST_REQUIRE_EQUAL(aMacK, bMacK);
	BOOST_REQUIRE_EQUAL(aEgressMac, bIngressMac);
	BOOST_REQUIRE_EQUAL(bEgressMac, aIngressMac);

}

BOOST_AUTO_TEST_CASE(ecies_aes128_ctr_unaligned)
{
	SecureFixedHash<16> encryptK(sha3("..."), h128::AlignLeft);
	h256 egressMac(sha3("+++"));
	// TESTING: send encrypt magic sequence
	bytes magic {0x22,0x40,0x08,0x91};
	bytes magicCipherAndMac;
	magicCipherAndMac = encryptSymNoAuth(encryptK, h128(), &magic);

	magicCipherAndMac.resize(magicCipherAndMac.size() + 32);
	sha3mac(egressMac.ref(), &magic, egressMac.ref());
	egressMac.ref().copyTo(bytesRef(&magicCipherAndMac).cropped(magicCipherAndMac.size() - 32, 32));

	bytesConstRef cipher(&magicCipherAndMac[0], magicCipherAndMac.size() - 32);
	bytes plaintext = decryptSymNoAuth(encryptK, h128(), cipher).makeInsecure();

	plaintext.resize(magic.size());
	// @alex @subtly TODO: FIX: this check is pointless with the above line.
	BOOST_REQUIRE(plaintext.size() > 0);
	BOOST_REQUIRE(magic == plaintext);
}

BOOST_AUTO_TEST_CASE(ecies_aes128_ctr)
{
	SecureFixedHash<16> k(sha3("0xAAAA"), h128::AlignLeft);
	string m = "AAAAAAAAAAAAAAAA";
	bytesConstRef msg((byte*)m.data(), m.size());

	bytes ciphertext;
	h128 iv;
	tie(ciphertext, iv) = encryptSymNoAuth(k, msg);

	bytes plaintext = decryptSymNoAuth(k, iv, &ciphertext).makeInsecure();
	BOOST_REQUIRE_EQUAL(asString(plaintext), m);
}

BOOST_AUTO_TEST_CASE(cryptopp_aes128_ctr)
{
	const int aesKeyLen = 16;
	BOOST_REQUIRE(sizeof(char) == sizeof(byte));

	// generate test key
	AutoSeededRandomPool rng;
	SecByteBlock key(0x00, aesKeyLen);
	rng.GenerateBlock(key, key.size());

	// cryptopp uses IV as nonce/counter which is same as using nonce w/0 ctr
	FixedHash<AES::BLOCKSIZE> ctr;
	rng.GenerateBlock(ctr.data(), sizeof(ctr));

	// used for decrypt
	FixedHash<AES::BLOCKSIZE> ctrcopy(ctr);

	string text = "Now is the time for all good persons to come to the aid of humanity.";
	unsigned char const* in = (unsigned char*)&text[0];
	unsigned char* out = (unsigned char*)&text[0];
	string original = text;
	string doublespeak = text + text;

	string cipherCopy;
	try
	{
		CTR_Mode<AES>::Encryption e;
		e.SetKeyWithIV(key, key.size(), ctr.data());

		// 68 % 255 should be difference of counter
		e.ProcessData(out, in, text.size());
		ctr = h128(u128(ctr) + text.size() / 16);

		BOOST_REQUIRE(text != original);
		cipherCopy = text;
	}
	catch (CryptoPP::Exception& _e)
	{
		cerr << _e.what() << endl;
	}

	try
	{
		CTR_Mode< AES >::Decryption d;
		d.SetKeyWithIV(key, key.size(), ctrcopy.data());
		d.ProcessData(out, in, text.size());
		BOOST_REQUIRE(text == original);
	}
	catch (CryptoPP::Exception& _e)
	{
		cerr << _e.what() << endl;
	}

	// reencrypt ciphertext...
	try
	{
		BOOST_REQUIRE(cipherCopy != text);
		in = (unsigned char*)&cipherCopy[0];
		out = (unsigned char*)&cipherCopy[0];

		CTR_Mode<AES>::Encryption e;
		e.SetKeyWithIV(key, key.size(), ctrcopy.data());
		e.ProcessData(out, in, text.size());

		// yep, ctr mode.
		BOOST_REQUIRE(cipherCopy == original);
	}
	catch (CryptoPP::Exception& _e)
	{
		cerr << _e.what() << endl;
	}

}

BOOST_AUTO_TEST_CASE(cryptopp_aes128_cbc)
{
	const int aesKeyLen = 16;
	BOOST_REQUIRE(sizeof(char) == sizeof(byte));

	AutoSeededRandomPool rng;
	SecByteBlock key(0x00, aesKeyLen);
	rng.GenerateBlock(key, key.size());

	// Generate random IV
	byte iv[AES::BLOCKSIZE];
	rng.GenerateBlock(iv, AES::BLOCKSIZE);

	string string128("AAAAAAAAAAAAAAAA");
	string plainOriginal = string128;

	CryptoPP::CBC_Mode<Rijndael>::Encryption cbcEncryption(key, key.size(), iv);
	cbcEncryption.ProcessData((byte*)&string128[0], (byte*)&string128[0], string128.size());
	BOOST_REQUIRE(string128 != plainOriginal);

	CBC_Mode<Rijndael>::Decryption cbcDecryption(key, key.size(), iv);
	cbcDecryption.ProcessData((byte*)&string128[0], (byte*)&string128[0], string128.size());
	BOOST_REQUIRE(plainOriginal == string128);

	// plaintext whose size isn't divisible by block size must use stream filter for padding
	string string192("AAAAAAAAAAAAAAAABBBBBBBB");
	plainOriginal = string192;

	string cipher;
	StreamTransformationFilter* aesStream = new StreamTransformationFilter(cbcEncryption, new StringSink(cipher));
	StringSource source(string192, true, aesStream);
	BOOST_REQUIRE(cipher.size() == 32);

	byte* pOut = reinterpret_cast<byte*>(&string192[0]);
	byte const* pIn = reinterpret_cast<byte const*>(cipher.data());
	cbcDecryption.ProcessData(pOut, pIn, cipher.size());
	BOOST_REQUIRE(string192 == plainOriginal);
}

BOOST_AUTO_TEST_CASE(recoverVgt3)
{
	// base secret
	Secret secret(sha3("privacy"));

	// we get ec params from signer
	ECDSA<ECP, Keccak_256>::Signer signer;

	// e := sha3(msg)
	bytes e(fromHex("0x01"));
	e.resize(32);
	int tests = 13;
	while (sha3(&e, &e), secret = sha3(secret), tests--)
	{
		KeyPair key(secret);
		Public pkey = key.pub();
		signer.AccessKey().Initialize(params(), Integer(secret.data(), Secret::size));

		h256 he(sha3(e));
		Integer heInt(he.asBytes().data(), 32);
		h256 k(crypto::kdf(secret, he));
		Integer kInt(k.asBytes().data(), 32);
		kInt %= params().GetSubgroupOrder()-1;

		ECP::Point rp = params().ExponentiateBase(kInt);
		Integer const& q = params().GetGroupOrder();
		Integer r = params().ConvertElementToInteger(rp);

		Integer kInv = kInt.InverseMod(q);
		Integer s = (kInv * (Integer(secret.data(), 32) * r + heInt)) % q;
		BOOST_REQUIRE(!!r && !!s);

		//try recover function on diffrent v values (should be invalid)
		for (size_t i = 0; i < 10; i++)
		{
			Signature sig;
			sig[64] = i;
			r.Encode(sig.data(), 32);
			s.Encode(sig.data() + 32, 32);

			Public p = dev::recover(sig, he);
			size_t expectI = rp.y.IsOdd() ? 1 : 0;
			if (i == expectI)
				BOOST_REQUIRE(p == pkey);
			else
				BOOST_REQUIRE(p != pkey);
		}
	}
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


// MIT License
//
// Copyright (c) 2021 Aditya Shridhar Hegde
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <HEAAN.h>
#include <limits.h>
#include <stdlib.h>

#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>
#include <ckp19/dataset.hpp>
#include <ckp19/mean_shift.hpp>
#include <ckp19/plaintext_clustering.hpp>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <vector>

#include "common.hpp"

namespace utf = boost::unit_test;

// Test fixture
struct MeanShiftFixture {
  Ring ring;
  SecretKey secretKey;
  Scheme scheme;
  MeanShift meanshift;

  MeanShiftFixture()
      : secretKey(ring),
        scheme(secretKey, ring),
        meanshift(scheme, SchemeConstants.logn, SchemeConstants.logq,
                  SchemeConstants.logq_boot, SchemeConstants.logp,
                  SchemeConstants.logt, SchemeConstants.seed) {
    BOOST_TEST_REQUIRE(SchemeConstants.logn < logN - 1);

    scheme.addLeftRotKeys(secretKey);
    scheme.addRightRotKeys(secretKey);

    // Tests will involve bootstrap operations on ciphertext with 2 or 4 slots.
    scheme.addBootKey(secretKey, 3, SchemeConstants.logq_boot + 4);
    scheme.addBootKey(secretKey, 4, SchemeConstants.logq_boot + 4);
  }
};

// Utility functions

void scale_vector(double *v, long n, double r) {
  double sum;
  for (int i = 0; i < n; ++i) sum += v[i] * v[i];

  sum = std::sqrt(sum);
  sum /= r;

  for (int i = 0; i < n; ++i) v[i] /= sum;
};

void getDustCiphertextFromPlaintext(
    Ciphertext &dusts, MeanShift &meanshift,
    const std::vector<std::vector<double>> &points, long dim, long npoints,
    long num_dusts, long slots) {
  std::vector<complex<double>> pdusts(slots, 0);
  for (long i = 0; i < num_dusts; ++i) {
    for (long j = 0; j < npoints; ++j) {
      for (long k = 0; k < dim; ++k) {
        pdusts[i * npoints * dim + j * dim + k] = points[i % points.size()][k];
      }
    }
  }

  meanshift.scheme.encrypt(dusts, pdusts.data(), slots, meanshift.logp,
                           meanshift.logq);
}

// Since most of these algorithms take signfificant time to execute if `logN`
// is large, the tests are quite minimal and don't verify across a lot of
// different parameter values.
BOOST_FIXTURE_TEST_SUITE(mean_shift, MeanShiftFixture)

BOOST_AUTO_TEST_CASE(inverse, *utf::tolerance(1e-4)) {
  srand(SchemeConstants.seed);
  meanshift.refreshSeed(SchemeConstants.seed);

  double m = 20;
  double steps = 11;

  std::unique_ptr<double[]> pval(
      EvaluatorUtils::randomRealArray(meanshift.slots, m));

  Ciphertext cval;
  scheme.encrypt(cval, pval.get(), meanshift.slots, meanshift.logp,
                 meanshift.logq);
  Ciphertext cinv;
  meanshift.inverse(cinv, cval, m, steps);

  BOOST_TEST(cinv.logq == cval.logq - (steps + 2) * meanshift.logp,
             "cinv.logq: " << cinv.logq << ", cval.logq: " << cval.logq);
  BOOST_TEST(cinv.logp == cval.logp,
             "cinv.logp: " << cinv.logp << ", cval.logp: " << cval.logp);

  std::unique_ptr<complex<double>[]> pinv(scheme.decrypt(secretKey, cinv));

  for (long i = 0; i < meanshift.slots; ++i) {
    double exp = 1.0 / pval[i];
    double output = pinv[i].real();
    BOOST_TEST(exp == output,
               "output: " << pinv[i].real() << ", expected: " << exp);
  }
}

BOOST_TEST_DECORATOR(*utf::tolerance(1e-5))
BOOST_DATA_TEST_CASE(kernelSIMD, utf::data::xrange<long>(2, 4), dim) {
  srand(SchemeConstants.seed);
  meanshift.refreshSeed(SchemeConstants.seed);

  long degree = 5;
  // Deliberate choice to not make `npoints` a power of 2. Having buffer values
  // in plaintext slots can help catch errors in implementation.
  long npoints = 6;

  // `npoints` points should be encrypted within the same ciphertext.
  BOOST_TEST_REQUIRE(npoints * dim < meanshift.slots);

  long req_slots = npoints * dim;
  std::unique_ptr<double[]> x(EvaluatorUtils::randomRealArray(meanshift.slots));
  std::unique_ptr<double[]> y(EvaluatorUtils::randomRealArray(meanshift.slots));
  scale_vector(x.get(), req_slots, 0.5);
  scale_vector(y.get(), req_slots, 0.5);

  Ciphertext cx, cy;
  scheme.encrypt(cx, x.get(), meanshift.slots, meanshift.logp, meanshift.logq);
  scheme.encrypt(cy, y.get(), meanshift.slots, meanshift.logp, meanshift.logq);

  Ciphertext ckernel;
  meanshift.kernelSIMD(ckernel, cx, cy, dim, npoints, degree);

  BOOST_TEST(ckernel.logq == cx.logq - (degree + 2) * meanshift.logp,
             "ckernel.logq: " << ckernel.logq << ", cx.logq: " << cx.logq);
  BOOST_TEST(ckernel.logp == cx.logp,
             "ckernel.logp: " << ckernel.logp << ", cx.logp: " << cx.logp);

  std::unique_ptr<complex<double>[]> output(
      meanshift.scheme.decrypt(secretKey, ckernel));

  PlainMeanShift pmeanshift(meanshift.seed);
  for (long i = 0; i < npoints; ++i) {
    double kernel = pmeanshift.kernel(
        std::vector<double>(x.get() + i * dim, x.get() + (i + 1) * dim),
        std::vector<double>(y.get() + i * dim, y.get() + (i + 1) * dim),
        degree);

    for (long j = 0; j < dim; ++j) {
      BOOST_TEST(
          output[i * dim + j].real() == kernel,
          "output: " << output[i * dim + j].real() << ", expected: " << kernel);
    }
  }
}

BOOST_TEST_DECORATOR(*utf::tolerance(5e-4))
BOOST_DATA_TEST_CASE(refreshDusts, utf::data::xrange(2, 4), dim) {
  srand(SchemeConstants.seed);
  meanshift.refreshSeed(SchemeConstants.seed);

  long num_dusts = 4;
  // Compute `npoints` (data points per ciphertext) based on given params
  long npoints = meanshift.slots / (dim * num_dusts);

  BOOST_TEST_REQUIRE(npoints >= 1);

  Dataset ds(dim, npoints, SchemeConstants.seed);
  Ciphertext cdusts;
  getDustCiphertextFromPlaintext(cdusts, meanshift, ds.getPoints(), dim,
                                 npoints, num_dusts, meanshift.slots);
  std::unique_ptr<complex<double>[]> orig(scheme.decrypt(secretKey, cdusts));

  meanshift.refreshDusts(cdusts, dim, npoints, num_dusts);
  std::unique_ptr<complex<double>[]> bootstrap(
      scheme.decrypt(secretKey, cdusts));

  BOOST_TEST(
      cdusts.logq > cdusts.logp,
      "cdusts.logq: " << cdusts.logq << ", cdusts.logp: " << cdusts.logp);
  BOOST_TEST(cdusts.logq > SchemeConstants.logq_boot,
             "cdusts.logq: " << cdusts.logq
                             << ", logq_boot: " << SchemeConstants.logq_boot);

  for (long i = 0; i < num_dusts; ++i) {
    for (long j = 0; j < dim; ++j) {
      // `refreshDusts` sets only the first instance of the dust
      long pos = i * dim * npoints + j;
      BOOST_TEST(orig[pos].real() == bootstrap[pos].real(),
                 "output: " << bootstrap[pos].real()
                            << ", expected: " << orig[pos].real());
    }
  }
}

BOOST_TEST_DECORATOR(*utf::tolerance(5e-4))
BOOST_DATA_TEST_CASE(modeSeekingSIMD,
                     utf::data::xrange(2, 4) *
                         utf::data::make(std::vector<long>{1, 5}),
                     dim, steps) {
  srand(SchemeConstants.seed);
  meanshift.refreshSeed(SchemeConstants.seed);

  long num_point_cts = 2;
  long num_dusts = 4;
  long kdegree = 5;
  long inv_steps = 8;

  // Compute `npoints` (data points per ciphertext) based on given params
  long npoints = meanshift.slots / (dim * num_dusts);

  BOOST_TEST_REQUIRE(npoints >= 1);

  Dataset ds(dim, num_point_cts * npoints, SchemeConstants.seed);
  ds.rescaleDatasetInPlace(0.5);

  EncryptedDataset eds;
  ds.encryptSIMD(eds, scheme, num_dusts, meanshift.slots, meanshift.logp,
                 meanshift.logq);

  // Meanshift object should ideally be constructed with `eds.log_slots`. For
  // testing, we work our way backwards and set `eds.log_slots` to be equal to
  // `meanshift.logn`.
  BOOST_TEST_REQUIRE(eds.log_slots == meanshift.logn);

  std::vector<std::vector<double>> dusts;
  PlainMeanShift pmeanshift(meanshift.seed);
  pmeanshift.modeSeeking(dusts, ds.getPoints(), num_dusts, steps, kdegree,
                         inv_steps);

  Ciphertext cdusts;
  meanshift.modeSeekingSIMD(cdusts, eds.points, eds.dim, eds.npoints, num_dusts,
                            steps, kdegree, inv_steps);

  if (steps == 1) {
    BOOST_TEST(cdusts.logq == (eds.points[0].logq -
                               (kdegree + inv_steps + 7) * meanshift.logp),
               "cdusts.logq: " << cdusts.logq
                               << ", dataset.logq: " << eds.points[0].logq);
    BOOST_TEST(cdusts.logp == eds.points[0].logp,
               "cdusts.logp: " << cdusts.logp
                               << ", dataset.logp: " << eds.points[0].logp);
  } else {
    BOOST_TEST(
        cdusts.logq > cdusts.logp,
        "cdusts.logq: " << cdusts.logq << ", cdusts.logp: " << cdusts.logp);
    BOOST_TEST(cdusts.logq > SchemeConstants.logq_boot,
               "cdusts.logq: " << cdusts.logq
                               << ", logq_boot: " << SchemeConstants.logq_boot);
  }

  std::unique_ptr<complex<double>[]> pdusts(
      meanshift.scheme.decrypt(secretKey, cdusts));

  for (long i = 0; i < num_dusts; ++i) {
    for (long j = 0; j < npoints; ++j) {
      for (long k = 0; k < dim; ++k) {
        long pos = i * npoints * dim + j * dim + k;
        BOOST_TEST(
            pdusts[pos].real() == dusts[i][k],
            "output: " << pdusts[pos].real() << ", expected: " << dusts[i][k]);
      }
    }
  }
}

BOOST_TEST_DECORATOR(*utf::tolerance(5e-1))
BOOST_DATA_TEST_CASE(pointLabelingSIMD, utf::data::xrange(2, 4), dim) {
  srand(SchemeConstants.seed);
  meanshift.refreshSeed(SchemeConstants.seed);

  long num_point_cts = 2;
  long num_dusts = 4;
  long kdegree = 4;
  long inv_steps = 13;
  long minidx_t = 4;
  long npoints = meanshift.slots / (dim * num_dusts);

  BOOST_TEST_REQUIRE(npoints >= 1);

  Dataset ds(dim, num_point_cts * npoints, SchemeConstants.seed);
  ds.rescaleDatasetInPlace(0.4);

  EncryptedDataset eds;
  ds.encryptSIMD(eds, scheme, num_dusts, meanshift.slots, meanshift.logp,
                 meanshift.logq);

  // Meanshift object should ideally be constructed with `eds.log_slots`. For
  // testing, we work our way backwards and set `eds.log_slots` to be equal to
  // `meanshift.logn`.
  BOOST_TEST_REQUIRE(eds.log_slots == meanshift.logn);

  std::vector<std::vector<double>> dusts(ds.getPoints().begin(),
                                         ds.getPoints().begin() + num_dusts);

  Ciphertext cdusts;
  getDustCiphertextFromPlaintext(cdusts, meanshift, dusts, dim, npoints,
                                 num_dusts, meanshift.slots);

  BOOST_TEST_REQUIRE(cdusts.logq == eds.points[0].logq);
  BOOST_TEST_REQUIRE(cdusts.logp == eds.points[0].logp);

  std::vector<std::vector<double>> labels;
  PlainMeanShift pmeanshift(meanshift.seed);
  pmeanshift.pointLabeling(labels, ds.getPoints(), dusts, kdegree, inv_steps,
                           minidx_t);

  std::vector<Ciphertext> clabels;
  meanshift.pointLabelingSIMD(clabels, cdusts, eds.points, dim, npoints,
                              num_dusts, kdegree, inv_steps, minidx_t);

  for (long p = 0; p < num_point_cts; ++p) {
    std::unique_ptr<complex<double>[]> plabel(
        meanshift.scheme.decrypt(secretKey, clabels[p]));

    for (long i = 0; i < num_dusts; ++i) {
      for (long j = 0; j < npoints; ++j) {
        for (long k = 0; k < dim; ++k) {
          double output = plabel[i * npoints * dim + j * dim + k].real();
          double exp = labels[p * npoints + j][i];
          BOOST_TEST(output == exp,
                     "output: " << output << ", expected: " << exp);
        }
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Dense>
#include <lib/rrt/StateSpace.hpp>
#include <lib/rrt/Tree.hpp>

namespace RRT {

/**
 * This creates an instance of an RRT Tree with the callbacks
 * configured from the given parameters.
 *
 * @param w, h The dimensions of the 2d plane.  These are used when
 * picking random points for the Tree to move towards.

 * @param goal The point representing the goal that the tree is
 * trying to find a path to
 *
 * @param step The fixed step size that the tree uses.  This is the
 * maximum distance between nodes unless adaptive stepsize control is utilized.
 *
 * @return An RRT::Tree with its callbacks and parameters configured.
 * You'll probably want to override the transitionValidator callback
 * if your 2d plane has any obstacles.
 */
RRT::Tree<Eigen::Vector2f>* TreeFor2dPlane(
    std::shared_ptr<StateSpace<Eigen::Vector2f>> stateSpace,
    Eigen::Vector2f goal, float step);

}  // namespace RRT


#include <time.h>

#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <cblox/core/submap_collection.h>
#include <cblox/io/submap_io.h>
#include <cblox/utils/quat_transformation_protobuf_utils.h>
#include <glog/logging.h>
#include <ros/ros.h>
#include <voxblox/io/layer_io.h>
#include <voxblox_ros/ptcloud_vis.h>
#include <voxblox_ros/ros_params.h>

#include "voxgraph/frontend/submap_collection/voxgraph_submap.h"
#include "voxgraph/tools/submap_registration_helper.h"
#include "voxgraph/tools/tf_helper.h"
#include "voxgraph/tools/visualization/submap_visuals.h"

int main(int argc, char** argv) {
  enum SolverReportStyle { kBrief, kFull, kNone };
  using voxblox::Transformation;
  using voxgraph::RegistrationCostFunction;
  using voxgraph::SubmapRegistrationHelper;
  using voxgraph::TfHelper;
  using voxgraph::VoxgraphSubmap;

  // Start logging
  google::InitGoogleLogging(argv[0]);

  // Register with ROS master
  ros::init(argc, argv, "voxgraph_registration_test_bench");

  // Create node handles
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  // Read ROS params: General experiment settings
  std::string submap_collection_file_path, log_folder_path;
  CHECK(nh_private.getParam("submap_collection_file_path",
                            submap_collection_file_path))
      << "Rosparam submap_collection_file_path must be set" << std::endl;
  CHECK(nh_private.getParam("log_folder_path", log_folder_path))
      << "Rosparam log_folder_path must be set" << std::endl;
  cblox::SubmapID reference_submap_id, reading_submap_id;
  {
    int fixed_submap_id_tmp, reading_submap_id_tmp;
    CHECK(nh_private.getParam("reference_submap_id", fixed_submap_id_tmp))
        << "Rosparam reference_submap_id must be set" << std::endl;
    CHECK(nh_private.getParam("reading_submap_id", reading_submap_id_tmp))
        << "Rosparam reading_submap_id must be set" << std::endl;
    reference_submap_id = static_cast<cblox::SubmapID>(fixed_submap_id_tmp);
    reading_submap_id = static_cast<cblox::SubmapID>(reading_submap_id_tmp);
  }
  std::vector<float> range_x, range_y, range_z;
  std::vector<float> range_yaw, range_pitch, range_roll;
  nh_private.param("test_range/x", range_x, {0});
  nh_private.param("test_range/y", range_y, {0});
  nh_private.param("test_range/z", range_z, {0});
  nh_private.param("test_range/yaw", range_yaw, {0});
  nh_private.param("test_range/pitch", range_pitch, {0});
  nh_private.param("test_range/roll", range_roll, {0});
  SolverReportStyle solver_report_style;
  {
    std::string report_style_str;
    nh_private.param<std::string>("solver_report_style", report_style_str,
                                  "brief");
    if (report_style_str == "brief") {
      solver_report_style = kBrief;
    } else if (report_style_str == "full") {
      solver_report_style = kFull;
    } else if (report_style_str == "none") {
      solver_report_style = kNone;
    } else {
      ROS_FATAL(
          "Param \"report_style\" must be "
          "\"brief\" (default), \"full\" or \"none\"");
      ros::shutdown();
      return -1;
    }
  }

  // Read ROS params: Submap registration settings
  ros::NodeHandle nh_registration(nh_private, "submap_registration");
  VoxgraphSubmap::Config::RegistrationFilter registration_filter_config;
  SubmapRegistrationHelper::Options registerer_options;
  nh_registration.param("solver/max_num_iterations",
                        registerer_options.solver.max_num_iterations, 40);
  nh_registration.param("solver/parameter_tolerance",
                        registerer_options.solver.parameter_tolerance, 3e-9);
  nh_registration.param("cost/no_correspondence_cost",
                        registerer_options.registration.no_correspondence_cost,
                        registerer_options.registration.no_correspondence_cost);
  nh_registration.param("cost/use_esdf_distance",
                        registerer_options.registration.use_esdf_distance,
                        registerer_options.registration.use_esdf_distance);
  nh_registration.param("cost/visualize_residuals",
                        registerer_options.registration.visualize_residuals,
                        registerer_options.registration.visualize_residuals);
  nh_registration.param("cost/visualize_gradients",
                        registerer_options.registration.visualize_gradients,
                        registerer_options.registration.visualize_gradients);
  nh_registration.param("cost/visualize_transforms",
                        registerer_options.registration.visualize_transforms_,
                        registerer_options.registration.visualize_transforms_);
  nh_registration.param("cost/sampling_ratio",
                        registerer_options.registration.sampling_ratio,
                        registerer_options.registration.sampling_ratio);
  // Get the registration method (from string)
  {
    std::string registration_method_str;
    nh_registration.param<std::string>("cost/registration_method",
                                       registration_method_str,
                                       "implicit_to_implicit");
    if (registration_method_str == "implicit_to_implicit") {
      registerer_options.registration.registration_point_type =
          VoxgraphSubmap::RegistrationPointType::kVoxels;
    } else if (registration_method_str == "explicit_to_implicit") {
      registerer_options.registration.registration_point_type =
          VoxgraphSubmap::RegistrationPointType::kIsosurfacePoints;
    } else {
      ROS_FATAL(
          "Param \"submap_registration/cost/registration_method\" must be "
          "\"implicit_to_implicit\" (default) or \"explicit_to_implicit\"");
      ros::shutdown();
      return -1;
    }
  }
  // Get the Jacobian evaluation method (from string)
  {
    std::string jacobian_evaluation_method_str;
    nh_registration.param<std::string>("cost/jacobian_evaluation_method",
                                       jacobian_evaluation_method_str,
                                       "analytic");
    if (jacobian_evaluation_method_str == "analytic") {
      registerer_options.registration.jacobian_evaluation_method =
          RegistrationCostFunction::JacobianEvaluationMethod::kAnalytic;
    } else if (jacobian_evaluation_method_str == "numeric") {
      registerer_options.registration.jacobian_evaluation_method =
          RegistrationCostFunction::JacobianEvaluationMethod::kNumeric;
    } else {
      ROS_FATAL(
          "Param \"submap_registration/cost/jacobian_evaluation_method\" "
          "must be \"analytic\" (default) or \"numeric\"");
      ros::shutdown();
      return -1;
    }
  }
  // Get the submap relevant voxel thresholds
  nh_registration.param("cost/min_voxel_weight",
                        registration_filter_config.min_voxel_weight,
                        registration_filter_config.min_voxel_weight);
  nh_registration.param("cost/max_voxel_distance",
                        registration_filter_config.max_voxel_distance,
                        registration_filter_config.max_voxel_distance);

  // Announce ROS topics for Rviz debug visuals
  ros::Publisher reference_mesh_pub =
      nh_private.advertise<visualization_msgs::Marker>("reference_mesh_pub", 1,
                                                       true);
  ros::Publisher reference_tsdf_pub =
      nh_private.advertise<pcl::PointCloud<pcl::PointXYZI>>(
          "reference_tsdf_pointcloud", 1, true);
  ros::Publisher perturbed_reading_mesh_pub =
      nh_private.advertise<visualization_msgs::Marker>(
          "perturbed_reading_mesh_pub", 1, true);
  ros::Publisher optimized_reading_mesh_pub =
      nh_private.advertise<visualization_msgs::Marker>(
          "optimized_reading_mesh_pub", 1, true);

  // Load the submap collection
  cblox::SubmapCollection<VoxgraphSubmap>::Ptr submap_collection_ptr;
  cblox::io::LoadSubmapCollection<VoxgraphSubmap>(submap_collection_file_path,
                                                  &submap_collection_ptr);

  // If both submaps IDs are the same, duplicate the reference submap
  if (reference_submap_id == reading_submap_id) {
    ROS_INFO(
        "Reference and reading submap IDs are the same, "
        "duplicating the reference...");
    reading_submap_id = INT32_MAX;
    CHECK(submap_collection_ptr->duplicateSubmap(reference_submap_id,
                                                 reading_submap_id));
  }

  // Setup the submap to submap registerer
  SubmapRegistrationHelper submap_registerer(submap_collection_ptr,
                                             registerer_options);

  // Setup visualization tools
  voxgraph::SubmapVisuals submap_vis(
      submap_collection_ptr->getConfig(),
      voxblox::getMeshIntegratorConfigFromRosParam(nh_private));

  // Save the reference submap pose and the original reading submap pose
  Transformation transform_getter;
  CHECK(submap_collection_ptr->getSubmapPose(reference_submap_id,
                                             &transform_getter));
  const Transformation T_mission__reference = transform_getter;
  CHECK(submap_collection_ptr->getSubmapPose(reading_submap_id,
                                             &transform_getter));
  const Transformation T_mission__reading_original = transform_getter;
  const Eigen::Vector3f& ground_truth_position =
      T_mission__reading_original.getPosition();

  // Publish TFs for the reference and reading submap
  TfHelper::publishTransform(T_mission__reference, "mission",
                             "reference_submap", true);
  TfHelper::publishTransform(T_mission__reading_original, "mission",
                             "reading_submap", true);

  // Publish the meshes used to visualize the submaps
  {
    // Wait for Rviz to launch so that it receives the meshes
    ros::Rate wait_rate(1);
    while (reference_mesh_pub.getNumSubscribers() == 0) {
      ROS_INFO(
          "Waiting for Rviz to launch "
          "and subscribe to topic 'reference_mesh_pub'");
      wait_rate.sleep();
    }
    // Publish reference submap Mesh
    submap_vis.publishMesh(*submap_collection_ptr, reference_submap_id,
                           voxblox::Color::Green(), "reference_submap",
                           reference_mesh_pub);
    // Publish temporary TFs for the moving meshes, such that Rviz
    // doesn't discard them due to missing frame position information
    TfHelper::publishTransform(T_mission__reading_original, "mission",
                               "perturbed_submap", true);
    TfHelper::publishTransform(T_mission__reading_original, "mission",
                               "optimized_submap", true);
    // Publish the reading submap mesh used to indicate its perturbed pose
    submap_vis.publishMesh(*submap_collection_ptr, reading_submap_id,
                           voxblox::Color::Red(), "perturbed_submap",
                           perturbed_reading_mesh_pub);
    // Publish the reading submap mesh used to indicate its optimized pose
    submap_vis.publishMesh(*submap_collection_ptr, reading_submap_id,
                           voxblox::Color::Blue(), "optimized_submap",
                           optimized_reading_mesh_pub);
  }

  // Finish the submaps such that their cached members are generated
  {
    VoxgraphSubmap::Ptr ref_submap_ptr =
        submap_collection_ptr->getSubmapPtr(reference_submap_id);
    VoxgraphSubmap::Ptr reading_submap_ptr =
        submap_collection_ptr->getSubmapPtr(reading_submap_id);
    CHECK_NOTNULL(ref_submap_ptr);
    CHECK_NOTNULL(reading_submap_ptr);
    ref_submap_ptr->setRegistrationFilterConfig(registration_filter_config);
    reading_submap_ptr->setRegistrationFilterConfig(registration_filter_config);
    ref_submap_ptr->finishSubmap();
    reading_submap_ptr->finishSubmap();
  }

  // Create the log file with the current timestamp as its file name
  time_t raw_time = std::time(nullptr);
  struct tm time_struct;
  localtime_r(&raw_time, &time_struct);
  char time_char_buffer[80];
  strftime(time_char_buffer, 80, "%Y-%m-%d_%H-%M-%S", &time_struct);
  boost::filesystem::path log_file_path;
  log_file_path = log_folder_path;
  log_file_path /= time_char_buffer;
  log_file_path += ".csv";
  ROS_INFO_STREAM("Log file will be saved as: " << log_file_path);
  std::ofstream log_file;
  log_file.open(log_file_path.string());

  // Create log file and write header
  // TODO(victorr): Check if log_folder_path exists and create it if it doesn't
  log_file << "reference_submap_id, reading_submap_id, "
           << "visuals_enabled, using_esdf_distance, git_branch, git_hash\n"
           << reference_submap_id << ",";
  if (reading_submap_id == INT32_MAX) {
    log_file << reference_submap_id;
  } else {
    log_file << reading_submap_id;
  }
  log_file << ","
           << (registerer_options.registration.visualize_residuals ||
               registerer_options.registration.visualize_gradients)
           << "," << registerer_options.registration.use_esdf_distance << ","
           << GIT_BRANCH << "," << GIT_COMMIT_HASH << "\n"
           << "x_true, y_true, z_true, yaw_true, pitch_true, roll_true\n"
           << ground_truth_position.x() << "," << ground_truth_position.y()
           << "," << ground_truth_position.z() << ","
           << "0,0,0\n";
  log_file << "x_disturbance, y_disturbance, z_disturbance,"
           << "yaw_disturbance, pitch_disturbance, roll_disturbance, "
           << "x_error, y_error, z_error, yaw_error, pitch_error, roll_error, "
           << "solve_time \n";

  // Loop over all ranges
  int counter(0);
  ROS_INFO("Looping over all starting positions:");
  for (const float& x : range_x) {
    for (const float& y : range_y)
      for (const float& z : range_z)
        for (const float& yaw : range_yaw)
          for (const float& pitch : range_pitch)
            for (const float& roll : range_roll) {
              // Append test to log file
              log_file << x << "," << y << "," << z << "," << yaw << ","
                       << pitch << "," << roll << ",";

              // Move reading_submap to T(x,y,z,yaw,pitch,roll)
              // TODO(victorr): Implement disturbance over pitch & roll
              Transformation::Vector3 rot_vec(0, 0, yaw);
              voxblox::Rotation rotation(rot_vec);
              Transformation T_mission__reading_perturbed;
              T_mission__reading_perturbed.getRotation() =
                  T_mission__reading_original.getRotation() * rotation;
              Transformation::Vector3 translation(x, y, z);
              T_mission__reading_perturbed.getPosition() =
                  T_mission__reading_original.getPosition() + translation;
              submap_collection_ptr->setSubmapPose(
                  reading_submap_id, T_mission__reading_perturbed);

              // Announce progress
              const Eigen::Vector3f& perturbed_position =
                  T_mission__reading_perturbed.getPosition();
              printf(
                  "-- % 2i disturbance:        "
                  "x % 4.6f    y % 4.6f    z % 4.6f    "
                  "yaw % 4.2f    pitch % 4.2f    roll % 4.2f\n",
                  counter, perturbed_position.x() - ground_truth_position.x(),
                  perturbed_position.y() - ground_truth_position.y(),
                  perturbed_position.z() - ground_truth_position.z(), yaw,
                  pitch, roll);

              // Publish the TF of perturbed mesh
              TfHelper::publishTransform(T_mission__reading_perturbed,
                                         "mission", "perturbed_submap", true);

              // Set initial conditions
              ceres::Solver::Summary summary;
              Transformation::Vector6 T_vec_read =
                  T_mission__reading_perturbed.log();
              double mission_pose_reading[4] = {T_vec_read[0], T_vec_read[1],
                                                T_vec_read[2], T_vec_read[5]};

              // Optimize submap registration
              bool registration_successful = submap_registerer.testRegistration(
                  reference_submap_id, reading_submap_id, mission_pose_reading,
                  &summary);
              if (registration_successful) {
                // Update reading submap pose with the optimization result
                T_vec_read[0] = mission_pose_reading[0];
                T_vec_read[1] = mission_pose_reading[1];
                T_vec_read[2] = mission_pose_reading[2];
                T_vec_read[5] = mission_pose_reading[3];
                const Transformation T_mission__reading_optimized =
                    Transformation::exp(T_vec_read);
                submap_collection_ptr->setSubmapPose(
                    reading_submap_id, T_mission__reading_optimized);

                // Announce results
                const Eigen::Vector3f& optimized_position =
                    T_mission__reading_optimized.getPosition();
                printf(
                    "-- % 2i remaining error:    "
                    "x % 4.6f    y % 4.6f    z % 4.6f    "
                    "yaw % 4.2f    pitch % 4.2f    roll % 4.2f    "
                    "time % 4.4f\n",
                    counter++,
                    optimized_position.x() - ground_truth_position.x(),
                    optimized_position.y() - ground_truth_position.y(),
                    optimized_position.z() - ground_truth_position.z(),
                    T_mission__reading_optimized.log()[5] -
                        T_mission__reading_original.log()[5],
                    0.0, 0.0, summary.total_time_in_seconds);

                // Append stats to log file
                log_file << optimized_position.x() - ground_truth_position.x()
                         << ","
                         << optimized_position.y() - ground_truth_position.y()
                         << ","
                         << optimized_position.z() - ground_truth_position.z()
                         << ",0,0,0," << summary.total_time_in_seconds << "\n";
              } else {
                ROS_WARN("Ceres could not find a solution");

                // Append failure to log file
                log_file << "X,X,X,X,X,X\n";
              }

              // Report solver stats
              if (solver_report_style == kBrief) {
                std::cout << summary.BriefReport() << std::endl;
              } else if (solver_report_style == kFull) {
                std::cout << summary.FullReport() << std::endl;
              }

              // Exit if CTRL+C was pressed
              if (!ros::ok()) {
                ROS_INFO("Shutting down...");
                goto endloop;
              }
            }
  }
endloop:

  // Close the log file
  log_file.close();

  // Keep the ROS node alive in order to interact with its topics in Rviz
  ros::spin();

  // Exit normally
  return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/sym_mat3.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/return_arg.hpp>

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, sym_mat3<double> const& value)
  {
    for(std::size_t i=0;i<6;i++) start = to_string(start, value[i]);
    return start;
  }

  template <>
  struct from_string<sym_mat3<double> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<6;i++) {
        from_string<double> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    sym_mat3<double> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af { namespace boost_python {

  template <>
  struct flex_default_element<sym_mat3<double> >
  {
    static sym_mat3<double>
    get() { return sym_mat3<double>(0,0,0,0,0,0); }
  };

  namespace {

    flex<sym_mat3<double> >::type*
    join(
      af::const_ref<double> const& a00,
      af::const_ref<double> const& a11,
      af::const_ref<double> const& a22,
      af::const_ref<double> const& a01,
      af::const_ref<double> const& a02,
      af::const_ref<double> const& a12)
    {
      SCITBX_ASSERT(a00.size() == a11.size());
      SCITBX_ASSERT(a00.size() == a22.size());
      SCITBX_ASSERT(a00.size() == a01.size());
      SCITBX_ASSERT(a00.size() == a02.size());
      SCITBX_ASSERT(a00.size() == a12.size());
      af::shared<sym_mat3<double> > result((af::reserve(a00.size())));
      for(std::size_t i=0;i<a00.size();i++) {
        result.push_back(sym_mat3<double>(a00[i], a11[i], a22[i], a01[i], a02[i], a12[i]));
      }
      return new flex<sym_mat3<double> >::type(result, result.size());
    }

    flex<sym_mat3<double> >::type*
    from_double(
      af::const_ref<double> const& x)
    {
      SCITBX_ASSERT(x.size() % 6 == 0);
      std::size_t result_size = x.size() / 6;
      af::shared<sym_mat3<double> > result((af::reserve(result_size)));
      const double* d = x.begin();
      for(std::size_t i=0;i<result_size;i++) {
        result.push_back(sym_mat3<double>(d));
        d += 6;
      }
      return new flex<sym_mat3<double> >::type(result, result.size());
    }

    flex_double
    as_double(const_ref<sym_mat3<double> > const& a)
    {
      flex_double result(a.size()*6, init_functor_null<double>());
      double* r = result.begin();
      for(std::size_t i=0;i<a.size();i++) {
        for(std::size_t j=0;j<6;j++) {
          *r++ = a[i][j];
        }
      }
      return result;
    }

    void
    imul_a_scalar(
      af::ref<sym_mat3<double> > const& a,
      double f)
    {
      for(std::size_t i=0;i<a.size();i++) a[i] *= f;
    }

    flex_double
    norms(const_ref<sym_mat3<double> > const& a)
    {
      flex_double result(a.size(), init_functor_null<double>());
      double* r = result.begin();
      for(std::size_t i=0;i<a.size();i++) {
        *r++ = std::sqrt(a[i].dot(a[i]));
      }
      return result;
    }

  } // namespace <anonymous>

  void wrap_flex_sym_mat3_double()
  {
    using namespace boost::python;
    typedef flex_wrapper<sym_mat3<double> > f_w;
    f_w::plain("sym_mat3_double")
      .def_pickle(flex_pickle_single_buffered<sym_mat3<double>,
        6*pickle_size_per_element<double>::value>())
      .def("__init__", boost::python::make_constructor(join))
      .def("__init__", boost::python::make_constructor(from_double))
      .def("as_double", as_double)
      .def("norms", norms)
      .def("__add__", f_w::add_a_a)
      .def("__sub__", f_w::sub_a_a)
      .def("__imul__", imul_a_scalar, return_self<>())
    ;
  }

}}} // namespace scitbx::af::boost_python


#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <nt2/sdk/aligned/is_aligned.hpp>

using nt2::meta::is_aligned_c;

int main()
{
  BOOST_MPL_ASSERT(( is_aligned_c<0,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<2,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<4,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<8,2>::type ));
  BOOST_MPL_ASSERT_NOT(( is_aligned_c<17,2>::type ));
}


#pragma once

#include <cstdint>
#include <fstream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <boost/archive/iterators/transform_width.hpp>

namespace wonder_rabbit_project
{
  namespace wonderland
  {
    namespace storage
    {
      namespace base64_helper
      {
        template < class T_input_iterator >
        inline auto base64_impl_copy
        ( const T_input_iterator& begin
        , const T_input_iterator& end
        )
          -> std::string
        {
          std::ostringstream result( std::stringstream::out | std::stringstream::binary );

          std::copy
          ( begin
          , end
          , std::ostream_iterator< char >( result )
          );

          return result.str();
        }

        template < class T >
        inline auto base64_encode_impl( const std::vector< T >& input )
          -> std::string
        {
          using base64_encode_iterator
            = boost::archive::iterators::base64_from_binary
              < boost::archive::iterators::transform_width
                < typename std::vector< T >::const_iterator
                , 6
                , 8 * sizeof( T )
                >
              >
            ;

          const auto begin = base64_encode_iterator( std::begin( input ) );
          const auto end   = base64_encode_iterator( std::end  ( input ) );

          return base64_impl_copy( begin, end );
        }

        template < class T = char >
        inline auto base64_encode_impl( const std::basic_string< T >& input )
          -> std::string
        {
          using base64_encode_iterator
            = boost::archive::iterators::base64_from_binary
              < boost::archive::iterators::transform_width
                < typename std::basic_string< T >::const_iterator
                , 6
                , 8 * sizeof( T )
                >
              >
            ;

          const auto begin = base64_encode_iterator( std::begin( input ) );
          const auto end   = base64_encode_iterator( std::end  ( input ) );

          return base64_impl_copy( begin, end );
        }

        template < class T = char >
        inline auto base64_encode_impl( const T* input )
          -> std::string
        {
          return base64_encode_impl( std::string( input ) );
        }

        template < class T = char >
        auto base64_encode_impl( std::basic_istream< T >& input )
          -> std::string
        {
          using base64_encode_iterator
            = boost::archive::iterators::base64_from_binary
              < boost::archive::iterators::transform_width
                < std::istream_iterator< T >
                , 6
                , 8 * sizeof( T )
                >
              >
            ;

          const auto begin = base64_encode_iterator( std::istream_iterator< T >( input ) );
          const auto end   = base64_encode_iterator( std::istream_iterator< T >() );

          return base64_impl_copy( begin, end );
        }

        template < class T >
        inline auto base64_encode( T&& value, const bool padding = true )
          -> std::string
        {
          auto result = base64_encode_impl( value );

          if ( padding )
            switch( result.size() % 3 )
            {
              case 1: result += "=";
              case 2: result += "=";
            }
          return result;
        }

        template < class T_out = std::string >
        auto base64_decode( const std::string& input )
          -> T_out
        {
          using out_value_type = typename T_out::value_type;

          const auto result = base64_decode< std::string >( input );

          const auto result_begin = reinterpret_cast< const out_value_type* >( result.data() );
          const auto result_end   = result_begin + result.size() / sizeof( out_value_type );

          return T_out( result_begin, result_end );
        }

        template < >
        auto base64_decode< std::string >( const std::string& input )
          -> std::string
        {
          using base64_decode_iterator
            = boost::archive::iterators::transform_width
              < boost::archive::iterators::binary_from_base64
                < boost::archive::iterators::remove_whitespace
                  < std::string::const_iterator
                  >
                >
              , 8 * sizeof( char )
              , 6
              >
            ;

          const auto begin = base64_decode_iterator( std::begin( input ) );
          const auto end   = base64_decode_iterator( std::end  ( input ) );

          return base64_impl_copy( begin, end );
        }

      }
    }
  }
}


#include <iostream>
#include <boost/python.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_value_policy.hpp>

#include "prx/utilities/general/transforms.hpp"
#include <Eigen/Dense>
#include <Eigen/Core>

using namespace boost::python;
typedef Eigen::Matrix<int,1,1>::Index Index;
typedef Eigen::Matrix<int,1,1>::Scalar Scalar;
typedef Eigen::AngleAxis<double> AngleAxisT;
// enum{Dim=VectorT::RowsAtCompileTime};
// static bool dyn(){ return Dim==Eigen::Dynamic; }

static inline void IDX_CHECK(Index i, Index MAX)
    { if(i<0 || i>=MAX)
        { PyErr_SetString(PyExc_IndexError,("Index " + boost::lexical_cast<std::string>(i)+" out of range 0.." + boost::lexical_cast<std::string>(MAX-1)).c_str()); throw_error_already_set(); } }
static inline void IDX2_CHECKED_TUPLE_INTS(tuple tuple,const Index max2[2], Index arr2[2])
    {Index l=len(tuple); if(l!=2) { PyErr_SetString(PyExc_IndexError,"Index must be integer or a 2-tuple"); throw_error_already_set(); } for(int _i=0; _i<2; _i++) { extract<Index> val(tuple[_i]); if(!val.check()){ PyErr_SetString(PyExc_ValueError,("Unable to convert "+boost::lexical_cast<std::string>(_i)+"-th index to integer.").c_str()); throw_error_already_set(); } Index v=val(); IDX_CHECK(v,max2[_i]); arr2[_i]=v; }  }

template<typename T>
static void set_vector_item(T m, Index ix, Scalar value)
    { IDX_CHECK(ix, (Index) m.size() ); m[ix]=value; }

template<typename T>
static void set_matrix_item(T m, Index ix, Index iy, Scalar value)
    { IDX_CHECK(ix, (Index) m.size() ); m(ix, iy)=value; }

template<typename T>
static Scalar get_item(const T& a, tuple _idx){ Index idx[2]; Index mx[2]={a.rows(),a.cols()}; IDX2_CHECKED_TUPLE_INTS(_idx,mx,idx); return a(idx[0],idx[1]); }
template<typename T>
static void set_item(T& a, tuple _idx, const Scalar& value){ Index idx[2]; Index mx[2]={a.rows(),a.cols()}; IDX2_CHECKED_TUPLE_INTS(_idx,mx,idx); a(idx[0],idx[1])=value; }

template<typename T>
static T transpose(const T& m){ return m.transpose(); }
template<typename T, typename R>
static R diagonal(const T& m){ return m.diagonal(); }

template<typename T>
static T __imul__(T& a, const T& b){ a*=b; return a; };
template<typename T>
static T __mul__(const T& a, const T& b){ return a*b; }
template<typename T, typename R>
static R __mul__vec(const T& m, const R& v){ return m*v; }
// float matrices only
template<typename T>
static T inverse(const T& m){ return m.inverse(); }
template<typename T>
static T __div__(const T& a, const T& b){ return a/b; }

template<typename T>
static T Ones(Index rows, Index cols){     return T::Ones(rows,cols); }
template<typename T>
static T Zero(Index rows, Index cols){     return T::Zero(rows,cols); }
template<typename T>
static T Random(Index rows, Index cols){   return T::Random(rows,cols); }
template<typename T>
static T Identity(Index rows, Index cols){ return T::Identity(rows,cols); }

static prx::quaternion_t* fromAxisAngle(const prx::vector_t& axis, const Scalar& angle){ prx::quaternion_t* ret=new prx::quaternion_t(AngleAxisT(angle, axis)); ret->normalize(); return ret; }
static prx::quaternion_t* fromAngleAxis(const Scalar& angle, const prx::vector_t& axis){ prx::quaternion_t* ret=new prx::quaternion_t(AngleAxisT(angle, axis)); ret->normalize(); return ret; }
static prx::quaternion_t* fromTwoVectors(const prx::vector_t& u, const prx::vector_t& v){ prx::quaternion_t* q(new prx::quaternion_t); q->setFromTwoVectors(u,v); return q; }

// static
// static auto translation(Eigen::Transform<double, 3, Eigen::AffineCompact> Tr)
//     {return Tr.translation();}
static void translation(prx::transform_t& Tr, prx::vector_t v)
    {Tr.translation() = (v);}

void pyprx_utilities_general_transforms()
{

    // using n_vector_t = Eigen::Matrix<double, N, 1>;
    // template <int N>
    // using n_matrix_t = Eigen::Matrix<double, N, N>;
    // using vector_t = n_vector_t<3>;
    // using matrix_t = n_matrix_t<3>;
    // using quaternion_t = Eigen::Quaternion<double>;
    // using axis_angle_t = Eigen::AngleAxis<double>;

    class_< prx::n_vector_t<Eigen::Dynamic> >("n_vector", init< prx::n_vector_t<Eigen::Dynamic> >() )
        .def("__setitem__", &set_vector_item< prx::n_vector_t<Eigen::Dynamic> >)
        ;

    class_< prx::n_matrix_t<Eigen::Dynamic> >("n_matrix", init< prx::n_matrix_t<Eigen::Dynamic> >() )
        .def("__setitem__", &set_matrix_item< prx::n_matrix_t<Eigen::Dynamic> >)
        ;

    class_< prx::vector_t >("vector", init< prx::vector_t >() )
        .def(init<double, double, double>())
        .def("__setitem__", &set_vector_item< prx::vector_t >)
        ;

    class_< prx::matrix_t >("matrix", init< prx::matrix_t >() )
        .def("__setitem__", &set_matrix_item< prx::matrix_t >)
        .def("determinant",&prx::matrix_t::determinant,"Return matrix determinant.")
        .def("trace",&prx::matrix_t::trace,"Return sum of diagonal elements.")
        .def("transpose",&transpose<prx::matrix_t>,"Return transposed matrix.")
        .def("diagonal",&diagonal<prx::matrix_t, prx::vector_t>,"Return diagonal as vector.")
        // // matrix*matrix product
        .def("__mul__",&__mul__<prx::matrix_t>).def("__imul__",&__imul__<prx::matrix_t>)
        // // matrix*vector product
        .def("__mul__",&__mul__vec<prx::matrix_t, prx::vector_t>).def("__rmul__",&__mul__vec<prx::matrix_t, prx::vector_t>)
        .def("Zero",&Zero<prx::matrix_t>,(arg("rows"),arg("cols")),"Create zero matrix of given dimensions").staticmethod("Zero")
        .def("Ones",&Zero<prx::matrix_t>,(arg("rows"),arg("cols")),"Create matrix of given dimensions where all elements are set to 1.").staticmethod("Ones")
        .def("Random",&Zero<prx::matrix_t>,(arg("rows"),arg("cols")),"Create matrix with given dimensions where all elements are set to number between 0 and 1 (uniformly-distributed).").staticmethod("Random")
        .def("Identity",&Zero<prx::matrix_t>,(arg("rank")),"Create identity matrix with given rank (square).").staticmethod("Identity")
        // .def("__setitem__",&prx::matrix_t::set_row).def("__getitem__",&prx::matrix_t::get_row)
        // .def("__setitem__",&prx::matrix_t::set_item< prx::matrix_t >).def("__getitem__",&prx::matrix_t::get_item< prx::matrix_t >)
        // .def("__str__",&prx::matrix_t::__str__).def("__repr__",&prx::matrix_t::__str__)
        ;

    class_< prx::quaternion_t >("quaternion" )
        .def("__init__", make_constructor(&fromAxisAngle, default_call_policies(),(arg("axis"),  arg("angle"))))
        .def("__init__", make_constructor(&fromAngleAxis, default_call_policies(),(arg("angle"), arg("axis"))))
        .def("__init__", make_constructor(&fromTwoVectors,default_call_policies(),(arg("u"),     arg("v"))))
        .def(init<Scalar,Scalar,Scalar,Scalar>((arg("w"),arg("x"),arg("y"),arg("z")),"Initialize from coefficients.\n\n.. note:: The order of coefficients is *w*, *x*, *y*, *z*. The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
        .def(init<prx::matrix_t>((arg("rotMatrix")))) //,"Initialize from given rotation matrix.")
        .def(init<prx::quaternion_t>((arg("other"))))
        ;

    class_<prx::transform_t >("transform")
        // .def("__init__", make_constructor(&fromAxisAngle, default_call_policies(),(arg("axis"),  arg("angle"))))
        .def("setIdentity", &prx::transform_t::setIdentity)
        .def("translation", &translation)
        ;

}


#include <boost_adaptbx/graph/graph_type.hpp>
#include <boost_adaptbx/graph/graph_export_adaptor.hpp>
#include <boost_adaptbx/graph/vertex_map.hpp>

#include <boost_adaptbx/exporting.hpp>

#include <boost/python/module.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/list.hpp>
#include <boost/python/def.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/connected_components.hpp>

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

namespace boost_adaptbx
{
namespace
{

template< typename Graph >
struct connected_components_export
{
  typedef boost::graph_traits< Graph > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::vertices_size_type vertex_index_type;
  typedef graph_export_adaptor::vertex_descriptor_converter< vertex_descriptor > converter;
  typedef vertex_map::generic_vertex_map< Graph, vertex_index_type > component_map_type;
  typedef typename component_map_type::property_map_type component_property_map_type;
  typedef vertex_map::index_map< Graph > index_map_type;

  static boost::python::list connected_components(Graph const& graph)
  {
    using namespace boost;
    index_map_type index_map( graph );
    component_map_type component_map( graph );
    component_property_map_type component_property_map( component_map.get() );

    int num = boost::connected_components(
      graph,
      component_property_map,
      vertex_index_map( index_map.get()  )
      );

    vertex_iterator di, dj;
    boost::python::list result;

    for( boost::tie( di, dj ) = boost::vertices( graph ); di != dj; ++di )
    {
      result.append(
        boost::python::make_tuple(
          converter::forward( *di ),
          boost::get( component_property_map, *di )
          )
        );
    }

    return result;
  }

  static void process()
  {
    using namespace boost::python;

    def( "connected_components", connected_components, arg( "graph" ) );
  }
};

struct connected_component_algorithm_exporter
{
  template< typename Export >
  void operator ()(boost::mpl::identity< Export > myexport) const
  {
    typedef typename Export::first graph_type;
    typedef typename Export::second name_type;

    typedef boost::graph_traits< graph_type > graph_traits;
    typedef typename boost::mpl::if_<
      boost::is_same<
        typename  graph_traits::directed_category,
        boost::undirected_tag
        >,
      connected_components_export< graph_type >,
      graph_export_adaptor::no_export< graph_type >
      >::type exporter_type;
    exporter_type::process();
  }
};

} // namespace <anonymous>
} // namespace boost_adaptbx

BOOST_PYTHON_MODULE(boost_adaptbx_graph_connected_component_algorithm_ext)
{
  boost_adaptbx::exporting::class_list< boost_adaptbx::graph_type::exports >::process(
    boost_adaptbx::connected_component_algorithm_exporter()
    );
}


#include <chrono>
#include <iostream>
#include <cmath>

#include <Eigen/Dense>

#include "structure.h"
#include "local_environment.h"

void construct_structure(Structure** struc) {
    int noa = 50;
    int nos = 5;

    std::vector<int> species = std::vector<int>(noa);
    for (int i = 0; i < noa; i++) {
        species[i] = rand() % nos;
    }

    Eigen::MatrixXd cell {3, 3};
    Eigen::MatrixXd positions_1 = 10 * Eigen::MatrixXd::Random(noa, 3);

    cell << 10, 0.52, 0.12,
           -0.93, 10, 0.32,
            0.1, -0.2, 10;

    (*struc) = new Structure(cell, species, positions_1);
}

void deconstruct_structure(Structure* struc) {
    delete struc;
}

void construct_environment(LocalEnvironment** env, Structure* struc) {
    double rcut = 10;
    (*env) = new LocalEnvironment(*struc, 0, rcut);
}

void deconstruct_environment(LocalEnvironment* env) {
    delete env;
}

void construct_calculator(B2_Calculator** calc) {
    std::string radial_string = "chebyshev";
    std::string cutoff_string = "quadratic";

    std::vector<double> cutoff_hyps;

    double first_gauss = 0;
    double final_gauss = 10;
    std::vector<double> radial_hyps = {first_gauss, final_gauss};

    std::vector<int> descriptor_settings {5, 10, 10};

    (*calc) = new B2_Calculator(radial_string, cutoff_string, radial_hyps,
                            cutoff_hyps, descriptor_settings, 0);
}

void deconstruct_calculator(B2_Calculator* calc) {
    delete calc;
}

int main() {
    Structure* struc;
    LocalEnvironment* env;
    B2_Calculator* calc;
    double duration = 0;
    int num_reps = 20;

    construct_calculator(&calc);

    for (int i = 0; i < num_reps; i++) {
        construct_structure(&struc);
        construct_environment(&env, struc);

        std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();

        calc->compute(*env);

        std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
        duration += (double) std::chrono::duration_cast<std::chrono::milliseconds>( t2 - t1 ).count();

        deconstruct_structure(struc);
        deconstruct_environment(env);
    }

    std::cout << "time: " << duration << " ms" << std::endl;

    deconstruct_calculator(calc);
    return 0;
}


//============================================================================
// Name        : DWT_model.cpp
// Author      : Saeed Omidi
// Version     :
// Copyright   : Your copyright notice
// Description : Positional Dependency based on DWT model in C++, Ansi-style
//============================================================================

#include <iostream>
#include <boost/math/special_functions/gamma.hpp>
#include <time.h>
#include <vector>
#include <iomanip>
#include "Motif.h"
#include "constants.h"
#include "Alignment.h"
#include "Decomposition.h"
#include "Score.h"
#include "Window.h"
#include "ParameterFile.h"

using namespace std;

int main(int argc, char* argv[]){
	if (argc != 2){
		cerr << "Calculates the pairwise dependency posterior between all pairs of positions." << endl;
		cerr << "e.g. CTCF.dwt" << endl;
		exit(1);
	}

	string dwt_model_file = argv[1];
	Alignment alignment(dwt_model_file);
	ParameterFile parameters;
	Score S(alignment, alignment.get_TF(), parameters);
	for(unsigned int i=0; i<alignment.ncols(); i++){
		for(unsigned int j=0;j<i;j++){
			printf("%d\t%d\t%.4lf\n", j, i, S.posterior(i,j));
		}
	}
	return 0;
}


/**
 * @file EuclideanParamError.hpp
 * @brief Header file for the EuclideanParamError class.
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_CERES_EUCLIDEANPARAMERROR_HPP_
#define INCLUDE_OKVIS_CERES_EUCLIDEANPARAMERROR_HPP_

#include <vector>
#include <Eigen/Core>
#include "ceres/ceres.h"
#include <okvis/assert_macros.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/ceres/ErrorInterface.hpp>

namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {
template<int kParamDim>
class EuclideanParamError : public ::ceres::SizedCostFunction<
    kParamDim /* number of residuals */,
    kParamDim /* size of first parameter */>,
    public ErrorInterface {
 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  /// \brief The base class type.
  typedef ::ceres::SizedCostFunction<kParamDim, kParamDim> base_t;

  /// \brief Number of residuals (kParamDim)
  static const int kNumResiduals = kParamDim;

  /// \brief The information matrix type.
  typedef Eigen::Matrix<double, kParamDim, kParamDim> information_t;

  /// \brief The covariance matrix type (same as information).
  typedef Eigen::Matrix<double, kParamDim, kParamDim> covariance_t;

  /// \brief Default constructor.
  EuclideanParamError();

  /// \brief Construct with measurement and information matrix
  /// @param[in] measurement The measurement.
  /// @param[in] information The information (weight) matrix.
  EuclideanParamError(const Eigen::Matrix<double, kParamDim, 1> & measurement,
                      const information_t & information);

  /// \brief Construct with measurement and variance.
  /// @param[in] measurement The measurement.
  /// @param[in] variance The variance of each dim of the measurement, i.e. information_ has variance in its diagonal.
  EuclideanParamError(const Eigen::Matrix<double, kParamDim, 1>& measurement,
                      const Eigen::Matrix<double, kParamDim, 1>& variance);

  /// \brief Trivial destructor.
  virtual ~EuclideanParamError() {
  }

  // setters
  /// \brief Set the measurement.
  /// @param[in] measurement The measurement.
  void setMeasurement(const Eigen::Matrix<double, kParamDim, 1> & measurement) {
    measurement_ = measurement;
  }

  /// \brief Set the information.
  /// @param[in] information The information (weight) matrix.
  void setInformation(const information_t & information);

  // getters
  /// \brief Get the measurement.
  /// \return The measurement vector.
  const Eigen::Matrix<double, kParamDim, 1>& measurement() const {
    return measurement_;
  }

  /// \brief Get the information matrix.
  /// \return The information (weight) matrix.
  const information_t& information() const {
    return information_;
  }

  /// \brief Get the covariance matrix.
  /// \return The inverse information (covariance) matrix.
  const covariance_t& covariance() const {
    return covariance_;
  }

  // error term and Jacobian implementation
  /**
   * @brief This evaluates the error term and additionally computes the Jacobians.
   * @param parameters Pointer to the parameters (see ceres)
   * @param residuals Pointer to the residual vector (see ceres)
   * @param jacobians Pointer to the Jacobians (see ceres)
   * @return success of th evaluation.
   */
  virtual bool Evaluate(double const* const * parameters, double* residuals,
                        double** jacobians) const;

  /**
   * @brief This evaluates the error term and additionally computes
   *        the Jacobians in the minimal internal representation.
   * @param parameters Pointer to the parameters (see ceres)
   * @param residuals Pointer to the residual vector (see ceres)
   * @param jacobians Pointer to the Jacobians (see ceres)
   * @param jacobiansMinimal Pointer to the minimal Jacobians (equivalent to jacobians).
   * @return Success of the evaluation.
   */
  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
                                            double* residuals,
                                            double** jacobians,
                                            double** jacobiansMinimal) const;

  // sizes
  /// \brief Residual dimension.
  size_t residualDim() const {
    return kNumResiduals;
  }

  /// \brief Number of parameter blocks.
  size_t parameterBlocks() const {
    return base_t::parameter_block_sizes().size();
  }

  /// \brief Dimension of an individual parameter block.
  /// @param[in] parameterBlockId ID of the parameter block of interest.
  /// \return The dimension.
  size_t parameterBlockDim(size_t parameterBlockId) const {
    return base_t::parameter_block_sizes().at(parameterBlockId);
  }

  /// @brief Residual block type as string
  virtual std::string typeInfo() const {
    return "EuclideanParamError";
  }

 protected:

  // the measurement
  Eigen::Matrix<double, kParamDim, 1> measurement_; ///< The measurement.

  // weighting related
  information_t information_; ///< The information matrix.
  information_t squareRootInformation_; ///< The square root information matrix.
  covariance_t covariance_; ///< The covariance matrix.

};
}  // namespace ceres
}  // namespace okvis

#include "implementation/EuclideanParamError.hpp"
#endif /* INCLUDE_OKVIS_CERES_EUCLIDEANPARAMERROR_HPP_ */


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Cartography.h>
#include <vw/Math.h>

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

using namespace vw;
using namespace vw::cartography;

int main( int argc, char *argv[] ) {
  string dem1_name, dem2_name, output_prefix;
  double default_value;

  po::options_description desc("Options");
  desc.add_options()
    ("help,h", "Display this help message")
    ("default-value", po::value(&default_value), "The value of missing pixels in the first dem")
    ("output-prefix,o", po::value(&output_prefix), "Specify the output prefix");

  po::options_description positional("");
  positional.add_options()
    ("dem1", po::value(&dem1_name), "Explicitly specify the first dem")
    ("dem2", po::value(&dem2_name), "Explicitly specify the second dem");

  po::positional_options_description p;
  p.add("dem1", 1);
  p.add("dem2", 1);

  po::options_description all_options;
  all_options.add(desc).add(positional);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(desc).positional(p).run(), vm );
    po::notify( vm );
  } catch (po::error const& e) {
    cout << "Error parsing: " << e.what() << "\n\t" << desc << "\n";
    return 1;
  }

  if ( dem1_name.empty() || dem2_name.empty() || vm.count("help" ) ) {
    cout << "Usage: " << argv[0] << " <dem1> <dem2> " << endl;
    cout << desc << endl;
    return 1;
  }

  if ( output_prefix.empty() ) {
    fs::path dem1_path(dem1_name), dem2_path(dem2_name);
    output_prefix = (dem1_path.branch_path() / (fs::basename(dem1_path) + "__" + fs::basename(dem2_path))).string();
  }

  DiskImageResourceGDAL dem1_rsrc(dem1_name), dem2_rsrc(dem2_name);
  if ( !vm.count("default-value") && dem1_rsrc.has_nodata_read() ) {
    default_value = dem1_rsrc.nodata_read();
    vw_out() << "\tFound input nodata value: " << default_value << endl;
  }
  DiskImageView<double> dem1_dmg(dem1_name), dem2_dmg(dem2_name);

  GeoReference dem1_georef, dem2_georef;
  read_georeference(dem1_georef, dem1_rsrc);
  read_georeference(dem2_georef, dem2_rsrc);

  // dem2_reproj is calculated in the event that two dem's datums are
  // different (for example, USGS uses 3396190m for the radius of Mars,
  // while we use 3396000m)
  ImageViewRef<double> dem2_reproj = select_channel(reproject_point_image(dem_to_point_image(dem2_dmg, dem2_georef), dem2_georef, dem1_georef), 2);
  ImageViewRef<double> dem2_trans = crop(geo_transform(dem2_reproj, dem2_georef, dem1_georef), 0, 0, dem1_dmg.cols(), dem1_dmg.rows());

  ImageViewRef<PixelMask<double> > diff_masked = copy_mask(dem1_dmg - dem2_trans, create_mask(dem1_dmg, default_value));
  ImageViewRef<double> diff = apply_mask(diff_masked, default_value);

  DiskImageResourceGDAL output_rsrc(output_prefix + "-diff.tif", diff.format(),
                                         Vector2i(vw_settings().default_tile_size(),
                                                  vw_settings().default_tile_size()));
  write_georeference(output_rsrc, dem1_georef);
  block_write_image(output_rsrc, diff,
                    TerminalProgressCallback("asp", "\t--> Differencing: "));

  return 0;
}