#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 2019,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <iostream>

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/sample-models.hpp>

#include <pinocchio/multibody/liegroup/liegroup.hpp>

#define BOOST_TEST_MODULE features
#include <boost/test/unit_test.hpp>

#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-generic.hh>
#include <sot/core/feature-pose.hh>
#include <sot/core/gain-adaptive.hh>
#include <sot/core/sot.hh>
#include <sot/core/task.hh>

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;

#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),                            \
                      "check " #Va ".isApprox(" #Vb ") failed "                \
                      "[\n"                                                    \
                          << (Va).transpose() << "\n!=\n"                      \
                          << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),                            \
                      "check " #Va ".isApprox(" #Vb ") failed "                \
                      "[\n"                                                    \
                          << (Va) << "\n!=\n"                                  \
                          << (Vb) << "\n]")

class FeatureTestBase {
public:
  Task task_;
  int time_;
  dynamicgraph::Vector expectedTaskOutput_;

  FeatureTestBase(unsigned dim, const std::string &name)
      : task_("task" + name), time_(0) {
    expectedTaskOutput_.resize(dim);
  }

  virtual void init() {
    task_.addFeature(featureAbstract());
    task_.setWithDerivative(true);
  }

  void computeExpectedTaskOutput(const Vector &error,
                                 const Vector &errorDrift) {
    double gain = task_.controlGainSIN;
    expectedTaskOutput_ = -gain * error - errorDrift;
  }

  template <typename LG_t>
  void computeExpectedTaskOutput(const Vector &s, const Vector &sdes,
                                 const Vector &sDesDot, const LG_t &lg) {
    Vector s_sd(lg.nv());
    lg.difference(sdes, s, s_sd);

    Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(),
                                                                    lg.nv());
    lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);

    computeExpectedTaskOutput(s_sd, Jminus * sDesDot);
  }

  void checkTaskOutput() {
    BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(),
                        expectedTaskOutput_.size());
    Vector taskOutput(expectedTaskOutput_.size());
    for (int i = 0; i < expectedTaskOutput_.size(); ++i)
      taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
    EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
  }

  void setGain(double gain) {
    task_.controlGainSIN = gain;
    task_.controlGainSIN.access(time_);
    task_.controlGainSIN.setReady();
  }

  template <typename SignalType, typename ValueType>
  void setSignal(SignalType &sig, const ValueType &v) {
    sig = v;
    sig.access(time_);
    sig.setReady();
  }

  virtual FeatureAbstract &featureAbstract() = 0;

  virtual void setInputs() = 0;

  virtual void printInputs() {}

  virtual void recompute() {
    task_.taskSOUT.recompute(time_);

    // Check that recomputing went fine.
    FeatureAbstract &f(featureAbstract());
    BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, task_.errorTimeDerivativeSOUT.getTime());

    // TODO check that the output is correct
    // computeExpectedTaskOutput (s - sd, - vd);
  }

  virtual void printOutputs() {}

  virtual void checkValue() {
    time_++;
    setInputs();
    printInputs();
    recompute();
    printOutputs();
  }
};

class TestFeatureGeneric : public FeatureTestBase {
public:
  FeatureGeneric feature_, featureDes_;
  int dim_;

  TestFeatureGeneric(unsigned dim, const std::string &name)
      : FeatureTestBase(dim, name), feature_("feature" + name),
        featureDes_("featureDes" + name), dim_(dim) {
    feature_.setReference(&featureDes_);
    feature_.selectionSIN = Flags(true);

    dynamicgraph::Matrix Jq(dim, dim);
    Jq.setIdentity();
    feature_.jacobianSIN.setReference(&Jq);

    init();
  }

  FeatureAbstract &featureAbstract() { return feature_; }

  void setInputs() {
    dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
    double gain;

    switch (time_) {
    case 0:
      BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
      s.setZero();
      sd.setZero();
      vd.setZero();
      gain = 0.0;
      break;
    case 1:
      BOOST_TEST_MESSAGE(" ----- Test Position -----");
      s.setZero();
      sd.setConstant(2.);
      vd.setZero();
      gain = 1.0;
      break;
    case 2:
      BOOST_TEST_MESSAGE(" ----- Test both -----");
      s.setZero();
      sd.setConstant(2.);
      vd.setConstant(1.);
      gain = 3.0;
      break;
    default:
      s.setRandom();
      sd.setRandom();
      vd.setRandom();
      gain = 1.0;
    }

    feature_.errorSIN = s;
    feature_.errorSIN.access(time_);
    feature_.errorSIN.setReady();

    featureDes_.errorSIN = sd;
    featureDes_.errorSIN.access(time_);
    featureDes_.errorSIN.setReady();

    featureDes_.errordotSIN = vd;
    featureDes_.errordotSIN.access(time_);
    featureDes_.errordotSIN.setReady();

    setGain(gain);
  }

  void printInputs() {
    BOOST_TEST_MESSAGE("----- inputs -----");
    BOOST_TEST_MESSAGE(
        "feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE(
        "featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE("featureDes_.errordotSIN: "
                       << featureDes_.errordotSIN(time_).transpose());
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
  }

  void recompute() {
    FeatureTestBase::recompute();

    dynamicgraph::Vector s = feature_.errorSIN;
    dynamicgraph::Vector sd = featureDes_.errorSIN;
    dynamicgraph::Vector vd = featureDes_.errordotSIN;

    computeExpectedTaskOutput(s - sd, -vd);

    checkTaskOutput();
  }

  void printOutputs() {
    BOOST_TEST_MESSAGE("----- output -----");
    BOOST_TEST_MESSAGE("time: " << time_);
    BOOST_TEST_MESSAGE(
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
    BOOST_TEST_MESSAGE(
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
    // task_.errorTimeDerivativeSOUT(time_));
    BOOST_TEST_MESSAGE(
        "Expected task output: " << expectedTaskOutput_.transpose());
  }
};

BOOST_AUTO_TEST_SUITE(feature_generic)

BOOST_AUTO_TEST_CASE(check_value) {
  std::string srobot("test");
  unsigned int dim = 6;

  TestFeatureGeneric testFeatureGeneric(dim, srobot);

  for (int i = 0; i < 10; ++i)
    testFeatureGeneric.checkValue();
}

BOOST_AUTO_TEST_SUITE_END() // feature_generic

MatrixHomogeneous randomM() {
  MatrixHomogeneous M;
  M.translation().setRandom();
  Eigen::Vector3d w(Eigen::Vector3d::Random());
  M.linear() = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix();
  return M;
}

typedef pinocchio::SE3 SE3;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
typedef pinocchio::CartesianProductOperation<
    pinocchio::VectorSpaceOperationTpl<3, double>,
    pinocchio::SpecialOrthogonalOperationTpl<3, double> >
    R3xSO3_t;
template <Representation_t representation> struct LG_t {
  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
                                    R3xSO3_t>::type type;
};

Vector7 toVector(const pinocchio::SE3 &M) {
  Vector7 v;
  v.head<3>() = M.translation();
  QuaternionMap(v.tail<4>().data()) = M.rotation();
  return v;
}

Vector toVector(const std::vector<MultiBound> &in) {
  Vector out(in.size());
  for (int i = 0; i < (int)in.size(); ++i)
    out[i] = in[i].getSingleBound();
  return out;
}

template <Representation_t representation>
class TestFeaturePose : public FeatureTestBase {
public:
  typedef typename LG_t<representation>::type LieGroup_t;
  FeaturePose<representation> feature_;
  bool relative_;
  pinocchio::Model model_;
  pinocchio::Data data_;
  pinocchio::JointIndex ja_, jb_;
  pinocchio::FrameIndex fa_, fb_;

  TestFeaturePose(bool relative, const std::string &name)
      : FeatureTestBase(6, name), feature_("feature" + name),
        relative_(relative), data_(model_) {
    pinocchio::buildModels::humanoid(model_, true); // use freeflyer
    model_.lowerPositionLimit.head<3>().setConstant(-1.);
    model_.upperPositionLimit.head<3>().setConstant(1.);
    jb_ = model_.getJointId("rarm_wrist2_joint");
    fb_ = model_.addFrame(pinocchio::Model::Frame(
        "frame_b", jb_,
        model_.getFrameId("rarm_wrist2_joint", pinocchio::JOINT),
        SE3::Identity(), pinocchio::OP_FRAME));
    if (relative) {
      ja_ = model_.getJointId("larm_wrist2_joint");
      fa_ = model_.addFrame(pinocchio::Model::Frame(
          "frame_a", ja_,
          model_.getFrameId("larm_wrist2_joint", pinocchio::JOINT),
          SE3::Identity(), pinocchio::OP_FRAME));
    } else {
      ja_ = 0;
      fa_ = model_.addFrame(pinocchio::Model::Frame(
          "frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME));
    }
    data_ = pinocchio::Data(model_);

    init();

    setJointFrame();
  }

  void _setFrame() {
    setSignal(
        feature_.jaMfa,
        MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix()));
    setSignal(
        feature_.jbMfb,
        MatrixHomogeneous(model_.frames[fb_].placement.toHomogeneousMatrix()));
  }

  void setJointFrame() {
    model_.frames[fa_].placement.setIdentity();
    model_.frames[fb_].placement.setIdentity();
    _setFrame();
  }

  void setRandomFrame() {
    model_.frames[fa_].placement.setRandom();
    model_.frames[fb_].placement.setRandom();
    _setFrame();
  }

  FeatureAbstract &featureAbstract() { return feature_; }

  void setInputs() {
    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    // Desired
    setSignal(feature_.faMfbDes, randomM());
    setSignal(feature_.faNufafbDes, Vector::Random(6));

    double gain = 0;
    // if (time_ % 5 != 0)
    // gain = 2 * (double)rand() / RAND_MAX;
    if (time_ % 2 != 0)
      gain = 1;
    setGain(gain);
  }

  void printInputs() {
    BOOST_TEST_MESSAGE("----- inputs -----");
    // BOOST_TEST_MESSAGE("feature_.errorSIN: " <<
    // feature_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
  }

  void recompute() {
    FeatureTestBase::recompute();

    const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_],
              faMfb(feature_.faMfb.accessCopy().matrix()),
              faMfbDes(feature_.faMfbDes.accessCopy().matrix());
    const Vector &nu(feature_.faNufafbDes.accessCopy());

    computeExpectedTaskOutput(
        toVector(oMfa.inverse() * oMfb), toVector(faMfbDes),
        (boost::is_same<LieGroup_t, SE3_t>::value
             ? faMfbDes.toActionMatrixInverse() * nu
             : (Vector6d() << nu.head<3>() -
                                  faMfb.translation().cross(nu.tail<3>()),
                faMfbDes.rotation().transpose() * nu.tail<3>())
                   .finished()),
        LieGroup_t());

    checkTaskOutput();
  }

  void printOutputs() {
    BOOST_TEST_MESSAGE("----- output -----");
    BOOST_TEST_MESSAGE("time: " << time_);
    BOOST_TEST_MESSAGE(
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
    BOOST_TEST_MESSAGE(
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
    // task_.errorTimeDerivativeSOUT(time_));
    BOOST_TEST_MESSAGE(
        "Expected task output: " << expectedTaskOutput_.transpose());
  }

  virtual void checkJacobian() {
    time_++;
    // We want to check that e (q+dq, t) ~ e(q, t) + de/dq(q,t) dq

    setGain(1.);

    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    // Desired
    setSignal(feature_.faMfbDes, randomM());

    // Get task jacobian
    Vector e_q = task_.errorSOUT.access(time_);
    J = task_.jacobianSOUT.access(time_);

    Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "",
                                "[", "]\n");
    Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
                                "[", "]\n");

    Vector qdot(Vector::Zero(model_.nv));
    double eps = 1e-6;
    BOOST_TEST_MESSAGE(
        data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt)
        << data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt)
        << model_.frames[fa_].placement.toHomogeneousMatrix().format(
               PyMatrixFmt)
        << model_.frames[fb_].placement.toHomogeneousMatrix().format(
               PyMatrixFmt)
        << J.format(PyMatrixFmt));

    for (int i = 0; i < model_.nv; ++i) {
      time_++;
      qdot(i) = eps;

      // Update pose signals
      Vector q_qdot = pinocchio::integrate(model_, q, qdot);
      pinocchio::framesForwardKinematics(model_, data_, q_qdot);
      setSignal(feature_.oMjb,
                MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
      if (relative_) {
        setSignal(feature_.oMja,
                  MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
      }

      // Recompute output and check finite diff
      Vector e_q_dq = task_.errorSOUT.access(time_);
      BOOST_CHECK_MESSAGE(
          (((e_q_dq - e_q) / eps) - J.col(i)).maxCoeff() < 1e-4,
          "Jacobian col " << i << " does not match finite difference.\n"
                          << ((e_q_dq - e_q) / eps).format(PyVectorFmt) << '\n'
                          << J.col(i).format(PyVectorFmt) << '\n');

      qdot(i) = 0.;
    }
    time_++;
  }

  virtual void checkFeedForward() {
    setGain(0.);
    // random config
    // set inputs
    // compute e = task_.taskSOUT and J = task_.jacobianSOUT
    // check that e (q + eps*qdot) - e (q) ~= eps * J * qdot
    // compute qdot = J^+ * e
    // check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faNufafbDes

    time_++;
    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    Matrix faJfafb;
    J.setZero();
    pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J);
    faJfafb = faMfb.toActionMatrix() * J;
    J.setZero();
    pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J);
    faJfafb -= J;

    // Desired
    setSignal(feature_.faMfbDes, randomM());

    // Get task jacobian
    task_.jacobianSOUT.recompute(time_);
    J = task_.jacobianSOUT.accessCopy();
    Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Vector faNufafbDes(Vector::Zero(6));
    double eps = 1e-6;
    for (int i = 0; i < 6; ++i) {
      time_++;
      faNufafbDes(i) = 1.;
      setSignal(feature_.faNufafbDes, faNufafbDes);
      task_.taskSOUT.recompute(time_);

      Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy()));

      Vector faNufafb = faJfafb * qdot;
      EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps);

      faNufafbDes(i) = 0.;
    }
    time_++;
  }
};

template <typename TestClass>
void runTest(TestClass &runner, int N = 2)
// int N = 10)
{
  for (int i = 0; i < N; ++i)
    runner.checkValue();
  for (int i = 0; i < N; ++i)
    runner.checkJacobian();
  for (int i = 0; i < N; ++i)
    runner.checkFeedForward();
}

BOOST_AUTO_TEST_SUITE(feature_pose)

template <Representation_t representation>
void feature_pose_absolute_tpl(const std::string &repr) {
  BOOST_TEST_MESSAGE("absolute " << repr);
  TestFeaturePose<representation> testAbsolute(false, "abs" + repr);
  testAbsolute.setJointFrame();
  runTest(testAbsolute);

  testAbsolute.setRandomFrame();
  runTest(testAbsolute);
}

BOOST_AUTO_TEST_SUITE(absolute)

BOOST_AUTO_TEST_CASE(r3xso3) {
  feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3");
}

BOOST_AUTO_TEST_CASE(se3) {
  feature_pose_absolute_tpl<SE3Representation>("SE3");
}

BOOST_AUTO_TEST_SUITE_END() // absolute

template <Representation_t representation>
void feature_pose_relative_tpl(const std::string &repr) {
  BOOST_TEST_MESSAGE("relative " << repr);
  TestFeaturePose<representation> testRelative(true, "rel" + repr);
  runTest(testRelative);

  testRelative.setRandomFrame();
  runTest(testRelative);
}

BOOST_AUTO_TEST_SUITE(relative)

BOOST_AUTO_TEST_CASE(r3xso3) {
  feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3");
}

BOOST_AUTO_TEST_CASE(se3) {
  feature_pose_relative_tpl<SE3Representation>("SE3");
}

BOOST_AUTO_TEST_SUITE_END() // relative

BOOST_AUTO_TEST_SUITE_END() // feature_pose


#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>

#include "base/eigen2hdf.hpp"
#include "fft/fft2.hpp"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

using namespace std;

TEST(base, rca)
{
  for (int j = 0; j < 9; ++j) {
    RidgeletFrame rf(j, j, 1, 1);

    typedef RidgeletCellArray<> rca_t;

    rca_t rca(rf);
    auto& vcoeffs = rca.coeffs();
    RCLinearize rcl(rf);
  }
}


#include <complex>
#include <exception>
#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <vector>
#include <range/v3/all.hpp>
#include <cstdint>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_set.hpp>
#include <../aoc2020/matrix.h>
#include <../aoc2020/combinations_algo.h>
namespace icl = boost::icl;
typedef FixedMatrix<bool, 1000> Mat;
typedef icl::interval<int>::type Interval;
class Fab
{
  Mat m_;
  int value_;
  int id_;

public:
  int count() const
  {
    return value_;
  }
  int id() const
  {
    return id_;
  }
  friend std::istream &operator>>(std::istream &is, Fab &fab)
  {
    std::vector<std::tuple<int, Interval, Interval>> interval;
    int top, left, h, w;
    int i = 0;
    while (is >> top >> left >> h >> w) {
      icl::interval<int>::type p(top, top + h);
      icl::interval<int>::type pb(left, left + w);
      interval.emplace_back(i, p, pb);
      ++i;
    }
    std::vector<bool> ids(interval.size());
    for_each_combination(interval.begin(), interval.begin() + 2, interval.end(), [&fab, &ids](auto b, auto e) {
      auto [aid, afirst, asecond] = *b;
      auto [bid, bfirst, bsecond] = *(b + 1);

      auto atop = afirst.lower();
      auto abottem = afirst.upper();
      auto btop = bfirst.lower();
      auto bbottem = bfirst.upper();
      auto aleft = asecond.lower();
      auto aright = asecond.upper();
      auto bleft = bsecond.lower();
      auto bright = bsecond.upper();
      for (auto i = std::max(atop, btop); i < std::min(abottem, bbottem); ++i) {
        for (auto j = std::max(aleft, bleft); j < std::min(aright, bright); ++j) {
          fab.m_(i, j) = true;
          ids[aid] = true;
          ids[bid] = true;
        }
      }
      return false;
    });
    fab.value_ = ranges::count(fab.m_.array(), true);
    fab.id_ = ranges::find(ids, false) - ids.begin();
    return is;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    Fab fab;
    ifs >> fab;
    std::cout << fab.count() << std::endl;
    std::cout << fab.id() + 1 << std::endl;
    return 0;
  }
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_POW_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_POW_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/pow_expander.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/assert.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/is_not_nan.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/negif.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          , bd::constant_< bd::scalar_< bd::uint_<A1> > >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0 const& a0, A1) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::constant_< bd::scalar_< bd::int_<A1> > >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0 const& a0, A1) const BOOST_NOEXCEPT
    {
      return eval(a0, boost::mpl::bool_<(A1::value >= 0)>());
    }

    BOOST_FORCEINLINE result_type eval( A0 const& a0, boost::mpl::true_) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }

    BOOST_FORCEINLINE result_type eval( A0 const& a0, boost::mpl::false_) const BOOST_NOEXCEPT
    {
      return pow_expander<-A1::value>::call(rec(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::uint_<A1> >
                          )
  {
    using result_type = A0;

    A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      A0 base = a0;
      A1 exp = a1;

      result_type result = One<result_type>();
      while(exp)
      {
        if(is_odd(exp))
          result *= base;
        exp >>= 1;
        base = sqr(base);
      }

      return result;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::integer_<A0> >
                          , bd::generic_< bd::int_<A1> >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( boost::simd::assert_all(a1 >= 0), "integral pow with signed exponent" );

      using u_t =  bd::as_integer_t<A1, unsigned>;
      return pow(a0, bitwise_cast<u_t>(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::int_<A1> >
                          )
  {
    using result_type = A0;

    A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      using u_t =  bd::as_integer_t<A1, unsigned>;
      auto ltza1 = is_ltz(a1);
      A0 p = pow(a0, bitwise_cast<u_t>(negif(ltza1, a1)));
      return if_else(ltza1, rec(p), p);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , boost::simd::assert_tag
                           )
  {
    BOOST_FORCEINLINE A0 operator() ( A0  const& a0, A0 const&  a1
                                    , assert_tag const& ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(bs::assert_all((is_positive(a0)&&is_not_nan(a0)) || is_flint(a1)),
                       "pow(a0, a1, assert_) cannot produce complex result." );
      return  pow(a0, a1);
    }
  };

} } }

#endif