#include <Eigen/Core>

#include <geos/geom.h>
#include <geos/opBuffer.h>
#include <geos/opDistance.h>

#include <Pita/Node.hpp>
#include <Pita/Context.hpp>
#include <Pita/Vectorizer.hpp>

namespace cgl
{
	struct Color
	{
		int r = 0, g = 0, b = 0;

		std::string toString()const
		{
			std::stringstream ss;
			ss << "rgb(" << r << ", " << g << ", " << b << ")";
			return ss.str();
		}
	};

	class PitaGeometry
	{
	public:
		PitaGeometry() = default;

		PitaGeometry(gg::Geometry* shape, const Color& color) :
			shape(shape),
			color(color)
		{}

		gg::Geometry* shape;
		Color color;
	};

	std::vector<gg::Geometry*> GeosFromList(const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform);

	std::vector<gg::Geometry*> GeosFromListPacked(const cgl::PackedList& list, std::shared_ptr<cgl::Context> pEnv, const cgl::TransformPacked& transform);

	std::vector<PitaGeometry> GeosFromListDrawable(const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform);

	std::vector<PitaGeometry> GeosFromRecordDrawable(const cgl::Val& value, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform = cgl::Transform());

	bool ReadDouble(double& output, const std::string& name, const Record& record, std::shared_ptr<Context> environment)
	{
		const auto& values = record.values;

		auto it = values.find(name);
		if (it == values.end())
		{
			return false;
		}
		auto opt = environment->expandOpt(it->second);
		if (!opt)
		{
			return false;
		}
		const Val& value = opt.value();
		if (!IsType<int>(value) && !IsType<double>(value))
		{
			return false;
		}
		output = IsType<int>(value) ? static_cast<double>(As<int>(value)) : As<double>(value);
		return true;
	}

	std::tuple<double, double> ReadVec2Packed(const PackedRecord& record)
	{
		const auto& values = record.values;

		auto itX = values.find("x");
		auto itY = values.find("y");

		return std::tuple<double, double>(AsDouble(itX->second.value), AsDouble(itY->second.value));
	}

	Path ReadPathPacked(const PackedRecord& pathRecord)
	{
		Path resultPath;

		resultPath.cs = std::make_unique<gg::CoordinateArraySequence>();
		auto& cs = resultPath.cs;
		auto& distances = resultPath.distances;

		auto itLine = pathRecord.values.find("line");
		if (itLine == pathRecord.values.end())
			CGL_Error("不正な式です");

		const PackedVal lineVal = itLine->second.value;
		if (auto pointListOpt = AsOpt<PackedList>(lineVal))
		{
			for (const auto pointVal : pointListOpt.value().data)
			{
				if (auto posRecordOpt = AsOpt<PackedRecord>(pointVal.value))
				{
					const auto v = ReadVec2Packed(posRecordOpt.value());
					const double x = std::get<0>(v);
					const double y = std::get<1>(v);
					cs->add(gg::Coordinate(x, y));
				}
				else CGL_Error("不正な式です");
			}

			distances.push_back(0);
			for (int i = 1; i < cs->size(); ++i)
			{
				const double newDistance = cs->getAt(i - 1).distance(cs->getAt(i));
				distances.push_back(distances.back() + newDistance);
			}
		}
		else CGL_Error("不正な式です");

		return std::move(resultPath);
	}

	bool ReadDoublePacked(double& output, const std::string& name, const PackedRecord& record, std::shared_ptr<Context> environment)
	{
		const auto& values = record.values;

		auto it = values.find(name);
		if (it == values.end())
		{
			return false;
		}
		const PackedVal& value = it->second.value;
		if (!IsType<int>(value) && !IsType<double>(value))
		{
			return false;
		}
		output = IsType<int>(value) ? static_cast<double>(As<int>(value)) : As<double>(value);
		return true;
	}

	Transform::Transform(const Record& record, std::shared_ptr<Context> pEnv)
	{
		double px = 0, py = 0;
		double sx = 1, sy = 1;
		double angle = 0;

		for (const auto& member : record.values)
		{
			auto valOpt = AsOpt<Record>(pEnv->expand(member.second));

			if (member.first == "pos" && valOpt)
			{
				ReadDouble(px, "x", valOpt.value(), pEnv);
				ReadDouble(py, "y", valOpt.value(), pEnv);
			}
			else if (member.first == "scale" && valOpt)
			{
				ReadDouble(sx, "x", valOpt.value(), pEnv);
				ReadDouble(sy, "y", valOpt.value(), pEnv);
			}
			else if (member.first == "angle")
			{
				ReadDouble(angle, "angle", record, pEnv);
			}
		}

		init(px, py, sx, sy, angle);
	}

	void Transform::init(double px, double py, double sx, double sy, double angle)
	{
		const double pi = 3.1415926535;
		const double cosTheta = std::cos(pi*angle / 180.0);
		const double sinTheta = std::sin(pi*angle / 180.0);

		mat <<
			sx*cosTheta, -sy*sinTheta, px,
			sx*sinTheta, sy*cosTheta, py,
			0, 0, 1;
	}

	Eigen::Vector2d Transform::product(const Eigen::Vector2d& v)const
	{
		Eigen::Vector3d xs;
		xs << v.x(), v.y(), 1;
		Eigen::Vector3d result = mat * xs;
		Eigen::Vector2d result2d;
		result2d << result.x(), result.y();
		return result2d;
	}

	void Transform::printMat()const
	{
		std::cout << "Matrix(\n";
		for (int y = 0; y < 3; ++y)
		{
			std::cout << "    ";
			for (int x = 0; x < 3; ++x)
			{
				std::cout << mat(y, x) << " ";
			}
			std::cout << "\n";
		}
		std::cout << ")\n";
	}

	TransformPacked::TransformPacked(const PackedRecord& record, std::shared_ptr<Context> pEnv)
	{
		double px = 0, py = 0;
		double sx = 1, sy = 1;
		double angle = 0;

		for (const auto& member : record.values)
		{
			const PackedVal& value = member.second.value;
			const auto valOpt = AsOpt<PackedRecord>(value);

			if (valOpt)
			{
				const PackedRecord& childRecord = valOpt.value();
				if (member.first == "pos")
				{
					ReadDoublePacked(px, "x", childRecord, pEnv);
					ReadDoublePacked(py, "y", childRecord, pEnv);
				}
				else if (member.first == "scale")
				{
					ReadDoublePacked(sx, "x", childRecord, pEnv);
					ReadDoublePacked(sy, "y", childRecord, pEnv);
				}
			}
			else if (member.first == "angle")
			{
				ReadDoublePacked(angle, "angle", record, pEnv);
			}
		}

		init(px, py, sx, sy, angle);
	}

	void TransformPacked::init(double px, double py, double sx, double sy, double angle)
	{
		const double pi = 3.1415926535;
		const double cosTheta = std::cos(pi*angle / 180.0);
		const double sinTheta = std::sin(pi*angle / 180.0);

		mat <<
			sx*cosTheta, -sy*sinTheta, px,
			sx*sinTheta, sy*cosTheta, py,
			0, 0, 1;
	}

	Eigen::Vector2d TransformPacked::product(const Eigen::Vector2d& v)const
	{
		Eigen::Vector3d xs;
		xs << v.x(), v.y(), 1;
		Eigen::Vector3d result = mat * xs;
		Eigen::Vector2d result2d;
		result2d << result.x(), result.y();
		return result2d;
	}

	void TransformPacked::printMat()const
	{
		std::cout << "Matrix(\n";
		for (int y = 0; y < 3; ++y)
		{
			std::cout << "    ";
			for (int x = 0; x < 3; ++x)
			{
				std::cout << mat(y, x) << " ";
			}
			std::cout << "\n";
		}
		std::cout << ")\n";
	}

	void BoundingRect::add(const Eigen::Vector2d& v)
	{
		if (v.x() < m_min.x())
		{
			m_min.x() = v.x();
		}
		if (v.y() < m_min.y())
		{
			m_min.y() = v.y();
		}
		if (m_max.x() < v.x())
		{
			m_max.x() = v.x();
		}
		if (m_max.y() < v.y())
		{
			m_max.y() = v.y();
		}
	}

	void BoundingRect::add(const Vector<Eigen::Vector2d>& vs)
	{
		for (const auto& v : vs)
		{
			add(v);
		}
	}

	bool ReadPolygon(Vector<Eigen::Vector2d>& output, const List& vertices, std::shared_ptr<Context> pEnv, const Transform& transform)
	{
		output.clear();

		for (const Address vertex : vertices.data)
		{
			const Val value = pEnv->expand(vertex);

			if (IsType<Record>(value))
			{
				double x = 0, y = 0;
				const Record& pos = As<Record>(value);
				if (!ReadDouble(x, "x", pos, pEnv) || !ReadDouble(y, "y", pos, pEnv))
				{
					return false;
				}
				Eigen::Vector2d v;
				v << x, y;
				output.push_back(transform.product(v));
			}
			else
			{
				return false;
			}
		}

		return true;
	}

	bool ReadColor(Color& output, const Record& record, std::shared_ptr<Context> pEnv, const Transform& transform)
	{
		double r = 0, g = 0, b = 0;

		if (!(
			ReadDouble(r, "r", record, pEnv) &&
			ReadDouble(g, "g", record, pEnv) &&
			ReadDouble(b, "b", record, pEnv)
			))
		{
			return false;
		}

		output.r = r;
		output.g = g;
		output.b = b;

		return true;
	}

	bool ReadPolygonPacked(Vector<Eigen::Vector2d>& output, const PackedList& vertices, std::shared_ptr<Context> pEnv, const TransformPacked& transform)
	{
		output.clear();

		for (const auto& val: vertices.data)
		{
			const PackedVal& value = val.value;

			if (IsType<PackedRecord>(value))
			{
				double x = 0, y = 0;
				const PackedRecord& pos = As<PackedRecord>(value);
				if (!ReadDoublePacked(x, "x", pos, pEnv) || !ReadDoublePacked(y, "y", pos, pEnv))
				{
					return false;
				}
				Eigen::Vector2d v;
				v << x, y;
				output.push_back(transform.product(v));
			}
			else
			{
				return false;
			}
		}

		return true;
	}

	void GetBoundingBoxImpl(BoundingRect& output, const Record& record, std::shared_ptr<Context> pEnv, const Transform& parent)
	{
		const Transform current(record, pEnv);
		const Transform transform = parent * current;

		for (const auto& member : record.values)
		{
			const Val value = pEnv->expand(member.second);

			if ((member.first == "polygon" || member.first == "line") && IsType<List>(value))
			{
				Vector<Eigen::Vector2d> polygon;
				if (ReadPolygon(polygon, As<List>(value), pEnv, transform) && !polygon.empty())
				{
					for (const auto& vertex : polygon)
					{
						output.add(vertex);
					}
				}
			}
			else if (member.first == "polygons" && IsType<List>(value))
			{
				const List& polygons = As<List>(value);
				for (const auto& polygonAddress : polygons.data)
				{
					const Val& polygonVertices = pEnv->expand(polygonAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(polygonVertices), pEnv, transform) && !polygon.empty())
					{
						for (const auto& vertex : polygon)
						{
							output.add(vertex);
						}
					}
				}
			}
			else if (IsType<Record>(value))
			{
				GetBoundingBoxImpl(output, As<Record>(value), pEnv, transform);
			}
			else if (IsType<List>(value))
			{
				GetBoundingBoxImpl(output, As<List>(value), pEnv, transform);
			}
		}
	}

	void GetBoundingBoxImpl(BoundingRect& output, const List& list, std::shared_ptr<Context> pEnv, const Transform& transform)
	{
		for (const Address member : list.data)
		{
			const Val value = pEnv->expand(member);

			if (IsType<Record>(value))
			{
				GetBoundingBoxImpl(output, As<Record>(value), pEnv, transform);
			}
			else if (IsType<List>(value))
			{
				GetBoundingBoxImpl(output, As<List>(value), pEnv, transform);
			}
		}
	}

	boost::optional<BoundingRect> GetBoundingBox(const Val& value, std::shared_ptr<Context> pEnv)
	{
		if (IsType<Record>(value))
		{
			const Record& record = As<Record>(value);
			BoundingRect rect;
			GetBoundingBoxImpl(rect, record, pEnv);
			return rect;
		}

		return boost::none;
	}

	std::string GetGeometryType(gg::Geometry* geometry)
	{
		switch (geometry->getGeometryTypeId())
		{
		case geos::geom::GEOS_POINT:              return "Point";
		case geos::geom::GEOS_LINESTRING:         return "LineString";
		case geos::geom::GEOS_LINEARRING:         return "LinearRing";
		case geos::geom::GEOS_POLYGON:            return "Polygon";
		case geos::geom::GEOS_MULTIPOINT:         return "MultiPoint";
		case geos::geom::GEOS_MULTILINESTRING:    return "MultiLineString";
		case geos::geom::GEOS_MULTIPOLYGON:       return "MultiPolygon";
		case geos::geom::GEOS_GEOMETRYCOLLECTION: return "GeometryCollection";
		}

		return "Unknown";
	}

	gg::Polygon* ToPolygon(const Vector<Eigen::Vector2d>& exterior)
	{
		gg::CoordinateArraySequence pts;

		for (int i = 0; i < exterior.size(); ++i)
		{
			pts.add(gg::Coordinate(exterior[i].x(), exterior[i].y()));
		}

		if (!pts.empty())
		{
			pts.add(pts.front());
		}

		//gg::GeometryFactory::unique_ptr factory = gg::GeometryFactory::create();
		auto factory = gg::GeometryFactory::create();
		return factory->createPolygon(factory->createLinearRing(pts), {});
	}

	gg::LineString* ToLineString(const Vector<Eigen::Vector2d>& exterior)
	{
		gg::CoordinateArraySequence pts;

		for (int i = 0; i < exterior.size(); ++i)
		{
			pts.add(gg::Coordinate(exterior[i].x(), exterior[i].y()));
		}

		auto factory = gg::GeometryFactory::create();
		return factory->createLineString(pts);
	}

	void GeosPolygonsConcat(std::vector<gg::Geometry*>& head, const std::vector<gg::Geometry*>& tail)
	{
		/*if (tail.empty())
		{
			return;
		}

		gg::GeometryFactory::unique_ptr factory = gg::GeometryFactory::create();

		gg::Geometry* pgeometry = factory->createEmptyGeometry();
		for (int i = 0; i < tail.size(); ++i)
		{
			pgeometry = pgeometry->Union(tail[i]);
		}

		for (int i = 0; i < head.size(); ++i)
		{
			pgeometry = pgeometry->Union(head[i]);
		}

		head.clear();
		head.push_back(pgeometry);*/
		head.insert(head.end(), tail.begin(), tail.end());
	}

	void GeosPolygonsConcatDrawable(std::vector<PitaGeometry>& head, const std::vector<PitaGeometry>& tail)
	{
		head.insert(head.end(), tail.begin(), tail.end());
	}

	std::vector<gg::Geometry*> GeosFromRecordImpl(const cgl::Record& record, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& parent)
	{
		const cgl::Transform current(record, pEnv);

		const cgl::Transform transform = parent * current;

		std::vector<gg::Geometry*> currentPolygons;
		std::vector<gg::Geometry*> currentHoles;

		std::vector<gg::Geometry*> currentLines;

		for (const auto& member : record.values)
		{
			const cgl::Val value = pEnv->expand(member.second);

			if (member.first == "polygon" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					//shapes.emplace_back(CGLPolygon(polygon));
					currentPolygons.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "hole" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					//shapes.emplace_back(CGLPolygon(polygon));
					currentHoles.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "polygons" && IsType<List>(value))
			{
				const List& polygons = As<List>(value);
				for (const auto& polygonAddress : polygons.data)
				{
					const Val& polygonVertices = pEnv->expand(polygonAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(polygonVertices), pEnv, transform) && !polygon.empty())
					{
						currentPolygons.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "holes" && IsType<List>(value))
			{
				const List& holes = As<List>(value);
				for (const auto& holeAddress : holes.data)
				{
					const Val& hole = pEnv->expand(holeAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(hole), pEnv, transform) && !polygon.empty())
					{
						currentHoles.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "line" && IsType<List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentLines.push_back(ToLineString(polygon));
				}
			}
			else if (cgl::IsType<cgl::Record>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromRecordImpl(cgl::As<cgl::Record>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromList(cgl::As<cgl::List>(value), pEnv, transform));
			}
		}

		//gg::GeometryFactory::unique_ptr factory = gg::GeometryFactory::create();
		auto factory = gg::GeometryFactory::create();

		if (currentPolygons.empty())
		{
			return currentLines;
		}
		else if (currentHoles.empty())
		{
			currentPolygons.insert(currentPolygons.end(), currentLines.begin(), currentLines.end());
			return currentPolygons;
		}
		else
		{
			for (int s = 0; s < currentPolygons.size(); ++s)
			{
				//std::cout << __FUNCTION__ << " : " << __LINE__ << std::endl;
				gg::Geometry* erodeGeometry = currentPolygons[s];

				for (int d = 0; d < currentHoles.size(); ++d)
				{
					erodeGeometry = erodeGeometry->difference(currentHoles[d]);
					//std::cout << __FUNCTION__ << " : " << __LINE__ << std::endl;

					if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_POLYGON)
					{
					}
					else if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_MULTIPOLYGON)
					{
						currentPolygons.erase(currentPolygons.begin() + s);

						const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(erodeGeometry);
						for (int i = 0; i < polygons->getNumGeometries(); ++i)
						{
							currentPolygons.insert(currentPolygons.begin() + s, polygons->getGeometryN(i)->clone());
						}

						erodeGeometry = currentPolygons[s];
					}
					else
					{
						std::cout << __FUNCTION__ << " Differenceの結果が予期せぬデータ形式" << __LINE__ << std::endl;
					}
				}
				//std::cout << __FUNCTION__ << " : " << __LINE__ << std::endl;
				currentPolygons[s] = erodeGeometry;
			}

			currentPolygons.insert(currentPolygons.end(), currentLines.begin(), currentLines.end());
			return currentPolygons;

			/*gg::MultiPolygon* accumlatedPolygons = factory->createMultiPolygon(currentPolygons);
			gg::MultiPolygon* accumlatedHoles = factory->createMultiPolygon(currentHoles);

			return{ accumlatedPolygons->difference(accumlatedHoles) };*/
		}
	}

	std::vector<gg::Geometry*> GeosFromList(const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform)
	{
		std::vector<gg::Geometry*> currentPolygons;
		for (const cgl::Address member : list.data)
		{
			const cgl::Val value = pEnv->expand(member);

			if (cgl::IsType<cgl::Record>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromRecordImpl(cgl::As<cgl::Record>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromList(cgl::As<cgl::List>(value), pEnv, transform));
			}
		}
		return currentPolygons;
	}

	std::vector<gg::Geometry*> GeosFromRecord(const Val& value, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform)
	{
		if (cgl::IsType<cgl::Record>(value))
		{
			const cgl::Record& record = cgl::As<cgl::Record>(value);
			return GeosFromRecordImpl(record, pEnv, transform);
		}
		if (cgl::IsType<cgl::List>(value))
		{
			const cgl::List& list = cgl::As<cgl::List>(value);
			return GeosFromList(list, pEnv, transform);
		}

		return{};
	}

	std::vector<PitaGeometry> GeosFromRecordImplDrawable(const cgl::Record& record, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& parent)
	{
		const cgl::Transform current(record, pEnv);

		const cgl::Transform transform = parent * current;

		std::vector<PitaGeometry> wholePolygons;

		std::vector<gg::Geometry*> currentPolygons;
		std::vector<gg::Geometry*> currentHoles;

		gg::Geometry* currentLine = nullptr;

		Color currentColor;

		for (const auto& member : record.values)
		{
			const cgl::Val value = pEnv->expand(member.second);

			if (member.first == "polygon" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentPolygons.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "hole" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentHoles.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "polygons" && IsType<List>(value))
			{
				const List& polygons = As<List>(value);
				for (const auto& polygonAddress : polygons.data)
				{
					const Val& polygonVertices = pEnv->expand(polygonAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(polygonVertices), pEnv, transform) && !polygon.empty())
					{
						currentPolygons.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "holes" && IsType<List>(value))
			{
				const List& holes = As<List>(value);
				for (const auto& holeAddress : holes.data)
				{
					const Val& hole = pEnv->expand(holeAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(hole), pEnv, transform) && !polygon.empty())
					{
						currentHoles.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "line" && IsType<List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentLine = ToLineString(polygon);
				}
			}
			else if (member.first == "color" && IsType<Record>(value))
			{
				cgl::ReadColor(currentColor, cgl::As<cgl::Record>(value), pEnv, transform);
			}
			else if (cgl::IsType<cgl::Record>(value))
			{
				GeosPolygonsConcatDrawable(wholePolygons, GeosFromRecordImplDrawable(cgl::As<cgl::Record>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				GeosPolygonsConcatDrawable(wholePolygons, GeosFromListDrawable(cgl::As<cgl::List>(value), pEnv, transform));
			}
		}

		//gg::GeometryFactory::unique_ptr factory = gg::GeometryFactory::create();
		auto factory = gg::GeometryFactory::create();

		if(currentPolygons.empty() && currentLine == nullptr)
		{
			return wholePolygons;
		}
		else if(currentPolygons.empty())
		{
			wholePolygons.emplace_back(currentLine, currentColor);
			return wholePolygons;
		}
		else if(currentHoles.empty())
		{
			for(gg::Geometry* geometry : currentPolygons)
			{
				wholePolygons.emplace_back(geometry, currentColor);
			}

			if(currentLine)
			{
				wholePolygons.emplace_back(currentLine, currentColor);
			}

			return wholePolygons;
		}
		else
		{
			for (int s = 0; s < currentPolygons.size(); ++s)
			{
				gg::Geometry* erodeGeometry = currentPolygons[s];

				for (int d = 0; d < currentHoles.size(); ++d)
				{
					erodeGeometry = erodeGeometry->difference(currentHoles[d]);

					if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_MULTIPOLYGON)
					{
						currentPolygons.erase(currentPolygons.begin() + s);

						const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(erodeGeometry);
						for (int i = 0; i < polygons->getNumGeometries(); ++i)
						{
							currentPolygons.insert(currentPolygons.begin() + s, polygons->getGeometryN(i)->clone());
						}

						erodeGeometry = currentPolygons[s];
					}
					else if (erodeGeometry->getGeometryTypeId() != geos::geom::GEOS_POLYGON
						&& erodeGeometry->getGeometryTypeId() != geos::geom::GEOS_GEOMETRYCOLLECTION)
					{
						std::cout << __FUNCTION__ << " Differenceの結果が予期せぬデータ形式" << __LINE__ << std::endl;
					}
				}

				currentPolygons[s] = erodeGeometry;
			}

			for (gg::Geometry* geometry : currentPolygons)
			{
				wholePolygons.emplace_back(geometry, currentColor);
			}

			if (currentLine)
			{
				wholePolygons.emplace_back(currentLine, currentColor);
			}

			return wholePolygons;
		}
	}

	std::vector<PitaGeometry> GeosFromListDrawable(const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform)
	{
		std::vector<PitaGeometry> currentPolygons;

		for (const cgl::Address member : list.data)
		{
			const cgl::Val value = pEnv->expand(member);

			if (cgl::IsType<cgl::Record>(value))
			{
				GeosPolygonsConcatDrawable(currentPolygons, GeosFromRecordImplDrawable(cgl::As<cgl::Record>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				GeosPolygonsConcatDrawable(currentPolygons, GeosFromListDrawable(cgl::As<cgl::List>(value), pEnv, transform));
			}
		}

		return currentPolygons;
	}

	std::vector<PitaGeometry> GeosFromRecordDrawable(const Val& value, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform)
	{
		if (cgl::IsType<cgl::Record>(value))
		{
			const cgl::Record& record = cgl::As<cgl::Record>(value);
			return GeosFromRecordImplDrawable(record, pEnv, transform);
		}
		if (cgl::IsType<cgl::List>(value))
		{
			const cgl::List& list = cgl::As<cgl::List>(value);
			return GeosFromListDrawable(list, pEnv, transform);
		}

		return{};
	}

	std::vector<gg::Geometry*> GeosFromRecordPackedImpl(const cgl::PackedRecord& record, std::shared_ptr<cgl::Context> pEnv, const cgl::TransformPacked& parent)
	{
		const cgl::TransformPacked current(record, pEnv);

		const cgl::TransformPacked transform = parent * current;

		std::vector<gg::Geometry*> currentPolygons;
		std::vector<gg::Geometry*> currentHoles;

		std::vector<gg::Geometry*> currentLines;

		for (const auto& member : record.values)
		{
			const cgl::PackedVal& value = member.second.value;

			if (member.first == "polygon" && cgl::IsType<cgl::PackedList>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygonPacked(polygon, cgl::As<cgl::PackedList>(value), pEnv, transform) && !polygon.empty())
				{
					currentPolygons.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "hole" && cgl::IsType<cgl::PackedList>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygonPacked(polygon, cgl::As<cgl::PackedList>(value), pEnv, transform) && !polygon.empty())
				{
					currentHoles.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "polygons" && IsType<PackedList>(value))
			{
				const PackedList& polygons = As<PackedList>(value);
				for (const auto& polygonAddress : polygons.data)
				{
					const PackedVal& polygonVertices = polygonAddress.value;

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygonPacked(polygon, As<PackedList>(polygonVertices), pEnv, transform) && !polygon.empty())
					{
						currentPolygons.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "holes" && IsType<PackedList>(value))
			{
				const PackedList& holes = As<PackedList>(value);
				for (const auto& holeAddress : holes.data)
				{
					const PackedVal& hole = holeAddress.value;

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygonPacked(polygon, As<PackedList>(holes), pEnv, transform) && !polygon.empty())
					{
						currentHoles.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "line" && IsType<PackedList>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygonPacked(polygon, cgl::As<cgl::PackedList>(value), pEnv, transform) && !polygon.empty())
				{
					currentLines.push_back(ToLineString(polygon));
				}
			}
			else if (cgl::IsType<cgl::PackedRecord>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromRecordPackedImpl(cgl::As<cgl::PackedRecord>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::PackedList>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromListPacked(cgl::As<cgl::PackedList>(value), pEnv, transform));
			}
		}

		auto factory = gg::GeometryFactory::create();

		if (currentPolygons.empty())
		{
			return currentLines;
		}
		else if (currentHoles.empty())
		{
			currentPolygons.insert(currentPolygons.end(), currentLines.begin(), currentLines.end());
			return currentPolygons;
		}
		else
		{
			for (int s = 0; s < currentPolygons.size(); ++s)
			{
				gg::Geometry* erodeGeometry = currentPolygons[s];

				for (int d = 0; d < currentHoles.size(); ++d)
				{
					erodeGeometry = erodeGeometry->difference(currentHoles[d]);

					if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_POLYGON)
					{
					}
					else if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_MULTIPOLYGON)
					{
						currentPolygons.erase(currentPolygons.begin() + s);

						const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(erodeGeometry);
						for (int i = 0; i < polygons->getNumGeometries(); ++i)
						{
							currentPolygons.insert(currentPolygons.begin() + s, polygons->getGeometryN(i)->clone());
						}

						erodeGeometry = currentPolygons[s];
					}
					else
					{
						std::cout << __FUNCTION__ << " Differenceの結果が予期せぬデータ形式" << __LINE__ << std::endl;
					}
				}

				currentPolygons[s] = erodeGeometry;
			}

			currentPolygons.insert(currentPolygons.end(), currentLines.begin(), currentLines.end());
			return currentPolygons;
		}
	}

	std::vector<gg::Geometry*> GeosFromListPacked(const cgl::PackedList& list, std::shared_ptr<cgl::Context> pEnv, const cgl::TransformPacked& transform)
	{
		std::vector<gg::Geometry*> currentPolygons;
		for (const auto& val : list.data)
		{
			const PackedVal& value = val.value;
			if (cgl::IsType<cgl::PackedRecord>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromRecordPackedImpl(cgl::As<cgl::PackedRecord>(value), pEnv, transform));
			}
			else if (cgl::IsType<cgl::PackedList>(value))
			{
				GeosPolygonsConcat(currentPolygons, GeosFromListPacked(cgl::As<cgl::PackedList>(value), pEnv, transform));
			}
		}
		return currentPolygons;
	}

	std::vector<gg::Geometry*> GeosFromRecordPacked(const PackedVal& value, std::shared_ptr<cgl::Context> pEnv, const cgl::TransformPacked& transform)
	{
		if (cgl::IsType<cgl::PackedRecord>(value))
		{
			return GeosFromRecordPackedImpl(cgl::As<cgl::PackedRecord>(value), pEnv, transform);
		}
		if (cgl::IsType<cgl::PackedList>(value))
		{
			return GeosFromListPacked(cgl::As<cgl::PackedList>(value), pEnv, transform);
		}

		return{};
	}

	Record GetPolygon(const gg::Polygon* poly, std::shared_ptr<cgl::Context> pEnv)
	{
		const auto coord = [&](double x, double y)
		{
			Record record;
			record.append("x", pEnv->makeTemporaryValue(x));
			record.append("y", pEnv->makeTemporaryValue(y));
			return record;
		};

		const auto appendCoord = [&](List& list, double x, double y)
		{
			list.append(pEnv->makeTemporaryValue(coord(x, y)));
		};

		Record result;
		{
			List polygonList;
			const gg::LineString* outer = poly->getExteriorRing();
			//for (int i = static_cast<int>(outer->getNumPoints()) - 1; 0 < i; --i)//始点と終点は同じ座標なので最後だけ飛ばす
			for (int i = 1; i < static_cast<int>(outer->getNumPoints()); ++i)//始点と終点は同じ座標なので最後だけ飛ばす
			{
				const gg::Coordinate& p = outer->getCoordinateN(i);
				appendCoord(polygonList, p.x, p.y);
			}
			result.append("polygon", pEnv->makeTemporaryValue(polygonList));
		}

		{
			List holeList;
			for (size_t i = 0; i < poly->getNumInteriorRing(); ++i)
			{
				List holeVertexList;

				const gg::LineString* hole = poly->getInteriorRingN(i);

				for (int n = static_cast<int>(hole->getNumPoints()) - 1; 0 < n; --n)
					//for (int n = 0; n < hole->getNumPoints(); ++n)
				{
					gg::Point* pp = hole->getPointN(n);
					appendCoord(holeVertexList, pp->getX(), pp->getY());
					//const gg::Coordinate& p = hole->getCoordinateN(n);
					//holePoints.emplace_back(p.x, p.y);
				}

				holeList.append(pEnv->makeTemporaryValue(holeVertexList));
			}
			result.append("holes", pEnv->makeTemporaryValue(List(holeList)));
		}

		return result;
	}

	PackedRecord GetPolygonPacked(const gg::Polygon* poly)
	{
		const auto appendCoord = [&](PackedList& list, double x, double y)
		{
			list.add(MakeRecord("x", x, "y", y));
		};

		PackedRecord result;
		{
			PackedList polygonList;
			const gg::LineString* outer = poly->getExteriorRing();

			for (int i = 1; i < static_cast<int>(outer->getNumPoints()); ++i)//始点と終点は同じ座標なので最後だけ飛ばす
			{
				const gg::Coordinate& p = outer->getCoordinateN(i);
				appendCoord(polygonList, p.x, p.y);
			}

			result.add("polygon", polygonList);
		}

		{
			PackedList holeList;
			for (size_t i = 0; i < poly->getNumInteriorRing(); ++i)
			{
				PackedList holeVertexList;
				const gg::LineString* hole = poly->getInteriorRingN(i);

				for (int n = static_cast<int>(hole->getNumPoints()) - 1; 0 < n; --n)
				{
					gg::Point* pp = hole->getPointN(n);
					appendCoord(holeVertexList, pp->getX(), pp->getY());
				}

				holeList.add(holeVertexList);
			}

			result.add("holes", holeList);
		}

		return result;
	}

	List GetShapesFromGeos(const std::vector<gg::Geometry*>& polygons, std::shared_ptr<cgl::Context> pEnv)
	{
		List resultShapes;

		for (size_t i = 0; i < polygons.size(); ++i)
		{
			const gg::Geometry* shape = polygons[i];

			gg::GeometryTypeId id;
			switch (shape->getGeometryTypeId())
			{
			case geos::geom::GEOS_POINT:
				break;
			case geos::geom::GEOS_LINESTRING:
				break;
			case geos::geom::GEOS_LINEARRING:
				break;
			case geos::geom::GEOS_POLYGON:
			{
				const Record record = GetPolygon(dynamic_cast<const gg::Polygon*>(shape), pEnv);
				resultShapes.append(pEnv->makeTemporaryValue(record));
				break;
			}
			case geos::geom::GEOS_MULTIPOINT:
				break;
			case geos::geom::GEOS_MULTILINESTRING:
				break;
			case geos::geom::GEOS_MULTIPOLYGON:
				break;
			case geos::geom::GEOS_GEOMETRYCOLLECTION:
				break;
			default:
				break;
			}
		}

		return resultShapes;
	}

	PackedList GetPackedShapesFromGeos(const std::vector<gg::Geometry*>& polygons)
	{
		PackedList resultShapes;

		for (size_t i = 0; i < polygons.size(); ++i)
		{
			const gg::Geometry* shape = polygons[i];

			gg::GeometryTypeId id;
			switch (shape->getGeometryTypeId())
			{
			case geos::geom::GEOS_POINT:
				break;
			case geos::geom::GEOS_LINESTRING:
				break;
			case geos::geom::GEOS_LINEARRING:
				break;
			case geos::geom::GEOS_POLYGON:
			{
				resultShapes.add(GetPolygonPacked(dynamic_cast<const gg::Polygon*>(shape)));
				break;
			}
			case geos::geom::GEOS_MULTIPOINT:
				break;
			case geos::geom::GEOS_MULTILINESTRING:
				break;
			case geos::geom::GEOS_MULTIPOLYGON:
				break;
			case geos::geom::GEOS_GEOMETRYCOLLECTION:
				break;
			default:
				break;
			}
		}

		return resultShapes;
	}

	void OutputPolygonsStream(PolygonsStream& ps, const gg::Polygon* polygon)
	{
		{
			std::stringstream ss;

			const gg::LineString* outer = polygon->getExteriorRing();
			const double area = polygon->getEnvelope()->getArea();

			ss << "<polygon fill=\"black\" points=\"";
			for (int i = static_cast<int>(outer->getNumPoints()) - 1; 0 < i; --i)
			{
				const gg::Coordinate& p = outer->getCoordinateN(i);
				ss << p.x << "," << p.y << " ";
			}
			ss << "\"/>\n";

			ps.emplace(area, ss.str());
		}

		for (size_t i = 0; i < polygon->getNumInteriorRing(); ++i)
		{
			std::stringstream ss;

			const gg::LineString* hole = polygon->getInteriorRingN(i);
			const double area = hole->getEnvelope()->getArea();

			ss << "<polygon fill=\"white\" points=\"";
			for (int n = static_cast<int>(hole->getNumPoints()) - 1; 0 < n; --n)
				//for (int n = 0; n < hole->getNumPoints(); ++n)
			{
				gg::Point* p = hole->getPointN(n);
				ss << p->getX() << "," << p->getY() << " ";
			}
			ss << "\"/>\n";

			ps.emplace(area, ss.str());
		}
	}

	void OutputLineStringStream(PolygonsStream& ps, const gg::LineString* lineString)
	{
		std::stringstream ss;

		const double area = lineString->getLength();

		ss << "<polyline stroke=\"rgb(0, 255, 255)\" fill=\"none\" points=\"";
		for (int i = 0; i < lineString->getNumPoints(); ++i)
		{
			const gg::Point* p = lineString->getPointN(i);
			ss << p->getX() << "," << p->getY() << " ";
		}
		ss << "\"/>\n";

		ps.emplace(area, ss.str());
	}

	void OutputPolygonsStreamDrawable(PolygonsStream& ps, const gg::Polygon* polygon, const Color& color)
	{
		{
			std::stringstream ss;

			const gg::LineString* outer = polygon->getExteriorRing();
			const double area = polygon->getEnvelope()->getArea();

			ss << "<polygon fill=\"" << color.toString() << "\" points=\"";
			//ss << "<polygon fill=\"black\" points=\"";
			for (int i = static_cast<int>(outer->getNumPoints()) - 1; 0 < i; --i)
			{
				const gg::Coordinate& p = outer->getCoordinateN(i);
				ss << p.x << "," << p.y << " ";
			}
			ss << "\"/>\n";

			ps.emplace(area, ss.str());
		}

		for (size_t i = 0; i < polygon->getNumInteriorRing(); ++i)
		{
			std::stringstream ss;

			const gg::LineString* hole = polygon->getInteriorRingN(i);
			const double area = hole->getEnvelope()->getArea();

			ss << "<polygon fill=\"white\" points=\"";
			for (int n = static_cast<int>(hole->getNumPoints()) - 1; 0 < n; --n)
				//for (int n = 0; n < hole->getNumPoints(); ++n)
			{
				gg::Point* p = hole->getPointN(n);
				ss << p->getX() << "," << p->getY() << " ";
			}
			ss << "\"/>\n";

			ps.emplace(area, ss.str());
		}
	}

	void OutputLineStringStreamDrawable(PolygonsStream& ps, const gg::LineString* lineString, const Color& color)
	{
		std::stringstream ss;

		const double area = lineString->getLength();

		ss << "<polyline stroke=\"" << color.toString() << "\" fill=\"none\" points=\"";
		//ss << "<polyline stroke=\"rgb(0, 255, 255)\" fill=\"none\" points=\"";
		for (int i = 0; i < lineString->getNumPoints(); ++i)
		{
			const gg::Point* p = lineString->getPointN(i);
			ss << p->getX() << "," << p->getY() << " ";
		}
		ss << "\"/>\n";

		ps.emplace(area, ss.str());
	}

	bool OutputSVG(std::ostream& os, const Val& value, std::shared_ptr<Context> pEnv)
	{
		auto boundingBoxOpt = GetBoundingBox(value, pEnv);
		if (IsType<Record>(value) && boundingBoxOpt)
		{
			const BoundingRect& rect = boundingBoxOpt.value();

			//const auto pos = rect.pos();
			const auto widthXY = rect.width();
			const auto center = rect.center();

			const double width = std::max(widthXY.x(), widthXY.y());
			const double halfWidth = width*0.5;

			const Eigen::Vector2d pos = center - Eigen::Vector2d(halfWidth, halfWidth);

			os << R"(<svg xmlns="http://www.w3.org/2000/svg" width=")" << width << R"(" height=")" << width << R"(" viewBox=")" << pos.x() << " " << pos.y() << " " << width << " " << width << R"(">)" << "\n";

			PolygonsStream ps;
			std::vector<PitaGeometry> geometries = GeosFromRecordDrawable(value, pEnv);
			for (const auto& geometry : geometries)
			{
				if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_POLYGON)
				{
					const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(geometry.shape);
					OutputPolygonsStreamDrawable(ps, polygon, geometry.color);
				}
				else if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_MULTIPOLYGON)
				{
					const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(geometry.shape);
					for (int i = 0; i < polygons->getNumGeometries(); ++i)
					{
						const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(polygons->getGeometryN(i));
						OutputPolygonsStreamDrawable(ps, polygon, geometry.color);
					}
				}
				else if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_LINESTRING)
				{
					const gg::LineString* lineString = dynamic_cast<const gg::LineString*>(geometry.shape);
					OutputLineStringStreamDrawable(ps, lineString, geometry.color);
				}
			}

			/*
			std::vector<gg::Geometry*> geometries = GeosFromRecord(value, pEnv);
			for (gg::Geometry* geometry : geometries)
			{
				if (geometry->getGeometryTypeId() == gg::GeometryTypeId::GEOS_POLYGON)
				{
					const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(geometry);
					OutputPolygonsStream(ps, polygon);
				}
				else if (geometry->getGeometryTypeId() == gg::GeometryTypeId::GEOS_MULTIPOLYGON)
				{
					const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(geometry);
					for (int i = 0; i < polygons->getNumGeometries(); ++i)
					{
						const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(polygons->getGeometryN(i));
						OutputPolygonsStream(ps, polygon);
					}
				}
				else if (geometry->getGeometryTypeId() == gg::GeometryTypeId::GEOS_LINESTRING)
				{
					const gg::LineString* lineString = dynamic_cast<const gg::LineString*>(geometry);
					OutputLineStringStream(ps, lineString);
				}
			}
			*/

			//const Record& record = As<Record>(value);
			//OutputSVGImpl(ps, record, pEnv);
			for (auto it = ps.rbegin(); it != ps.rend(); ++it)
			{
				os << it->second;
			}

			os << "</svg>" << "\n";

			return true;
		}

		return false;
	}

	std::string getIndent(int depth)
	{
		std::string indent;
		const std::string indentStr = "  ";
		for (int i = 0; i < depth; ++i)
		{
			indent += indentStr;
		}
		return indent;
	}

	bool GeosFromList2(std::ostream& os, const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const std::string& name, int depth, const cgl::Transform& transform);

	bool GeosFromRecord2(std::ostream& os, const cgl::Val& value, std::shared_ptr<cgl::Context> pEnv, const std::string& name, int depth, const cgl::Transform& transform = cgl::Transform());

	bool GeosFromRecordImpl2(std::ostream& os, const cgl::Record& record, std::shared_ptr<cgl::Context> pEnv, const std::string& name, int depth, const cgl::Transform& parent)
	{
		const cgl::Transform current(record, pEnv);

		const cgl::Transform transform = parent * current;

		std::vector<PitaGeometry> wholePolygons;

		std::vector<gg::Geometry*> currentPolygons;
		std::vector<gg::Geometry*> currentHoles;

		gg::Geometry* currentLine = nullptr;

		//現時点では実際に描画されるデータを持っているかどうかわからないため、一旦別のストリームに保存しておく
		std::stringstream currentStream;
		currentStream << getIndent(depth) << "<g id=\"" << name << "\" ";

		std::stringstream currentChildStream;

		//Color currentColor;

		bool hasShape = false;

		for (const auto& member : record.values)
		{
			const cgl::Val value = pEnv->expand(member.second);

			if (member.first == "polygon" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentPolygons.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "hole" && cgl::IsType<cgl::List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentHoles.push_back(ToPolygon(polygon));
				}
			}
			else if (member.first == "polygons" && IsType<List>(value))
			{
				const List& polygons = As<List>(value);
				for (const auto& polygonAddress : polygons.data)
				{
					const Val& polygonVertices = pEnv->expand(polygonAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(polygonVertices), pEnv, transform) && !polygon.empty())
					{
						currentPolygons.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "holes" && IsType<List>(value))
			{
				const List& holes = As<List>(value);
				for (const auto& holeAddress : holes.data)
				{
					const Val& hole = pEnv->expand(holeAddress);

					Vector<Eigen::Vector2d> polygon;
					if (ReadPolygon(polygon, As<List>(hole), pEnv, transform) && !polygon.empty())
					{
						currentHoles.push_back(ToPolygon(polygon));
					}
				}
			}
			else if (member.first == "line" && IsType<List>(value))
			{
				cgl::Vector<Eigen::Vector2d> polygon;
				if (cgl::ReadPolygon(polygon, cgl::As<cgl::List>(value), pEnv, transform) && !polygon.empty())
				{
					currentLine = ToLineString(polygon);
				}
			}
			else if (member.first == "fill" && IsType<Record>(value))
			{
				Color currentColor;
				cgl::ReadColor(currentColor, cgl::As<cgl::Record>(value), pEnv, transform);
				currentStream << "fill=\"" << currentColor.toString() << "\" ";
			}
			else if (member.first == "stroke" && IsType<Record>(value))
			{
				Color currentColor;
				cgl::ReadColor(currentColor, cgl::As<cgl::Record>(value), pEnv, transform);
				currentStream << "stroke=\"" << currentColor.toString() << "\" ";
			}
			else if (cgl::IsType<cgl::Record>(value))
			{
				hasShape = GeosFromRecordImpl2(currentChildStream, cgl::As<cgl::Record>(value), pEnv, member.first, depth + 1, transform) || hasShape;
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				hasShape = GeosFromList2(currentChildStream, cgl::As<cgl::List>(value), pEnv, member.first, depth + 1, transform) || hasShape;
			}
		}

		currentStream << ">\n";

		const auto writePolygon = [&depth](std::ostream& os, const gg::Polygon* polygon)
		{
			//穴がない場合ー＞Polygonで描画
			if (polygon->getNumInteriorRing() == 0)
			{
				const gg::LineString* outer = polygon->getExteriorRing();

				os << getIndent(depth+1) << "<polygon " << "points=\"";
				for (int i = 0; i < outer->getNumPoints(); ++i)
				{
					const gg::Coordinate& p = outer->getCoordinateN(i);
					os << p.x << "," << p.y << " ";
				}
				os << "\"/>\n";
			}
			//穴がある場合ー＞Pathで描画
			else
			{
				os << getIndent(depth + 1) << "<path " << "d=\"";

				{
					const gg::LineString* outer = polygon->getExteriorRing();

					if (outer->getNumPoints() != 0)
					{
						const gg::Coordinate& p = outer->getCoordinateN(0);
						os << "M" << p.x << "," << p.y << " ";
					}

					for (int i = 1; i < outer->getNumPoints(); ++i)
					{
						const gg::Coordinate& p = outer->getCoordinateN(i);
						os << "L" << p.x << "," << p.y << " ";
					}
					os << "z ";
				}

				for (size_t i = 0; i < polygon->getNumInteriorRing(); ++i)
				{
					const gg::LineString* hole = polygon->getInteriorRingN(i);

					if (hole->getNumPoints() != 0)
					{
						const gg::Coordinate& p = hole->getCoordinateN(0);
						os << "M" << p.x << "," << p.y << " ";
					}

					for (int n = 1; n < hole->getNumPoints(); ++n)
					{
						gg::Point* p = hole->getPointN(n);
						os << "L" << p->getX() << "," << p->getY() << " ";
					}
					os << "z ";
				}

				os << "\"/>\n";
			}
		};

		const auto writeLine = [&depth](std::ostream& os, const gg::LineString* lineString)
		{
			os << getIndent(depth + 1) << "<polyline " << "fill=\"none\" points=\"";
			//os << getIndent(depth + 1) << "<polyline " << "points=\"";
			for (int i = 0; i < lineString->getNumPoints(); ++i)
			{
				const gg::Point* p = lineString->getPointN(i);
				os << p->getX() << "," << p->getY() << " ";
			}
			os << "\"/>\n";
		};

		const auto writePolygons = [&wholePolygons, &writePolygon, &writeLine](std::ostream& os)->bool
		{
			bool hasShape = false;
			for (const auto& geometry : wholePolygons)
			{
				if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_POLYGON)
				{
					hasShape = true;
					const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(geometry.shape);
					writePolygon(os, polygon);
				}
				else if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_MULTIPOLYGON)
				{
					const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(geometry.shape);
					for (int i = 0; i < polygons->getNumGeometries(); ++i)
					{
						hasShape = true;
						const gg::Polygon* polygon = dynamic_cast<const gg::Polygon*>(polygons->getGeometryN(i));
						writePolygon(os, polygon);
					}
				}
				else if (geometry.shape->getGeometryTypeId() == gg::GeometryTypeId::GEOS_LINESTRING)
				{
					hasShape = true;
					const gg::LineString* lineString = dynamic_cast<const gg::LineString*>(geometry.shape);
					writeLine(os, lineString);
				}
			}
			return hasShape;
		};

		const auto writeWholeData = [&]()
		{
			hasShape = writePolygons(currentStream) || hasShape;
			currentStream << currentChildStream.str();
			currentStream << getIndent(depth) << "</g>\n";

			if (hasShape)
			{
				os << currentStream.str();
			}
		};

		auto factory = gg::GeometryFactory::create();

		if (currentPolygons.empty() && currentLine == nullptr)
		{
			writeWholeData();

			return hasShape;
		}
		else if (currentPolygons.empty())
		{
			//パスの色はどうするか　別で指定する必要がある？
			//図形の境界線も考慮すると、塗りつぶしの色と線の色は別の名前で指定できるようにすべき
			wholePolygons.emplace_back(currentLine, Color());
			writeWholeData();
			return true;
		}
		else if (currentHoles.empty())
		{
			for (gg::Geometry* geometry : currentPolygons)
			{
				wholePolygons.emplace_back(geometry, Color());
			}

			if (currentLine)
			{
				wholePolygons.emplace_back(currentLine, Color());
			}

			writeWholeData();
			return true;
		}
		else
		{
			for (int s = 0; s < currentPolygons.size(); ++s)
			{
				gg::Geometry* erodeGeometry = currentPolygons[s];

				for (int d = 0; d < currentHoles.size(); ++d)
				{
					erodeGeometry = erodeGeometry->difference(currentHoles[d]);

					if (erodeGeometry->getGeometryTypeId() == geos::geom::GEOS_MULTIPOLYGON)
					{
						currentPolygons.erase(currentPolygons.begin() + s);

						const gg::MultiPolygon* polygons = dynamic_cast<const gg::MultiPolygon*>(erodeGeometry);
						for (int i = 0; i < polygons->getNumGeometries(); ++i)
						{
							currentPolygons.insert(currentPolygons.begin() + s, polygons->getGeometryN(i)->clone());
						}

						erodeGeometry = currentPolygons[s];
					}
					else if (erodeGeometry->getGeometryTypeId() != geos::geom::GEOS_POLYGON
						&& erodeGeometry->getGeometryTypeId() != geos::geom::GEOS_GEOMETRYCOLLECTION)
					{
						std::cout << __FUNCTION__ << " Differenceの結果が予期せぬデータ形式" << __LINE__ << std::endl;
					}
				}

				currentPolygons[s] = erodeGeometry;
			}

			for (gg::Geometry* geometry : currentPolygons)
			{
				wholePolygons.emplace_back(geometry, Color());
			}

			if (currentLine)
			{
				wholePolygons.emplace_back(currentLine, Color());
			}

			writeWholeData();
			return true;
		}
	}

	bool GeosFromList2(std::ostream& os, const cgl::List& list, std::shared_ptr<cgl::Context> pEnv, const std::string& name, int depth, const cgl::Transform& transform)
	{
		std::vector<PitaGeometry> currentPolygons;

		bool hasShape = false;
		int i = 0;
		for (const cgl::Address member : list.data)
		{
			const cgl::Val value = pEnv->expand(member);

			std::stringstream currentName;
			currentName << name << "[" << i << "]";

			if (cgl::IsType<cgl::Record>(value))
			{
				hasShape = GeosFromRecordImpl2(os, cgl::As<cgl::Record>(value), pEnv, currentName.str(), depth + 1, transform) || hasShape;
			}
			else if (cgl::IsType<cgl::List>(value))
			{
				hasShape = GeosFromList2(os, cgl::As<cgl::List>(value), pEnv, currentName.str(), depth + 1, transform) || hasShape;
			}

			++i;
		}

		return hasShape;
	}

	bool GeosFromRecord2(std::ostream& os, const Val& value, std::shared_ptr<cgl::Context> pEnv, const std::string& name, int depth, const cgl::Transform& transform)
	{
		if (cgl::IsType<cgl::Record>(value))
		{
			const cgl::Record& record = cgl::As<cgl::Record>(value);
			return GeosFromRecordImpl2(os, record, pEnv, name, depth, transform);
		}
		if (cgl::IsType<cgl::List>(value))
		{
			const cgl::List& list = cgl::As<cgl::List>(value);
			return GeosFromList2(os, list, pEnv, name, depth, transform);
		}

		return{};
	}

	bool OutputSVG2(std::ostream& os, const Val& value, std::shared_ptr<Context> pEnv, const std::string& name)
	{
		auto boundingBoxOpt = GetBoundingBox(value, pEnv);
		if (IsType<Record>(value) && boundingBoxOpt)
		{
			const BoundingRect& rect = boundingBoxOpt.value();

			//const auto pos = rect.pos();
			const auto widthXY = rect.width();
			const auto center = rect.center();

			//const double width = std::max(widthXY.x(), widthXY.y());
			//const double halfWidth = width * 0.5;
			const double width = widthXY.x();
			const double height = widthXY.y();

			const Eigen::Vector2d pos = center - Eigen::Vector2d(width*0.5, height*0.5);

			/*os << R"(<svg xmlns="http://www.w3.org/2000/svg" version="1.2" baseProfile="tiny" width=")" << width << R"(" height=")" << height << R"(" viewBox=")" << pos.x() << " " << pos.y() << " " << width << " " << height
				<< R"(" viewport-fill="black" viewport-fill-opacity="0.1)"  << R"(">)" << "\n";*/
			os << R"(<svg xmlns="http://www.w3.org/2000/svg" width=")" << width << R"(" height=")" << height << R"(" viewBox=")" << pos.x() << " " << pos.y() << " " << width << " " << height << R"(">)" << "\n";

			GeosFromRecord2(os, value, pEnv, name, 0);

			os << "</svg>" << "\n";

			return true;
		}

		return false;
	}

}


///////////////////////////////////////////////////////////////
//  Copyright 2013 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#ifndef BOOST_MP_CPP_INT_LITERALS_HPP
#define BOOST_MP_CPP_INT_LITERALS_HPP

#include <boost/multiprecision/cpp_int/cpp_int_config.hpp>

namespace boost{ namespace multiprecision{

namespace literals{ namespace detail{

template <char> struct hex_value;
template <> struct hex_value<'0'> { static constexpr limb_type value = 0; };
template <> struct hex_value<'1'> { static constexpr limb_type value = 1; };
template <> struct hex_value<'2'> { static constexpr limb_type value = 2; };
template <> struct hex_value<'3'> { static constexpr limb_type value = 3; };
template <> struct hex_value<'4'> { static constexpr limb_type value = 4; };
template <> struct hex_value<'5'> { static constexpr limb_type value = 5; };
template <> struct hex_value<'6'> { static constexpr limb_type value = 6; };
template <> struct hex_value<'7'> { static constexpr limb_type value = 7; };
template <> struct hex_value<'8'> { static constexpr limb_type value = 8; };
template <> struct hex_value<'9'> { static constexpr limb_type value = 9; };
template <> struct hex_value<'a'> { static constexpr limb_type value = 10; };
template <> struct hex_value<'b'> { static constexpr limb_type value = 11; };
template <> struct hex_value<'c'> { static constexpr limb_type value = 12; };
template <> struct hex_value<'d'> { static constexpr limb_type value = 13; };
template <> struct hex_value<'e'> { static constexpr limb_type value = 14; };
template <> struct hex_value<'f'> { static constexpr limb_type value = 15; };
template <> struct hex_value<'A'> { static constexpr limb_type value = 10; };
template <> struct hex_value<'B'> { static constexpr limb_type value = 11; };
template <> struct hex_value<'C'> { static constexpr limb_type value = 12; };
template <> struct hex_value<'D'> { static constexpr limb_type value = 13; };
template <> struct hex_value<'E'> { static constexpr limb_type value = 14; };
template <> struct hex_value<'F'> { static constexpr limb_type value = 15; };

template <class Pack, limb_type value>
struct combine_value_to_pack;
template <limb_type first, limb_type...ARGS, limb_type value>
struct combine_value_to_pack<value_pack<first, ARGS...>, value>
{
   typedef value_pack<first | value, ARGS...> type;
};

template <char NextChar, char...CHARS>
struct pack_values
{
   static constexpr unsigned chars_per_limb = sizeof(limb_type) * CHAR_BIT / 4;
   static constexpr unsigned shift = ((sizeof...(CHARS)) % chars_per_limb) * 4;
   static constexpr limb_type value_to_add = shift ? hex_value<NextChar>::value << shift : hex_value<NextChar>::value;

   typedef typename pack_values<CHARS...>::type recursive_packed_type;
   typedef typename boost::mpl::if_c<shift == 0,
      typename recursive_packed_type::next_type,
      recursive_packed_type>::type pack_type;
   typedef typename combine_value_to_pack<pack_type, value_to_add>::type type;
};
template <char NextChar>
struct pack_values<NextChar>
{
   static constexpr limb_type value_to_add = hex_value<NextChar>::value;

   typedef value_pack<value_to_add> type;
};

template <class T>
struct strip_leading_zeros_from_pack;
template <limb_type...PACK>
struct strip_leading_zeros_from_pack<value_pack<PACK...> >
{
   typedef value_pack<PACK...> type;
};
template <limb_type...PACK>
struct strip_leading_zeros_from_pack<value_pack<0u, PACK...> >
{
   typedef typename strip_leading_zeros_from_pack<value_pack<PACK...> >::type type;
};

template <limb_type v, class PACK>
struct append_value_to_pack;
template <limb_type v, limb_type...PACK>
struct append_value_to_pack<v, value_pack<PACK...> >
{
   typedef value_pack<PACK..., v> type;
};

template <class T>
struct reverse_value_pack;
template <limb_type v, limb_type...VALUES>
struct reverse_value_pack<value_pack<v, VALUES...> >
{
   typedef typename reverse_value_pack<value_pack<VALUES...> >::type lead_values;
   typedef typename append_value_to_pack<v, lead_values>::type type;
};
template <limb_type v>
struct reverse_value_pack<value_pack<v> >
{
   typedef value_pack<v> type;
};
template <>
struct reverse_value_pack<value_pack<> >
{
   typedef value_pack<> type;
};

template <char l1, char l2, char...STR>
struct make_packed_value_from_str
{
   BOOST_STATIC_ASSERT_MSG(l1 == '0', "Multi-precision integer literals must be in hexadecimal notation.");
   BOOST_STATIC_ASSERT_MSG((l2 == 'X') || (l2 == 'x'), "Multi-precision integer literals must be in hexadecimal notation.");
   typedef typename pack_values<STR...>::type packed_type;
   typedef typename strip_leading_zeros_from_pack<packed_type>::type stripped_type;
   typedef typename reverse_value_pack<stripped_type>::type type;
};

template <class Pack, class B>
struct make_backend_from_pack
{
   static constexpr Pack p = {};
   static constexpr B value = p;
};

template <class Pack, class B>
constexpr B make_backend_from_pack<Pack, B>::value;

template <unsigned Digits>
struct signed_cpp_int_literal_result_type
{
   static constexpr unsigned bits = Digits * 4;
   typedef boost::multiprecision::backends::cpp_int_backend<bits, bits, signed_magnitude, unchecked, void> backend_type;
   typedef number<backend_type, et_off> number_type;
};

template <unsigned Digits>
struct unsigned_cpp_int_literal_result_type
{
   static constexpr unsigned bits = Digits * 4;
   typedef boost::multiprecision::backends::cpp_int_backend<bits, bits, unsigned_magnitude, unchecked, void> backend_type;
   typedef number<backend_type, et_off> number_type;
};

}

template <char... STR>
constexpr typename boost::multiprecision::literals::detail::signed_cpp_int_literal_result_type<(sizeof...(STR)) - 2>::number_type operator "" _cppi()
{
   typedef typename boost::multiprecision::literals::detail::make_packed_value_from_str<STR...>::type pt;
   return boost::multiprecision::literals::detail::make_backend_from_pack<pt, typename boost::multiprecision::literals::detail::signed_cpp_int_literal_result_type<(sizeof...(STR)) - 2>::backend_type>::value;
}

template <char... STR>
constexpr typename boost::multiprecision::literals::detail::unsigned_cpp_int_literal_result_type<(sizeof...(STR)) - 2>::number_type operator "" _cppui()
{
   typedef typename boost::multiprecision::literals::detail::make_packed_value_from_str<STR...>::type pt;
   return boost::multiprecision::literals::detail::make_backend_from_pack<pt, typename boost::multiprecision::literals::detail::unsigned_cpp_int_literal_result_type<(sizeof...(STR)) - 2>::backend_type>::value;
}

#define BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(Bits)\
template <char... STR> \
constexpr boost::multiprecision::number<boost::multiprecision::backends::cpp_int_backend<Bits, Bits, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> > operator "" BOOST_JOIN(_cppi, Bits)()\
{\
   typedef typename boost::multiprecision::literals::detail::make_packed_value_from_str<STR...>::type pt;\
   return boost::multiprecision::literals::detail::make_backend_from_pack<\
      pt, \
      boost::multiprecision::backends::cpp_int_backend<Bits, Bits, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> \
   >::value;\
}\
template <char... STR> \
constexpr boost::multiprecision::number<boost::multiprecision::backends::cpp_int_backend<Bits, Bits, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> > operator "" BOOST_JOIN(_cppui, Bits)()\
{\
   typedef typename boost::multiprecision::literals::detail::make_packed_value_from_str<STR...>::type pt;\
   return boost::multiprecision::literals::detail::make_backend_from_pack<\
      pt, \
      boost::multiprecision::backends::cpp_int_backend<Bits, Bits, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void>\
   >::value;\
}\

BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(128)
BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(256)
BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(512)
BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(1024)

}

//
// Overload unary minus operator for constexpr use:
//
template <unsigned MinBits, cpp_int_check_type Checked>
constexpr number<cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>, et_off>
   operator - (const number<cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>, et_off>& a)
{
   return cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>(a.backend(), boost::multiprecision::literals::detail::make_negate_tag());
}
template <unsigned MinBits, cpp_int_check_type Checked>
constexpr number<cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>, et_off>
   operator - (number<cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>, et_off>&& a)
{
   return cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>(static_cast<const number<cpp_int_backend<MinBits, MinBits, signed_magnitude, Checked, void>, et_off>&>(a).backend(), boost::multiprecision::literals::detail::make_negate_tag());
}

}} // namespaces

#endif // BOOST_MP_CPP_INT_CORE_HPP


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 1, 4> test_double_1_4(const Eigen::Matrix<double, 1, 4>& M) { return M; }
void export_double_1_4() { boost::python::def("test_double_1_4", test_double_1_4); }


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_SSE_SSSE3_NEGATE_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_SSE_SSSE3_NEGATE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
#include <boost/simd/ieee/functions/negate.hpp>
#include <boost/simd/ieee/functions/simd/common/negate.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         (negate_, boost::simd::tag::ssse3_,
                       (A0),
                       ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                       ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                      )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return _mm_sign_epi32(a0, a1);  }
  };

  BOOST_DISPATCH_IMPLEMENT         (negate_, boost::simd::tag::ssse3_,
                       (A0),
                       ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                      )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return _mm_sign_epi16(a0, a1);  }
  };

  BOOST_DISPATCH_IMPLEMENT         (negate_, boost::simd::tag::ssse3_,
                       (A0),
                       ((simd_<int8_<A0>,boost::simd::tag::sse_>))
                      )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return _mm_sign_epi8(a0, a1);  }
  };
} } }
#endif
#endif


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2015 by Wenzel Jakob

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <nori/accel.h>
#include <Eigen/Geometry>
#include <chrono>

using namespace std::chrono;

NORI_NAMESPACE_BEGIN

void Accel::addMesh(Mesh *mesh) {
    if (m_num_meshes >= MAX_NUM_MESHES)
        throw NoriException("Accel: only %d meshes are supported!", MAX_NUM_MESHES);
    m_meshes[m_num_meshes] = mesh;
    m_bbox.expandBy(mesh->getBoundingBox());
    m_num_meshes++;
}

void Accel::build() {
    if (m_num_meshes == 0)
        throw NoriException("No mesh found, could not build acceleration structure");

    auto start = high_resolution_clock::now();
    // delete old hierarchy if present
    delete m_root;

    uint32_t num_triangles = 0;
    for (uint32_t mesh_idx = 0; mesh_idx < m_num_meshes; mesh_idx++) {
        num_triangles += m_meshes[mesh_idx]->getTriangleCount();
    }

    std::vector<uint32_t> triangles(num_triangles);
    std::vector<uint32_t> mesh_indices(num_triangles);
    uint32_t offset = 0;

    for (uint32_t current_mesh_idx = 0; current_mesh_idx < m_num_meshes; current_mesh_idx++) {
        uint32_t num_triangles_mesh = m_meshes[current_mesh_idx]->getTriangleCount();
        for (uint32_t i = 0; i < num_triangles_mesh; i++) {
            triangles[offset + i] = i;
            mesh_indices[offset + i] = current_mesh_idx;
        }
        offset += num_triangles_mesh;
    }

    m_root = buildRecursive(m_bbox, triangles, mesh_indices, 0);
    printf("Octree build time: %ldms \n", duration_cast<milliseconds>(high_resolution_clock::now() - start).count());
    printf("Num nodes: %d \n", m_num_nodes);
    printf("Num leaf nodes: %d \n", m_num_leaf_nodes);
    printf("Num non-empty leaf nodes: %d \n", m_num_nonempty_leaf_nodes);
    printf("Total number of saved triangles: %d \n", m_num_triangles_saved);
    printf("Avg triangles per node: %f \n", (float)m_num_triangles_saved / (float)m_num_nodes);
    printf("Recursion depth: %d \n", m_recursion_depth);
}

bool Accel::rayIntersect(const Ray3f &ray_, Intersection &its, bool shadowRay) const {
    bool foundIntersection;  // Was an intersection found so far?
    uint32_t f = (uint32_t) -1;      // Triangle index of the closest intersection

    Ray3f ray(ray_); /// Make a copy of the ray (we will need to update its '.maxt' value)

    foundIntersection = traverseRecursive(*m_root, ray, its, shadowRay, f);
    if (shadowRay)
        return foundIntersection;

    if (foundIntersection) {
        /* At this point, we now know that there is an intersection,
           and we know the triangle index of the closest such intersection.

           The following computes a number of additional properties which
           characterize the intersection (normals, texture coordinates, etc..)
        */

        /* Find the barycentric coordinates */
        Vector3f bary;
        bary << 1-its.uv.sum(), its.uv;

        /* References to all relevant mesh buffers */
        const Mesh *mesh   = its.mesh;
        const MatrixXf &V  = mesh->getVertexPositions();
        const MatrixXf &N  = mesh->getVertexNormals();
        const MatrixXf &UV = mesh->getVertexTexCoords();
        const MatrixXu &F  = mesh->getIndices();

        /* Vertex indices of the triangle */
        uint32_t idx0 = F(0, f), idx1 = F(1, f), idx2 = F(2, f);

        Point3f p0 = V.col(idx0), p1 = V.col(idx1), p2 = V.col(idx2);

        its.bary = bary;

        its.tri_index = Point3f(idx0, idx1, idx2);

        /* Compute the intersection positon accurately
           using barycentric coordinates */
        its.p = bary.x() * p0 + bary.y() * p1 + bary.z() * p2;

        /* Compute proper texture coordinates if provided by the mesh */
        if (UV.size() > 0)
            its.uv = bary.x() * UV.col(idx0) +
                bary.y() * UV.col(idx1) +
                bary.z() * UV.col(idx2);

        /* Compute the geometry frame */
        its.geoFrame = Frame((p1-p0).cross(p2-p0).normalized());

        if (N.size() > 0) {
            /* Compute the shading frame. Note that for simplicity,
               the current implementation doesn't attempt to provide
               tangents that are continuous across the surface. That
               means that this code will need to be modified to be able
               use anisotropic BRDFs, which need tangent continuity */

            its.shFrame = Frame(
                (bary.x() * N.col(idx0) +
                 bary.y() * N.col(idx1) +
                 bary.z() * N.col(idx2)).normalized());
        } else {
            its.shFrame = its.geoFrame;
        }
    }

    return foundIntersection;
}

Accel::Node* Accel::buildRecursive(const BoundingBox3f& bbox, std::vector<uint32_t>& triangle_indices,
        std::vector<uint32_t>& mesh_indices, uint32_t recursion_depth) {
    // a node is created in any case
    m_num_nodes++;

    uint32_t num_triangles = triangle_indices.size();

    // return empty node if no triangles are left
    if (num_triangles == 0) {
        Node* node = new Node();
        node->bbox = BoundingBox3f(bbox);

        // add to statistics
        m_num_leaf_nodes++;
        return node;
    }

    // create leaf node if 10 or less triangles are left or if the max recursion depth is reached.
    if (num_triangles <= MAX_TRIANGLES_PER_NODE || recursion_depth >= MAX_RECURSION_DEPTH) {
        Node* node = new Node();
        node->num_triangles = num_triangles;
        node->triangle_indices = new uint32_t[num_triangles];
        node->mesh_indices = new uint32_t [num_triangles];

        for (uint32_t i = 0; i < num_triangles; i++) {
            node->triangle_indices[i] = triangle_indices[i];
            node->mesh_indices[i] = mesh_indices[i];
        }
        node->bbox = BoundingBox3f(bbox);

        // add to statistics
        m_num_leaf_nodes++;
        m_num_nonempty_leaf_nodes++;
        m_num_triangles_saved += num_triangles;
        return node;
    }

    // create new parent node
    Node* node = new Node();
    node->bbox = BoundingBox3f(bbox);

    BoundingBox3f child_bboxes[8] = {};
    subdivideBBox(bbox, child_bboxes);

    std::vector<std::vector<uint32_t>> child_triangle_indices(8);
    std::vector<std::vector<uint32_t>> child_mesh_indices(8);

    uint32_t child_num_triangles[8] = {};

    // place every triangle in the children it overlaps with
    // for every child bbox
    for (uint32_t i = 0; i < 8; i++) {
        // for every triangle inside of the parent create triangle bounding box
        for (uint32_t j = 0; j < num_triangles; j++) {
            // for every triangle vertex expand triangle bbox
            uint32_t triangle_idx = triangle_indices[j];
            uint32_t mesh_idx = mesh_indices[j];
            BoundingBox3f triangle_bbox = m_meshes[mesh_idx]->getBoundingBox(triangle_idx);

            // check if triangle is in bbox, if so put triangle index into triangle list of child
            if (child_bboxes[i].overlaps(triangle_bbox)) {
                child_triangle_indices[i].emplace_back(triangle_idx);
                child_mesh_indices[i].emplace_back(mesh_idx);
                child_num_triangles[i]++;
            }
        }
    }

    // release memory to avoid stack overflow
    triangle_indices = std::vector<uint32_t>();
    mesh_indices = std::vector<uint32_t>();

    // for every child bbox
    Node* last_child = nullptr;
    for (uint32_t i = 0; i < 8; i++) {
        // first child
        if (i == 0) {
            node->child = buildRecursive(child_bboxes[i], child_triangle_indices[i], child_mesh_indices[i], recursion_depth + 1);
            last_child = node->child;
        // neighbour children
        } else {
            last_child->next = buildRecursive(child_bboxes[i], child_triangle_indices[i], child_mesh_indices[i], recursion_depth + 1);
            last_child = last_child->next;
        }
        m_recursion_depth = std::max(m_recursion_depth, recursion_depth + 1);
    }
    return node;
}

bool Accel::traverseRecursive(const Node& node, Ray3f &ray, Intersection &its, bool shadowRay, uint32_t& hit_idx) const {
    bool foundIntersection = false;

    // only check triangles of node and its children if ray intersects with node bbox
    if (!node.bbox.rayIntersect(ray)) {
        return false;
    }

    // search through all triangles in node
    for (uint32_t i = 0; i < node.num_triangles; ++i) {
        float u, v, t;
        uint32_t triangle_idx = node.triangle_indices[i];
        uint32_t mesh_idx = node.mesh_indices[i];
        if (m_meshes[mesh_idx]->rayIntersect(triangle_idx, ray, u, v, t) && t < ray.maxt) {
            /* An intersection was found! Can terminate
               immediately if this is a shadow ray query */
            if (shadowRay)
                return true;
            ray.maxt = t;
            its.t = t;
            its.uv = Point2f(u, v);
            its.mesh = m_meshes[mesh_idx];
            hit_idx = triangle_idx;
            foundIntersection = true;
        }
    }

    if (node.child) {
        std::pair<Node*, float> children[8];
        Node* current_child = node.child;
        int i = 0;
        do {
            children[i] = std::pair<Node*, float>(current_child, current_child->bbox.distanceTo(ray.o));
            current_child = current_child->next;
            i++;
        } while (current_child);

        std::sort(children, children + 8, [ray](const std::pair<Node*, float>& l, const std::pair<Node*, float>& r) {
            return l.second < r.second;
        });

        for (auto child: children) {
            foundIntersection = traverseRecursive(*child.first, ray, its, shadowRay, hit_idx) || foundIntersection;
            if (shadowRay && foundIntersection)
                return true;
        }
    }
    return foundIntersection;
}

void Accel::subdivideBBox(const nori::BoundingBox3f &parent, nori::BoundingBox3f *bboxes) {
    Point3f extents = parent.getExtents();

    Point3f x0_y0_z0 = parent.min;
    Point3f x1_y0_z0 = Point3f(parent.min.x() + extents.x() / 2.f, parent.min.y(), parent.min.z());
    Point3f x0_y1_z0 = Point3f(parent.min.x(), parent.min.y() + extents.y() / 2.f, parent.min.z());
    Point3f x1_y1_z0 = Point3f(parent.min.x() + extents.x() / 2.f, parent.min.y() + extents.y() / 2.f, parent.min.z());

    Point3f x0_y0_z1 = Point3f(parent.min.x(), parent.min.y(), parent.min.z() + extents.z() / 2.f);
    Point3f x1_y0_z1 = Point3f(parent.min.x() + extents.x() / 2.f, parent.min.y(), parent.min.z() + extents.z() / 2.f);
    Point3f x0_y1_z1 = Point3f(parent.min.x(), parent.min.y() + extents.y() / 2.f, parent.min.z() + extents.z() / 2.f);
    Point3f x1_y1_z1 = Point3f(parent.min.x() + extents.x() / 2.f, parent.min.y() + extents.y() / 2.f, parent.min.z() + extents.z() / 2.f);
    Point3f x2_y1_z1 = Point3f(parent.max.x(), parent.min.y() + extents.y() / 2.f, parent.min.z() + extents.z() / 2.f);
    Point3f x1_y2_z1 = Point3f(parent.min.x() + extents.x() / 2.f, parent.max.y(), parent.min.z() + extents.z() / 2.f);
    Point3f x2_y2_z1 = Point3f(parent.max.x(), parent.max.y(), parent.min.z() + extents.z() / 2.f);

    Point3f x1_y1_z2 = Point3f(parent.min.x() + extents.x() / 2.f, parent.min.y() + extents.y() / 2.f, parent.max.z());
    Point3f x2_y1_z2 = Point3f(parent.max.x(), parent.min.y() + extents.y() / 2.f, parent.max.z());
    Point3f x1_y2_z2 = Point3f(parent.min.x() + extents.x() / 2.f, parent.max.y(), parent.max.z());
    Point3f x2_y2_z2 = Point3f(parent.max.x(), parent.max.y(), parent.max.z());

    bboxes[0] = BoundingBox3f(x0_y0_z0, x1_y1_z1);
    bboxes[1] = BoundingBox3f(x1_y0_z0, x2_y1_z1);
    bboxes[2] = BoundingBox3f(x0_y1_z0, x1_y2_z1);
    bboxes[3] = BoundingBox3f(x1_y1_z0, x2_y2_z1);
    bboxes[4] = BoundingBox3f(x0_y0_z1, x1_y1_z2);
    bboxes[5] = BoundingBox3f(x1_y0_z1, x2_y1_z2);
    bboxes[6] = BoundingBox3f(x0_y1_z1, x1_y2_z2);
    bboxes[7] = BoundingBox3f(x1_y1_z1, x2_y2_z2);
}

NORI_NAMESPACE_END


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  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_X86_AVX2_SIMD_FUNCTION_SHIFT_LEFT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_SHIFT_LEFT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

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

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0, typename A1)
                          , bs::avx2_
                          , bs::pack_<bd::ints16_<A0>, bs::avx_>
                          , bd::scalar_<bd::integer_<A1>>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 a1) const BOOST_NOEXCEPT
      {
        return _mm256_slli_epi16(a0, int(a1));
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sllv_epi32(a0, a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0, typename A1)
                          , bs::avx2_
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                          , bd::scalar_<bd::integer_<A1>>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 a1) const BOOST_NOEXCEPT
      {
        return _mm256_slli_epi32(a0, int(a1));
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm_sllv_epi32(a0, a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sllv_epi64(a0, a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0, typename A1)
                          , bs::avx2_
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                          , bd::scalar_<bd::integer_<A1>>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 a1) const BOOST_NOEXCEPT
      {
        return _mm256_slli_epi64(a0, int(a1));
      }
   };

   BOOST_DISPATCH_OVERLOAD( shift_left_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints64_<A0>, bs::sse_>
                          , bs::pack_<bd::ints64_<A0>, bs::sse_>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm_sllv_epi64(a0, a1);
      }
   };
} } }

#endif


#include "util.h"
#include "wallet/wallet.h"
#include "core/ecc_native.h"
#include "core/serialization_adapters.h"
#include "utility/logger.h"
#include <boost/filesystem.hpp>
#include <iterator>
#include <future>

using namespace std;
using namespace grimm;
using namespace ECC;

namespace grimm {

bool ReadTreasury(ByteBuffer& bb, const string& sPath)
{
	if (sPath.empty())
		return false;

	std::FStream f;
	if (!f.Open(sPath.c_str(), true))
		return false;

	size_t nSize = static_cast<size_t>(f.get_Remaining());
	if (!nSize)
		return false;

	bb.resize(f.get_Remaining());
	return f.read(&bb.front(), nSize) == nSize;
}

wallet::IWalletDB::Ptr init_wallet_db(const std::string& path, uintBig* walletSeed, io::Reactor::Ptr reactor) {
    static const std::string TEST_PASSWORD("12321");

    if (boost::filesystem::exists(path)) boost::filesystem::remove_all(path);

    std::string password(TEST_PASSWORD);
    password += path;

    NoLeak<uintBig> seed;
    Hash::Value hv;
    Hash::Processor() << password >> hv;
    seed.V = hv;

    auto walletDB = wallet::WalletDB::init(path, password, seed, reactor);

    if (walletSeed)
        *walletSeed = seed.V;

    return walletDB;
}

} //namespace

std::ostream& operator<<(std::ostream& os, const ECC::Scalar::Native& sn) {
    Scalar s;
    sn.Export(s);
    os << s.m_Value;
    return os;
}


//
//  face.hpp
//  SFaceCompare
//
//  Created by Anton Khrolenko on 5/31/18.
//  Copyright © 2018 Bohdan Mihiliev. All rights reserved.
//

#ifndef face_hpp
#define face_hpp

#ifdef __cplusplus
#undef NO
#include <opencv2/opencv.hpp>
#include <dlib/image_processing.h>
#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include "general.hpp"

/**
 @brief Aligns face for the further neural network processing in face comparing.
 @note Aligned face only fits 96x96 neural network input.

 @param inputImage Image with human face, which need to be aligned.
 @param shapePredictor dlib shape predictor to find face landmarks.
 @param haarCascadeClassifier Haar Cascade Classifier to find face on the image if necessery.
 @param faceTemplate Constants for the 64 face landmarks for aligning.
 @param inputFaceRectangle Face rectangle coordinates on the image if it is known.

 @return Aligned face image 96x96.
 */
cv::Mat faceAlign(cv::Mat inputImage,
                  const dlib::shape_predictor& shapePredictor,
                  cv::CascadeClassifier haarCascadeClassifier,
                  cv::Point2f *faceTemplate,
                  cv::Rect inputFaceRectangle = cv::Rect()
                  );
#endif

#endif /* face_hpp */


/*******************************************************************************
*
*  Filename    : JetLepMgr.cc
*  Description : Definition of histograms and how they are filled.
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/SignalMCStudy/interface/Common.hpp"
#include "TstarAnalysis/SignalMCStudy/interface/JetLepMgr.hpp"

#include <boost/format.hpp>
#include <iostream>

#include "TFile.h"
#include "TTree.h"

using namespace std;

/******************************************************************************/

JetLepMgr::JetLepMgr( const std::string& name ) :
  Named( name ),
  HistMgr( name )
{
  AddHist( "NumLep",        "Number of good leptons",                    "",  5, 0,   5 );
  AddHist( "JetLepSep",     "Jet-lepton angular seperation",             "", 50, 0, 0.5 );
  AddHist( "JetLepSep1Lep", "Jet-lepton angular seperation (=1 lepton)", "", 50, 0, 0.5 );
  FillFromFile();
}

/******************************************************************************/

JetLepMgr::~JetLepMgr()
{

}

/******************************************************************************/

void
JetLepMgr::FillFromFile()
{
  const string filename = ResultsDir() / ( "jetlep_M"+Name()+".root" );
  TFile* file           = TFile::Open( filename.c_str() );
  TTree* tree           = (TTree*)( file->Get( "jetlepsep/JetLepTest" ) );

  unsigned numlep;
  unsigned numjetlep;
  double jetlepsep[64];

  tree->SetBranchAddress( "NLepton",   &numlep );
  tree->SetBranchAddress( "NJetLep",   &numjetlep );
  tree->SetBranchAddress( "JetLepSep", jetlepsep );

  boost::format entryfmt( "\r%s Entry[%d/%d]" );

  for( int i = 0; i< tree->GetEntries(); ++i ){
    tree->GetEntry( i );

    cout << entryfmt
      % Name()
      % i % tree->GetEntries()
      << flush;

    Hist( "NumLep" )->Fill( numlep );

    for( unsigned j = 0; j < numjetlep; ++j ){
      Hist( "JetLepSep" )->Fill( jetlepsep[j] );
      if( numlep == 1 ){
        Hist( "JetLepSep1Lep" )->Fill( jetlepsep[j] );
      }
    }
  }
  cout << "Done!" << endl;
}


// Copyright (c) 2015-2021 The Dogecoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

#include <arith_uint256.h>
#include <chain.h>
#include <dogecoin.h>
#include <logging.h>

#ifdef __clang__
__attribute__((no_sanitize("unsigned-integer-overflow")))
#endif // __clang__
int static generateMTRandom(unsigned int s, int range)
{
    boost::mt19937 gen(s);
    boost::uniform_int<> dist(1, range);
    return dist(gen);
}

CAmount GetDogecoinBlockSubsidy(int nHeight, const Consensus::Params& consensusParams, uint256 prevHash)
{
    int halvings = nHeight / consensusParams.nSubsidyHalvingInterval;

    if (nHeight < 145000 && !consensusParams.fSimplifiedRewards)
    {
        // Old-style rewards derived from the previous block hash

        // This extracts 7*4 bits out of the middle of the previous block hash to use as a seed.
        // It's longer than the 1.14 code, but avoids the locale-sensitive strtol() function.
        const int bitsPerNibble = 4;
        const int totalNibbles = 256 / bitsPerNibble;
        const int nibblesToClear = 7;
        const int nibblesToKeep = 7;
        arith_uint256 upperBits = UintToArith256(prevHash) << (nibblesToClear * bitsPerNibble);
        arith_uint256 cleanedBits = upperBits >> ((totalNibbles - nibblesToKeep) * bitsPerNibble);
        uint64_t seed = ArithToUint256(cleanedBits).GetUint64(0);

        // Convert the seed into a subsidy value.
        CAmount maxReward = (1000000 >> halvings) - 1;
        int rand = generateMTRandom(seed, maxReward);

        return (1 + rand) * COIN;
    } else if (nHeight < (6 * consensusParams.nSubsidyHalvingInterval)) {
        // New-style constant rewards for each halving interval
        return (500000 * COIN) >> halvings;
    } else {
        // Constant inflation
        return 10000 * COIN;
    }
}

// Dogecoin: Normally minimum difficulty blocks can only occur in between
// retarget blocks. However, once we introduce Digishield every block is
// a retarget, so we need to handle minimum difficulty on all blocks.
bool AllowDigishieldMinDifficultyForBlock(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
    // check if the chain allows minimum difficulty blocks
    if (!params.fPowAllowMinDifficultyBlocks)
        return false;

    // check if the chain allows minimum difficulty blocks on recalc blocks
    if (pindexLast->nHeight < 157500)
    // if (!params.fPowAllowDigishieldMinDifficultyBlocks)
        return false;

    // Allow for a minimum block time if the elapsed time > 2*nTargetSpacing
    return (pblock->GetBlockTime() > pindexLast->GetBlockTime() + params.nPowTargetSpacing*2);
}

unsigned int CalculateDogecoinNextWorkRequired(const CBlockIndex* pindexLast, int64_t nFirstBlockTime, const Consensus::Params& params)
{
    if (params.fPowNoRetargeting)
        return pindexLast->nBits;

    int nHeight = pindexLast->nHeight + 1;
    bool fNewDifficultyProtocol = (nHeight >= 145000);
    // bool fNewDifficultyProtocol = (nHeight >= params.GetDigiShieldForkBlock());
    const int64_t nRetargetTimespan = fNewDifficultyProtocol
                             ? 60 // params.DigiShieldTargetTimespan()
                             : params.nPowTargetTimespan;

    int64_t nTimespan = pindexLast->GetBlockTime() - nFirstBlockTime;
    int64_t nMaxTimespan;
    int64_t nMinTimespan;

    if (fNewDifficultyProtocol) //DigiShield implementation - thanks to RealSolid & WDC for this code
    {
        // amplitude filter - thanks to daft27 for this code
        nTimespan = nRetargetTimespan + (nTimespan - nRetargetTimespan) / 8;

        nMinTimespan = nRetargetTimespan - (nRetargetTimespan / 4);
        nMaxTimespan = nRetargetTimespan + (nRetargetTimespan / 2);
    } else if (nHeight > 10000) {
        nMinTimespan = nRetargetTimespan / 4;
        nMaxTimespan = nRetargetTimespan * 4;
    } else if (nHeight > 5000) {
        nMinTimespan = nRetargetTimespan / 8;
        nMaxTimespan = nRetargetTimespan * 4;
    } else {
        nMinTimespan = nRetargetTimespan / 16;
        nMaxTimespan = nRetargetTimespan * 4;
    }

    // Limit adjustment step
    if (nTimespan < nMinTimespan)
        nTimespan = nMinTimespan;
    else if (nTimespan > nMaxTimespan)
        nTimespan = nMaxTimespan;

    // Retarget
    const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
    arith_uint256 bnNew;
    bnNew.SetCompact(pindexLast->nBits);
    bnNew *= nTimespan;
    bnNew /= nRetargetTimespan;

    if (bnNew > bnPowLimit)
        bnNew = bnPowLimit;

    return bnNew.GetCompact();
}


#ifndef ASLAM_TRANSFORMATION_EXPRESSION_NODE
#define ASLAM_TRANSFORMATION_EXPRESSION_NODE

#include <Eigen/Core>
#include <aslam/backend/JacobianContainer.hpp>
#include <boost/shared_ptr.hpp>
#include <set>

namespace aslam {
namespace backend {

class TransformationExpressionNode {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef Eigen::Matrix<double, 6, 6> Matrix6d;
    typedef Eigen::Matrix<double, 4, 6> Matrix4x6d;

    TransformationExpressionNode();
    virtual ~TransformationExpressionNode();

    /// \brief Evaluate the rotation matrix.
    Eigen::Matrix4d toTransformationMatrix();

    /// \brief Evaluate the Jacobians
    void evaluateJacobians(JacobianContainer& outJacobians) const;

    /// \brief Evaluate the Jacobians and apply the chain rule.
    void evaluateJacobians(JacobianContainer& outJacobians, const Eigen::MatrixXd& applyChainRule) const;
    void getDesignVariables(DesignVariable::set_t& designVariables) const;

  protected:
    // These functions must be implemented by child classes.
    virtual Eigen::Matrix4d toTransformationMatrixImplementation() = 0;
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians) const = 0;
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians,
                                                 const Eigen::MatrixXd& applyChainRule) const = 0;
    virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const = 0;
};

/**
 * \class TransformationExpressionNodeMultiply
 *
 * \brief A class representing the multiplication of two transformation matrices.
 *
 */
class TransformationExpressionNodeMultiply : public TransformationExpressionNode {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef Eigen::Matrix<double, 6, 6> Matrix6d;

    TransformationExpressionNodeMultiply(boost::shared_ptr<TransformationExpressionNode> lhs,
                                         boost::shared_ptr<TransformationExpressionNode> rhs);
    virtual ~TransformationExpressionNodeMultiply();

  private:
    virtual Eigen::Matrix4d toTransformationMatrixImplementation();
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians) const;
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians,
                                                 const Eigen::MatrixXd& applyChainRule) const;
    virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const;

    boost::shared_ptr<TransformationExpressionNode> _lhs;
    Eigen::Matrix4d _T_lhs;
    boost::shared_ptr<TransformationExpressionNode> _rhs;
    Eigen::Matrix4d _T_rhs;
};

/**
 * \class TransformationExpressionNodeInverse
 *
 * \brief A class representing the inverse of a transformation matrix.
 *
 */
class TransformationExpressionNodeInverse : public TransformationExpressionNode {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef Eigen::Matrix<double, 6, 6> Matrix6d;

    TransformationExpressionNodeInverse(boost::shared_ptr<TransformationExpressionNode> dvTransformation);

    virtual ~TransformationExpressionNodeInverse();

  private:
    virtual Eigen::Matrix4d toTransformationMatrixImplementation();
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians) const;
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians,
                                                 const Eigen::MatrixXd& applyChainRule) const;
    virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const;

    boost::shared_ptr<TransformationExpressionNode> _dvTransformation;
    Eigen::Matrix4d _T;
};

/**
 * \class TransformationExpressionNodeMultiply
 *
 * \brief A class representing the multiplication of two transformation matrices.
 *
 */
class TransformationExpressionNodeConstant : public TransformationExpressionNode {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    TransformationExpressionNodeConstant(const Eigen::Matrix4d& T);
    virtual ~TransformationExpressionNodeConstant();

  private:
    virtual Eigen::Matrix4d toTransformationMatrixImplementation();
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians) const;
    virtual void evaluateJacobiansImplementation(JacobianContainer& outJacobians,
                                                 const Eigen::MatrixXd& applyChainRule) const;
    virtual void getDesignVariablesImplementation(DesignVariable::set_t& designVariables) const;

    Eigen::Matrix4d _T;
};

}  // namespace backend
}  // namespace aslam

#endif /* ASLAM_TRANSFORMATION_EXPRESSION_NODE */


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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 <camera/camera_params.h>
#include <ff_common/init.h>
#include <ff_common/utils.h>
#include <localization_common/logger.h>
#include <localization_common/utilities.h>
#include <optimization_common/fov_distorter.h>
#include <optimization_common/rad_distorter.h>
#include <optimization_common/radtan_distorter.h>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <opencv2/imgcodecs.hpp>

namespace lc = localization_common;
namespace oc = optimization_common;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

int main(int argc, char** argv) {
  std::string robot_config_file;
  std::string world;
  std::string output_undistorted_images_directory;
  po::options_description desc(
    "Creates undistorted images using the intrinsics and distortion params of the selected camera.");
  desc.add_options()("help", "produce help message")("distorted-images-directory", po::value<std::string>()->required(),
                                                     "Distorted images directory")(
    "distortion-type", po::value<std::string>()->required(), "fov, rad, or radtan.")(
    "camera-name", po::value<std::string>()->required(), "Selected camera name from robot config file.")(
    "config-path", po::value<std::string>()->required(), "Config path.")(
    "output-directory,o",
    po::value<std::string>(&output_undistorted_images_directory)->default_value("undistorted_images"),
    "Output directory for the undistorted images.")(
    "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"),
    "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name.");
  po::positional_options_description p;
  p.add("distorted-images-directory", 1);
  p.add("distortion-type", 1);
  p.add("camera-name", 1);
  p.add("config-path", 1);
  po::variables_map vm;
  try {
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  try {
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  const std::string distorted_images_directory = vm["distorted-images-directory"].as<std::string>();
  const std::string distortion_type = vm["distortion-type"].as<std::string>();
  const std::string camera_name = vm["camera-name"].as<std::string>();
  const std::string config_path = vm["config-path"].as<std::string>();

  // Only pass program name to free flyer so that boost command line options
  // are ignored when parsing gflags.
  int ff_argc = 1;
  ff_common::InitFreeFlyerApplication(&ff_argc, &argv);

  lc::SetEnvironmentConfigs(config_path, world, robot_config_file);
  config_reader::ConfigReader config;
  config.AddFile("geometry.config");
  config.AddFile("cameras.config");
  config.AddFile("tools/camera_target_based_intrinsics_calibrator.config");
  if (!config.ReadFiles()) {
    LogFatal("Failed to read config files.");
  }

  if (!fs::is_directory(distorted_images_directory)) {
    LogFatal("Distorted images directory " << distorted_images_directory << " not found.");
  }
  if (fs::is_directory(output_undistorted_images_directory)) {
    LogFatal("Output undistorted images directory " << output_undistorted_images_directory << " already exists.");
  }
  fs::create_directory(output_undistorted_images_directory);

  const camera::CameraParameters camera_params(&config, camera_name.c_str());
  std::vector<cv::String> image_file_names;
  const std::string images_file_path = (fs::path(distorted_images_directory) / fs::path("*.jpg")).string();
  cv::glob(images_file_path, image_file_names, false);
  const Eigen::Matrix3d intrinsics = camera_params.GetIntrinsicMatrix<camera::DISTORTED>();
  const Eigen::VectorXd distortion_params = camera_params.GetDistortion();
  for (const auto& image_file_name : image_file_names) {
    const cv::Mat distorted_image = cv::imread(image_file_name);
    cv::Mat undistorted_image;
    // TODO(rsoussan): Avoid checking distortion param value/recreating distortion object each time
    if (distortion_type == "fov") {
      oc::FovDistorter distorter;
      undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params);
    } else if (distortion_type == "rad") {
      oc::RadDistorter distorter;
      undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params);
    } else if (distortion_type == "radtan") {
      oc::RadTanDistorter distorter;
      undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params);
    } else {
      LogFatal("Invalid distortion type provided.");
    }
    const fs::path image_filepath(image_file_name);
    const std::string undistorted_image_filename =
      (fs::path(output_undistorted_images_directory) / image_filepath.filename()).string();
    cv::imwrite(undistorted_image_filename, undistorted_image);
  }
}