#include <iostream>

//using namespace std;
#include "tests_cryptoTools/UnitTests.h"
#include "libOTe_Tests/UnitTests.h"

#include <cryptoTools/Common/Defines.h>
using namespace osuCrypto;

#include <cryptoTools/Network/Channel.h>
#include <cryptoTools/Network/Session.h>
#include <cryptoTools/Network/IOService.h>
#include <numeric>
#include <cryptoTools/Common/Timer.h>
#include <cryptoTools/Common/Log.h>
int miraclTestMain();

#include "libOTe/TwoChooseOne/KosOtExtReceiver.h"
#include "libOTe/TwoChooseOne/KosOtExtSender.h"
#include "libOTe/TwoChooseOne/KosDotExtReceiver.h"
#include "libOTe/TwoChooseOne/KosDotExtSender.h"
#include "libOTe/TwoChooseOne/IknpOtExtReceiver.h"
#include "libOTe/TwoChooseOne/IknpOtExtSender.h"
#include "libOTe/TwoChooseOne/IknpDotExtReceiver.h"
#include "libOTe/TwoChooseOne/IknpDotExtSender.h"

#include "libOTe/NChooseOne/Oos/OosNcoOtReceiver.h"
#include "libOTe/NChooseOne/Oos/OosNcoOtSender.h"
#include "libOTe/NChooseOne/Kkrt/KkrtNcoOtReceiver.h"
#include "libOTe/NChooseOne/Kkrt/KkrtNcoOtSender.h"

#include "libOTe/TwoChooseOne/SilentOtExtReceiver.h"
#include "libOTe/TwoChooseOne/SilentOtExtSender.h"

#include "libOTe/NChooseK/AknOtReceiver.h"
#include "libOTe/NChooseK/AknOtSender.h"

#include <cryptoTools/Common/CLP.h>
#include "util.h"
#include <iomanip>
#include <boost/preprocessor/variadic/size.hpp>

template<typename NcoOtSender, typename  NcoOtReceiver>
void NChooseOne_example(Role role, int totalOTs, int numThreads, std::string ip, std::string tag, CLP&)
{
	const u64 step = 1024;

	if (totalOTs == 0)
		totalOTs = 1 << 20;

	bool randomOT = true;
	auto numOTs = totalOTs / numThreads;
	auto numChosenMsgs = 256;

	// get up the networking
	auto rr = role == Role::Sender ? SessionMode::Server : SessionMode::Client;
	IOService ios;
	Session  ep0(ios, ip, rr);
	PRNG prng(sysRandomSeed());

	// for each thread we need to construct a channel (socket) for it to communicate on.
	std::vector<Channel> chls(numThreads);
	for (int i = 0; i < numThreads; ++i)
		chls[i] = ep0.addChannel();

	std::vector<NcoOtReceiver> recvers(numThreads);
	std::vector<NcoOtSender> senders(numThreads);

	// all Nco Ot extenders must have configure called first. This determines
	// a variety of parameters such as how many base OTs are required.
	bool maliciousSecure = false;
	u64 statSecParam = 40;
	u64 inputBitCount = 76; // the kkrt protocol default to 128 but oos can only do 76.
	recvers[0].configure(maliciousSecure, statSecParam, inputBitCount);
	senders[0].configure(maliciousSecure, statSecParam, inputBitCount);

	// Generate new base OTs for the first extender. This will use
	// the default BaseOT protocol. You can also manually set the
	// base OTs with setBaseOts(...);
	if (role == Role::Sender)
		senders[0].genBaseOts(prng, chls[0]);
	else
		recvers[0].genBaseOts(prng, chls[0]);

	// now that we have one valid pair of extenders, we can call split on
	// them to get more copies which can be used concurrently.
	for (int i = 1; i < numThreads; ++i)
	{
		recvers[i] = recvers[0].splitBase();
		senders[i] = senders[0].splitBase();
	}

	// create a lambda function that performs the computation of a single receiver thread.
	auto recvRoutine = [&](int k)
	{
		auto& chl = chls[k];
		PRNG prng(sysRandomSeed());

		if (randomOT)
		{
			// once configure(...) and setBaseOts(...) are called,
			// we can compute many batches of OTs. First we need to tell
			// the instance how mant OTs we want in this batch. This is done here.
			recvers[k].init(numOTs, prng, chl);

			// now we can iterate over the OTs and actaully retreive the desired
			// messages. However, for efficieny we will do this in steps where
			// we do some computation followed by sending off data. This is more
			// efficient since data will be sent in the background :).
			for (int i = 0; i < numOTs; )
			{
				// figure out how many OTs we want to do in this step.
				auto min = std::min<u64>(numOTs - i, step);

				// iterate over this step.
				for (u64 j = 0; j < min; ++j, ++i)
				{
					// For the OT index by i, we need to pick which
					// one of the N OT messages that we want. For this
					// example we simply pick a random one. Note only the
					// first log2(N) bits of choice is considered.
					block choice = prng.get<block>();

					// this will hold the (random) OT message of our choice
					block otMessage;

					// retreive the desired message.
					recvers[k].encode(i, &choice, &otMessage);

					// do something cool with otMessage
					//otMessage;
				}

				// Note that all OTs in this region must be encode. If there are some
				// that you don't actually care about, then you can skip them by calling
				//
				//    recvers[k].zeroEncode(i);
				//

				// Now that we have gotten out the OT messages for this step,
				// we are ready to send over network some information that
				// allows the sender to also compute the OT messages. Since we just
				// encoded "min" OT messages, we will tell the class to send the
				// next min "correction" values.
				recvers[k].sendCorrection(chl, min);
			}

			// once all numOTs have been encoded and had their correction values sent
			// we must call check. This allows to sender to make sure we did not cheat.
			// For semi-honest protocols, this can and will be skipped.
			recvers[k].check(chl, ZeroBlock);

		}
		else
		{
			std::vector<block>recvMsgs(numOTs);
			std::vector<u64> choices(numOTs);

			// define which messages the receiver should learn.
			for (int i = 0; i < numOTs; ++i)
				choices[i] = prng.get<u8>();

			// the messages that were learned are written to recvMsgs.
			recvers[k].receiveChosen(numChosenMsgs, recvMsgs, choices, prng, chl);
		}
	};

	// create a lambda function that performs the computation of a single sender thread.
	auto sendRoutine = [&](int k)
	{
		auto& chl = chls[k];
		PRNG prng(sysRandomSeed());

		if (randomOT)
		{

			// Same explanation as above.
			senders[k].init(numOTs, prng, chl);

			// Same explanation as above.
			for (int i = 0; i < numOTs; )
			{
				// Same explanation as above.
				auto min = std::min<u64>(numOTs - i, step);

				// unlike for the receiver, before we call encode to get
				// some desired OT message, we must call recvCorrection(...).
				// This receivers some information that the receiver had sent
				// and allows the sender to compute any OT message that they desired.
				// Note that the step size must match what the receiver used.
				// If this is unknown you can use recvCorrection(chl) -> u64
				// which will tell you how many were sent.
				senders[k].recvCorrection(chl, min);

				// we now encode any OT message with index less that i + min.
				for (u64 j = 0; j < min; ++j, ++i)
				{
					// in particular, the sender can retreive many OT messages
					// at a single index, in this case we chose to retreive 3
					// but that is arbitrary.
					auto choice0 = prng.get<block>();
					auto choice1 = prng.get<block>();
					auto choice2 = prng.get<block>();

					// these we hold the actual OT messages.
					block
						otMessage0,
						otMessage1,
						otMessage2;

					// now retreive the messages
					senders[k].encode(i, &choice0, &otMessage0);
					senders[k].encode(i, &choice1, &otMessage1);
					senders[k].encode(i, &choice2, &otMessage2);
				}
			}

			// This call is required to make sure the receiver did not cheat.
			// All corrections must be recieved before this is called.
			senders[k].check(chl, ZeroBlock);
		}
		else
		{
			// populate this with the messages that you want to send.
			Matrix<block> sendMessages(numOTs, numChosenMsgs);
			prng.get(sendMessages.data(), sendMessages.size());

			// perform the OTs with the given messages.
			senders[k].sendChosen(sendMessages, prng, chl);
		}
	};

	std::vector<std::thread> thds(numThreads);
	std::function<void(int)> routine;

	if (role == Role::Sender)
		routine = sendRoutine;
	else
		routine = recvRoutine;

	Timer time;
	auto s = time.setTimePoint("start");

	for (int k = 0; k < numThreads; ++k)
		thds[k] = std::thread(routine, k);

	for (int k = 0; k < numThreads; ++k)
		thds[k].join();

	auto e = time.setTimePoint("finish");
	auto milli = std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count();

	if (role == Role::Sender)
		std::cout << tag << " n=" << totalOTs << " " << milli << " ms" << std::endl;
}

template<typename OtExtSender, typename OtExtRecver>
void TwoChooseOne_example(Role role, int totalOTs, int numThreads, std::string ip, std::string tag, CLP & cmd)
{
	if (totalOTs == 0)
		totalOTs = 1 << 20;

	bool randomOT = true;

	auto numOTs = totalOTs / numThreads;

	// get up the networking
	auto rr = role == Role::Sender ? SessionMode::Server : SessionMode::Client;
	IOService ios;
	Session  ep0(ios, ip, rr);
	PRNG prng(sysRandomSeed());

	// for each thread we need to construct a channel (socket) for it to communicate on.
	std::vector<Channel> chls(numThreads);
	for (int i = 0; i < numThreads; ++i)
		chls[i] = ep0.addChannel();

	Timer timer, sendTimer, recvTimer;
	timer.reset();
	auto s = timer.setTimePoint("start");
	sendTimer.setTimePoint("start");
	recvTimer.setTimePoint("start");

	std::vector<OtExtSender> senders(numThreads);
	std::vector<OtExtRecver> receivers(numThreads);

#ifdef LIBOTE_HAS_BASE_OT
	// Now compute the base OTs, we need to set them on the first pair of extenders.
	// In real code you would only have a sender or reciever, not both. But we do
	// here just showing the example.
	if (role == Role::Receiver)
	{
        DefaultBaseOT base;
		std::array<std::array<block, 2>, 128> baseMsg;
		base.send(baseMsg, prng, chls[0], numThreads);
		receivers[0].setBaseOts(baseMsg, prng, chls[0]);

		//receivers[0].genBaseOts(prng, chls[0]);
	}
	else
	{

        DefaultBaseOT base;
		BitVector bv(128);
		std::array<block, 128> baseMsg;
		bv.randomize(prng);
		base.receive(bv, baseMsg, prng, chls[0], numThreads);
		senders[0].setBaseOts(baseMsg, bv,chls[0]);
	}
#endif

	// for the rest of the extenders, call split. This securely
	// creates two sets of extenders that can be used in parallel.
	for (auto i = 1; i < numThreads; ++i)
	{
		if (role == Role::Receiver)
			receivers[i] = receivers[0].splitBase();
		else
			senders[i] = senders[0].splitBase();
	}

	auto routine = [&](int i)
	{
		// get a random number generator seeded from the system
		PRNG prng(sysRandomSeed());

		if (role == Role::Receiver)
		{
			// construct the choices that we want.
			BitVector choice(numOTs);
			// in this case pick random messages.
			choice.randomize(prng);

			// construct a vector to stored the received messages.
			std::vector<block> msgs(numOTs);

			if (randomOT)
			{
				// perform  numOTs random OTs, the results will be written to msgs.
				receivers[i].receive(choice, msgs, prng, chls[i]);
			}
			else
			{
				// perform  numOTs chosen message OTs, the results will be written to msgs.
				receivers[i].receiveChosen(choice, msgs, prng, chls[i]);
			}
		}
		else
		{
			// construct a vector to stored the random send messages.
			std::vector<std::array<block, 2>> msgs(numOTs);

			// if delta OT is used, then the user can call the following
			// to set the desired XOR difference between the zero messages
			// and the one messages.
			//
			//     senders[i].setDelta(some 128 bit delta);
			//

			if (randomOT)
			{
				// perform the OTs and write the random OTs to msgs.
				senders[i].send(msgs, prng, chls[i]);
			}
			else
			{
				// Populate msgs with something useful...
				prng.get(msgs.data(), msgs.size());

				// perform the OTs. The receiver will learn one
				// of the messages stored in msgs.
				senders[i].sendChosen(msgs, prng, chls[i]);
			}
		}
	};

	senders[0].setTimer(sendTimer);
	receivers[0].setTimer(recvTimer);

	std::vector<std::thread> thrds(numThreads);
	for (int i = 0; i < numThreads; ++i)
		thrds[i] = std::thread(routine, i);

	for (int i = 0; i < numThreads; ++i)
		thrds[i].join();

	auto e = timer.setTimePoint("finish");
	auto milli = std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count();

	auto com = (chls[0].getTotalDataRecv() + chls[0].getTotalDataSent()) * numThreads;

	if (role == Role::Sender)
		lout << tag << " n=" << Color::Green << totalOTs << " " << milli << " ms  " << com << " bytes" << std::endl << Color::Default;

	if (cmd.isSet("v"))
	{
		if (role == Role::Sender)
			lout << " **** sender ****\n" << sendTimer << std::endl;

		if (role == Role::Receiver)
			lout << " **** receiver ****\n" << recvTimer << std::endl;
	}
}

//template<typename OtExtSender, typename OtExtRecver>
void TwoChooseOneG_example(Role role, int numOTs, int numThreads, std::string ip, std::string tag, CLP & cmd)
{
#ifdef ENABLE_SILENTOT

	if (numOTs == 0)
		numOTs = 1 << 20;
	using OtExtSender = SilentOtExtSender;
	using OtExtRecver = SilentOtExtReceiver;

	// get up the networking
	auto rr = role == Role::Sender ? SessionMode::Server : SessionMode::Client;
	IOService ios;
	Session  ep0(ios, ip, rr);
	PRNG prng(sysRandomSeed());

	// for each thread we need to construct a channel (socket) for it to communicate on.
	std::vector<Channel> chls(numThreads);
	for (int i = 0; i < numThreads; ++i)
		chls[i] = ep0.addChannel();

	bool mal = cmd.isSet("mal");
	OtExtSender sender;
	OtExtRecver receiver;

	auto routine = [&](int s, int sec, SilentBaseType type)
	{
		// get a random number generator seeded from the system
		PRNG prng(sysRandomSeed());

		sync(chls[0], role);

		if (role == Role::Receiver)
		{
			// construct the choices that we want.
			BitVector choice(numOTs);
			// in this case pick random messages.
			choice.randomize(prng);

			// construct a vector to stored the received messages.
			std::vector<block> msgs(numOTs);

			receiver.genBase(numOTs, chls[0], prng, s, sec, mal, type, chls.size());
			// perform  numOTs random OTs, the results will be written to msgs.
			receiver.receive(msgs, choice, prng, chls);
		}
		else
		{
			std::vector<std::array<block, 2>> msgs(numOTs);

			sender.genBase(numOTs, chls[0], prng, s, sec,mal, type, chls.size());
			// construct a vector to stored the random send messages.

			// if delta OT is used, then the user can call the following
			// to set the desired XOR difference between the zero messages
			// and the one messages.
			//
			//     senders[i].setDelta(some 128 bit delta);
			//

			// perform the OTs and write the random OTs to msgs.
			sender.send(msgs, prng, chls);
		}
	};

	cmd.setDefault("s", "4");
	cmd.setDefault("sec", "80");
	std::vector<int> ss = cmd.getMany<int>("s");
	std::vector<int> secs = cmd.getMany<int>("sec");
	std::vector< SilentBaseType> types;

	if (cmd.isSet("base"))
		types.push_back(SilentBaseType::Base);
	if (cmd.isSet("baseExtend"))
		types.push_back(SilentBaseType::BaseExtend);
	if (cmd.isSet("extend"))
		types.push_back(SilentBaseType::Extend);
	if (types.size() == 0 || cmd.isSet("none"))
		types.push_back(SilentBaseType::None);

	for (auto s : ss)
		for (auto sec : secs)
			for (auto type : types)
			{

				chls[0].resetStats();

				Timer timer, sendTimer, recvTimer;
				timer.reset();
				auto b = timer.setTimePoint("start");
				sendTimer.setTimePoint("start");
				recvTimer.setTimePoint("start");

				sender.setTimer(sendTimer);
				receiver.setTimer(recvTimer);

				routine(s, sec, type);

				auto e = timer.setTimePoint("finish");
				auto milli = std::chrono::duration_cast<std::chrono::milliseconds>(e - b).count();

				u64 com = 0;
				for(auto &c : chls)
					com += (c.getTotalDataRecv() + c.getTotalDataSent());

				std::string typeStr = "n ";
				switch (type)
				{
				case SilentBaseType::Base:
					typeStr = "b ";
					break;
				case SilentBaseType::Extend:
					typeStr = "e ";
					break;
				case SilentBaseType::BaseExtend:
					typeStr = "be";
					break;
				default:
					break;
				}

				if (role == Role::Sender)
					lout << tag <<
					" n:" << Color::Green << std::setw(6) << std::setfill(' ')<<numOTs << Color::Default <<
					" type: " << Color::Green << typeStr << Color::Default <<
					" sec: " << Color::Green << std::setw(3) << std::setfill(' ') << sec << Color::Default <<
					" s: " << Color::Green << s << Color::Default <<
					"   ||   " << Color::Green <<
					std::setw(6) << std::setfill(' ') << milli << " ms   " <<
					std::setw(6) << std::setfill(' ') << com << " bytes" << std::endl << Color::Default;

				if (cmd.isSet("v"))
				{
					if (role == Role::Sender)
						lout << " **** sender ****\n" << sendTimer << std::endl;

					if (role == Role::Receiver)
						lout << " **** receiver ****\n" << recvTimer << std::endl;
				}
			}

#endif
}

template<typename BaseOT>
void baseOT_example(Role role, int totalOTs, int numThreads, std::string ip, std::string tag, CLP&)
{
	IOService ios;
	PRNG prng(sysRandomSeed());

	if (totalOTs == 0)
		totalOTs = 128;

	if (numThreads > 1)
		std::cout << "multi threading for the base OT example is not implemented.\n" << std::flush;

	if (role == Role::Receiver)
	{
		auto chl0 = Session(ios, ip, SessionMode::Server).addChannel();
		BaseOT recv;

		std::vector<block> msg(totalOTs);
		BitVector choice(totalOTs);
		choice.randomize(prng);

		Timer t;
		auto s = t.setTimePoint("base OT start");

		recv.receive(choice, msg, prng, chl0);

		auto e = t.setTimePoint("base OT end");
		auto milli = std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count();

		std::cout << tag << " n=" << totalOTs << " " << milli << " ms" << std::endl;
	}
	else
	{

		auto chl1 = Session(ios, ip, SessionMode::Client).addChannel();

		BaseOT send;

		std::vector<std::array<block, 2>> msg(totalOTs);

		send.send(msg, prng, chl1);
	}
}

static const std::vector<std::string>
unitTestTag{ "u", "unitTest" },
kos{ "k", "kos" },
dkos{ "d", "dkos" },
kkrt{ "kk", "kkrt" },
iknp{ "i", "iknp" },
diknp{ "diknp" },
oos{ "o", "oos" },
Silent{ "s", "Silent" },
akn{ "a", "akn" },
np{ "np" },
simple{ "simplest" };

using ProtocolFunc = std::function<void(Role, int, int, std::string, std::string, CLP&)>;

bool runIf(ProtocolFunc protocol, CLP & cmd, std::vector<std::string> tag)
{
	auto n = cmd.isSet("nn")
		? (1 << cmd.get<int>("nn"))
		: cmd.getOr("n", 0);

	auto t = cmd.getOr("t", 1);
	auto ip = cmd.getOr<std::string>("ip", "localhost:1212");

	if (cmd.isSet(tag))
	{
		if (cmd.hasValue("r"))
		{
			auto role = cmd.get<int>("r") ? Role::Sender : Role::Receiver;
			protocol(role, n, t, ip, tag.back(), cmd);
		}
		else
		{
			auto thrd = std::thread([&] {
				try { protocol(Role::Sender, n, t, ip, tag.back(), cmd); }
				catch (std::exception & e)
				{
					lout << e.what() << std::endl;
				}
				});

			try { protocol(Role::Receiver, n, t, ip, tag.back(), cmd); }
			catch (std::exception & e)
			{
				lout << e.what() << std::endl;
			}
			thrd.join();
		}

		return true;
	}

	return false;
}

void minimal()
{
	// Setup networking. See cryptoTools\frontend_cryptoTools\Tutorials\Network.cpp
	IOService ios;
	Channel senderChl = Session(ios, "localhost:1212", SessionMode::Server).addChannel();
	Channel recverChl = Session(ios, "localhost:1212", SessionMode::Client).addChannel();

	// The number of OTs.
	int n = 100;

	// The code to be run by the OT receiver.
	auto recverThread = std::thread([&]() {
		PRNG prng(sysRandomSeed());
		IknpOtExtReceiver recver;
		recver.genBaseOts(prng, recverChl);

		// Choose which messages should be received.
		BitVector choices(n);
		choices[0] = 1;
		//...

		// Receive the messages
		std::vector<block> messages(n);
		recver.receiveChosen(choices, messages, prng, recverChl);

		// messages[i] = sendMessages[i][choices[i]];
		});

	PRNG prng(sysRandomSeed());
	IknpOtExtSender sender;
	sender.genBaseOts(prng, senderChl);

	// Choose which messages should be sent.
	std::vector<std::array<block, 2>> sendMessages(n);
	sendMessages[0] = { toBlock(54), toBlock(33) };
	//...

	// Send the messages.
	sender.sendChosen(sendMessages, prng, senderChl);
	recverThread.join();
}

void getLatency(CLP & cmd)
{
	auto ip = cmd.getOr<std::string>("ip", "localhost:1212");

	if (cmd.hasValue("r"))
	{
		auto mode = cmd.get<int>("r") != 0 ? SessionMode::Server : SessionMode::Client;
		IOService ios;
		Session session(ios, ip, mode);
		auto chl = session.addChannel();
		if (mode == SessionMode::Server)
			senderGetLatency(chl);
		else
			recverGetLatency(chl);
	}
	else
	{
		IOService ios;
		Session s(ios, ip, SessionMode::Server);
		Session r(ios, ip, SessionMode::Client);
		auto cs = s.addChannel();
		auto cr = r.addChannel();

		auto thrd = std::thread([&]() {senderGetLatency(cs); });
		recverGetLatency(cr);

		thrd.join();
	}
}

#ifdef ENABLE_SIMPLESTOT
const bool spEnabled = true;
#else
const bool spEnabled = false;
#endif
#ifdef NAOR_PINKAS
const bool npEnabled = true;
#else
const bool npEnabled = false;
#endif

#ifdef ENABLE_SILENTOT
const bool silentEnabled = true;
#else
const bool silentEnabled = false;
#endif

int main(int argc, char** argv)
{
	CLP cmd;
	cmd.parse(argc, argv);
	bool flagSet = false;

	if (cmd.isSet(unitTestTag))
	{
		flagSet = true;
		auto tests = tests_cryptoTools::Tests;
		tests += tests_libOTe::Tests;

		tests.runIf(cmd);
		return 0;
	}

	if (cmd.isSet("latency"))
	{
		getLatency(cmd);
		flagSet = true;
	}

#ifdef ENABLE_SIMPLESTOT
	flagSet |= runIf(baseOT_example<SimplestOT>, cmd, simple);
#endif
#ifdef NAOR_PINKAS
	flagSet |= runIf(baseOT_example<NaorPinkas>, cmd, np);
#endif
	flagSet |= runIf(TwoChooseOne_example<IknpOtExtSender, IknpOtExtReceiver>, cmd, iknp);
	flagSet |= runIf(TwoChooseOne_example<IknpDotExtSender, IknpDotExtReceiver>, cmd, diknp);
	flagSet |= runIf(TwoChooseOne_example<KosOtExtSender, KosOtExtReceiver>, cmd, kos);
	flagSet |= runIf(TwoChooseOne_example<KosDotExtSender, KosDotExtReceiver>, cmd, dkos);

	flagSet |= runIf(NChooseOne_example<KkrtNcoOtSender, KkrtNcoOtReceiver>, cmd, kkrt);
	flagSet |= runIf(NChooseOne_example<OosNcoOtSender, OosNcoOtReceiver>, cmd, oos);

	//<SilentOtExtSender, SilentOtExtReceiver>
	flagSet |= runIf(TwoChooseOneG_example, cmd, Silent);

	if (flagSet == false)
	{

		std::cout
			<< "#######################################################\n"
			<< "#                      - libOTe -                     #\n"
			<< "#               A library for performing              #\n"
			<< "#                  oblivious transfer.                #\n"
			<< "#                     Peter Rindal                    #\n"
			<< "#######################################################\n" << std::endl;

		std::cout
			<< "Protocols:\n"
			<< Color::Green << "  -simplest" << Color::Default << "  : to run the SimplestOT active secure 1-out-of-2 base OT" << (spEnabled ? "" : "(disabled)") << "\n"
			<< Color::Green << "  -np      " << Color::Default << "  : to run the NaorPinkas active secure 1-out-of-2 base OT" << (npEnabled ? "" : "(disabled)") << "\n"
			<< Color::Green << "  -iknp    " << Color::Default << "  : to run the IKNP   passive secure 1-out-of-2       OT\n"
			<< Color::Green << "  -diknp   " << Color::Default << "  : to run the IKNP   passive secure 1-out-of-2 Delta-OT\n"
			<< Color::Green << "  -Silent  " << Color::Default << "  : to run the Silent passive secure 1-out-of-2       OT"<< (silentEnabled ? "" : "(disabled)") <<"\n"
			<< Color::Green << "  -kos     " << Color::Default << "  : to run the KOS    active secure  1-out-of-2       OT\n"
			<< Color::Green << "  -dkos    " << Color::Default << "  : to run the KOS    active secure  1-out-of-2 Delta-OT\n"
			<< Color::Green << "  -oos     " << Color::Default << "  : to run the OOS    active secure  1-out-of-N OT for N=2^76\n"
			<< Color::Green << "  -kkrt    " << Color::Default << "  : to run the KKRT   passive secure 1-out-of-N OT for N=2^128\n\n"

			<< "Other Options:\n"
			<< Color::Green << "  -n         " << Color::Default << ": the number of OTs to perform\n"
			<< Color::Green << "  -r 0/1     " << Color::Default << ": Do not play both OT roles. r 1 -> OT sender and network server. r 0 -> OT receiver and network cleint.\n"
			<< Color::Green << "  -ip        " << Color::Default << ": the IP and port of the netowrk server, default = localhost:1212\n"
			<< Color::Green << "  -t         " << Color::Default << ": the number of threads that should be used\n"
			<< Color::Green << "  -u         " << Color::Default << ": to run the unit tests\n"
			<< Color::Green << "  -u -list   " << Color::Default << ": to list the unit tests\n"
			<< Color::Green << "  -u 1 2 15  " << Color::Default << ": to run the unit tests indexed by {1, 2, 15}.\n"
			<< std::endl;
	}

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.arithmetic toolbox - sqrti/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 28/11/2010
///
#include <nt2/include/functions/bitwise_cast.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/functions/extract.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/constants/sqrti.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/real.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/functions/load.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>

NT2_TEST_CASE_TPL ( abs_cplx__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;

  // specific values tests
  {
    std::cout << nt2::Sqrti<vcT >() << std::endl;
    NT2_TEST_EQUAL(nt2::Sqrti<vcT>(),  vcT(nt2::Sqrt_2o_2<vT>(),nt2::Sqrt_2o_2<vT>()));

  }

} // end of test for floating_


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/Properties/Reactivity/ConceptualDft.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <boost/optional.hpp>

using namespace Scine::Utils::ConceptualDft;

void init_conceptual_dft(pybind11::module& m) {
  auto conceptual_dft_submodule = m.def_submodule("conceptual_dft", R"delim(
      Functionality to calculate various conceptual DFT properties based on the finite difference approximation.

      To use this functionality the energy and/or atomic charges of the optimized structure of interest and of the same
      structure (not reoptimized!) with one additional electron and one electron less are needed.

      An introduction into conceptual DFT can be found in:
      Chattaraj, P. K. Chemical Reactivity Theory : A Density Functional View; CRC Press, 2009.
      https://doi.org/10.1201/9781420065442.
    )delim");

  pybind11::class_<GlobalConceptualDftContainer> globalConceptualDftContainer(conceptual_dft_submodule,
                                                                              "GlobalConceptualDftContainer");
  globalConceptualDftContainer.def_readonly("chemical_potential", &GlobalConceptualDftContainer::chemicalPotential);
  globalConceptualDftContainer.def_readonly("electronegativity", &GlobalConceptualDftContainer::electronegativity);
  globalConceptualDftContainer.def_readonly("hardness", &GlobalConceptualDftContainer::hardness);
  globalConceptualDftContainer.def_readonly("softness", &GlobalConceptualDftContainer::softness);
  globalConceptualDftContainer.def_readonly("electrophilicity", &GlobalConceptualDftContainer::electrophilicity);

  pybind11::class_<LocalConceptualDftContainer> localConceptualDftContainer(conceptual_dft_submodule,
                                                                            "LocalConceptualDftContainer");
  localConceptualDftContainer.def_readonly("fukui_plus", &LocalConceptualDftContainer::fukuiPlus);
  localConceptualDftContainer.def_readonly("fukui_minus", &LocalConceptualDftContainer::fukuiMinus);
  localConceptualDftContainer.def_readonly("fukui_radical", &LocalConceptualDftContainer::fukuiRadical);
  localConceptualDftContainer.def_readonly("dual_descriptor", &LocalConceptualDftContainer::dualDescriptor);

  pybind11::class_<ConceptualDftContainer> conceptualDftContainer(conceptual_dft_submodule, "ConceptualDftContainer");
  // NOTE: Cannot define global or local, those are reserved keywords in python
  conceptualDftContainer.def_readonly("global_v", &ConceptualDftContainer::global);
  conceptualDftContainer.def_readonly("local_v", &ConceptualDftContainer::local);

  conceptual_dft_submodule.def("calculate", &calculate, pybind11::arg("energy"), pybind11::arg("atomic_charges"),
                               pybind11::arg("energy_plus"), pybind11::arg("atomic_charges_plus"),
                               pybind11::arg("energy_minus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates a set of global and local conceptual DFT parameters.
      Note: The quality of the resulting Fukui and dual descriptor indices heavily depends on the quality of the atomic
      charges.
      We recommend Hirshfeld charges.

      :param energy: The energy of the structure of interest
      :param: atomic_charges The atomic charges of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: A conceptualDftContainer with local and global cDFT properties
      )delim");

  conceptual_dft_submodule.def("calculate_global", &calculateGlobal, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates a set of global conceptual DFT parameters.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: A globalConceptualDftContainer with global cDFT properties
      )delim");

  conceptual_dft_submodule.def("calculate_local", &calculateLocal, pybind11::arg("atomic_charges"),
                               pybind11::arg("atomic_charges_plus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates the condensed to atom Fukui and dual descriptor indices.

      Note: The quality of the resulting indices heavily depends on the quality of the atomic charges.
      We recommend Hirshfeld charges.

      :param: atomic_charges The atomic charges of the structure of interest
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: A localConceptualDftContainer with global cDFT properties
      )delim");

  conceptual_dft_submodule.def("calculate_chemical_potential", &calculateChemicalPotential, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates the conceptual DFT chemical potential.

      Parr, R. G.; Pearson, R. G., J. Am. Chem. Soc. 1983, 105 (26), 7512–7516. https://doi.org/10.1021/ja00364a005.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: The chemical potential
      )delim");

  conceptual_dft_submodule.def("calculate_electronegativity", &calculateElectronegativity, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates the Mulliken electronegativity.

      Mulliken, R. S.; J. Chem. Phys. 1934, 2 (11), 782–793. https://doi.org/10.1063/1.1749394.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: The electronegativity
      )delim");

  conceptual_dft_submodule.def("calculate_hardness", &calculateHardness, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates the chemical hardness.

      Parr, R. G.; Pearson, R. G.; J. Am. Chem. Soc. 1983, 105 (26), 7512–7516. https://doi.org/10.1021/ja00364a005.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: The chemical hardness.
      )delim");

  conceptual_dft_submodule.def("calculate_softness", &calculateSoftness, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates the chemical softness.

      Yang, W.; Parr, R. G.; PNAS 1985, 82 (20), 6723–6726. https://doi.org/10.1073/pnas.82.20.6723.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: The chemical softness.
      )delim");

  conceptual_dft_submodule.def("calculate_electrophilicity", &calculateElectrophilicity, pybind11::arg("energy"),
                               pybind11::arg("energy_plus"), pybind11::arg("energy_minus"), R"delim(
      Calculates the electrophilicity.

      Parr, R. G.; Szentpály, L. v.; Liu, S.; J. Am. Chem. Soc. 1999, 121 (9), 1922–1924.
      https://doi.org/10.1021/ja983494x.

      :param energy: The energy of the structure of interest
      :param energy_plus: The energy for the same geometry with one additional electron
      :param energy_minus: The energy for the same geometry with one electron less
      :return: The electrophilicity.
      )delim");

  conceptual_dft_submodule.def("calculate_fukui_plus", &calculateFukuiPlus, pybind11::arg("atomic_charges"),
                               pybind11::arg("atomic_charges_plus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates the Fukui indices for nucleophilic attacks.
      Note: The quality of the results heavily depends on the quality of the atomic charges.
      We recommend Hirshfeld charges.

      Parr, R. G.; Yang, W.; J. Am. Chem. Soc. 1984, 106 (14), 4049–4050. https://doi.org/10.1021/ja00326a036.

      :param: atomic_charges The atomic charges of the structure of interest
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: The Fukui indices for nucleophilic attacks.
      )delim");

  conceptual_dft_submodule.def("calculate_fukui_minus", &calculateFukuiMinus, pybind11::arg("atomic_charges"),
                               pybind11::arg("atomic_charges_plus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates the Fukui indices for nucleophilic attacks.
      Note: The quality of the results heavily depends on the quality of the atomic charges.
      We recommend Hirshfeld charges.

      Parr, R. G.; Yang, W.; J. Am. Chem. Soc. 1984, 106 (14), 4049–4050. https://doi.org/10.1021/ja00326a036.

      :param: atomic_charges The atomic charges of the structure of interest
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: The Fukui indices for electrophilic attacks.
      )delim");

  conceptual_dft_submodule.def("calculate_fukui_radical", &calculateFukuiRadical, pybind11::arg("atomic_charges"),
                               pybind11::arg("atomic_charges_plus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates the Fukui indices for radical attacks.
      Note: The quality of the results heavily depends on the quality of the atomic charges.
      We recommend Hirshfeld charges.
      The relevance of the radical Fukui function is known to be limited. It is included here for completeness only.

      Parr, R. G.; Yang, W.; J. Am. Chem. Soc. 1984, 106 (14), 4049–4050. https://doi.org/10.1021/ja00326a036.

      :param: atomic_charges The atomic charges of the structure of interest
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: The Fukui indices for radical attacks.
      )delim");

  conceptual_dft_submodule.def("calculate_dual_descriptor", &calculateDualDescriptor, pybind11::arg("atomic_charges"),
                               pybind11::arg("atomic_charges_plus"), pybind11::arg("atomic_charges_minus"), R"delim(
      Calculates the the condensed to atom dual descriptor.
      Note: The quality of the results heavily depends on the quality of the atomic charges.
      We recommend Hirshfeld charges.

      Morell, C.; Grand, A.; Toro-Labbé, A.; J. Phys. Chem. A 2005, 109 (1), 205–212. https://doi.org/10.1021/jp046577a.

      :param: atomic_charges The atomic charges of the structure of interest
      :param: atomic_charges_plus The atomic charges for the same geometry with one additional electron
      :param: atomic_charges_minus The atomic charges for the same geometry with one electron less
      :return: The condensed-to-atom dual descriptor.
      )delim");
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprGatherTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <vector>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/half.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

template <typename DATA_IN_TYPE, typename INDICES_TYPE>
void ConstExprTest_Gather_Type(std::string data_in_type,
                               std::string indices_type) {
  // The compute graph :
  //
  // data ---- GATHER ----> Add([0]) ----> output

  // We will gather a rank-4 tensor:
  Shape inShape = {2, 5, 3, 4};
  // Expect a rank-5 tensor result, with a smaller axis 1 and a new axis 2
  Shape expectedOutShape = {2, 2, 3, 3, 4};
  // Gather along axis 1
  int64_t axis = 1;

  TensorInfo inDataInfo{data_in_type, inShape};
  std::vector<DATA_IN_TYPE> inData(inDataInfo.nelms());
  for (int64_t i = 0; i < inDataInfo.nelms(); ++i) {
    // Fill the array with the indices as value
    inData[i] = static_cast<DATA_IN_TYPE>(i);
  }

  Shape indicesShape = {2, 3};
  TensorInfo inIndicesInfo{indices_type, indicesShape};
  std::vector<INDICES_TYPE> indices({1, 3, 0, 2, 4, 1});

  TensorInfo expectedOutdataInfo{data_in_type, expectedOutShape};
  std::vector<DATA_IN_TYPE> outData(expectedOutdataInfo.nelms());
  std::vector<DATA_IN_TYPE> expectedOutData(expectedOutdataInfo.nelms());
  for (int64_t d0 = 0; d0 < expectedOutShape[0]; ++d0) {
    for (int64_t d1 = 0; d1 < expectedOutShape[1]; ++d1) {
      for (int64_t d2 = 0; d2 < expectedOutShape[2]; ++d2) {
        for (int64_t d3 = 0; d3 < expectedOutShape[3]; ++d3) {
          for (int64_t d4 = 0; d4 < expectedOutShape[4]; ++d4) {

            // Derive the expected value
            int64_t value =
                d4 +
                inShape[3] *
                    (d3 + inShape[2] * (indices[d2 + indicesShape[1] * d1] +
                                        inShape[1] * d0));

            // Compute the array index
            int64_t index =
                d4 + expectedOutShape[4] *
                         (d3 + expectedOutShape[3] *
                                   (d2 + expectedOutShape[2] *
                                             (d1 + expectedOutShape[1] * d0)));

            expectedOutData[index] = static_cast<DATA_IN_TYPE>(value);
          }
        }
      }
    }
  }

  ConstVoidData constInData      = {inData.data(), inDataInfo};
  ConstVoidData constIndicesData = {indices.data(), inIndicesInfo};

  // Build an onnx model
  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto inId        = aiOnnx.constant(constInData, "constInData");
  auto indicesId   = aiOnnx.constant(constIndicesData, "constIndicesData");
  auto gatherOutId = aiOnnx.gather({inId, indicesId}, axis);

  // Dummy add op appended at the end since gather will not be folded if the
  // output is an anchor
  Shape add0Shape = {1};
  TensorInfo add0DataInfo(data_in_type, add0Shape);
  std::vector<DATA_IN_TYPE> add0Data(1);
  add0Data[0]                 = static_cast<DATA_IN_TYPE>(0);
  ConstVoidData constAdd0Data = {add0Data.data(), add0DataInfo};

  auto add0Id = aiOnnx.constant(constAdd0Data, "constAdd0Data");
  auto outId  = aiOnnx.add({gatherOutId, add0Id});

  builder->addOutputTensor(outId);

  popart::NDArrayWrapper<DATA_IN_TYPE> output(outData.data(), expectedOutShape);
  std::map<popart::TensorId, popart::IArray &> anchors = {{outId, output}};

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{outId, art}});
  auto device   = createTestDevice(TEST_TARGET);

  auto session = popart::InferenceSession::createFromOnnxModel(
      proto,
      dataFlow,
      device,
      InputShapeInfo(),
      {}, // no SessionOptions
      Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false));

  session->prepareDevice();
  popart::StepIO stepio({}, anchors);
  session->run(stepio);

  // Check that the values are as expected
  BOOST_CHECK_EQUAL_COLLECTIONS(outData.begin(),
                                outData.end(),
                                expectedOutData.begin(),
                                expectedOutData.end());
}

BOOST_AUTO_TEST_CASE(ConstExprTest_Gather_Types) {
  // ConstExprTest_Gather_Type<uint32_t>("UINT32");
  // ConstExprTest_Gather_Type<uint64_t>("UINT64");
  ConstExprTest_Gather_Type<int32_t, int32_t>("INT32", "INT32");
  ConstExprTest_Gather_Type<int32_t, int64_t>("INT32", "INT64");
  // ConstExprTest_Gather_Type<int64_t>("INT64");
  ConstExprTest_Gather_Type<popart::float16_t, int32_t>("FLOAT16", "INT32");
  ConstExprTest_Gather_Type<popart::float16_t, int64_t>("FLOAT16", "INT64");
  ConstExprTest_Gather_Type<float, int32_t>("FLOAT", "INT32");
  ConstExprTest_Gather_Type<float, int64_t>("FLOAT", "INT64");
  // ConstExprTest_Gather_Type<double>("DOUBLE");
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testNonlinearOptimizer.cpp
 * @brief   Unit tests for NonlinearOptimizer class
 * @author  Frank Dellaert
 */

#include <tests/smallExample.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/range/adaptor/map.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using boost::adaptors::map_values;

#include <iostream>
#include <fstream>

using namespace std;
using namespace gtsam;

const double tol = 1e-5;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( NonlinearOptimizer, paramsEquals )
{
  // default constructors lead to two identical params
  GaussNewtonParams gnParams1;
  GaussNewtonParams gnParams2;
  CHECK(gnParams1.equals(gnParams2));

  // but the params become different if we change something in gnParams2
  gnParams2.setVerbosity("DELTA");
  CHECK(!gnParams1.equals(gnParams2));
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, iterateLM )
{
  // really non-linear factor graph
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  // config far from minimum
  Point2 x0(3,0);
  Values config;
  config.insert(X(1), x0);

  // normal iterate
  GaussNewtonParams gnParams;
  GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
  gnOptimizer.iterate();

  // LM iterate with lambda 0 should be the same
  LevenbergMarquardtParams lmParams;
  lmParams.lambdaInitial = 0.0;
  LevenbergMarquardtOptimizer lmOptimizer(fg, config, lmParams);
  lmOptimizer.iterate();

  CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  // test error at minimum
  Point2 xstar(0,0);
  Values cstar;
  cstar.insert(X(1), xstar);
  DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

  // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);
  DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

  // optimize parameters
  Ordering ord;
  ord.push_back(X(1));

  // Gauss-Newton
  GaussNewtonParams gnParams;
  gnParams.ordering = ord;
  Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual1),tol);

  // Levenberg-Marquardt
  LevenbergMarquardtParams lmParams;
  lmParams.ordering = ord;
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  DoglegParams dlParams;
  dlParams.ordering = ord;
  Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = GaussNewtonOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleDLOptimizer )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = DoglegOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, optimization_method )
{
  LevenbergMarquardtParams paramsQR;
  paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR;
  LevenbergMarquardtParams paramsChol;
  paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
  DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);

  Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize();
  DOUBLES_EQUAL(0,fg.error(actualMFChol),tol);
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
  Values config;
  config.insert(X(1), Pose2(0.,0.,0.));
  config.insert(X(2), Pose2(1.5,0.,0.));

  NonlinearFactorGraph graph;
  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
  graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));

  Ordering ordering;
  ordering.push_back(X(1));
  ordering.push_back(X(2));

  LevenbergMarquardtParams params;
  LevenbergMarquardtParams::SetLegacyDefaults(&params);
  LevenbergMarquardtOptimizer optimizer(graph, config, ordering, params);
  optimizer.iterate();

  Values expected;
  expected.insert(X(1), Pose2(0.,0.,0.));
  expected.insert(X(2), Pose2(1.,0.,0.));
  CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, NullFactor) {

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  // Add null factor
  fg.push_back(NonlinearFactorGraph::sharedFactor());

  // test error at minimum
  Point2 xstar(0,0);
  Values cstar;
  cstar.insert(X(1), xstar);
  DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

  // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);
  DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

  // optimize parameters
  Ordering ord;
  ord.push_back(X(1));

  // Gauss-Newton
  Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual1),tol);

  // Levenberg-Marquardt
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}

/* ************************************************************************* */
TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {

  NonlinearFactorGraph fg;
  fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(1, 0, M_PI / 2),
      noiseModel::Isotropic::Sigma(3, 1));
  fg += BetweenFactor<Pose2>(1, 2, Pose2(1, 0, M_PI / 2),
      noiseModel::Isotropic::Sigma(3, 1));

  Values init;
  init.insert(0, Pose2(3, 4, -M_PI));
  init.insert(1, Pose2(10, 2, -M_PI));
  init.insert(2, Pose2(11, 7, -M_PI));

  Values expected;
  expected.insert(0, Pose2(0, 0, 0));
  expected.insert(1, Pose2(1, 0, M_PI / 2));
  expected.insert(2, Pose2(1, 1, M_PI));

  VectorValues expectedGradient;
  expectedGradient.insert(0,Z_3x1);
  expectedGradient.insert(1,Z_3x1);
  expectedGradient.insert(2,Z_3x1);

  // Try LM and Dogleg
  LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults();
  {
    LevenbergMarquardtOptimizer optimizer(fg, init, params);

    // test convergence
    Values actual = optimizer.optimize();
    EXPECT(assert_equal(expected, actual));

    // Check that the gradient is zero
    GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
    EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
  }
  EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));

//  cout << "===================================================================================" << endl;

  // Try LM with diagonal damping
  Values initBetter;
    initBetter.insert(0, Pose2(3,4,0));
    initBetter.insert(1, Pose2(10,2,M_PI/3));
    initBetter.insert(2, Pose2(11,7,M_PI/2));

  {
    params.diagonalDamping = true;
    LevenbergMarquardtOptimizer optimizer(fg, initBetter, params);

    // test the diagonal
    GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
    VectorValues d = linear->hessianDiagonal();
    VectorValues sqrtHessianDiagonal = d;
    for (Vector& v : sqrtHessianDiagonal | map_values) v = v.cwiseSqrt();
    GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear, sqrtHessianDiagonal);
    VectorValues  expectedDiagonal = d + params.lambdaInitial * d;
    EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal()));

    // test convergence (does not!)
    Values actual = optimizer.optimize();
    EXPECT(assert_equal(expected, actual));

    // Check that the gradient is zero (it is not!)
    linear = optimizer.linearize();
    EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));

    // Check that the gradient is zero for damped system (it is not!)
    damped = optimizer.buildDampedSystem(*linear, sqrtHessianDiagonal);
    VectorValues actualGradient = damped.gradientAtZero();
    EXPECT(assert_equal(expectedGradient,actualGradient));

    /* This block was made to test the original initial guess "init"
    // Check errors at convergence and errors in direction of gradient (decreases!)
    EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5);
    EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5);

    // Check that solve yields gradient (it's not equal to gradient, as predicted)
    VectorValues delta = damped.optimize();
    double factor = actualGradient[0][0]/delta[0][0];
    EXPECT(assert_equal(actualGradient,factor*delta));

    // Still pointing downhill wrt actual gradient !
    EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3);

    // delta.print("This is the delta value computed by LM with diagonal damping");

    // Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular)
    EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5);

    // Check errors at convergence and errors in direction of solution (does not decrease!)
    EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5);

    // Check errors at convergence and errors at a small step in direction of solution (does not decrease!)
    EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3);
    */
  }
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {

  NonlinearFactorGraph fg;
  fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
                                                         noiseModel::Isotropic::Sigma(3,1)));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0.9,M_PI/2),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0),
                                                         noiseModel::Isotropic::Sigma(3,1)));

  Values init;
  init.insert(0, Pose2(0,0,0));
  init.insert(1, Pose2(0.961187, 0.99965, 1.1781));

  Values expected;
  expected.insert(0, Pose2(0,0,0));
  expected.insert(1, Pose2(0.961187, 0.99965, 1.1781));

  LevenbergMarquardtParams lm_params;

  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
  auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize();
  auto dl_result = DoglegOptimizer(fg, init).optimize();

  EXPECT(assert_equal(expected, gn_result, 3e-2));
  EXPECT(assert_equal(expected, lm_result, 3e-2));
  EXPECT(assert_equal(expected, dl_result, 3e-2));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {

  NonlinearFactorGraph fg;
  fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
  fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
                                                         noiseModel::Isotropic::Sigma(2,1)));
  fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
                                                         noiseModel::Isotropic::Sigma(2,1)));
  fg += BetweenFactor<Point2>(0, 1, Point2(1,90),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
                                                         noiseModel::Isotropic::Sigma(2,1)));

  Values init;
  init.insert(0, Point2(1,1));
  init.insert(1, Point2(1,0));

  Values expected;
  expected.insert(0, Point2(0,0));
  expected.insert(1, Point2(1,1.85));

  LevenbergMarquardtParams params;

  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
  auto dl_result = DoglegOptimizer(fg, init).optimize();

  EXPECT(assert_equal(expected, gn_result, 1e-4));
  EXPECT(assert_equal(expected, lm_result, 1e-4));
  EXPECT(assert_equal(expected, dl_result, 1e-4));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {

  NonlinearFactorGraph fg;
  fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
                                                         noiseModel::Isotropic::Sigma(3,1)));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
                                                         noiseModel::Isotropic::Sigma(3,1)));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
                             noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
                                                        noiseModel::Isotropic::Sigma(3,1)));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, 0),
                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
                                                         noiseModel::Isotropic::Sigma(3,1)));

  Values init;
  init.insert(0, Pose2(0, 0, 0));
  init.insert(1, Pose2(0, 10, M_PI/4));

  Values expected;
  expected.insert(0, Pose2(0, 0, 0));
  expected.insert(1, Pose2(0, 10, 1.45212));

  LevenbergMarquardtParams params;

  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
  auto dl_result = DoglegOptimizer(fg, init).optimize();

  EXPECT(assert_equal(expected, gn_result, 1e-1));
  EXPECT(assert_equal(expected, lm_result, 1e-1));
  EXPECT(assert_equal(expected, dl_result, 1e-1));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, RobustMeanCalculation) {

  NonlinearFactorGraph fg;

  Values init;

  Values expected;

  auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20),
                                          noiseModel::Isotropic::Sigma(1, 1));

  vector<double> pts{-10,-3,-1,1,3,10,1000};
  for(auto pt : pts) {
    fg.addPrior(0, pt, huber);
  }

  init.insert(0, 100.0);
  expected.insert(0, 3.33333333);

  DoglegParams params_dl;
  params_dl.setRelativeErrorTol(1e-10);

  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
  auto lm_result = LevenbergMarquardtOptimizer(fg, init).optimize();
  auto dl_result = DoglegOptimizer(fg, init, params_dl).optimize();

  EXPECT(assert_equal(expected, gn_result, tol));
  EXPECT(assert_equal(expected, lm_result, tol));
  EXPECT(assert_equal(expected, dl_result, tol));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, disconnected_graph) {
  Values expected;
  expected.insert(X(1), Pose2(0.,0.,0.));
  expected.insert(X(2), Pose2(1.5,0.,0.));
  expected.insert(X(3), Pose2(3.0,0.,0.));

  Values init;
  init.insert(X(1), Pose2(0.,0.,0.));
  init.insert(X(2), Pose2(0.,0.,0.));
  init.insert(X(3), Pose2(0.,0.,0.));

  NonlinearFactorGraph graph;
  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
  graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
  graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));

  EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize()));
}

/* ************************************************************************* */
#include <gtsam/linear/iterative.h>

class IterativeLM : public LevenbergMarquardtOptimizer {
  /// Solver specific parameters
  ConjugateGradientParameters cgParams_;
  Values initial_;

 public:
  /// Constructor
  IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues,
              const ConjugateGradientParameters& p,
              const LevenbergMarquardtParams& params =
                  LevenbergMarquardtParams::LegacyDefaults())
      : LevenbergMarquardtOptimizer(graph, initialValues, params),
        cgParams_(p),
        initial_(initialValues) {}

  /// Solve that uses conjugate gradient
  VectorValues solve(const GaussianFactorGraph& gfg,
                             const NonlinearOptimizerParams& params) const override {
    VectorValues zeros = initial_.zeroVectors();
    return conjugateGradientDescent(gfg, zeros, cgParams_);
  }
};

/* ************************************************************************* */
TEST(NonlinearOptimizer, subclass_solver) {
  Values expected;
  expected.insert(X(1), Pose2(0., 0., 0.));
  expected.insert(X(2), Pose2(1.5, 0., 0.));
  expected.insert(X(3), Pose2(3.0, 0., 0.));

  Values init;
  init.insert(X(1), Pose2(0., 0., 0.));
  init.insert(X(2), Pose2(0., 0., 0.));
  init.insert(X(3), Pose2(0., 0., 0.));

  NonlinearFactorGraph graph;
  graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
  graph += BetweenFactor<Pose2>(X(1), X(2), Pose2(1.5, 0., 0.),
                                noiseModel::Isotropic::Sigma(3, 1));
  graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));

  ConjugateGradientParameters p;
  Values actual = IterativeLM(graph, init, p).optimize();
  EXPECT(assert_equal(expected, actual, 1e-4));
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, logfile )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  // Levenberg-Marquardt
  LevenbergMarquardtParams lmParams;
  static const string filename("testNonlinearOptimizer.log");
  lmParams.logFile = filename;
  LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();

//  stringstream expected,actual;
//  ifstream ifs(("../../gtsam/tests/" + filename).c_str());
//  if(!ifs) throw std::runtime_error(filename);
//  expected << ifs.rdbuf();
//  ifs.close();
//  ifstream ifs2(filename.c_str());
//  if(!ifs2) throw std::runtime_error(filename);
//  actual << ifs2.rdbuf();
//  EXPECT(actual.str()==expected.str());
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_LM )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  // Levenberg-Marquardt
  LevenbergMarquardtParams lmParams;
  size_t lastIterCalled = 0;
  lmParams.iterationHook = [&](size_t iteration, double oldError, double newError)
  {
    // Tests:
    lastIterCalled = iteration;
    EXPECT(newError<oldError);

    // Example of evolution printout:
    //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
  };
  LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();

  EXPECT(lastIterCalled>5);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_CG )
{
  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  // Levenberg-Marquardt
  NonlinearConjugateGradientOptimizer::Parameters cgParams;
  size_t lastIterCalled = 0;
  cgParams.iterationHook = [&](size_t iteration, double oldError, double newError)
  {
    // Tests:
    lastIterCalled = iteration;
    EXPECT(newError<oldError);

    // Example of evolution printout:
    //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
  };
  NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize();

  EXPECT(lastIterCalled>5);
}

/* ************************************************************************* */
//// Minimal traits example
struct MyType : public Vector3 {
  using Vector3::Vector3;
};

namespace gtsam {
template <>
struct traits<MyType> {
  static bool Equals(const MyType& a, const MyType& b, double tol) {
    return (a - b).array().abs().maxCoeff() < tol;
  }
  static void Print(const MyType&, const string&) {}
  static int GetDimension(const MyType&) { return 3; }
  static MyType Retract(const MyType& a, const Vector3& b) { return a + b; }
  static Vector3 Local(const MyType& a, const MyType& b) { return b - a; }
};
}

TEST(NonlinearOptimizer, Traits) {
  NonlinearFactorGraph fg;
  fg.addPrior(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));

  Values init;
  init.insert(0, MyType(0,0,0));

  LevenbergMarquardtOptimizer optimizer(fg, init);
  Values actual = optimizer.optimize();
  EXPECT(assert_equal(init, actual));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// Copyright John Maddock 2010.

// 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)

#include <boost/math/constants/info.hpp>
#include <boost/math/concepts/real_concept.hpp>

int main()
{
   boost::math::constants::print_info_on_type<float>();
   boost::math::constants::print_info_on_type<double>();
   boost::math::constants::print_info_on_type<long double>();
   boost::math::constants::print_info_on_type<boost::math::concepts::real_concept>();

   return 0;
}

/*

------ Rebuild All started: Project: test_print_info_on_type, Configuration: Debug Win32 ------
  test_print_info_on_type.cpp
  test_print_info_on_type.vcxproj -> J:\Cpp\math_constants\Debug\test_print_info_on_type.exe
  Information on the Implementation and Handling of
  Mathematical Constants for Type float

  Checking for std::numeric_limits<float> specialisation: yes
  std::numeric_limits<float>::digits reports that the precision is
  24 binary digits.
  boost::math::policies::precision<float, Policy> reports that the compile time precision is
  24 binary digits.
  The constant will be constructed from a float.

  Information on the Implementation and Handling of
  Mathematical Constants for Type double

  Checking for std::numeric_limits<double> specialisation: yes
  std::numeric_limits<double>::digits reports that the precision is
  53 binary digits.
  boost::math::policies::precision<double, Policy> reports that the compile time precision is
  53 binary digits.
  The constant will be constructed from a double.

  Information on the Implementation and Handling of
  Mathematical Constants for Type long double

  Checking for std::numeric_limits<long double> specialisation: yes
  std::numeric_limits<long double>::digits reports that the precision is
  53 binary digits.
  boost::math::policies::precision<long double, Policy> reports that the compile time precision is
  53 binary digits.
  The constant will be constructed from a double.

  Information on the Implementation and Handling of
  Mathematical Constants for Type class boost::math::concepts::real_concept

  Checking for std::numeric_limits<class boost::math::concepts::real_concept> specialisation: no
  boost::math::policies::precision<class boost::math::concepts::real_concept, Policy>
  reports that there is no compile type precision available.
  boost::math::tools::digits<class boost::math::concepts::real_concept>()
  reports that the current runtime precision is
  53 binary digits.
  No compile time precision is available, the construction method
  will be decided at runtime and results will not be cached
  - this may lead to poor runtime performance.
  Current runtime precision indicates that
  the constant will be constructed from a string on each call.

========== Rebuild All: 1 succeeded, 0 failed, 0 skipped ==========

*/


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm convolution neighborhood square
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/convolution/neighborhood/square_traits.h"
#include "fern/algorithm/algebra/elementary/multiply.h"

namespace fa = fern::algorithm;

BOOST_AUTO_TEST_CASE(square)
{
    {
        fern::Square<int, 1> square({
            {1, 2, 3},
            {4, 5, 6},
            {7, 8, 9}
        });

        BOOST_CHECK_EQUAL(square.weight(0), 1);
        BOOST_CHECK_EQUAL(square.weight(4), 5);
        BOOST_CHECK_EQUAL(square.weight(8), 9);
    }
}

BOOST_AUTO_TEST_CASE(verify_use_in_algorithm)
{
    // Verify we can pass a square to an operation.

    fa::SequentialExecutionPolicy sequential;

    // Multiply.
    {
        fern::Square<int, 1> square({
            {1, 2, 3},
            {4, 5, 6},
            {7, 8, 9}
        });

        fa::algebra::multiply(sequential, square, 2, square);

        BOOST_CHECK_EQUAL(square.weight(0), 2);
        BOOST_CHECK_EQUAL(square.weight(1), 4);
        BOOST_CHECK_EQUAL(square.weight(2), 6);
        BOOST_CHECK_EQUAL(square.weight(3), 8);
        BOOST_CHECK_EQUAL(square.weight(4), 10);
        BOOST_CHECK_EQUAL(square.weight(5), 12);
        BOOST_CHECK_EQUAL(square.weight(6), 14);
        BOOST_CHECK_EQUAL(square.weight(7), 16);
        BOOST_CHECK_EQUAL(square.weight(8), 18);
    }
}


#include <nonius/nonius.h++>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <cassert>
#include <limits>
#include <random>

#ifdef NDEBUG
static constexpr std::size_t points_per_matrix = 50;
#else
static constexpr std::size_t points_per_matrix = 500000;
#endif
static constexpr unsigned int seed = 0xdeadbeef;
static constexpr unsigned int dims = 3;

template<typename Scalar>
using pointlist = Eigen::Matrix<Scalar, dims, Eigen::Dynamic, Eigen::RowMajor>;
template<typename Scalar>
using point = Eigen::Matrix<Scalar, dims, 1>;
template<typename Scalar>
using transform = Eigen::Transform<Scalar, dims, Eigen::Isometry>;

template<typename Scalar>
pointlist<Scalar> create_random_points(const std::size_t num_points)
{
    std::default_random_engine eng{seed};
    std::uniform_real_distribution<Scalar> dist{0, 1};

    assert(num_points <= std::numeric_limits<typename pointlist<Scalar>::Index>::max());
    pointlist<Scalar> pts{dims, static_cast<typename pointlist<Scalar>::Index>(num_points)};

    for(auto c = pts.cols(); c > 0; --c)
    {
        pts.col(c-1) = point<Scalar>{dist(eng), dist(eng), dist(eng)};
    }

    return pts;
}

template<typename Scalar>
transform<Scalar> create_random_transformation()
{
    std::default_random_engine eng{seed};
    std::normal_distribution<Scalar> ndist{};
    std::uniform_real_distribution<Scalar> dist{-10, 10};

    using quaternion = Eigen::Quaternion<Scalar>;
    using translation = Eigen::Translation<Scalar, dims>;

    transform<Scalar> rand_trans;
    rand_trans = translation{typename translation::VectorType{dist(eng), dist(eng), dist(eng)}};
    rand_trans.rotate(quaternion{ndist(eng), ndist(eng), ndist(eng), ndist(eng)}.normalized());
    return rand_trans;
}

template<typename Scalar>
pointlist<Scalar> eigen_matrix_transformation(pointlist<Scalar> pts,
                                                     const transform<Scalar> &trans)
{
    return (trans * pts.colwise().homogeneous());
}

template<typename Scalar>
pointlist<Scalar> raw_loop_transformation(pointlist<Scalar> pts,
                                                 const transform<Scalar> &trans)
{
#ifndef NDEBUG
    pointlist<Scalar> check = eigen_matrix_transformation(pts, trans);
#endif

    for(auto c = pts.cols(); c > 0; --c)
    {
        pts.col(c-1) = trans * pts.col(c-1).homogeneous();
    }

#ifndef NDEBUG
    assert(pts.isApprox(check));
#endif

    return pts;
}

NONIUS_BENCHMARK("raw loop double",
                 [](nonius::chronometer meter)
                 {
                     using scalar = double;
                     const pointlist<scalar> pts = create_random_points<scalar>(points_per_matrix);
                     const transform<scalar> t = create_random_transformation<scalar>();

                     meter.measure([&]{ raw_loop_transformation<scalar>(pts, t); });
                 })

NONIUS_BENCHMARK("eigen matrix double",
                 [](nonius::chronometer meter)
                 {
                     using scalar = double;
                     const pointlist<scalar> pts = create_random_points<scalar>(points_per_matrix);
                     const transform<scalar> t = create_random_transformation<scalar>();

                     meter.measure([&]{ eigen_matrix_transformation<scalar>(pts, t); });
                 })

NONIUS_BENCHMARK("raw loop float",
                 [](nonius::chronometer meter)
                 {
                     using scalar = float;
                     const pointlist<scalar> pts = create_random_points<scalar>(points_per_matrix);
                     const transform<scalar> t = create_random_transformation<scalar>();

                     meter.measure([&]{ raw_loop_transformation<scalar>(pts, t); });
                 })

NONIUS_BENCHMARK("eigen matrix float",
                 [](nonius::chronometer meter)
                 {
                     using scalar = float;
                     const pointlist<scalar> pts = create_random_points<scalar>(points_per_matrix);
                     const transform<scalar> t = create_random_transformation<scalar>();

                     meter.measure([&]{ eigen_matrix_transformation<scalar>(pts, t); });
                 })


/* @file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Shapes/Partitioner.h"

using namespace Scine::Molassembler;
using namespace Shapes;

BOOST_AUTO_TEST_CASE(Partitions, *boost::unit_test::label("Shapes")) {
  for(unsigned i = 1; i < 4; ++i) {
    for(unsigned j = 1; j < 4; ++j) {
      Partitioner partitioner {i, j};
      do {
        BOOST_CHECK(Partitioner::isOrderedMapping(partitioner.map()));
      } while(partitioner.next_partition());
    }
  }

  // Test a few specific counts
  auto countPartitions = [](const unsigned S, const unsigned E) -> unsigned {
    Partitioner partitioner {S, E};
    unsigned partitions = 0;
    do {
      ++partitions;
    } while(partitioner.next_partition());
    return partitions;
  };

  BOOST_CHECK_EQUAL(countPartitions(1, 1), 1);
  BOOST_CHECK_EQUAL(countPartitions(1, 2), 1);
  BOOST_CHECK_EQUAL(countPartitions(2, 1), 1);
  BOOST_CHECK_EQUAL(countPartitions(2, 2), 3);
  BOOST_CHECK_EQUAL(countPartitions(2, 3), 10);
}