#pragma warning(disable:4996)
#include <functional>
#include <thread>
#include <mutex>

#include <boost/functional/hash.hpp>
#include <boost/algorithm/string.hpp>

#include <cppoptlib/meta.h>
#include <cppoptlib/problem.h>
#include <cppoptlib/solver/bfgssolver.h>

#include <cmaes.h>

#include <Unicode.hpp>

//#define USE_OPTIONAL_LIB

#ifdef USE_OPTIONAL_LIB
#define USE_NLOPT
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include <nlopt.hpp>

#include <limbo/acqui.hpp>
#include <limbo/bayes_opt/boptimizer.hpp>
#include <limbo/kernel/matern_five_halves.hpp>
#include <limbo/mean/data.hpp>
#include <limbo/model/gp.hpp>
#include <limbo/stat.hpp>
#include <limbo/tools/macros.hpp>

#ifdef _DEBUG
#pragma comment(lib, "Debug/nlopt_cxx.lib")
#else
#pragma comment(lib, "Release/nlopt_cxx.lib")
#endif

//struct Params {
//	struct acqui_gpucb : public limbo::defaults::acqui_gpucb {
//	};
//
//#ifdef USE_NLOPT
//	struct opt_nloptnograd : public limbo::defaults::opt_nloptnograd {
//	};
//#elif defined(USE_LIBCMAES)
//	struct opt_cmaes : public defaults::opt_cmaes {
//	};
//#else
//	struct opt_gridsearch : public limbo::defaults::opt_gridsearch {
//	};
//#endif
//	struct acqui_ucb {
//		BO_PARAM(double, alpha, 0.1);
//	};
//
//	struct kernel : public limbo::defaults::kernel {
//		BO_PARAM(double, noise, 0.001);
//	};
//
//	struct kernel_maternfivehalves {
//		BO_PARAM(double, sigma_sq, 1);
//		BO_PARAM(double, l, 0.2);
//	};
//	struct kernel_exp : public limbo::defaults::kernel_exp {
//	};
//	struct bayes_opt_bobase : public limbo::defaults::bayes_opt_bobase {
//		BO_PARAM(bool, stats_enabled, true);
//	};
//
//	struct bayes_opt_boptimizer : public limbo::defaults::bayes_opt_boptimizer {
//	};
//
//	struct init_randomsampling {
//		BO_PARAM(int, samples, 5);
//	};
//
//	struct stop_maxiterations {
//		BO_PARAM(int, iterations, 20);
//	};
//	struct stat_gp {
//		BO_PARAM(int, bins, 20);
//	};
//
//	struct kernel_squared_exp_ard : public limbo::defaults::kernel_squared_exp_ard {
//	};
//
//	struct opt_rprop : public limbo::defaults::opt_rprop {
//	};
//};

struct Params {
	struct bayes_opt_boptimizer : public limbo::defaults::bayes_opt_boptimizer {
		BO_PARAM(int, hp_period, 10);
	};
	struct bayes_opt_bobase : public limbo::defaults::bayes_opt_bobase {
		BO_PARAM(int, stats_enabled, true);
	};

	struct init_randomsampling {
		BO_PARAM(int, samples, 100);
	};
	struct stop_maxiterations {
		BO_PARAM(int, iterations, 500);
	};
	struct stop_mintolerance {
		BO_PARAM(double, tolerance, -0.1);
	};

	struct acqui_ei {
		BO_PARAM(double, jitter, 0.0);
	};

	//struct acqui_gpucb : public limbo::defaults::acqui_gpucb {
	//};
	//struct acqui_ucb {
	//	//BO_PARAM(double, alpha, 0.1);
	//	//BO_PARAM(double, alpha, 0.3);
	//	BO_PARAM(double, alpha, 0.5);
	//};

	/*struct kernel : public limbo::defaults::kernel {
		BO_PARAM(double, noise, 0.001);
	};*/
	struct kernel : public limbo::defaults::kernel {
		//BO_PARAM(double, noise, 1.e-10);
		BO_PARAM(double, noise, 0.01);
	};
	struct kernel_squared_exp_ard : public limbo::defaults::kernel_squared_exp_ard {
	};
	struct kernel_maternfivehalves {
		BO_PARAM(double, sigma_sq, 1);
		BO_PARAM(double, l, 0.2);
	};
	struct kernel_exp : public limbo::defaults::kernel_exp {
	};

	struct stat_gp {
		BO_PARAM(int, bins, 3);
	};

	struct opt_nloptnograd : public limbo::defaults::opt_nloptnograd {
	};
	struct opt_rprop : public limbo::defaults::opt_rprop {
	};
};

struct LimboFitFunc {
	std::function<double(const Eigen::VectorXd& x)> func;
	size_t numOfVars;
	size_t dim_in()const { return numOfVars; }
	size_t dim_out()const { return 1; }

	Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
	{
		return limbo::tools::make_vector(func(x));
	}
};
#endif

#include <Pita/Node.hpp>
#include <Pita/Context.hpp>
#include <Pita/OptimizationEvaluator.hpp>
#include <Pita/Parser.hpp>
#include <Pita/Evaluator.hpp>
#include <Pita/Printer.hpp>
#include <Pita/IntrinsicGeometricFunctions.hpp>

extern bool printAddressInsertion;
extern double cloneTime;
extern unsigned cloneCount;
extern bool isDebugMode;

namespace cgl
{
	std::string UnaryOpToStr(UnaryOp op)
	{
		switch (op)
		{
		case UnaryOp::Not:     return "Not";
		case UnaryOp::Plus:    return "Plus";
		case UnaryOp::Minus:   return "Minus";
		case UnaryOp::Dynamic: return "Dynamic";
		}

		return "UnknownUnaryOp";
	}

	std::string BinaryOpToStr(BinaryOp op)
	{
		switch (op)
		{
		case BinaryOp::And: return "And";
		case BinaryOp::Or:  return "Or";

		case BinaryOp::Equal:        return "Equal";
		case BinaryOp::NotEqual:     return "NotEqual";
		case BinaryOp::LessThan:     return "LessThan";
		case BinaryOp::LessEqual:    return "LessEqual";
		case BinaryOp::GreaterThan:  return "GreaterThan";
		case BinaryOp::GreaterEqual: return "GreaterEqual";

		case BinaryOp::Add: return "Add";
		case BinaryOp::Sub: return "Sub";
		case BinaryOp::Mul: return "Mul";
		case BinaryOp::Div: return "Div";

		case BinaryOp::Pow:    return "Pow";
		case BinaryOp::Assign: return "Assign";

		case BinaryOp::Concat: return "Concat";

		case BinaryOp::SetDiff: return "SetDiff";
		}

		return "UnknownBinaryOp";
	}

	bool IsVec2(const Val& value)
	{
		if (!IsType<Record>(value))
		{
			return false;
		}

		const auto& values = As<Record>(value).values;
		return values.find("x") != values.end() && values.find("y") != values.end();
	}

	bool IsShape(const Val& value)
	{
		if (!IsType<Record>(value))
		{
			return false;
		}

		const auto& values = As<Record>(value).values;
		return values.find("x") == values.end() || values.find("y") == values.end();
	}

	Eigen::Vector2d AsVec2(const Val& value, const Context& context)
	{
		const auto& values = As<Record>(value).values;

		const Val xval = context.expand(LRValue(values.find("x")->second), LocationInfo());
		const Val yval = context.expand(LRValue(values.find("y")->second), LocationInfo());

		return Eigen::Vector2d(AsDouble(xval), AsDouble(yval));
	}

	//manip::LocationInfoPrinter LocationInfo::printLoc() const { return { *this }; }
	std::string LocationInfo::getInfo() const
	{
		std::stringstream ss;
		ss << "[L" << locInfo_lineBegin << ":" << locInfo_posBegin << "-" << "L" << locInfo_lineEnd << ":" << locInfo_posEnd << "]";
		return ss.str();
	}

	Identifier Identifier::MakeIdentifier(const std::u32string& name_)
	{
		return Identifier(Unicode::UTF32ToUTF8(name_));
	}

	bool Identifier::isDeferredCall()const
	{
		return boost::starts_with(name, "#DeferredCall");
	}

	Identifier Identifier::asDeferredCall(const ScopeAddress& identifierScopeInfo)const
	{
		if (isDeferredCall())
		{
			return *this;
		}

		std::string head("#DeferredCall(");
		for (unsigned scopeIndex : identifierScopeInfo)
		{
			head += std::to_string(scopeIndex) + ",";
		}
		head += ")";

		return Identifier(head + name);
	}

	bool Identifier::isMakeClosure()const
	{
		return boost::starts_with(name, "#MakeClosure");
	}

	Identifier Identifier::asMakeClosure(const ScopeAddress& identifierScopeInfo)const
	{
		if (isMakeClosure())
		{
			return *this;
		}

		std::string head("#MakeClosure(");
		for (unsigned scopeIndex : identifierScopeInfo)
		{
			head += std::to_string(scopeIndex) + ",";
		}
		head += ")";

		return Identifier(head + name);
	}

	std::pair<ScopeAddress, Identifier> Identifier::decomposed()const
	{
		if (!isDeferredCall() && !isMakeClosure())
		{
			return { {},*this };
		}

		const size_t tagEndIndex = name.find_first_of(')');

		ScopeAddress scopeAddress;
		{
			const std::string scopeIndices(name.begin() + name.find_first_of('(') + 1, name.begin() + tagEndIndex);
			size_t currentPos = 0;
			size_t nextPos = scopeIndices.find_first_of(',', currentPos);
			while (nextPos != std::string::npos)
			{
				const std::string currentStr(scopeIndices.begin() + currentPos, scopeIndices.begin() + nextPos);
				const int scopeIndex = std::stoi(currentStr);
				scopeAddress.push_back(scopeIndex);
				currentPos = nextPos + 1;
				nextPos = scopeIndices.find_first_of(',', currentPos);
			}
		}

		const Identifier rawIdentifier(std::string(name.begin() + tagEndIndex + 1, name.end()));
		return { scopeAddress,rawIdentifier };
	}

	bool EitherReference::localReferenciable(const Context& context)const
	{
		return local && context.existsInLocalScope(local.get());
	}

	std::string EitherReference::toString()const
	{
		std::stringstream ss;
		ss << (local ? local.get().toString() : std::string("None"));
		ss << " | " << "Address(" << replaced.toString() << ")";
		return ss.str();
	}

	LRValue LRValue::Bool(bool a)
	{
		return LRValue(Val(a));
	}

	LRValue LRValue::Int(int a)
	{
		return LRValue(Val(a));
	}

	LRValue LRValue::Float(const std::u32string& str)
	{
		return LRValue(std::stod(Unicode::UTF32ToUTF8(str)));
	}

	LRValue& LRValue::setLocation(const LocationInfo& info)
	{
		locInfo_lineBegin = info.locInfo_lineBegin;
		locInfo_lineEnd = info.locInfo_lineEnd;
		locInfo_posBegin = info.locInfo_posBegin;
		locInfo_posEnd = info.locInfo_posEnd;
		return *this;
	}

	bool LRValue::isValid() const
	{
		return IsType<Address>(value)
			? As<Address>(value).isValid()
			: true; //EitherReference/Reference/Val は常に有効であるものとする
	}

	boost::optional<Address> LRValue::deref(const Context& env)const
	{
		if (isRValue())
		{
			return boost::none;
		}

		return address(env);
	}

	std::string LRValue::toString() const
	{
		if (isAddress())
		{
			return std::string("Address(") + As<Address>(value).toString() + std::string(")");
		}
		else if (isReference())
		{
			return std::string("Reference(") + As<Reference>(value).toString() + std::string(")");
		}
		else if (isEitherReference())
		{
			return std::string("EitherReference(") + As<EitherReference>(value).toString() + std::string(")");
		}
		else
		{
			return std::string("Val(...)");
		}
	}

	std::string LRValue::toString(Context& context) const
	{
		if (isAddress())
		{
			return std::string("Address(") + As<Address>(value).toString() + std::string(")");
		}
		else if (isReference())
		{
			return std::string("Reference(") + As<Reference>(value).toString() + std::string(")");
		}
		else if (isEitherReference())
		{
			return std::string("EitherReference(") + As<EitherReference>(value).toString() + std::string(")");
		}
		else
		{
			return std::string("Val(...)");
		}
	}

	Address LRValue::address(const Context & context) const
	{
		return IsType<Address>(value)
			? As<Address>(value)
			: (IsType<EitherReference>(value)
				? As<EitherReference>(value).replaced
				: context.getReference(As<Reference>(value)));
	}

	Address LRValue::getReference(const Context& context)const
	{
		return context.getReference(As<Reference>(value));
	}

	Address LRValue::makeTemporaryValue(Context& context)const
	{
		return context.makeTemporaryValue(evaluated());
	}

	class ExprImportForm : public boost::static_visitor<Expr>
	{
	public:
		ExprImportForm(bool isTopLevel)
			:isTopLevel(isTopLevel)
		{}

		bool isTopLevel;

		Expr operator()(const Lines& node)const
		{
			if (!isTopLevel)
			{
				return node;
			}

			RecordConstractor result;
			for (const auto& expr : node.exprs)
			{
				result.add(boost::apply_visitor(ExprImportForm(false), expr));
			}

			return result;
		}

		Expr operator()(const BinaryExpr& node)const
		{
			if (node.op != BinaryOp::Assign)
			{
				return node;
			}

			KeyExpr keyExpr(As<Identifier>(node.lhs));
			KeyExpr::SetExpr(keyExpr, node.rhs);
			return keyExpr;
		}

		Expr operator()(const LRValue& node)const { return node; }
		Expr operator()(const Identifier& node)const { return node; }
		Expr operator()(const Import& node)const { return node; }
		Expr operator()(const UnaryExpr& node)const { return node; }
		Expr operator()(const Range& node)const { return node; }
		Expr operator()(const DefFunc& node)const { return node; }
		Expr operator()(const If& node)const { return node; }
		Expr operator()(const For& node)const { return node; }
		Expr operator()(const Return& node)const { return node; }
		Expr operator()(const ListConstractor& node)const { return node; }
		Expr operator()(const KeyExpr& node)const { return node; }
		Expr operator()(const RecordConstractor& node)const { return node; }
		Expr operator()(const Accessor& node)const { return node; }
		Expr operator()(const DeclSat& node)const { return node; }
		Expr operator()(const DeclFree& node)const { return node; }
	};

	Expr ToImportForm(const Expr& expr)
	{
		ExprImportForm converter(true);
		return boost::apply_visitor(converter, expr);
	}

	Import::Import(const std::u32string& filePath)
	{
#ifdef USE_IMPORT
		const std::string u8FilePath = Unicode::UTF32ToUTF8(filePath);
		const auto path = cgl::filesystem::path(u8FilePath);

		CGL_DBG1(std::string("import path: \"") + u8FilePath + "\"");

		std::string sourceCode;

		if (path.is_absolute())
		{
			const std::string pathStr = filesystem::canonical(path).string();

			importPath = pathStr;

			/*if (alreadyImportedFiles.find(filesystem::canonical(path)) != alreadyImportedFiles.end())
			{
				std::cout << "File \"" << path.string() << "\" has been already imported.\n";
				originalParseTree = boost::none;
				return;
			}

			alreadyImportedFiles.emplace(filesystem::canonical(path));

			std::ifstream ifs(u8FilePath);
			if (!ifs.is_open())
			{
				CGL_Error(std::string() + "Error: import file \"" + u8FilePath + "\" does not exists.");
			}

			sourceCode = std::string((std::istreambuf_iterator<char>(ifs)), std::istreambuf_iterator<char>());
			cgl::workingDirectories.emplace(path.parent_path());*/
		}
		else
		{
			const auto currentDirectory = workingDirectories.top();
			const auto currentFilePath = currentDirectory / path;
			const std::string pathStr = filesystem::canonical(currentFilePath).string();

			CGL_DBG1(std::string("canonical path: \"") + pathStr + "\"");

			importPath = pathStr;
			/*if (alreadyImportedFiles.find(filesystem::canonical(currentFilePath)) != alreadyImportedFiles.end())
			{
				std::cout << "File \"" << filesystem::canonical(currentFilePath).string() << "\" has been already imported.\n";
				originalParseTree = boost::none;
				return;
			}

			alreadyImportedFiles.emplace(filesystem::canonical(currentFilePath));

			std::ifstream ifs(currentFilePath.string());
			if (!ifs.is_open())
			{
				CGL_Error(std::string() + "Error: import file \"" + currentFilePath.string() + "\" does not exists.");
			}

			sourceCode = std::string((std::istreambuf_iterator<char>(ifs)), std::istreambuf_iterator<char>());
			cgl::workingDirectories.emplace(currentFilePath.parent_path());*/
		}

		/*if (auto opt = Parse(sourceCode))
		{
			originalParseTree = opt;
		}
		else
		{
			CGL_Error("Parse failed.");
		}*/
		updateHash();
#else
		CGL_Error("Filesystem is disabled.");
#endif
	}

	Import::Import(const std::u32string& path, const Identifier& name):
		Import(path)
	{
		importName = name;
		updateHash();
	}

	LRValue Import::eval(std::shared_ptr<Context> pContext)const
	{
#ifdef USE_IMPORT
		auto it = importedParseTrees.find(seed);
		if (it == importedParseTrees.end() || !it->second)
		{
			CGL_Error("ファイルのimportに失敗");
		}

		const auto& expr = it->second.get();

		//通常のインポート
		//import filename
		if (importName.empty())
		{
			return ExecuteProgramWithRec(expr, pContext);
		}
		//修飾付きインポート
		//import filename as name
		//トップレベルの代入式をレコードの要素に包んで評価する
		else
		{
			const Expr importParseTree = BinaryExpr(Identifier(importName), ToImportForm(expr), BinaryOp::Assign);
			//printExpr(importParseTree, pContext, std::cout);
			return ExecuteProgramWithRec(importParseTree, pContext);
		}
#else
		CGL_Error("Import is disabled.");
#endif
	}

	void Import::SetName(Import& node, const Identifier& name)
	{
		node.importName = name;
		node.updateHash();
	}

	void Import::updateHash()
	{
		seed = 0;
		boost::hash_combine(seed, importPath);
		boost::hash_combine(seed, importName);
	}

	Expr BuildString(const std::u32string& str32)
	{
		Expr expr;
		expr = LRValue(CharString(str32));

		return expr;
	}

	//Expr BuildShapeExpander(const Accessor& accessor)
	//{
	//	Expr expr;
	//	//expr = LRValue(CharString(str32));
	//	//Accessor callFunction;
	//	//callFunction.AppendFunction(FunctionAccess());
	//	//FunctionAccess f;

	//	/*FuncVal({},
	//		MakeRecordConstructor(
	//			Identifier("line"), MakeListConstractor(
	//				MakeRecordConstructor(Identifier("x"), Expr(LRValue(minX)), Identifier("y"), Expr(LRValue(minY))),
	//				MakeRecordConstructor(Identifier("x"), Expr(LRValue(maxX)), Identifier("y"), Expr(LRValue(minY)))
	//			)
	//		)*/
	//	return expr;
	//}

#ifdef commentout
	class ConstraintProblem : public cppoptlib::Problem<double>
	{
	public:
		using typename cppoptlib::Problem<double>::TVector;

		std::function<double(const TVector&)> evaluator;
		Record originalRecord;
		std::vector<Identifier> keyList;
		std::shared_ptr<Context> pEnv;

		bool callback(const cppoptlib::Criteria<cppoptlib::Problem<double>::Scalar>& state, const TVector& x) override;

		double value(const TVector &x) override
		{
			return evaluator(x);
		}
	};

	bool ConstraintProblem::callback(const cppoptlib::Criteria<cppoptlib::Problem<double>::Scalar> &state, const TVector &x)
	{
		/*
		Record tempRecord = originalRecord;

		for (size_t i = 0; i < x.size(); ++i)
		{
		Address address = originalRecord.freeVariableRefs[i];
		pEnv->assignToObject(address, x[i]);
		}

		for (const auto& key : keyList)
		{
		Address address = pEnv->findAddress(key);
		tempRecord.append(key, address);
		}

		ProgressStore::TryWrite(pEnv, tempRecord);
		*/
		return true;
	}
#endif

	class ConstraintProblem : public cppoptlib::Problem<double>
	{
	public:
		using typename cppoptlib::Problem<double>::TVector;

		std::function<double(const TVector&)> evaluator;
		Record originalRecord;
		std::vector<Identifier> keyList;
		std::shared_ptr<Context> pEnv;

		double beginTime;

		bool callback(const cppoptlib::Criteria<cppoptlib::Problem<double>::Scalar>& state, const TVector& x) override;

		double value(const TVector &x) override
		{
			return evaluator(x);
		}
	};

	bool ConstraintProblem::callback(const cppoptlib::Criteria<cppoptlib::Problem<double>::Scalar> &state, const TVector &x)
	{
		if (!pEnv->hasTimeLimit())
		{
			return true;
		}

		if (pEnv->timeLimit() < GetSec() - beginTime)
		{
			return false;
		}

		return true;
	}

	void OptimizationProblemSat::addUnitConstraint(const Expr& logicExpr)
	{
		if (expr)
		{
			expr = BinaryExpr(expr.get(), logicExpr, BinaryOp::And);
		}
		else
		{
			expr = logicExpr;
		}
	}

	//void OptimizationProblemSat::constructConstraint(std::shared_ptr<Context> pEnv, std::vector<std::pair<Address, VariableRange>>& freeVariables)
	void OptimizationProblemSat::constructConstraint(std::shared_ptr<Context> pEnv)
	{
		refs.clear();
		invRefs.clear();
		hasPlateausFunction = false;

		//if (!expr || freeVariableRefs.empty())
		if (!expr)
		{
			CGL_DBG1("Warning: constraint expression is empty.");
			return;
		}
		//CGL_DBG1("Expr: ");
		//printExpr2(expr.get(), pEnv, std::cout);
		if (freeVariableRefs.empty())
		{
			CGL_DBG1("Warning: free variable set in constraint is empty.");
			return;
		}

		std::unordered_set<Address> appearingList;

		/*CGL_DBG1("freeVariables:");
		for (const auto& val : freeVariableRefs)
		{
			CGL_DBG1(std::string("  Address(") + val.first.toString() + ")");
		}*/

		std::vector<char> usedInSat(freeVariableRefs.size(), 0);

		SatVariableBinder binder(pEnv, freeVariableRefs, usedInSat, refs, appearingList, invRefs, hasPlateausFunction);

		/*CGL_DBG1("appearingList:");
		for (const auto& a : appearingList)
		{
			CGL_DBG1(std::string("  Address(") + a.toString() + ")");
		}*/

		if (boost::apply_visitor(binder, expr.get()))
		{
			//refs = binder.refs;
			//invRefs = binder.invRefs;
			//hasPlateausFunction = binder.hasPlateausFunction;

			//satに出てこないfreeVariablesの削除
			for (int i = static_cast<int>(freeVariableRefs.size()) - 1; 0 <= i; --i)
			{
				if (usedInSat[i] == 0)
				{
					freeVariableRefs.erase(freeVariableRefs.begin() + i);
				}
			}
		}
		else
		{
			refs.clear();
			invRefs.clear();
			freeVariableRefs.clear();
			hasPlateausFunction = false;
		}
	}

	bool OptimizationProblemSat::initializeData(std::shared_ptr<Context> pEnv)
	{
		data.resize(refs.size());

		for (size_t i = 0; i < data.size(); ++i)
		{
			const auto opt = pEnv->expandOpt(LRValue(refs[i]));
			if (!opt)
			{
				CGL_Error("参照エラー");
			}
			const Val& val = opt.get();
			if (auto opt = AsOpt<double>(val))
			{
				CGL_DebugLog(ToS(i) + " : " + ToS(opt.get()));
				data[i] = opt.get();
			}
			else if (auto opt = AsOpt<int>(val))
			{
				CGL_DebugLog(ToS(i) + " : " + ToS(opt.get()));
				data[i] = opt.get();
			}
			else
			{
				CGL_Error("存在しない参照をsatに指定した");
			}
		}

		return true;
	}

	std::vector<double> OptimizationProblemSat::solve(std::shared_ptr<Context> pEnv, const LocationInfo& info, const Record currentRecord, const std::vector<Identifier>& currentKeyList)
	{
		std::cerr << "OptimizationProblemSat::solve : " << std::endl;
		printExpr2(expr.get(), pEnv, std::cerr);

		constructConstraint(pEnv);
		CGL_DBG1(std::string("Current constraint freeVariablesSize: ") + ToS(freeVariableRefs.size()));

		/*if (isDebugMode && expr)
		{
			std::ofstream graphFile;
			graphFile.open("constraint_CFG.dot");
			MakeGraph(*pEnv, expr.get(), graphFile);
		}*/

		std::ofstream logger;
		if (isDebugMode)
		{
			logger.open("optimize_log.cgl");

			for (const auto& ref : freeVariableRefs)
			{
				CGL_DBG1(pEnv->makeLabel(ref.address));
			}

			logger << "{\n";
			logger << "\tsize: " << freeVariableRefs.size() << "\n";
			logger << "\tlabels: [\n";
			for (const auto& ref : freeVariableRefs)
			{
				logger << "\t\t\"" << pEnv->makeLabel(ref.address) << "\"\n";
			}
			logger << "\t]\n";

			logger << "\tdata: [\n";
		}

		/*{
			std::stringstream ss;
			for (const auto& r : optimizeRegions)
			{
				ss << "index(" << r.startIndex << "," << (r.startIndex + r.numOfIndices) << "), ";
			}
			CGL_DBG1(ss.str());
		}*/

		std::vector<Interval> rangeList;
		{
			for (const auto& r: optimizeRegions)
			{
				//std::cout << "index(" << r.startIndex << "," << (r.startIndex + r.numOfIndices) << ")\n";

				if (IsType<PackedVal>(r.region))
				{
					const auto& val = As<PackedVal>(r.region);

					//varに範囲指定がないときはmakePackedRangesを通るときに仮として0が設定されている。
					if (IsType<int>(val))
					{
						for (const Address address : r.addresses)
						{
							auto it = std::find_if(freeVariableRefs.begin(), freeVariableRefs.end(),
								[&](const RegionVariable& regionVariable) {
								return regionVariable.address == address;
							});

							if (it == freeVariableRefs.end())
							{
								CGL_Error("恐らく Evaluator.cpp の maskedRegionVariables() のバグ");
							}

							double minVal;
							double maxVal;
							if (it->has(RegionVariable::Position))
							{
								minVal = -1000;
								maxVal = +1000;
							}
							else if (it->has(RegionVariable::Scale))
							{
								minVal = 1.e-3;
								maxVal = 1.e+3;
							}
							else if (it->has(RegionVariable::Angle))
							{
								minVal = -180;
								maxVal = +180;
							}
							else if (it->has(RegionVariable::Other))
							{
								minVal = -1.e+6;
								maxVal = +1.e+6;
							}
							else
							{
								CGL_Error("不明な属性");
							}

							rangeList.push_back(Interval(minVal, maxVal));
						}

						/*for (int i = 0; i < r.numOfIndices; ++i)
						{
							const int currentIndex = r.startIndex + i;

							double minVal;
							double maxVal;
							if (freeVariableRefs[currentIndex].has(RegionVariable::Position))
							{
								minVal = -1000;
								maxVal = +1000;
							}
							else if (freeVariableRefs[currentIndex].has(RegionVariable::Scale))
							{
								minVal = 1.e-3;
								maxVal = 1.e+3;
							}
							else if (freeVariableRefs[currentIndex].has(RegionVariable::Angle))
							{
								minVal = -180;
								maxVal = +180;
							}
							else if (freeVariableRefs[currentIndex].has(RegionVariable::Other))
							{
								minVal = -1.e+6;
								maxVal = +1.e+6;
							}
							else
							{
								CGL_Error("不明な属性");
							}

							rangeList.push_back(Interval(minVal, maxVal));
						}*/
					}
					else if (IsType<PackedRecord>(val))
					{
						const auto& shapeRegion = As<PackedRecord>(val);
						const auto& values = shapeRegion.values;
						if (values.find("pos") == values.end() ||
							values.find("scale") == values.end() ||
							values.find("angle") == values.end())
						{
							CGL_Error("範囲の型が不正");
						}
						const auto bb = GetBoundingBox(shapeRegion, pEnv);
						const auto minRecord = As<PackedRecord>(bb.values.find("min")->second.value);
						const auto maxRecord = As<PackedRecord>(bb.values.find("max")->second.value);

						const double minX = AsDouble(minRecord.values.find("x")->second.value);
						const double minY = AsDouble(minRecord.values.find("y")->second.value);
						const double maxX = AsDouble(maxRecord.values.find("x")->second.value);
						const double maxY = AsDouble(maxRecord.values.find("y")->second.value);

						//TODO: ちゃんとインデックスを見て対応付ける
						//現在はvarはVec2のみでx,yの順に並んでいると仮定している
						rangeList.push_back(Interval(minX, maxX));
						rangeList.push_back(Interval(minY, maxY));
					}
					else if (IsType<PackedList>(val))
					{
						const auto& intervalRegion = As<PackedList>(val);
						if (intervalRegion.data.size() != 2 ||
							!IsNum(intervalRegion.data[0].value) ||
							!IsNum(intervalRegion.data[1].value))
						{
							CGL_Error("範囲の型が不正");
						}
						const double minV = AsDouble(intervalRegion.data[0].value);
						const double maxV = AsDouble(intervalRegion.data[1].value);

						/*for (int i = 0; i < r.numOfIndices; ++i)
						{
							rangeList.push_back(Interval(minV, maxV));
						}*/

						for (const Address address : r.addresses)
						{
							rangeList.push_back(Interval(minV, maxV));
						}
					}
					else
					{
						CGL_Error("範囲の型が不正");
					}
				}
				else
				{
					CGL_Error("範囲の型が不正");
				}
			}
		}

		if (!initializeData(pEnv))
		{
			CGL_Error("制約の初期化に失敗");
		}

		std::vector<double> resultxs;
		if (!freeVariableRefs.empty())
		{
			//varのアドレス(の内実際にsatに現れるもののリスト)から、OptimizationProblemSat中の変数リストへの対応付けを行うマップを作成
			std::unordered_map<int, int> variable2Data;
			for (size_t freeIndex = 0; freeIndex < freeVariableRefs.size(); ++freeIndex)
			{
				CGL_DebugLog(ToS(freeIndex));
				CGL_DebugLog(std::string("Address(") + freeVariableRefs[freeIndex].toString() + ")");
				const auto& ref1 = freeVariableRefs[freeIndex];

				bool found = false;
				for (size_t dataIndex = 0; dataIndex < refs.size(); ++dataIndex)
				{
					CGL_DebugLog(ToS(dataIndex));
					CGL_DebugLog(std::string("Address(") + refs[dataIndex].toString() + ")");

					const auto& ref2 = refs[dataIndex];

					if (ref1.address == ref2)
					{
						//std::cout << "    " << freeIndex << " -> " << dataIndex << std::endl;

						found = true;
						variable2Data[freeIndex] = dataIndex;
						break;
					}
				}

				//DeclFreeにあってDeclSatに無い変数は意味がない。
				//単に無視しても良いが、恐らく入力のミスと思われるので警告を出す
				if (!found)
				{
					CGL_WarnLog("freeに指定された変数が無効です");
				}
			}
			CGL_DebugLog("End Record MakeMap");
			if (hasPlateausFunction/*,false*/)
			{
				std::cout << "Solve constraint by CMA-ES...\n";

				libcmaes::FitFunc func = [&](const double *x, const int N)->double
				{
					for (int i = 0; i < N; ++i)
					{
						update(variable2Data[i], x[i]);
					}

					{
						for (const auto& keyval : invRefs)
						{
							pEnv->TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(keyval.first, data[keyval.second]);
						}
					}

					pEnv->switchFrontScope();
					pEnv->enterScope();
					double result = eval(pEnv, info);
					pEnv->exitScope();
					pEnv->switchBackScope();

					CGL_DebugLog(std::string("cost: ") + ToS(result, 17));

					return result;
				};

				std::vector<double> x0(freeVariableRefs.size());
				for (int i = 0; i < x0.size(); ++i)
				{
					x0[i] = data[variable2Data[i]];
					CGL_DebugLog(ToS(i) + " : " + ToS(x0[i]));
				}

				const double sigma = 0.1;

				const int lambda = 100;

				libcmaes::CMAParameters<> cmaparams(x0, sigma, lambda, 1);

				if (pEnv->hasTimeLimit())
				{
					cmaparams.set_max_calc_time(pEnv->timeLimit());
					cmaparams.set_current_time(GetSec());
				}

				libcmaes::CMASolutions cmasols = libcmaes::cmaes<>(func, cmaparams);
				resultxs = cmasols.best_candidate().get_x();

				std::cout << "solved\n";
			}
			else if(true)
			{
				std::cout << "Solve constraint by BFGS...\n";

				ConstraintProblem constraintProblem;
				constraintProblem.evaluator = [&](const ConstraintProblem::TVector& v)->double
				{
					for (int i = 0; i < v.size(); ++i)
					{
						update(variable2Data[i], v[i]);
					}

					for (const auto& keyval : invRefs)
					{
						pEnv->TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(keyval.first, data[keyval.second]);
					}

					pEnv->switchFrontScope();
					pEnv->enterScope();
					double result = eval(pEnv, info);
					pEnv->exitScope();
					pEnv->switchBackScope();

					//CGL_DebugLog(std::string("cost: ") + ToS(result, 17));
					//std::cout << std::string("cost: ") << ToS(result, 17) << "\n";
					return result*result;
				};
				constraintProblem.originalRecord = currentRecord;
				constraintProblem.keyList = currentKeyList;
				constraintProblem.pEnv = pEnv;

				Eigen::VectorXd x0s(freeVariableRefs.size());
				for (int i = 0; i < x0s.size(); ++i)
				{
					x0s[i] = data[variable2Data[i]];
					//x0s[i] = (problem.data[variable2Data[i]] / 2000.0) + 0.5;
					CGL_DebugLog(ToS(i) + " : " + ToS(x0s[i]));
				}

				constraintProblem.beginTime = GetSec();

				cppoptlib::BfgsSolver<ConstraintProblem> solver;
				solver.minimize(constraintProblem, x0s);

				resultxs.resize(x0s.size());
				for (int i = 0; i < x0s.size(); ++i)
				{
					resultxs[i] = x0s[i];
				}
			}
			else if(true)
			{
				std::cout << "Solve constraint by Random Search...\n";

				const auto targetFunc = [&](const std::vector<double>& v)->double
				{
					for (int i = 0; i < v.size(); ++i)
					{
						update(variable2Data[i], v[i]);
					}

					for (const auto& keyval : invRefs)
					{
						pEnv->TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(keyval.first, data[keyval.second]);
					}

					double result;

					pEnv->switchFrontScope();
					pEnv->enterScope();
					try
					{
						result = eval(pEnv, info);
					}
					catch (std::exception& e)
					{
						std::cout << "Eval: " << e.what() << std::endl;
						throw;
					}
					pEnv->exitScope();
					pEnv->switchBackScope();

					if (isDebugMode)
					{
						logger << "[";
						for (int i = 0; i < v.size(); ++i)
						{
							logger << v[i] << ", ";
						}
						logger << result << "], ";
					}

					//CGL_DebugLog(std::string("cost: ") + ToS(result, 17));
					return result;
				};

				std::vector<double> answer(freeVariableRefs.size());
				for (int i = 0; i < answer.size(); ++i)
				{
					answer[i] = data[variable2Data[i]];
				}

				double beginTime = GetSec();

				{
					double minimumCost = targetFunc(answer);
					std::vector<double> current(answer.size());
					if (current.size() != rangeList.size())
					{
						CGL_Error("範囲と変数の数が対応していない");
					}

					std::vector<std::uniform_real_distribution<double>> dists;
					for (size_t i = 0; i < rangeList.size(); ++i)
					{
						std::cout << "Range(" << i << "): [" << rangeList[i].minimum << ", " << rangeList[i].maximum << "]" << std::endl;
						dists.emplace_back(rangeList[i].minimum, rangeList[i].maximum);
					}

					int count = 0;
					std::mt19937 rng;
					//while (GetSec() - beginTime < 300.0)
					//while (count < 20000)
					//while (count < 6900)
					while(count < 20000)
					{
						cloneTime = 0.0;
						cloneCount = 0;
						/*if (6660 < count)
						{
							printAddressInsertion = true;
							std::cout << "----------------------------------\n";
							std::cout << count << ": ";
						}*/
						for (size_t i = 0; i < current.size(); ++i)
						{
							current[i] = dists[i](rng);
							if (printAddressInsertion)
							{
								std::cout << current[i] << ", ";
							}
						}

						if (printAddressInsertion)
						{
							std::cout << "\n";
						}

						const double currentCost = targetFunc(current);
						if (currentCost < minimumCost)
						{
							minimumCost = currentCost;
							answer = current;
						}
						if (printAddressInsertion)
						{
							std::cout << "cloneTime: " << cloneTime << " | " << cloneCount << "\n";
						}

						++count;

						if (count % 1000 == 0)
							//if (count % 10 == 0)
						{
							std::cout <<"it("<< count <<") | "<< "cloneTime: " << cloneTime << ", cloneCount: " << cloneCount << "\n";
							pEnv->garbageCollect(true);
						}
					}

					if (isDebugMode)
					{
						logger << "[";
						for (int i = 0; i < answer.size(); ++i)
						{
							logger << answer[i] << ", ";
						}
						logger << minimumCost << "]\n";

						logger << "\t]\n";
						logger << "}\n";
					}
				}

				resultxs.resize(answer.size());
				for (int i = 0; i < answer.size(); ++i)
				{
					resultxs[i] = answer[i];
				}
			}
			/*else if (true)
			{
				std::cout << "Solve constraint by nlopt...\n";

				auto targetFunc = [&](unsigned N, const double *x, double *grad, void *my_func_data)
				{
					for (int i = 0; i < N; ++i)
					{
						update(variable2Data[i], x[i]);
					}

					{
						for (const auto& keyval : invRefs)
						{
							pEnv->TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(keyval.first, data[keyval.second]);
						}
					}

					pEnv->switchFrontScope();
					pEnv->enterScope();
					double result = eval(pEnv, info);
					pEnv->exitScope();
					pEnv->switchBackScope();

					CGL_DebugLog(std::string("cost: ") + ToS(result, 17));

					return result;
				};

				std::vector<double> lb, ub;
				for (size_t i = 0; i < rangeList.size(); ++i)
				{
					lb.push_back(rangeList[i].minimum);
					ub.push_back(rangeList[i].maximum);
				}

				nlopt_opt opt;
				opt = nlopt_create(NLOPT_LD_MMA, freeVariableRefs.size());
				nlopt_set_lower_bounds(opt, lb.data());
				nlopt_set_upper_bounds(opt, ub.data());
				nlopt_set_min_objective(opt, targetFunc, NULL);

				std::vector<double> xs(freeVariableRefs.size());
				for (int i = 0; i < xs.size(); ++i)
				{
					xs[i] = data[variable2Data[i]];
				}
			}*/
#ifdef USE_OPTIONAL_LIB
			else
			{
				std::cout << "Solve constraint by Limbo...\n";

				const auto targetFunc = [&](const Eigen::VectorXd& x)->double
				{
					for (int i = 0; i < x.size(); ++i)
					{
						//std::cout << (x[i] * (rangeList[i].maximum - rangeList[i].minimum) + rangeList[i].minimum) << ", ";
						update(variable2Data[i], x[i] * (rangeList[i].maximum - rangeList[i].minimum) + rangeList[i].minimum);
					}
					//std::cout << "\n";

					for (const auto& keyval : invRefs)
					{
						pEnv->TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(keyval.first, data[keyval.second]);
					}

					double result;

					pEnv->switchFrontScope();
					pEnv->enterScope();
					try
					{
						result = eval(pEnv, info);
					}
					catch (std::exception& e)
					{
						std::cout << "Eval: " << e.what() << std::endl;
						throw;
					}
					pEnv->exitScope();
					pEnv->switchBackScope();

					//limbo maximizes target function
					return -result;
				};

				//*
				using Kernel_t = limbo::kernel::MaternFiveHalves<Params>;
				using Mean_t = limbo::mean::Data<Params>;

				using gp_opt_t = limbo::model::gp::KernelLFOpt<Params>;
				using GP_t = limbo::model::GP<Params, Kernel_t, Mean_t, gp_opt_t>;
				//using GP_t = limbo::model::GP<Params, Kernel_t, Mean_t>;

				//using Acqui_t = limbo::acqui::UCB<Params, GP_t>;
				using Acqui_t = limbo::acqui::EI<Params, GP_t>;

				using stat_t = boost::fusion::vector<limbo::stat::ConsoleSummary<Params>,
					limbo::stat::Samples<Params>,
					limbo::stat::Observations<Params>,
					limbo::stat::GP<Params>>;

				limbo::bayes_opt::BOptimizer<Params, limbo::modelfun<GP_t>, limbo::statsfun<stat_t>, limbo::acquifun<Acqui_t>> opt;
				//*/

				// example with basic HP opt
				//limbo::bayes_opt::BOptimizerHPOpt<Params> opt;

				LimboFitFunc target;

				target.numOfVars = freeVariableRefs.size();
				target.func = targetFunc;

				opt.optimize2(target);

				/*std::cout << opt.best_observation() << " res  "
					<< opt.best_sample().transpose() << std::endl;*/

				//const auto answer = opt.best_observation();
				const auto answer = opt.best_sample();
				resultxs.resize(answer.size());
				for (int i = 0; i < answer.size(); ++i)
				{
					std::cout << "answer[" << i << "]: " << answer[i] << "\n";
					resultxs[i] = answer[i] * (rangeList[i].maximum - rangeList[i].minimum) + rangeList[i].minimum;
				}
			}
#endif
		}

		return resultxs;
	}

	double OptimizationProblemSat::eval(std::shared_ptr<Context> pEnv, const LocationInfo& info)
	{
		if (!expr)
		{
			return 0.0;
		}

		if (data.empty())
		{
			CGL_WarnLog("free式に有効な変数が指定されていません。");
			return 0.0;
		}

		/*{
			CGL_DebugLog("data:");
			for(int i=0;i<data.size();++i)
			{
				CGL_DebugLog(std::string("  ID(") + ToS(i) + ") -> " + ToS(data[i]));
			}

			CGL_DebugLog("refs:");
			for (int i = 0; i<refs.size(); ++i)
			{
				CGL_DebugLog(std::string("  ID(") + ToS(i) + ") -> Address(" + refs[i].toString() + ")");
			}

			CGL_DebugLog("invRefs:");
			for(const auto& keyval : invRefs)
			{
				CGL_DebugLog(std::string("  Address(") + keyval.first.toString() + ") -> ID(" + ToS(keyval.second) + ")");
			}

			CGL_DebugLog("env:");
			pEnv->printContext();

			CGL_DebugLog("expr:");
			printExpr(expr.get());
		}*/

		EvalSatExpr evaluator(pEnv, data, refs, invRefs);
		const Val evaluated = pEnv->expand(boost::apply_visitor(evaluator, expr.get()), info);

		if (IsType<double>(evaluated))
		{
			return As<double>(evaluated);
		}
		else if (IsType<int>(evaluated))
		{
			return As<int>(evaluated);
		}

		CGL_Error("sat式の評価結果が不正");
	}

	//値同士が等しいかを知りたいのでAddressはハッシュには含めない
	class ValueHasher : public boost::static_visitor<size_t>
	{
	public:
		ValueHasher() = default;

		size_t operator()(bool node)const { return std::hash<bool>()(node); }
		size_t operator()(int node)const { return std::hash<int>()(node); }
		size_t operator()(double node)const { return std::hash<double>()(node); }
		size_t operator()(const CharString& node)const { return std::hash<std::u32string>()(node.toString()); }
		size_t operator()(const PackedList& node)const
		{
			size_t result = 0;
			for (const auto& val : node.data)
			{
				boost::hash_combine(result, boost::apply_visitor(*this, val.value));
			}
			return result;
		}
		size_t operator()(const KeyValue& node)const
		{
			return 0;
		}
		size_t operator()(const PackedRecord& node)const
		{
			size_t result = 0;
			for (const auto& keyval : node.values)
			{
				boost::hash_combine(result, std::hash<std::string>()(keyval.first));
				boost::hash_combine(result, boost::apply_visitor(*this, keyval.second.value));
			}
			return result;
		}
		size_t operator()(const FuncVal& node)const
		{
			return 0;
		}
		size_t operator()(const Jump& node)const
		{
			return 0;
		}
	};

	size_t GetHash(const PackedVal& val)
	{
		ValueHasher hasher;
		return boost::apply_visitor(hasher, val);
	}

	size_t GetHash(const Val& val, const Context& context)
	{
		const PackedVal& packed = Packed(val, context);
		return GetHash(packed);
	}

	//中身のアドレスを全て一つの値にまとめる
	class ValuePacker : public boost::static_visitor<PackedVal>
	{
	public:
		ValuePacker(const Context& context) :
			context(context)
		{}

		const Context& context;

		PackedVal operator()(bool node)const { return node; }
		PackedVal operator()(int node)const { return node; }
		PackedVal operator()(double node)const { return node; }
		PackedVal operator()(const CharString& node)const { return node; }
		PackedVal operator()(const List& node)const { return node.packed(context); }
		PackedVal operator()(const KeyValue& node)const { return node; }
		PackedVal operator()(const Record& node)const { return node.packed(context); }
		PackedVal operator()(const FuncVal& node)const { return node; }
		PackedVal operator()(const Jump& node)const { return node; }
	};

	//中身のアドレスを全て展開する
	class ValueUnpacker : public boost::static_visitor<Val>
	{
	public:
		ValueUnpacker(Context& context) :
			context(context)
		{}

		Context& context;

		Val operator()(bool node)const { return node; }
		Val operator()(int node)const { return node; }
		Val operator()(double node)const { return node; }
		Val operator()(const CharString& node)const { return node; }
		Val operator()(const PackedList& node)const { return node.unpacked(context); }
		Val operator()(const KeyValue& node)const { return node; }
		Val operator()(const PackedRecord& node)const { return node.unpacked(context); }
		Val operator()(const FuncVal& node)const { return node; }
		Val operator()(const Jump& node)const { return node; }
	};

	PackedVal Packed(const Val& value, const Context& context)
	{
		ValuePacker packer(context);
		return boost::apply_visitor(packer, value);
	}

	Val Unpacked(const PackedVal& packedValue, Context& context)
	{
		ValueUnpacker unpacker(context);
		return boost::apply_visitor(unpacker, packedValue);
	}

	Val PackedList::unpacked(Context& context)const
	{
		ValueUnpacker unpacker(context);

		List result;

		for (const auto& val : data)
		{
			const Address address = val.address;

			const PackedVal& packedValue = val.value;
			const Val value = boost::apply_visitor(unpacker, packedValue);

			if (address.isValid())
			{
				context.TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(address, value);
				result.add(address);
			}
			else
			{
				result.add(context.makeTemporaryValue(value));
			}
		}

		return result;
	}

	PackedVal List::packed(const Context& context)const
	{
		ValuePacker packer(context);

		PackedList result;

		for (const Address address : data)
		{
			const auto& opt = context.expandOpt(LRValue(address));
			if (!opt)
			{
				std::stringstream ss;
				ss << "リスト中の " << LRValue(address).toString() << " 参照に失敗しました。";
				CGL_Error("参照エラー: ");
			}
			const PackedVal packedValue = boost::apply_visitor(packer, opt.get());

			result.add(address, packedValue);
		}

		return result;
	}

	Val PackedRecord::unpacked(Context& context)const
	{
		ValueUnpacker unpacker(context);

		Record result;

		for (const auto& keyval : values)
		{
			const Address address = keyval.second.address;

			const PackedVal& packedValue = keyval.second.value;
			const Val value = boost::apply_visitor(unpacker, packedValue);

			if (address.isValid())
			{
				context.TODO_Remove__ThisFunctionIsDangerousFunction__AssignToObject(address, value);
				result.add(keyval.first, address);
			}
			else
			{
				result.add(keyval.first, context.makeTemporaryValue(value));
			}
		}

		result.problems = problems;
		result.boundedFreeVariables = freeVariables;
		//result.freeVariableRefs = freeVariableRefs;
		result.type = type;
		result.isSatisfied = isSatisfied;
		result.pathPoints = pathPoints;
		result.constraint = constraint;

		//result.unitConstraints = unitConstraints;
		//result.variableAppearances = variableAppearances;
		////result.constraintGroups = constraintGroups;
		//result.groupConstraints = groupConstraints;
		result.original = original;

		return result;
	}

	PackedVal Record::packed(const Context& context)const
	{
		ValuePacker packer(context);

		PackedRecord result;

		for (const auto& keyval : values)
		{
			const auto& opt = context.expandOpt(LRValue(keyval.second));
			if (!opt)
			{
				std::stringstream ss;
				ss << "レコード中のキー \"" << keyval.first << "\": " << LRValue(keyval.second).toString() << " の参照に失敗しました。";
				CGL_Error("参照エラー: " + ss.str());
			}
			const PackedVal packedValue = boost::apply_visitor(packer, opt.get());

			result.add(keyval.first, keyval.second, packedValue);
		}

		result.problems = problems;
		result.freeVariables = boundedFreeVariables;
		result.type = type;
		result.isSatisfied = isSatisfied;
		result.pathPoints = pathPoints;
		result.constraint = constraint;

		//result.unitConstraints = unitConstraints;
		//result.variableAppearances = variableAppearances;
		////result.constraintGroups = constraintGroups;
		//result.groupConstraints = groupConstraints;
		result.original = original;

		return result;
	}
}


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ 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
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "libRaptorQ/common.hpp"
#include "libRaptorQ/Parameters.hpp"
#include "libRaptorQ/Octet.hpp"
#include <Eigen/Dense>

namespace RaptorQ__v1 {
namespace Impl {

using DenseMtx = Eigen::Matrix<Octet, Eigen::Dynamic, Eigen::Dynamic,
                                                            Eigen::RowMajor>;

class RAPTORQ_LOCAL Operation
{
public:
    enum class _t : uint8_t {
        NONE = 0x00,
        SWAP = 0x01,
        ADD_MUL = 0x02,
        DIV = 0x03,
        BLOCK = 0x04,
        REORDER = 0x05
    };
    Operation() = delete;
    Operation (const Operation &rhs)
        :_type (rhs._type)
    {
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = rhs.swap;
            break;
        case _t::ADD_MUL:
            add_mul = rhs.add_mul;
            break;
        case _t::DIV:
            div = rhs.div;
            break;
        case _t::BLOCK:
            block = rhs.block;
            break;
        case _t::REORDER:
            reorder = rhs.reorder;
            break;
        case _t::NONE:
            break;
        }
    }
    Operation& operator= (const Operation &rhs)
    {
        assert (_type == rhs._type && "Operation types do not correspond.");
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = rhs.swap;
            break;
        case _t::ADD_MUL:
            add_mul = rhs.add_mul;
            break;
        case _t::DIV:
            div = rhs.div;
            break;
        case _t::BLOCK:
            block = rhs.block;
            break;
        case _t::REORDER:
            reorder = rhs.reorder;
            break;
        case _t::NONE:
            break;
        }
        return *this;
    }

    Operation (Operation &&rhs)
        :_type (rhs._type)
    {
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = std::move (rhs.swap);
            break;
        case _t::ADD_MUL:
            add_mul = std::move (rhs.add_mul);
            break;
        case _t::DIV:
            div = std::move (rhs.div);
            break;
        case _t::BLOCK:
            block = std::move (rhs.block);
            break;
        case _t::REORDER:
            reorder = std::move (rhs.reorder);
            break;
        case _t::NONE:
            break;
        }
    }

    Operation& operator= (Operation &&rhs)
    {
        assert (_type == rhs._type && "Operation types do not correspond.");
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = std::move (rhs.swap);
            break;
        case _t::ADD_MUL:
            add_mul = std::move (rhs.add_mul);
            break;
        case _t::DIV:
            div = std::move (rhs.div);
            break;
        case _t::BLOCK:
            block = std::move (rhs.block);
            break;
        case _t::REORDER:
            reorder = std::move (rhs.reorder);
            break;
        case _t::NONE:
            break;
        }
        return *this;
    }

    Operation (const _t type, const uint16_t row_1, const uint16_t row_2)
        : _type (type), swap (row_1, row_2) { assert (type == _t::SWAP); }
    Operation (const _t type, const uint16_t row_1, const uint16_t row_2,
                                                            const Octet scalar)
        : _type (type), add_mul (row_1, row_2, scalar)
                                            { assert (type == _t::ADD_MUL); }
    Operation (const _t type, const uint16_t row, const Octet scalar)
        : _type (type), div (row, scalar) { assert (type == _t::DIV); }
    Operation (const _t type, const DenseMtx &mtx)
        : _type (type), block (mtx) { assert (type == _t::BLOCK); }
    Operation (const _t type, const std::vector<uint16_t> &order)
        : _type (type), reorder (order) { assert (type == _t::REORDER); }

    ~Operation ()
    {
        if (_type == _t::BLOCK)
            block.clear();
        if (_type == _t::REORDER)
            reorder.clear();
    }

    void build_mtx (DenseMtx &mtx) const
    {
        switch (_type)
        {
        case _t::SWAP:
            return swap.build_mtx (mtx);
        case _t::ADD_MUL:
            return add_mul.build_mtx (mtx);
        case _t::DIV:
            return div.build_mtx (mtx);
        case _t::BLOCK:
            return block.build_mtx (mtx);
        case _t::REORDER:
            return reorder.build_mtx (mtx);
        case _t::NONE:
            break;
        }
    }
private:
    Operation (const _t type)
        :_type (type) {}
    class RAPTORQ_LOCAL Swap
    {
    public:
        Swap (const uint16_t row_1, const uint16_t row_2)
            : _row_1 (row_1), _row_2 (row_2) {}
        Swap (const Swap&) = default;
        Swap& operator= (const Swap&) = default;
        Swap (Swap &&) = default;
        Swap& operator= (Swap &&) = default;
        ~Swap() {}
        void build_mtx (DenseMtx &mtx) const
            { mtx.row(_row_1).swap (mtx.row(_row_2)); }
    private:
        uint16_t _row_1, _row_2;
    };

    class RAPTORQ_LOCAL Add_Mul
    {
    public:
        Add_Mul (const uint16_t row_1, const uint16_t row_2, const Octet scalar)
            : _row_1 (row_1), _row_2 (row_2), _scalar (scalar) {}
        Add_Mul (const Add_Mul&) = default;
        Add_Mul& operator= (const Add_Mul&) = default;
        Add_Mul (Add_Mul&&) = default;
        Add_Mul& operator= (Add_Mul&&) = default;
        ~Add_Mul() {}
        void build_mtx (DenseMtx &mtx) const
        {
            const auto row = mtx.row (_row_2) * _scalar;
            mtx.row (_row_1) += row;
        }
    private:
        uint16_t _row_1, _row_2;
        Octet _scalar;
    };

    class RAPTORQ_LOCAL Div
    {
    public:
        Div (const uint16_t row_1, const Octet scalar)
            : _row_1 (row_1), _scalar (scalar) {}
        Div (const Div&) = default;
        Div& operator= (const Div&) = default;
        Div (Div&&) = default;
        Div& operator= (Div&&) = default;
        ~Div() {}
        void build_mtx (DenseMtx &mtx) const
            { mtx.row (_row_1) /= _scalar; }
    private:
        uint16_t _row_1;
        Octet _scalar;
    };

    class RAPTORQ_LOCAL Block
    {
    public:
        Block (const DenseMtx &block)
            : _block (block) {}
        Block (const Block&) = default;
        Block& operator= (const Block&) = default;
        Block (Block&&) = default;
        Block& operator= (Block&&) = default;
        ~Block() {}
        void build_mtx (DenseMtx &mtx) const
        {
            const auto orig = mtx.block (0,0, _block.cols(), mtx.cols());
            mtx.block (0, 0, _block.cols(), mtx.cols()) = _block * orig;
        }
        void clear()
            { _block = DenseMtx(); }
    private:
        DenseMtx _block;
    };

    class RAPTORQ_LOCAL Reorder
    {
    public:
        Reorder (const std::vector<uint16_t> &order)
            : _order (order) {}
        Reorder (const Reorder&) = default;
        Reorder& operator= (const Reorder&) = default;
        Reorder (Reorder&&) = default;
        Reorder& operator= (Reorder&&) = default;
        ~Reorder() {}
        void build_mtx (DenseMtx &mtx) const
        {
            uint16_t overhead = static_cast<uint16_t> (
                                static_cast<uint16_t> (mtx.rows()) - _order.size());
            DenseMtx ret = DenseMtx (mtx.rows() - overhead , mtx.cols());

            // reorder some of the lines as requested by the _order vector
            uint16_t row = 0;
            for (const uint16_t pos : _order)
                ret.row (pos) = mtx.row (row++);
            mtx.swap (ret);
            // other lines will not influence the computation, ignore them
        }
        void clear()
            { _order = std::vector<uint16_t>(); }
    private:
        std::vector<uint16_t> _order;
    };

    const _t _type;
    union {
        Swap swap;
        Add_Mul add_mul;
        Div div;
        Block block;
        Reorder reorder;
    };
};

}   // namespace Impl
}   // namespace RaptorQ


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "ndm_spin_phase.hpp"

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Machine/rbm_spin.hpp"
#include "Utils/all_utils.hpp"
#include "Utils/log_cosh.hpp"

namespace netket {

using RealVectorType = NdmSpinPhase::RealVectorType;
using VectorType = AbstractMachine::VectorType;

void NdmSpinPhase::Init() {
  W1_.resize(nv_, nh_);
  U1_.resize(nv_, na_);
  b1_.resize(nv_);
  h1_.resize(nh_);
  d1_.resize(na_);

  W2_.resize(nv_, nh_);
  U2_.resize(nv_, na_);
  b2_.resize(nv_);
  h2_.resize(nh_);

  BatchSize(1);

  npar_ = 2 * nv_ * (nh_ + na_);

  if (useb_) {
    npar_ += 2 * nv_;
  } else {
    b1_.setZero();
    b2_.setZero();
  }

  if (useh_) {
    npar_ += 2 * nh_;
  } else {
    h1_.setZero();
    h2_.setZero();
  }

  if (used_) {
    npar_ += na_;
  } else {
    d1_.setZero();
  }

  InfoMessage() << "Phase NDM Initizialized with nvisible = " << nv_
                << " and nhidden  = " << nh_ << " and nancilla = " << na_
                << std::endl;
  InfoMessage() << "Using visible   bias = " << useb_ << std::endl;
  InfoMessage() << "Using hidden    bias  = " << useh_ << std::endl;
  InfoMessage() << "Using ancillary bias  = " << used_ << std::endl;
}

Index NdmSpinPhase::BatchSize() const noexcept { return thetas_r1_.rows(); }

void NdmSpinPhase::BatchSize(Index batch_size) {
  if (batch_size <= 0) {
    std::ostringstream msg;
    msg << "invalid batch size: " << batch_size
        << "; expected a positive number";
    throw InvalidInputError{msg.str()};
  }
  if (batch_size != BatchSize()) {
    thetas_r1_.resize(batch_size, nh_);
    thetas_r2_.resize(batch_size, nh_);
    thetas_c1_.resize(batch_size, nh_);
    thetas_c2_.resize(batch_size, nh_);

    lnthetas_r1_.resize(batch_size, nh_);
    lnthetas_r2_.resize(batch_size, nh_);
    lnthetas_c1_.resize(batch_size, nh_);
    lnthetas_c2_.resize(batch_size, nh_);

    thetasnew_r1_.resize(batch_size, nh_);
    thetasnew_r2_.resize(batch_size, nh_);
    thetasnew_c1_.resize(batch_size, nh_);
    thetasnew_c2_.resize(batch_size, nh_);

    lnthetasnew_r1_.resize(batch_size, nh_);
    lnthetasnew_r2_.resize(batch_size, nh_);
    lnthetasnew_c1_.resize(batch_size, nh_);
    lnthetasnew_c2_.resize(batch_size, nh_);

    thetas_a_.resize(batch_size, na_);
    lnthetas_a_.resize(batch_size, na_);
    thetas_a1_.resize(batch_size, na_);
    thetas_a2_.resize(batch_size, na_);
    thetasnew_a1_.resize(batch_size, na_);
    thetasnew_a2_.resize(batch_size, na_);
    pi_.resize(batch_size, na_);
    lnpi_.resize(batch_size, na_);
    lnpinew_.resize(batch_size, na_);

    vsum_.resize(batch_size, nv_);
    vdelta_.resize(batch_size, nv_);
  }
}

VectorType NdmSpinPhase::DerLogSingle(VisibleConstType vr, VisibleConstType vc,
                                      const any & /*cache*/) {
  VectorType der(npar_);

  const int impar = (npar_ + na_ * used_) / 2;

  if (useb_) {
    der.head(nv_) = 0.5 * (vr + vc);
    der.segment(impar, nv_) = I_ * 0.5 * (vr - vc);
  }
  lnthetas_r1_ = (W1_.transpose() * vr + h1_).array().tanh();
  lnthetas_r2_ = (W2_.transpose() * vr + h2_).array().tanh();
  lnthetas_c1_ = (W1_.transpose() * vc + h1_).array().tanh();
  lnthetas_c2_ = (W2_.transpose() * vc + h2_).array().tanh();

  if (useh_) {
    der.segment(useb_ * nv_, nh_) = 0.5 * (lnthetas_r1_ + lnthetas_c1_);
    der.segment(impar + useb_ * nv_, nh_) =
        I_ * 0.5 * (lnthetas_r2_ - lnthetas_c2_);
  }

  thetas_a1_ = 0.5 * U1_.transpose() * (vr + vc) + d1_;
  thetas_a2_ = 0.5 * U2_.transpose() * (vr - vc);
  lnpi_ = ((0.5 * U1_.transpose() * (vr + vc) + d1_).array() +
           I_ * (0.5 * U2_.transpose() * (vr - vc)).array())
              .tanh();

  if (used_) {
    der.segment(useb_ * nv_ + useh_ * nh_, na_) = lnpi_;
  }

  const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
  const int initw_2 = nv_ * useb_ + nh_ * useh_;

  MatrixType wder =
      0.5 * (vr * lnthetas_r1_.transpose() + vc * lnthetas_c1_.transpose());
  der.segment(initw_1, nv_ * nh_) =
      Eigen::Map<VectorType>(wder.data(), nv_ * nh_);

  wder = 0.5 * I_ *
         (vr * lnthetas_r2_.transpose() - vc * lnthetas_c2_.transpose());
  der.segment(impar + initw_2, nv_ * nh_) =
      Eigen::Map<VectorType>(wder.data(), nv_ * nh_);

  const int initu_1 = initw_1 + nv_ * nh_;
  const int initu_2 = initw_2 + nv_ * nh_;

  MatrixType uder = 0.5 * (vr + vc) * lnpi_.transpose();
  der.segment(initu_1, nv_ * na_) =
      Eigen::Map<VectorType>(uder.data(), nv_ * na_);

  uder = 0.5 * I_ * (vr - vc) * lnpi_.transpose();
  der.segment(impar + initu_2, nv_ * na_) =
      Eigen::Map<VectorType>(uder.data(), nv_ * na_);

  return der;
}

VectorType NdmSpinPhase::GetParameters() {
  VectorType pars(npar_);

  const int impar = (npar_ + na_ * used_) / 2;

  if (useb_) {
    pars.head(nv_) = b1_;
    pars.segment(impar, nv_) = b2_;
  }

  if (useh_) {
    pars.segment(nv_ * useb_, nh_) = h1_;
    pars.segment(impar + nv_ * useb_, nh_) = h2_;
  }

  if (used_) {
    pars.segment(useb_ * nv_ + useh_ * nh_, na_) = d1_;
  }

  const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
  const int initw_2 = nv_ * useb_ + nh_ * useh_;

  pars.segment(initw_1, nv_ * nh_) =
      Eigen::Map<RealVectorType>(W1_.data(), nv_ * nh_);
  pars.segment(impar + initw_2, nv_ * nh_) =
      Eigen::Map<RealVectorType>(W2_.data(), nv_ * nh_);

  const int initu_1 = initw_1 + nv_ * nh_;
  const int initu_2 = initw_2 + nv_ * nh_;

  pars.segment(initu_1, nv_ * na_) =
      Eigen::Map<RealVectorType>(U1_.data(), nv_ * na_);
  pars.segment(impar + initu_2, nv_ * na_) =
      Eigen::Map<RealVectorType>(U2_.data(), nv_ * na_);

  return pars;
}

void NdmSpinPhase::SetParameters(VectorConstRefType pars) {
  const int impar = (npar_ + na_ * used_) / 2;

  if (useb_) {
    b1_ = pars.head(nv_).real();
    b2_ = pars.segment(impar, nv_).real();
  }

  if (useh_) {
    h1_ = pars.segment(useb_ * nv_, nh_).real();
    h2_ = pars.segment(impar + useb_ * nv_, nh_).real();
  }

  if (used_) {
    d1_ = pars.segment(useb_ * nv_ + useh_ * nh_, na_).real();
  }

  const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
  const int initw_2 = nv_ * useb_ + nh_ * useh_;

  VectorType Wpars = pars.segment(initw_1, nv_ * nh_);
  W1_ = Eigen::Map<MatrixType>(Wpars.data(), nv_, nh_).real();

  Wpars = pars.segment(impar + initw_2, nv_ * nh_);
  W2_ = Eigen::Map<MatrixType>(Wpars.data(), nv_, nh_).real();

  const int initu_1 = initw_1 + nv_ * nh_;
  const int initu_2 = initw_2 + nv_ * nh_;

  VectorType Upars = pars.segment(initu_1, nv_ * na_);
  U1_ = Eigen::Map<MatrixType>(Upars.data(), nv_, na_).real();

  Upars = pars.segment(impar + initu_2, nv_ * na_);
  U2_ = Eigen::Map<MatrixType>(Upars.data(), nv_, na_).real();
}

// Value of the logarithm of the wave-function
// using pre-computed look-up tables for efficiency
Complex NdmSpinPhase::LogValSingle(VisibleConstType vr, VisibleConstType vc,
                                   const any& /*lookup*/) {
  auto r1s = SumLogCosh(W1_.transpose() * vr + h1_);
  auto r2s = SumLogCosh(W2_.transpose() * vr + h2_);
  auto c1s = SumLogCosh(W1_.transpose() * vc + h1_);
  auto c2s = SumLogCosh(W2_.transpose() * vc + h2_);

  thetas_a1_ = 0.5 * U1_.transpose() * (vr + vc) + d1_;
  thetas_a2_ = 0.5 * U2_.transpose() * (vr - vc);

  auto lnpis = SumLogCosh(thetas_a1_ + I_ * thetas_a2_);

  auto gamma_1 = 0.5 * (r1s + c1s + (vr + vc).dot(b1_));

  auto gamma_2 = 0.5 * (r2s - c2s + (vr - vc).dot(b2_));

  return (gamma_1 + I_ * gamma_2 + lnpis);
}

void NdmSpinPhase::LogVal(Eigen::Ref<const RowMatrix<double>> vr,
                          Eigen::Ref<const RowMatrix<double>> vc,
                          Eigen::Ref<VectorType> out, const any& /*lup*/) {
  CheckShape(__FUNCTION__, "vr", {vr.rows(), vr.cols()},
             {vc.rows(), NvisiblePhysical()});
  CheckShape(__FUNCTION__, "vc", {vc.rows(), vc.cols()},
             {vr.rows(), NvisiblePhysical()});
  CheckShape(__FUNCTION__, "out", out.size(), vr.rows());

  BatchSize(vr.rows());

  vsum_ = vr + vc;
  vdelta_ = vr - vc;

  thetas_r1_ = (vr * W1_).rowwise() + h1_.transpose();
  thetas_r2_ = (vr * W2_).rowwise() + h2_.transpose();
  thetas_c1_ = (vc * W1_).rowwise() + h1_.transpose();
  thetas_c2_ = (vc * W2_).rowwise() + h2_.transpose();
  thetas_a_ =
      (0.5 * (vsum_ * U1_ + I_ * vdelta_ * U2_)).rowwise() + d1_.transpose();

  out.noalias() = (vsum_ * b1_ + I_ * vdelta_ * b2_);

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    out(j) += SumLogCosh(thetas_r1_.row(j));
  }
#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    out(j) += I_ * SumLogCosh(thetas_r2_.row(j));
  }
#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    out(j) += SumLogCosh(thetas_c1_.row(j));
  }
#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    out(j) -= I_ * SumLogCosh(thetas_c2_.row(j));
  }

  // All previous term are multiplied by 0.5
  out = out * 0.5;

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    out(j) += SumLogCosh(thetas_a_.row(j));
  }
}

void NdmSpinPhase::DerLog(Eigen::Ref<const RowMatrix<double>> vr,
                          Eigen::Ref<const RowMatrix<double>> vc,
                          Eigen::Ref<RowMatrix<Complex>> out,
                          const any& /*cache*/) {
  CheckShape(__FUNCTION__, "vr", {vr.rows(), vr.cols()},
             {vc.rows(), NvisiblePhysical()});
  CheckShape(__FUNCTION__, "vc", {vc.rows(), vc.cols()},
             {vr.rows(), NvisiblePhysical()});
  CheckShape(__FUNCTION__, "out", {out.rows(), out.cols()},
             {vr.rows(), Npar()});
  BatchSize(vr.rows());

  vsum_ = vr + vc;
  vdelta_ = vr - vc;

  const int impar = (npar_ + na_ * used_) / 2;

  auto i = Index{0};
  auto i2 = Index{impar};
  if (useb_) {
    out.block(0, i, BatchSize(), nv_) = 0.5 * vsum_;
    out.block(0, impar, BatchSize(), nv_) = I_ * 0.5 * vdelta_;
    i += nv_;
    i2 += nv_;
  }

  thetas_r1_ = ((vr * W1_).rowwise() + h1_.transpose()).array().tanh();
  thetas_r2_ = ((vr * W2_).rowwise() + h2_.transpose()).array().tanh();
  thetas_c1_ = ((vc * W1_).rowwise() + h1_.transpose()).array().tanh();
  thetas_c2_ = ((vc * W2_).rowwise() + h2_.transpose()).array().tanh();
  thetas_a_ =
      ((0.5 * (vsum_ * U1_ + I_ * vdelta_ * U2_)).rowwise() + d1_.transpose())
          .array()
          .tanh();
  if (useh_) {
    out.block(0, i, BatchSize(), nh_) = 0.5 * (thetas_r1_ + thetas_c1_);
    out.block(0, i2, BatchSize(), nh_) = I_ * 0.5 * (thetas_r2_ - thetas_c2_);
    i += nh_;
    i2 += nh_;
  }

  if (used_) {
    out.block(0, i, BatchSize(), na_) = thetas_a_;
    i += na_;
  }

  // TODO: Rewrite all those using tensors

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    Eigen::Map<Eigen::MatrixXcd>{&out(j, i), W1_.rows(), W1_.cols()}.noalias() =
        0.5 * (vr.row(j).transpose() * thetas_r1_.row(j) +
               vc.row(j).transpose() * thetas_c1_.row(j));
  }

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    Eigen::Map<Eigen::MatrixXcd>{&out(j, i2), W1_.rows(), W1_.cols()}
        .noalias() = 0.5 * I_ *
                     (vr.row(j).transpose() * thetas_r2_.row(j) -
                      vc.row(j).transpose() * thetas_c2_.row(j));
  }

  i += nv_ * nh_;
  i2 += nv_ * nh_;

  // TODO: Rewrite all those using tensors

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    Eigen::Map<Eigen::MatrixXcd>{&out(j, i), U1_.rows(), U1_.cols()}.noalias() =
        0.5 * vsum_.row(j).transpose() * thetas_a_.row(j);
  }

#pragma omp parallel for schedule(static)
  for (auto j = Index{0}; j < BatchSize(); ++j) {
    Eigen::Map<Eigen::MatrixXcd>{&out(j, i2), U2_.rows(), U2_.cols()}
        .noalias() = 0.5 * I_ * vdelta_.row(j).transpose() * thetas_a_.row(j);
  }
}

void NdmSpinPhase::Save(const std::string &filename) const {
  json state;
  state["Name"] = "NdmSpinPhase";
  state["Nvisible"] = nv_;
  state["Nhidden"] = nh_;
  state["Nancilla"] = na_;
  state["UseVisibleBias"] = useb_;
  state["UseHiddenBias"] = useh_;
  state["UseAncillaBias"] = used_;
  state["b1"] = b1_;
  state["h1"] = h1_;
  state["d1"] = d1_;
  state["W1"] = W1_;
  state["U1"] = U1_;

  state["b2"] = b2_;
  state["h2"] = h2_;
  state["W2"] = W2_;
  state["U2"] = U2_;
  WriteJsonToFile(state, filename);
}

void NdmSpinPhase::Load(const std::string &filename) {
  auto pars = ReadJsonFromFile(filename);
  std::string name = FieldVal<std::string>(pars, "Name");
  if (name != "NdmSpinPhase") {
    throw InvalidInputError(
        "Error while constructing RbmSpinPhase from input parameters");
  }

  if (FieldExists(pars, "Nvisible")) {
    nv_ = FieldVal<int>(pars, "Nvisible");
  }
  if (nv_ != GetHilbertPhysical().Size()) {
    throw InvalidInputError(
        "Number of visible units is incompatible with given "
        "Hilbert space");
  }

  if (FieldExists(pars, "Nhidden")) {
    nh_ = FieldVal<int>(pars, "Nhidden");
  } else {
    nh_ = nv_ * double(FieldVal<double>(pars, "Alpha"));
  }

  if (FieldExists(pars, "Nancilla")) {
    na_ = FieldVal<int>(pars, "Nancilla");
  } else {
    na_ = nv_ * double(FieldVal<double>(pars, "Beta"));
  }

  useb_ = FieldOrDefaultVal(pars, "UseVisibleBias", true);
  useh_ = FieldOrDefaultVal(pars, "UseHiddenBias", true);
  used_ = FieldOrDefaultVal(pars, "UseAncillaBias", true);

  Init();

  // Loading parameters, if defined in the input
  if (FieldExists(pars, "b1")) {
    b1_ = FieldVal<RealVectorType>(pars, "b1");
    b2_ = FieldVal<RealVectorType>(pars, "b2");
  } else {
    b1_.setZero();
    b2_.setZero();
  }

  if (FieldExists(pars, "h1")) {
    h1_ = FieldVal<RealVectorType>(pars, "h1");
    h2_ = FieldVal<RealVectorType>(pars, "h2");
  } else {
    h1_.setZero();
    h2_.setZero();
  }

  if (FieldExists(pars, "d1")) {
    d1_ = FieldVal<RealVectorType>(pars, "d1");
  } else {
    d1_.setZero();
  }

  if (FieldExists(pars, "W1")) {
    W1_ = FieldVal<RealMatrixType>(pars, "W1");
    W2_ = FieldVal<RealMatrixType>(pars, "W2");
  }

  if (FieldExists(pars, "U1")) {
    U1_ = FieldVal<RealMatrixType>(pars, "U1");
    U2_ = FieldVal<RealMatrixType>(pars, "U2");
  }
}

bool NdmSpinPhase::IsHolomorphic() const noexcept { return false; }
};  // namespace netket


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#ifndef PARTICLE_AROUND_UNIFORMLY_ROTATING_ELLIPSOID
#define PARTICLE_AROUND_UNIFORMLY_ROTATING_ELLIPSOID

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"
#include "NAOS/ellipsoidGravitationalAcceleration.hpp"

namespace naos
{

//! particle around ellipsoid integration
/*!
 * integrate the equations of motion for a particle around an ellipsoid. The gravitational accelerations
 * calculated using the ellipsoid gravitational potential model.
 */
void executeParticleAroundEllipsoid( const double alpha,
                                     const double beta,
                                     const double gamma,
                                     const double gravParameter,
                                     std::vector< double > asteroidRotationVector,
                                     std::vector< double > &initialOrbitalElements,
                                     const double initialStepSize,
                                     const double startTime,
                                     const double endTime,
                                     std::ostringstream &outputFilePath,
                                     const int dataSaveIntervals );

//! Trajectory calculation for regolith around an asteroid (modelled as ellipsoid here)
/*!
 * Same as the previous function, except that the initial conditions are now given as a cartesian
 * state. The initial cartesian state should be given in body fixed frame of the asteroid.
 */
void singleRegolithTrajectoryCalculator( const double alpha,
                                         const double beta,
                                         const double gamma,
                                         const double gravParameter,
                                         std::vector< double > asteroidRotationVector,
                                         std::vector< double > &initialCartesianStateVector,
                                         const double initialStepSize,
                                         const double startTime,
                                         const double endTime,
                                         std::ostringstream &outputFilePath,
                                         const int dataSaveIntervals );

} // namespace naos

#endif // PARTICLE_AROUND_UNIFORMLY_ROTATING_ELLIPSOID


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <ext/numeric>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "mpi_kmeans.h"

namespace po = boost::program_options;

static unsigned int count_lines(const std::string& filename) {
	std::ifstream in(filename.c_str());
	if (in.fail()) {
		std::cerr << "count_lines, failed to open \"" << filename
			<< "\"." << std::endl;
		exit(EXIT_FAILURE);
	}
	unsigned int lines = 0;
	std::string line;
	while (in.eof() == false) {
		std::getline(in, line);
		if (line.size() == 0)
			continue;
		lines += 1;
	}
	in.close();
	return (lines);
}

static void write_cluster_centers(const std::string& output_filename,
								  const std::vector<std::vector<double> >& data_CX) {

	std::cout << "Writing cluster centers to \""
			  << output_filename << "\"" << std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				  << "\" for writing." << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(12);
	for (unsigned int m=0; m < data_CX.size(); ++m) {
		for (unsigned int n=0; n < data_CX[m].size(); ++n) {
			wout << (n==0? "":" ") << data_CX[m][n];
		}
		wout << std::endl;
	}
	wout.close();

	return;
}

static void write_cluster_centers(const std::string& output_filename,
								  double *data_CX,
								  unsigned int nof_clusters,
								  unsigned int dims) {

	std::cout << "Writing cluster centers to \""
			  << output_filename << "\"" << std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				  << "\" for writing." << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(12);
	unsigned int cntr = 0;
	for (unsigned int m=0; m < nof_clusters; ++m) {
		for (unsigned int n=0; n < dims; ++n) {
			wout << (n==0? "":" ") << data_CX[cntr];
			cntr += 1;
		}
		wout << std::endl;
	}
	wout.close();

	return;
}

static int read_problem_data(const std::string& train_filename,
							  std::vector<std::vector<double> >& data_X) {
	data_X.clear();

	std::ifstream in(train_filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \""
				  << train_filename << "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	unsigned int ndims = 0;
	while (in.eof() == false) {
		std::getline(in,line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ")+1);

		std::vector<double> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			double value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		data_X.push_back(current_data);
	}
	in.close();

	if (data_X.size() == 0) {
		std::cerr << "No points read from file \"" << train_filename
				  << "\"" << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	return(data_X.size());

}

static int read_problem_data(const std::string& train_filename,
							  double *data_X) {

	std::ifstream in(train_filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \""
				  << train_filename << "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	unsigned int nof_points = count_lines(train_filename);
	if (nof_points == 0) {
		std::cerr << "No points read from file \"" << train_filename
				  << "\"" << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	unsigned int ndims = 0;
	unsigned int cntr = 0;
	while (in.eof() == false) {
		std::getline(in,line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ")+1);

		std::vector<double> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			double value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		if (data_X==NULL)
			data_X = (double *)malloc(nof_points * ndims * sizeof(double));

		for (unsigned int i=0; i<ndims; ++i) {
			data_X[cntr] = current_data[i];
			cntr += 1;
		}
	}
	in.close();

	return(nof_points);

}

int main(int argc, char* argv[]) {

	std::string train_filename;
	std::string weight_filename;
	std::string output_filename;
	int nof_clusters;
	int nof_restarts;
	int maxiter;

	// Set Program options
	po::options_description generic("Generic Options");
	generic.add_options()
		("help","Produce help message")
		("verbose","Verbose output")
		;

	po::options_description input_options("Input/Output Options");
	input_options.add_options()
		("data",po::value<std::string>
		 (&train_filename)->default_value("data.txt"),
		 "Training file, one datum per line")
		("output",po::value<std::string>
		 (&output_filename)->default_value("output.txt"),
		 "Output file, one cluster center per line")
		("weights",po::value<std::string>
		 (&weight_filename)->default_value(""),
		 "Weighting of exmaples, one number per line")
		;

	po::options_description kmeans_options("K-Means Options");
	kmeans_options.add_options()
		("k",po::value<int>(&nof_clusters)->default_value(100),
		 "Number of clusters to generate")
		("restarts",po::value<int>(&nof_restarts)->default_value(0),
		 "Number of K-Means restarts. (0: single run)")
		("maxiter",po::value<int>(&maxiter)->default_value(0),
		 "Maximum number of K-Means iterations. (0: infinity)")
		;

	po::options_description all_options;
	all_options.add(generic).add(input_options).add(kmeans_options);
	po::variables_map vm;
	po::store(po::command_line_parser(argc,argv).options(all_options).run(), vm);
	po::notify(vm);

	bool verbose = vm.count("verbose");

	if (vm.count("help")) {
		std::cerr << "K-Means clustering" << std::endl;
		std::cerr << all_options << std::endl;
		std::cerr << std::endl;
		std::cerr << "Example:" << std::endl;
		std::cerr << "  mpi_kmeans --k 2 --data example.txt --output clusters.txt" << std::endl;
		exit(EXIT_SUCCESS);
	}

	// read in the problem
	std::cout << "Training file: " << train_filename << std::endl;
	std::vector<std::vector<double> > data_X; // so far kmeans does not support std::<vector>
	int nof_points = read_problem_data(train_filename,data_X);
	assert(nof_points>0);

	unsigned int dims = data_X[0].size();
	assert(dims>0);

	// convert points to double*
	double *X = (double *)malloc(nof_points * dims * sizeof(double));
	unsigned int cntr = 0;
	for (unsigned int m=0; m < data_X.size() ; ++m) {
		for (unsigned int n=0; n < data_X[m].size() ; ++n) {
			X[cntr] = data_X[m][n];
			cntr += 1;
		}
		data_X[m].clear();
	}
	data_X.clear();

	// read in weighting
	double *W = NULL;
	if (weight_filename != "") {
		std::cout << "Weighting file: " << weight_filename << std::endl;
		std::vector<std::vector<double> > data_W; // so far kmeans does not support std::<vector>
		int nof_points_wfile = read_problem_data(weight_filename,data_W);
		assert(nof_points_wfile == nof_points);

		unsigned int dims_wfile = data_W[0].size();
		assert(dims_wfile==1);

		// convert points to double*
		W = (double *)malloc(nof_points * sizeof(double));
		for (unsigned int m=0; m < data_W.size() ; ++m)
			W[m] = data_W[m][0];
		data_W.clear();

	}

	// start K-Means
	std::cout << "Starting Kmeans ..." << std::endl;
	std::cout << " ... with " << nof_points << " training points " <<std::endl;
	std::cout << " ... for " << nof_clusters << " clusters " <<std::endl;

	unsigned int *assignment = (unsigned int *)malloc(nof_points * sizeof(unsigned int));
	double *CX = (double *) calloc(nof_clusters * dims, sizeof(double));
	double sse = kmeans(CX, X, W, assignment, dims, nof_points, nof_clusters, maxiter, nof_restarts);
	free(X);
	assert(CX);

	std::cout << "Done!" << std::endl;
	std::cout << "Sum of Squared Error : " << sse << std::endl;

	// write the clusters
	// write_cluster_centers(output_filename,data_X);
	write_cluster_centers(output_filename,CX,nof_clusters,dims);

	// done
	exit(EXIT_SUCCESS);
}