// CPURenderer.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include "pch.h"
#include <iostream>
#include <sstream>
#include <array>
#include <algorithm>
#include <chrono>
#include <CLI11.hpp>
#include <tinyformat.h>
#include <openvdb/tools/RayTracer.h>
#include <openvdb/io/Stream.h>
#include <OpenEXR/ImfChannelList.h>
#include <OpenEXR/ImfFrameBuffer.h>
#include <OpenEXR/ImfHeader.h>
#include <OpenEXR/ImfOutputFile.h>
#include <OpenEXR/ImfPixelType.h>
#include <boost/algorithm/string/predicate.hpp>

#include <fcntl.h>
#include <io.h>

#include "PhongShader.h"
#include "IsoVolumeRayTracer.h"
#include "../CPURenderer/ExternalImporter.h"

template<typename Arg>
void parseStrImpl(const std::string& str, std::size_t pos, Arg& arg)
{
	if (str.find(',',pos) != std::string::npos)
		throw std::exception("more list entries specified than expected");
	std::stringstream ss(str.substr(pos));
	ss >> arg;
}

template<typename Arg, typename... Rest>
void parseStrImpl(const std::string& str, std::size_t pos, Arg& arg, Rest& ... rest)
{
	size_t p = str.find(',', pos);
	if (p == std::string::npos)
		throw std::exception("less list entries specified than expected");
	std::stringstream ss(str.substr(pos, p));
	ss >> arg;
	parseStrImpl(str, p + 1, rest...);
}

template<typename ...Args>
void parseStr(const std::string& str, Args & ... args)
{
	parseStrImpl(str, 0, args...);
}

//Does the string contain a '%d' or similiar?
bool hasIntFormat(const std::string& str)
{
	try
	{
		int index = 1;
		tfm::format(str, index);
		return true;
	} catch (std::exception& ex)
	{
		return false;
	}
}

struct Args
{
	//input/output
	std::string inputFilename;
	std::string outputFilename;
	//downscale
	int downscaleFactor = 1;
	std::string downscaleFilename;
	std::string downscaleDepthFilename;
	std::string highDepthFilename;
	//animation
	int animFrames = 0;
	std::string downscaleFlowFilename;
	openvdb::Vec3d cameraOriginEnd = { 0,0,-1 };
	openvdb::Vec3d cameraLookAtEnd = { 0,0,0 };
	//camera
	int resolutionX = 512;
	int resolutionY = 512;
	double cameraFov = 45;
	openvdb::Vec3d cameraOrigin = {0,0,-1};
	openvdb::Vec3d cameraLookAt = {0,0,0};
	openvdb::Vec3d cameraUp = {0,1,0};
	//render mode
	static const std::string RenderModeVolume;
	static const std::string RenderModeIso;
	static const std::string RenderModeConvert;
	std::string renderMode;
	int noShading = 0;
	//isosurface
	double isovalue = 0.0;
	openvdb::Vec3d materialDiffuse = { 0.7, 0.7, 0.7 };
	openvdb::Vec3d materialSpecular = { 1,1,1 };
	openvdb::Vec3d materialAmbient = { 0.1,0.1,0.1 };
	int materialSpecularExponent = 32;
	bool cameraLight = true; //true: light along camera view direction
	openvdb::Vec3d lightDirection = {0,0,0};
	int samples = 1;
	//iso-effects (unused)
	std::string aoMode = "";
	int aoSamples = 32;
	float aoRadius = 0.01;
	std::string highEffectsFilename;
	//volume
	//TODO
	//pipe
	std::string pipeOutputFormat = "RGBMXYZDUV"; //red, green, blue, mask, normal x, normal y, normal z, depth, flow u, flow v
};
const std::string Args::RenderModeVolume = "volume";
const std::string Args::RenderModeIso = "iso";
const std::string Args::RenderModeConvert = "convert";

Args parseArguments(int argc, const char* argv[])
{
	CLI::App app{ "CPU Volume Renderer" };
	Args args;

	//input and output
	app.add_option("input,-i,--input", args.inputFilename,
		"Input file name, supported file formats: .vdb")
		->required();
	app.add_option("output,-o,--output", args.outputFilename,
		"Output file name, supported file formats: .ppm, .exr\n"
		"Must contain a printf expression '%d' in animation mode, but the % replaced by & for compatibility.\n"
		"Alternative specify 'PIPE' to switch to pipe mode, downscaling and animation will be ignored.")
		->required();

	auto downscalePathOpt = app.add_option("--downscale_path", args.downscaleFilename,
		"Output file for additional downscaled image. If specified, downscale_factor has to be specified as well.\n"
		"Must contain a printf expression '%d' in animation mode, but the % replaced by & for compatibility.");
	auto downscaleFactorOpt = app.add_option("--downscale_factor", args.downscaleFactor,
		"Downscaling factor, to be used together with --downscale_path")
		->check(CLI::Range(1, 32));
	app.add_option("-d,--depth", args.downscaleDepthFilename,
		"Output file name for the downscaled normal+depth image, supported file formats: .ppm, .exr.\n"
		"Must contain a printf expression '%d' in animation mode, but the % replaced by & for compatibility.");
	downscalePathOpt->needs(downscaleFactorOpt);
	app.add_option("--highdepth", args.highDepthFilename,
		"Output file name for the high-resolution normal+depth image, supported file formats: .ppm, .exr.\n"
		"Must contain a printf expression '%d' in animation mode, but the % replaced by & for compatibility.");

	//animation
	auto animationOp = app.add_option("-a,--animation", args.animFrames,
		"Enables animation mode: specifies the number of frames to generate. Default: 0 = static image");
	app.add_option("-f,--flow", args.downscaleFlowFilename,
		"Output file name for the downscaled flow image, supported file formats: .ppm, .exr.\n"
		"Must contain a printf expression '%d'. Only used in animation mode, but the % replaced by & for compatibility.\n"
		"Flow is computed from the current frame to the next frame. Hence, the last frame does not contain flow information")
		->needs(animationOp);

	//camera
	app.add_option("--fov", args.cameraFov,
		"perspective camera field ov view (default: " + std::to_string(args.cameraFov) + ")");
	std::string resolutionStr = "512,512";
	app.add_option("--res,--resolution", resolutionStr,
		"image resolution (default: " + resolutionStr + ")");
	std::string originStr = "0,0,-2";
	app.add_option("--origin", originStr,
		"camera origin X,Y,Z (default: " + originStr + ").\n"
		"In animation mode must contain six elements: start X,Y,Z, end X,Y,Z");
	std::string lookatStr = "0,0,0";
	app.add_option("--lookat", lookatStr,
		"rotate the camera to point to X,Y,Z (default: " + lookatStr + ").\n"
		"In animation mode must contain six elements: start X,Y,Z, end X,Y,Z");
	std::string upStr = "0,1,0";
	app.add_option("--up", upStr, "camera up vector, in combination with --lookat (default: " + upStr + ")");

	//rendering mode
	app.add_set_ignore_case("-m,--mode", args.renderMode, { Args::RenderModeVolume, Args::RenderModeIso, Args::RenderModeConvert })
		->description("Render mode")->required();
	app.add_option("--noshading", args.noShading,
		"Set to one for no phong shading, color output contains only the albedo.");

	//isosurface
	app.add_option("--isovalue", args.isovalue,
		"Isovalue in world units for level set ray intersection (default: " + std::to_string(args.isovalue) + ')');
	std::string materialDiffuseStr = "0.7,0.7,0.7";
	app.add_option("--diffuse", materialDiffuseStr,
		"Iso: material diffuse color R,G,B (default: " + materialDiffuseStr + ")");
	std::string materialSpecularStr = "0.1,0.1,0.1";
	app.add_option("--specular", materialSpecularStr,
		"Iso: material specular color R,G,B (default: " + materialSpecularStr + ")");
	std::string materialAmbientStr = "0.1,0.1,0.1";
	app.add_option("--ambient", materialAmbientStr, "Iso: material ambient color R,G,B (default: " + materialAmbientStr + ")");
	app.add_option("--exponent", args.materialSpecularExponent, "ISO: specular exponent (default: " + std::to_string(args.materialSpecularExponent) + ")");
	std::string lightStr = "camera";
	app.add_option("--light", lightStr, "Iso: light direction, either X,Y,Z or 'camera' (default: " + lightStr + ")");
	app.add_option("--samples", args.samples, "The number of samples per pixel to reduce alising (default: " + std::to_string(args.samples) + ')')->check(CLI::Range(1, 64));

	//iso effects (unused)
	app.add_option("--ao", args.aoMode, "Ambient Occlusion Mode, supported values: 'screen' and 'world'");
	app.add_option("--aoradius", args.aoRadius, "radius of the ambient occlusion in world space");
	app.add_option("--aosamples", args.aoSamples, "Number of samples for the ambient occlusion");
	app.add_option("--higheffects", args.highEffectsFilename,
		"Output file name for the high-resolution effects image (ambient occlusion + shadow), supported file formats: .ppm, .exr.\n"
		"Must contain a printf expression '%d' in animation mode, but the % replaced by & for compatibility.");

	//pipe
	app.add_option("--pipeOutputFormat", args.pipeOutputFormat,
		"Output format for the pipe mode. Currently the only supported mode is " + args.pipeOutputFormat +
		", which is also the default setting");

	try
	{
		(app).parse((argc), (argv));
	} catch(const CLI::ParseError &e)
	{
		int code = (app).exit(e);
		exit(code);
	}

	//convert parameters
	if (args.animFrames > 1) {
		std::replace(args.outputFilename.begin(), args.outputFilename.end(), '&', '%');
		if (!hasIntFormat(args.outputFilename))
		{
			std::cout << "Animation mode! Output filename must be specified with '%d' or similar." << std::endl;
			exit(-1);
		}
		std::replace(args.highDepthFilename.begin(), args.highDepthFilename.end(), '&', '%');
		if (!args.highDepthFilename.empty() && !hasIntFormat(args.highDepthFilename))
		{
			std::cout << "Animation mode! Highres depth output filename must be specified with '%d' or similar." << std::endl;
			exit(-1);
		}
		std::replace(args.downscaleFilename.begin(), args.downscaleFilename.end(), '&', '%');
		if (!args.downscaleFilename.empty() && !hasIntFormat(args.downscaleFilename))
		{
			std::cout << "Animation mode! Downscale output filename must be specified with '%d' or similar." << std::endl;
			exit(-1);
		}
		std::replace(args.downscaleDepthFilename.begin(), args.downscaleDepthFilename.end(), '&', '%');
		if (!args.downscaleDepthFilename.empty() && !hasIntFormat(args.downscaleDepthFilename))
		{
			std::cout << "Animation mode! Downscale depth output filename must be specified with '%d' or similar." << std::endl;
			exit(-1);
		}
		std::replace(args.downscaleFlowFilename.begin(), args.downscaleFlowFilename.end(), '&', '%');
		if (!args.downscaleFlowFilename.empty() && !hasIntFormat(args.downscaleFlowFilename))
		{
			std::cout << "Animation mode! Downscale flow output filename must be specified with '%d' or similar." << std::endl;
			exit(-1);
		}
	}
	try
	{
		parseStr(resolutionStr, args.resolutionX, args.resolutionY);
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the image resolution, expected w,h: " << ex.what() << std::endl;
		exit(-1);
	}
	try
	{
		if (args.animFrames > 1)
			parseStr(originStr,
				args.cameraOrigin.x(), args.cameraOrigin.y(), args.cameraOrigin.z(),
				args.cameraOriginEnd.x(), args.cameraOriginEnd.y(), args.cameraOriginEnd.z());
		else
			parseStr(originStr, args.cameraOrigin.x(), args.cameraOrigin.y(), args.cameraOrigin.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the camera origin, expected X,Y,Z: " << ex.what() << std::endl;
		exit(-1);
	}
	try
	{
		if (args.animFrames > 1)
			parseStr(lookatStr,
				args.cameraLookAt.x(), args.cameraLookAt.y(), args.cameraLookAt.z(),
				args.cameraLookAtEnd.x(), args.cameraLookAtEnd.y(), args.cameraLookAtEnd.z());
		else
			parseStr(lookatStr, args.cameraLookAt.x(), args.cameraLookAt.y(), args.cameraLookAt.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the look-at position, expected X,Y,Z: " << ex.what() << std::endl;
		exit(-1);
	}
	try
	{
		parseStr(upStr, args.cameraUp.x(), args.cameraUp.y(), args.cameraUp.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the up vector, expected X,Y,Z: " << ex.what() << std::endl;
		exit(-1);
	}
	args.cameraUp.normalize();

	try
	{
		parseStr(materialDiffuseStr, args.materialDiffuse.x(), args.materialDiffuse.y(), args.materialDiffuse.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the diffuse color, expected R,G,B: " << ex.what() << std::endl;
		exit(-1);
	}
	try
	{
		parseStr(materialAmbientStr, args.materialAmbient.x(), args.materialAmbient.y(), args.materialAmbient.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the ambient color, expected R,G,B: " << ex.what() << std::endl;
		exit(-1);
	}
	try
	{
		parseStr(materialSpecularStr, args.materialSpecular.x(), args.materialSpecular.y(), args.materialSpecular.z());
	}
	catch (const std::exception& ex) {
		std::cout << "Wrong format for the diffuse color, expected R,G,B: " << ex.what() << std::endl;
		exit(-1);
	}
	if (lightStr == "camera")
		args.cameraLight = true;
	else
	{
		args.cameraLight = false;
		try
		{
			parseStr(lightStr, args.lightDirection.x(), args.lightDirection.y(), args.lightDirection.z());
			args.lightDirection.normalize();
		}
		catch (const std::exception& ex) {
			std::cout << "Wrong format for the light direction, expected X,Y,Z or 'camera': " << ex.what() << std::endl;
			exit(-1);
		}
	}

	//TEST
	std::cout << "input filename: " << args.inputFilename << std::endl;
	std::cout << "output filename: " << args.outputFilename << std::endl;
	std::cout << "fov: " << args.cameraFov << " -> focal length: " << openvdb::tools::PerspectiveCamera::fieldOfViewToFocalLength(args.cameraFov, 41.2) << std::endl;
	std::cout << "resolution: " << args.resolutionX << "x" << args.resolutionY << std::endl;
	std::cout << "camera origin: " << args.cameraOrigin << std::endl;
	std::cout << "camera look-at: " << args.cameraLookAt << std::endl;
	std::cout << "camera up: " << args.cameraUp << std::endl;
	std::cout << "render mode: " << args.renderMode << std::endl;
	std::cout << "isovalue: " << args.isovalue << std::endl;
	std::cout << "material ambient: " << args.materialAmbient << std::endl;
	std::cout << "material diffuse: " << args.materialDiffuse << std::endl;
	std::cout << "material specular: " << args.materialSpecular << std::endl;
	std::cout << "material specular exponent: " << args.materialSpecularExponent << std::endl;
	if (args.cameraLight) std::cout << "light direction: camera origin to camera look-at" << std::endl;
	else std::cout << "light direction: " << args.lightDirection << std::endl;
	std::cout << std::endl;

	return args;
}

//Convert from .vdb to .vdx
void convert(const Args& args)
{
	std::cout << "Convert from " << args.inputFilename << " to " << args.outputFilename << std::endl;
	if (!boost::ends_with(args.inputFilename, ".dat"))
	{
		std::cout << "Error: Input must end in ..dat" << std::endl;
		return;
	}
	if (!boost::ends_with(args.outputFilename, ".vdb"))
	{
		std::cout << "Error: Output must end in .vdb" << std::endl;
		return;
	}

	std::cout << "Load " << args.inputFilename << std::endl;
	openvdb::FloatGrid::Ptr grid = ExternalImporter::importRAW(args.inputFilename, args.downscaleFactor);
	if (grid == nullptr)
		return;

	//grid statistics
	grid->print(std::cout);
	std::cout << "Background value: " << grid->background() << std::endl;
	float minValue, maxValue; grid->evalMinMax(minValue, maxValue);
	std::cout << "Grid min value: " << minValue << ", max value: " << maxValue << std::endl;
	auto gridDim = grid->evalActiveVoxelDim(); auto gridCount = grid->activeVoxelCount();
	std::cout << "Grid number of voxels: " << gridCount << " (" << (gridCount * 100 / (gridDim.x()*gridDim.y()*gridDim.z())) << "%)" << std::endl;
	std::cout << "Grid uniform voxels: " << grid->hasUniformVoxels() << std::endl << std::endl;

	//scale grid to [-0.5,-0.5,-0.5] x [0.5,0.5,0.5]
	auto bbox = grid->evalActiveVoxelBoundingBox();
	auto center = bbox.getCenter();
	std::cout << "Original bounding box (index space): " << bbox.getStart() << " x " << bbox.getEnd() << std::endl;
	auto bboxWorld = grid->transformPtr()->indexToWorld(bbox);
	std::cout << "Original bounding box (world space): " << bboxWorld.min() << " x " << bboxWorld.max() << std::endl;
	auto size = bboxWorld.extents();
	double scale = 1.0 / std::max({ size.x(), size.y(), size.z() });
	grid->transformPtr()->postTranslate(-bboxWorld.getCenter());
	grid->transformPtr()->postScale(scale);
	bboxWorld = grid->transformPtr()->indexToWorld(bbox);
	std::cout << "Transformed bounding box: " << bboxWorld.min() << " x " << bboxWorld.max() << std::endl;
	std::cout << "Grid uniform voxels: " << grid->hasUniformVoxels() << std::endl;
	std::cout << "Transformation: " << grid->constTransform();

	//save grid
	std::cout << "Save grid" << std::endl;
	std::ofstream ofile(args.outputFilename, std::ofstream::binary);
	openvdb::GridPtrVecPtr grids(new openvdb::GridPtrVec);
	grids->push_back(grid);
	openvdb::io::Stream(ofile).write(*grids);
	std::cout << "Done" << std::endl;
}

openvdb::FloatGrid::Ptr loadGrid(const Args& args)
{
	std::cout << "Load Grid" << std::endl;

	openvdb::FloatGrid::Ptr grid;
	openvdb::io::File file(args.inputFilename);
	//No grid was specified by name, retrieve the first float grid from the file.
	file.open(/*delayLoad=*/false);
	openvdb::io::File::NameIterator it = file.beginName();
	openvdb::GridPtrVecPtr grids = file.readAllGridMetadata();
	for (size_t i = 0; i < grids->size(); ++i, ++it) {
		grid = openvdb::gridPtrCast<openvdb::FloatGrid>(grids->at(i));
		if (grid) {
			std::string gridName = *it;
			file.close();
			file.open();
			grid = openvdb::gridPtrCast<openvdb::FloatGrid>(file.readGrid(gridName));
			break;
		}
	}
	if (!grid)
	{
		std::cout << "No scalar, floating-point volume in file " << args.inputFilename << std::endl;
		exit(-2);
	}

	//TEST
	//makeSphere(*grid);

	//grid statistics
	grid->print(std::cout);
	std::cout << "Background value: " << grid->background() << std::endl;
	float minValue, maxValue; grid->evalMinMax(minValue, maxValue);
	std::cout << "Grid min value: " << minValue << ", max value: " << maxValue << std::endl;
	auto gridDim = grid->evalActiveVoxelDim(); auto gridCount = grid->activeVoxelCount();
	std::cout << "Grid number of voxels: " << gridCount << " (" << (gridCount * 100 / (gridDim.x()*gridDim.y()*gridDim.z())) << "%)" << std::endl;
	std::cout << "Grid uniform voxels: " << grid->hasUniformVoxels() << std::endl << std::endl;

	//scale grid to [-0.5,-0.5,-0.5] x [0.5,0.5,0.5]
	auto bbox = grid->evalActiveVoxelBoundingBox();
	auto center = bbox.getCenter();
	std::cout << "Original bounding box (index space): " << bbox.getStart() << " x " << bbox.getEnd() << std::endl;
	auto bboxWorld = grid->transformPtr()->indexToWorld(bbox);
	std::cout << "Original bounding box (world space): " << bboxWorld.min() << " x " << bboxWorld.max() << std::endl;
	auto size = bboxWorld.extents();
	double scale = 1.0 / std::max({ size.x(), size.y(), size.z() });
	grid->transformPtr()->postTranslate(-bboxWorld.getCenter());
	grid->transformPtr()->postScale(scale);
	bboxWorld = grid->transformPtr()->indexToWorld(bbox);
	std::cout << "Transformed bounding box: " << bboxWorld.min() << " x " << bboxWorld.max() << std::endl;
	std::cout << "Grid uniform voxels: " << grid->hasUniformVoxels() << std::endl;
	std::cout << "Transformation: " << grid->constTransform();

	return grid;
}

typedef std::shared_ptr<openvdb::tools::Film> FilmPtr;
std::array<FilmPtr, 3> //RGBA, NxNyNzD, VxVy
render(openvdb::FloatGrid::Ptr grid, const Args& args,
	const openvdb::Vec3d& currentOrigin, const openvdb::Vec3d& currentLookAt,
	const openvdb::Vec3d& nextOrigin, const const openvdb::Vec3d& nextLookAt,
	float* secondsOut = nullptr)
{
	//std::cout << "Render Image" << std::endl;

	//create output image
	FilmPtr filmRGB = std::make_shared<openvdb::tools::Film>(args.resolutionX, args.resolutionY);
	filmRGB->fill(openvdb::tools::Film::RGBA(0.0,0.0,0.0,0.0));
	FilmPtr filmDnormal = std::make_shared<openvdb::tools::Film>(args.resolutionX, args.resolutionY);
	filmDnormal->fill(openvdb::tools::Film::RGBA(0.0, 0.0, 0.0, 0.0));
	FilmPtr filmVelocities = std::make_shared<openvdb::tools::Film>(args.resolutionX, args.resolutionY);
	filmVelocities->fill(openvdb::tools::Film::RGBA(0.0, 0.0, 0.0, 0.0));

	//create camera
	double aperture = 0.01;
	double focalLength = openvdb::tools::PerspectiveCamera::fieldOfViewToFocalLength(args.cameraFov, aperture);
	openvdb::tools::PerspectiveCamera camera(*filmRGB,
		openvdb::Vec3R(0), currentOrigin,
		focalLength, aperture);
	camera.lookAt(currentLookAt, args.cameraUp);
	//std::cout << "Camera ray dir top-left: " << camera.getRay(0, 0).dir() << std::endl;
	//std::cout << "Camera ray dir bottom-right: " << camera.getRay(args.resolutionX-1, args.resolutionY-1).dir() << std::endl;

	openvdb::tools::PerspectiveCamera nextCamera(*filmRGB,
		openvdb::Vec3R(0), nextOrigin,
		focalLength, aperture);
	nextCamera.lookAt(nextLookAt, args.cameraUp);

	if (args.renderMode == Args::RenderModeIso)
	{
		//isosurface tracer
		float minValue, maxValue;
		grid->evalMinMax(minValue, maxValue);
		double isovalue = args.isovalue * maxValue;
		openvdb::Vec3R lightDir = args.cameraLight
			? (args.cameraLookAt - args.cameraOrigin)
			: args.lightDirection;
		lightDir.normalize();
		PhongShader<openvdb::tools::BoxSampler> phongShader(
			lightDir, args.materialAmbient, args.materialDiffuse, args.materialSpecular, args.materialSpecularExponent);
		openvdb::tools::MatteShader<openvdb::tools::Film::RGBA, openvdb::tools::BoxSampler> matteShader(
			openvdb::tools::Film::RGBA(args.materialDiffuse.x(), args.materialDiffuse.y(), args.materialDiffuse.z()));
		openvdb::tools::BaseShader* shader = args.noShading
		? static_cast<openvdb::tools::BaseShader*>(&matteShader)
		: static_cast<openvdb::tools::BaseShader*>(&phongShader);
		openvdb::tools::IsoVolumeRayIntersector<openvdb::FloatGrid> intersector(
			*grid, static_cast<typename openvdb::FloatGrid::ValueType>(isovalue));
		openvdb::tools::IsoVolumeRayTracer<openvdb::FloatGrid, openvdb::tools::IsoVolumeRayIntersector<openvdb::FloatGrid>> tracer(
			intersector, *shader, camera, args.samples, 42);
		tracer.setDepthNormalFilm(filmDnormal.get());
		tracer.setFlowFilm(filmVelocities.get(), &nextCamera);

		auto start = std::chrono::high_resolution_clock::now();
		tracer.render(true);
		auto finish = std::chrono::high_resolution_clock::now();
		if (secondsOut) *secondsOut = std::chrono::duration<double>(finish - start).count();
	}
	else
	{
		//volume tracer
		//TODO

		using IntersectorType = openvdb::tools::VolumeRayIntersector<openvdb::FloatGrid>;
		IntersectorType intersector(*grid);

		openvdb::tools::VolumeRender<IntersectorType> renderer(intersector, camera);
		openvdb::Vec3R lightDir = args.cameraLight
			? (args.cameraLookAt - args.cameraOrigin)
			: args.lightDirection;
		lightDir.normalize();
		renderer.setLightDir(lightDir.x(), lightDir.y(), lightDir.z());
		renderer.setLightColor(1, 1, 1);
		renderer.setPrimaryStep(1);
		renderer.setShadowStep(2);
		renderer.setScattering(1.5, 1.5, 1.5);
		renderer.setAbsorption(0.1, 0.1, 0.1);
		renderer.setLightGain(0.2);
		renderer.setCutOff(0.005);

		auto start = std::chrono::high_resolution_clock::now();
		renderer.render(true);
		auto finish = std::chrono::high_resolution_clock::now();
		if (secondsOut) *secondsOut = std::chrono::duration<double>(finish - start).count();
	}

	int numOn = 0, numOff = 0;
	for (int i=0; i< args.resolutionX; ++i)
		for (int j=0; j< args.resolutionY; ++j)
		{
			if (filmRGB->pixel(i, j).a == 0)
				numOff++;
			else
				numOn++;
		}
	//std::cout << "Num on: " << numOn << ", numOff: " << numOff << std::endl;

	return { filmRGB, filmDnormal, filmVelocities };
}

void saveImage(const openvdb::tools::Film& image, const std::string& outputFilename, int numChannels)
{
	std::cout << "Save Image to " << outputFilename << std::endl;
	if (boost::iends_with(outputFilename, ".ppm")) {
		// Save as PPM (fast, but large file size).
		std::string filename = outputFilename;
		filename.erase(filename.size() - 4); // strip .ppm extension
		const_cast<openvdb::tools::Film*>(&image)->savePPM(filename); //savePPM is non-const!!
	}
	else if (boost::iends_with(outputFilename, ".exr")) {
		// Save as EXR (slow, but small file size).
		Imf::setGlobalThreadCount(8);
		Imf::Header header(int(image.width()), int(image.height()));
		header.compression() = Imf::ZIP_COMPRESSION;
		header.channels().insert("R", Imf::Channel(Imf::FLOAT));
		if (numChannels >= 2) header.channels().insert("G", Imf::Channel(Imf::FLOAT));
		if (numChannels >= 3) header.channels().insert("B", Imf::Channel(Imf::FLOAT));
		if (numChannels >= 4) header.channels().insert("A", Imf::Channel(Imf::FLOAT));
		const size_t pixelBytes = sizeof(openvdb::tools::Film::RGBA), rowBytes = pixelBytes * image.width();
		openvdb::tools::Film::RGBA& pixel0 = const_cast<openvdb::tools::Film::RGBA*>(image.pixels())[0];
		Imf::FrameBuffer framebuffer;
		framebuffer.insert("R",
			Imf::Slice(Imf::FLOAT, reinterpret_cast<char*>(&pixel0.r), pixelBytes, rowBytes));
		if (numChannels >= 2)
			framebuffer.insert("G",
				Imf::Slice(Imf::FLOAT, reinterpret_cast<char*>(&pixel0.g), pixelBytes, rowBytes));
		if (numChannels >= 3)
			framebuffer.insert("B",
				Imf::Slice(Imf::FLOAT, reinterpret_cast<char*>(&pixel0.b), pixelBytes, rowBytes));
		if (numChannels >=4)
			framebuffer.insert("A",
				Imf::Slice(Imf::FLOAT, reinterpret_cast<char*>(&pixel0.a), pixelBytes, rowBytes));

		Imf::OutputFile imgFile(outputFilename.c_str(), header);
		imgFile.setFrameBuffer(framebuffer);
		imgFile.writePixels(int(image.height()));
	}
	else {
		std::cout << "unsupported image file format (" << outputFilename << ")" << std::endl;
	}
}

void renderSingle(openvdb::FloatGrid::Ptr grid, const Args& args)
{
	//high-res
	FilmPtr image = render(grid, args,
		args.cameraOrigin, args.cameraLookAt,
		args.cameraOrigin, args.cameraLookAt)[0];
	saveImage(*image, args.outputFilename, 4);

	//low-res
	if (!args.downscaleFilename.empty())
	{
		Args args2 = args;
		args2.samples = 1;
		args2.resolutionX = args.resolutionX / args.downscaleFactor;
		args2.resolutionY = args.resolutionY / args.downscaleFactor;
		auto images = render(grid, args2,
			args.cameraOrigin, args.cameraLookAt,
			args.cameraOrigin, args.cameraLookAt);
		saveImage(*images[0], args.downscaleFilename, 4);
		if (!args.downscaleDepthFilename.empty())
			saveImage(*images[1], args.downscaleDepthFilename, 4);
	}
}

openvdb::Vec3d interpolate(const openvdb::Vec3d& a, const openvdb::Vec3d& b, double alpha)
{
	return a * (1 - alpha) + b * alpha;
}
void renderAnimation(openvdb::FloatGrid::Ptr grid, const Args& args)
{
	for (int frame=0; frame < args.animFrames; ++frame)
	{
		openvdb::Vec3d currentCameraOrigin = interpolate(
			args.cameraOrigin, args.cameraOriginEnd,
			frame / double(args.animFrames - 1));
		openvdb::Vec3d nextCameraOrigin = interpolate(
			args.cameraOrigin, args.cameraOriginEnd,
			(frame+1) / double(args.animFrames - 1));
		openvdb::Vec3d currentCameraLookAt = interpolate(
			args.cameraLookAt, args.cameraLookAtEnd,
			frame / double(args.animFrames - 1));
		openvdb::Vec3d nextCameraLookAt = interpolate(
			args.cameraLookAt, args.cameraLookAtEnd,
			(frame + 1) / double(args.animFrames - 1));

		//high-res
		float time;
		auto images = render(grid, args,
			currentCameraOrigin, currentCameraLookAt,
			nextCameraOrigin, nextCameraLookAt,
			&time);
		saveImage(*images[0], tfm::format(args.outputFilename, frame), 4);
		if (!args.highDepthFilename.empty())
			saveImage(*images[1], tfm::format(args.highDepthFilename, frame), 4);
		std::cout << "Time to render high resolution: " << time << " sec" << std::endl;

		//low-res
		if (!args.downscaleFilename.empty())
		{
			Args args2 = args;
			args2.samples = 1;
			args2.resolutionX = args.resolutionX / args.downscaleFactor;
			args2.resolutionY = args.resolutionY / args.downscaleFactor;
			auto images = render(grid, args2,
				currentCameraOrigin, currentCameraLookAt,
				nextCameraOrigin, nextCameraLookAt,
				&time);
			saveImage(*images[0], tfm::format(args.downscaleFilename, frame), 4);
			if (!args.downscaleDepthFilename.empty())
				saveImage(*images[1], tfm::format(args.downscaleDepthFilename, frame), 4);
			if (!args.downscaleFlowFilename.empty())
				saveImage(*images[2], tfm::format(args.downscaleFlowFilename, frame), 4);
			std::cout << "Time to render low resolution: " << time << " sec" << std::endl;
		}
	}
}

void renderPipe(openvdb::FloatGrid::Ptr grid, Args args)
{
	std::cout << "Enter Pipe mode and wait for commands" << std::endl;

	openvdb::Vec3d lastOrigin = args.cameraOrigin;
	openvdb::Vec3d lastLookAt = args.cameraLookAt;

	if (args.pipeOutputFormat != "RGBMXYZDUV")
	{
		std::cout << "Unsupported pipe output format '" << args.pipeOutputFormat
			<< "', only 'RGBMXYZDUV' is supported" << std::endl;
		return;
	}
#define IDX(c, y, x) ((x) + args.resolutionX * ((y) + args.resolutionY * (c)))

	static const int32_t scale = 1000;

	while (true)
	{
		std::string command;
		std::getline(std::cin, command);
		if (command=="exit")
		{
			std::cout << "Exit program" << std::endl;
			return;
		} else if (command=="render")
		{
			//render
			float time;
			auto images = render(grid, args,
				args.cameraOrigin, args.cameraLookAt,
				lastOrigin, lastLookAt, &time); //use last camera as next camera and invert flow
			//write to stdout (cerr)
			std::vector<float> output(args.resolutionX * args.resolutionY * 12 + 1, 42.0f);
//#pragma omp parallel for
			for (int y=0; y<args.resolutionY; ++y)
				for (int x=0; x<args.resolutionX; ++x)
			{
				output[IDX(0, y, x)] = static_cast<float>(images[0]->pixel(x, y).r); //color
				output[IDX(1, y, x)] = static_cast<float>(images[0]->pixel(x, y).g);
				output[IDX(2, y, x)] = static_cast<float>(images[0]->pixel(x, y).b);
				output[IDX(3, y, x)] = static_cast<float>(images[0]->pixel(x, y).a); //mask
				output[IDX(4, y, x)] = static_cast<float>(images[1]->pixel(x, y).r); //normal
				output[IDX(5, y, x)] = static_cast<float>(images[1]->pixel(x, y).g);
				output[IDX(6, y, x)] = static_cast<float>(images[1]->pixel(x, y).b);
				output[IDX(7, y, x)] = static_cast<float>(images[1]->pixel(x, y).a); //depth
				output[IDX(8, y, x)] = -static_cast<float>(images[2]->pixel(x, y).r); //flow
				output[IDX(9, y, x)] = -static_cast<float>(images[2]->pixel(x, y).g);
				output[IDX(10,y, x)] = 1; //Ambient occlusion, not supported
				output[IDX(11,y, x)] = 0; //shadow, not supported
			}
			output[output.size() - 1] = time;
			//for (int c = 0; c < 10; ++c) for (int y = 0; y < args.resolutionY; ++y) {
			//	std::cerr.write(reinterpret_cast<const char*>(output.data() + args.resolutionX * (y + args.resolutionY*c)),
			//		args.resolutionX * sizeof(int32_t));
			//	std::cerr.flush();
			//}
			std::cerr.write(reinterpret_cast<const char*>(output.data()), output.size() * sizeof(float));
			std::cerr.flush();
			//save old camera position for flow
			lastOrigin = args.cameraOrigin;
			lastLookAt = args.cameraLookAt;
		} else
		{ //command=value
			auto idx = command.find('=');
			if (idx == std::string::npos)
			{
				std::cout << "Unknown command format: " << command << ", exit" << std::endl;
				return;
			}
			std::string cmd = command.substr(0, idx);
			std::string value = command.substr(idx + 1);
			if (cmd == "cameraOrigin")
				parseStr(value, args.cameraOrigin.x(), args.cameraOrigin.y(), args.cameraOrigin.z());
			else if (cmd == "cameraLookAt")
				parseStr(value, args.cameraLookAt.x(), args.cameraLookAt.y(), args.cameraLookAt.z());
			else if (cmd == "cameraUp")
				parseStr(value, args.cameraUp.x(), args.cameraUp.y(), args.cameraUp.z());
			else if (cmd == "cameraFoV")
				parseStr(value, args.cameraFov);
			else if (cmd == "resolution")
				parseStr(value, args.resolutionX, args.resolutionY);
			else if (cmd == "isovalue")
				parseStr(value, args.isovalue);
			else if (cmd == "unshaded")
				parseStr(value, args.noShading);
			else if (cmd == "aosamples")
				(void)0;
			else if (cmd == "aoradius")
				(void)0;
			else if (cmd == "viewport") //minX, minY, maxX, maxY
				(void)0;
			//TODO: more commands
			else {
				std::cout << "Unknown command: '" << cmd << "', exit" << std::endl;
				return;
			}
		}
	}
}

int main(int argc, const char* argv[])
{
	//https://stackoverflow.com/questions/36987437/data-corruption-piping-between-c-and-python
	_setmode(_fileno(stderr), O_BINARY);

	Args args = parseArguments(argc, argv);
	openvdb::initialize();

	if (args.renderMode == Args::RenderModeConvert) {
		convert(args);
		return 0;
	}

	openvdb::FloatGrid::Ptr grid = loadGrid(args);

	if (args.outputFilename == "PIPE")
		renderPipe(grid, args);
	else if (args.animFrames > 1)
		renderAnimation(grid, args);
	else
		renderSingle(grid, args);

}


/**
 * @copyright Copyright (c) 2017 B-com http://www.b-com.com/
 *
 * 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 <iostream>
#include <string>
#include <vector>
#include <set>
#include <cmath>
#include <boost/log/core.hpp>
 // ADD XPCF HEADERS HERE
#include "xpcf/xpcf.h"
#include "xpcf/threading/BaseTask.h"
#include "xpcf/threading/DropBuffer.h"
#include "core/Log.h"
// ADD COMPONENTS HEADERS HERE
#include "api/input/devices/ICamera.h"
#include "api/features/IKeypointDetector.h"
#include "api/features/IDescriptorsExtractor.h"
#include "api/solver/map/IMapper.h"
#include "api/display/I3DOverlay.h"
#include "api/display/IImageViewer.h"
#include "api/display/I3DPointsViewer.h"
#include "api/reloc/IKeyframeRetriever.h"
#include "api/storage/ICovisibilityGraph.h"
#include "api/storage/IKeyframesManager.h"
#include "api/storage/IPointCloudManager.h"
#include "api/solver/pose/IFiducialMarkerPose.h"
#include "api/solver/map/IBundler.h"
#include "api/geom/IUndistortPoints.h"
#include "api/loop/ILoopClosureDetector.h"
#include "api/loop/ILoopCorrector.h"
#include "api/slam/IBootstrapper.h"
#include "api/slam/ITracking.h"
#include "api/slam/IMapping.h"

#define NB_NEWKEYFRAMES_LOOP 5

using namespace SolAR;
using namespace SolAR::datastructure;
using namespace SolAR::api;
using namespace SolAR::api::storage;
using namespace SolAR::api::reloc;

namespace xpcf = org::bcom::xpcf;

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

#if NDEBUG
	boost::log::core::get()->set_logging_enabled(false);
#endif

	LOG_ADD_LOG_TO_CONSOLE();
	try {
		/* instantiate component manager*/
		/* this is needed in dynamic mode */
		SRef<xpcf::IComponentManager> xpcfComponentManager = xpcf::getComponentManagerInstance();

		std::string configxml = std::string("SolARSample_SLAM_Multi_conf.xml");
		if (argc == 2)
			configxml = std::string(argv[1]);
		if (xpcfComponentManager->load(configxml.c_str()) != org::bcom::xpcf::_SUCCESS)
		{
			LOG_ERROR("Failed to load the configuration file {}", configxml.c_str())
				return -1;
		}

		// declare and create components
		LOG_INFO("Start creating components");
		auto pointCloudManager = xpcfComponentManager->resolve<IPointCloudManager>();
		auto keyframesManager = xpcfComponentManager->resolve<IKeyframesManager>();
		auto covisibilityGraph = xpcfComponentManager->resolve<ICovisibilityGraph>();
		auto keyframeRetriever = xpcfComponentManager->resolve<IKeyframeRetriever>();
		auto mapper = xpcfComponentManager->resolve<solver::map::IMapper>();
		auto camera = xpcfComponentManager->resolve<input::devices::ICamera>();
		auto keypointsDetector = xpcfComponentManager->resolve<features::IKeypointDetector>();
		auto descriptorExtractor = xpcfComponentManager->resolve<features::IDescriptorsExtractor>();
		auto imageViewer = xpcfComponentManager->resolve<display::IImageViewer>();
		auto viewer3DPoints = xpcfComponentManager->resolve<display::I3DPointsViewer>();
		auto fiducialMarkerPoseEstimator = xpcfComponentManager->resolve<solver::pose::IFiducialMarkerPose>();
		auto bundler = xpcfComponentManager->resolve<api::solver::map::IBundler>();
		auto globalBundler = xpcfComponentManager->resolve<api::solver::map::IBundler>();
		auto undistortKeypoints = xpcfComponentManager->resolve<api::geom::IUndistortPoints>();
		auto loopDetector = xpcfComponentManager->resolve<loop::ILoopClosureDetector>();
		auto loopCorrector = xpcfComponentManager->resolve<loop::ILoopCorrector>();
		auto overlay3D = xpcfComponentManager->resolve<display::I3DOverlay>();
		auto bootstrapper = xpcfComponentManager->resolve<slam::IBootstrapper>();
		auto tracking = xpcfComponentManager->resolve<slam::ITracking>();
		auto mapping = xpcfComponentManager->resolve<slam::IMapping>();
		LOG_INFO("Loaded all components");

		// initialize pose estimation with the camera intrinsic parameters (please refeer to the use of intrinsec parameters file)
		CamCalibration calibration = camera->getIntrinsicsParameters();
		CamDistortion distortion = camera->getDistortionParameters();
		overlay3D->setCameraParameters(calibration, distortion);
		loopDetector->setCameraParameters(calibration, distortion);
		loopCorrector->setCameraParameters(calibration, distortion);
		fiducialMarkerPoseEstimator->setCameraParameters(calibration, distortion);
		undistortKeypoints->setCameraParameters(calibration, distortion);
		bootstrapper->setCameraParameters(calibration, distortion);
		tracking->setCameraParameters(calibration, distortion);
		mapping->setCameraParameters(calibration, distortion);
		LOG_DEBUG("Intrincic parameters : \n {}", calibration);

		// get properties
		float minWeightNeighbor = mapping->bindTo<xpcf::IConfigurable>()->getProperty("minWeightNeighbor")->getFloatingValue();
		float reprojErrorThreshold = mapper->bindTo<xpcf::IConfigurable>()->getProperty("reprojErrorThreshold")->getFloatingValue();

		if (camera->start() != FrameworkReturnCode::_SUCCESS)
		{
			LOG_ERROR("Camera cannot start");
			return -1;
		}

		// display point cloud function
		auto fnDisplay = [&keyframesManager, &pointCloudManager, &viewer3DPoints](const std::vector<Transform3Df>& framePoses) {
			// get all keyframes and point cloud
			std::vector<Transform3Df>   keyframePoses;
			std::vector<SRef<Keyframe>> allKeyframes;
			keyframesManager->getAllKeyframes(allKeyframes);
			for (auto const &it : allKeyframes)
				keyframePoses.push_back(it->getPose());
			std::vector<SRef<CloudPoint>> pointCloud;
			pointCloudManager->getAllPoints(pointCloud);
			// display point cloud
			if (framePoses.size() == 0
				|| viewer3DPoints->display(pointCloud, framePoses.back(), keyframePoses, framePoses) == FrameworkReturnCode::_STOP)
				return false;
			else
				return true;
		};

		xpcf::DropBuffer<SRef<Image>>												m_dropBufferCamImageCapture;
		xpcf::DropBuffer<SRef<Image>>												m_dropBufferCamImageCaptureBootstrap;
		xpcf::DropBuffer<SRef<Frame>>												m_dropBufferAddKeyframe;
		xpcf::DropBuffer<SRef<Keyframe>>											m_dropBufferNewKeyframe;
		xpcf::DropBuffer<SRef<Keyframe>>											m_dropBufferNewKeyframeLoop;
		xpcf::DropBuffer<SRef<Image>>												m_dropBufferDisplay;
		xpcf::DropBuffer< std::pair<SRef<Image>, std::vector<Keypoint>>>			m_dropBufferKeypoints;
		xpcf::DropBuffer<SRef<Frame>>												m_dropBufferFrameDescriptors;
		bool stop = false;
		bool bootstrapOk = false;
		SRef<Keyframe> keyframe2;
		std::vector<Transform3Df> framePoses;
		bool isStopMapping = false;
		int countNewKeyframes = 0;

		// Load map from file
		if (mapper->loadFromFile() == FrameworkReturnCode::_SUCCESS)
		{
			LOG_INFO("Load map done!");
		}
		else
		{
			LOG_WARNING("Failed to load map from file");
		}

		if (pointCloudManager->getNbPoints() > 0)
		{
			// Map loaded from file and not empty
			if (keyframesManager->getKeyframe(0, keyframe2) != FrameworkReturnCode::_SUCCESS)
			{
				return -1; //FrameworkReturnCode::_ERROR_;
			}
			tracking->updateReferenceKeyframe(keyframe2);
			framePoses.push_back(keyframe2->getPose());
			LOG_INFO("Number of initial point cloud: {}", pointCloudManager->getNbPoints());
			LOG_INFO("Number of initial keyframes: {}", keyframesManager->getNbKeyframes());
			bootstrapOk = true;
		}
		else
		{
			// Either no or empty map files
			LOG_INFO("Initialization from scratch");
		}

		// Camera image capture task
		auto fnCamImageCapture = [&]()
		{
			SRef<Image> view;
			if (camera->getNextImage(view) != SolAR::FrameworkReturnCode::_SUCCESS)
			{
				stop = true;
				return;
			}
			if (bootstrapOk)
				m_dropBufferCamImageCapture.push(view);
			else
				m_dropBufferCamImageCaptureBootstrap.push(view);
		};

		// Bootstrap task
		auto fnBootstrap = [&]()
		{
			SRef<Image> image;
			if (bootstrapOk || !m_dropBufferCamImageCaptureBootstrap.tryPop(image)) {
				xpcf::DelegateTask::yield();
				return;
			}
			SRef<Image> view;
			Transform3Df pose = Transform3Df::Identity();
			fiducialMarkerPoseEstimator->estimate(image, pose);
			if (bootstrapper->process(image, view, pose) == FrameworkReturnCode::_SUCCESS) {
				double bundleReprojError = bundler->bundleAdjustment(calibration, distortion);
				keyframesManager->getKeyframe(1, keyframe2);
				tracking->updateReferenceKeyframe(keyframe2);
				framePoses.push_back(keyframe2->getPose());
				LOG_INFO("Number of initial point cloud: {}", pointCloudManager->getNbPoints());
				LOG_INFO("Number of initial keyframes: {}", keyframesManager->getNbKeyframes());
				bootstrapOk = true;
			}
			if (!pose.isApprox(Transform3Df::Identity()))
				overlay3D->draw(pose, view);
			m_dropBufferDisplay.push(view);
		};

		// Keypoint detection task
		auto fnDetection = [&]()
		{
			SRef<Image> frame;
			if (!m_dropBufferCamImageCapture.tryPop(frame)) {
				xpcf::DelegateTask::yield();
				return;
			}
			std::vector<Keypoint> keypoints;
			keypointsDetector->detect(frame, keypoints);
			m_dropBufferKeypoints.push(std::make_pair(frame, keypoints));
		};

		// Feature extraction task
		auto fnExtraction = [&]()
		{
			std::pair<SRef<Image>, std::vector<Keypoint>> frameKeypoints;
			if (!m_dropBufferKeypoints.tryPop(frameKeypoints)) {
				xpcf::DelegateTask::yield();
				return;
			}
			std::vector<Keypoint> undistortedKeypoints;
			undistortKeypoints->undistort(frameKeypoints.second, undistortedKeypoints);
			SRef<DescriptorBuffer> descriptors;
			descriptorExtractor->extract(frameKeypoints.first, frameKeypoints.second, descriptors);
			SRef<Frame> frame = xpcf::utils::make_shared<Frame>(frameKeypoints.second, undistortedKeypoints, descriptors, frameKeypoints.first);
			m_dropBufferFrameDescriptors.push(frame);
		};

		// Tracking task
		auto fnTracking = [&]()
		{
			SRef<Frame> frame;
			if (!bootstrapOk || !m_dropBufferFrameDescriptors.tryPop(frame)) {
				xpcf::DelegateTask::yield();
				return;
			}
			SRef<Keyframe> newKeyframe;
			if (m_dropBufferNewKeyframe.tryPop(newKeyframe))
			{
				tracking->updateReferenceKeyframe(newKeyframe);
				SRef<Frame> tmpFrame;
				m_dropBufferAddKeyframe.tryPop(tmpFrame);
				isStopMapping = false;
			}
			// tracking
			SRef<Image>	displayImage;
			if (tracking->process(frame, displayImage) == FrameworkReturnCode::_SUCCESS) {
				// used for display
				framePoses.push_back(frame->getPose());
				// draw cube
				overlay3D->draw(frame->getPose(), displayImage);
				// send frame to mapping task
				m_dropBufferAddKeyframe.push(frame);
			}

			m_dropBufferDisplay.push(displayImage);
		};

		// check need new keyframe
		auto fnMapping = [&]()
		{
			SRef<Frame> frame;
			if (isStopMapping || (!m_dropBufferAddKeyframe.tryPop(frame))) {
				xpcf::DelegateTask::yield();
				return;
			}
			SRef<Keyframe> keyframe;
			if (mapping->process(frame, keyframe) == FrameworkReturnCode::_SUCCESS) {
				LOG_DEBUG("New keyframe id: {}", keyframe->getId());
				// Local bundle adjustment
				std::vector<uint32_t> bestIdx, bestIdxToOptimize;
				covisibilityGraph->getNeighbors(keyframe->getId(), minWeightNeighbor, bestIdx);
				if (bestIdx.size() < 10)
					bestIdxToOptimize.swap(bestIdx);
				else
					bestIdxToOptimize.insert(bestIdxToOptimize.begin(), bestIdx.begin(), bestIdx.begin() + 10);
				bestIdxToOptimize.push_back(keyframe->getId());
				double bundleReprojError = bundler->bundleAdjustment(calibration, distortion, bestIdxToOptimize);
				mapper->pruning();
				countNewKeyframes++;
				m_dropBufferNewKeyframeLoop.push(keyframe);
			}

			if (keyframe) {
				isStopMapping = true;
				m_dropBufferNewKeyframe.push(keyframe);
			}

		};

		// loop closure task
		auto fnLoopClosure = [&]() {
			SRef<Keyframe> lastKeyframe;
			if ((countNewKeyframes < NB_NEWKEYFRAMES_LOOP) || (!m_dropBufferNewKeyframeLoop.tryPop(lastKeyframe))) {
				xpcf::DelegateTask::yield();
				return;
			}
			uint32_t lastKeyframeId = lastKeyframe->getId();
			SRef<Keyframe> detectedLoopKeyframe;
			Transform3Df sim3Transform;
			std::vector<std::pair<uint32_t, uint32_t>> duplicatedPointsIndices;
			if (loopDetector->detect(lastKeyframe, detectedLoopKeyframe, sim3Transform, duplicatedPointsIndices) == FrameworkReturnCode::_SUCCESS) {
				// detected loop keyframe
				LOG_INFO("Detected loop keyframe id: {}", detectedLoopKeyframe->getId());
				LOG_INFO("Nb of duplicatedPointsIndices: {}", duplicatedPointsIndices.size());
				LOG_INFO("sim3Transform: \n{}", sim3Transform.matrix());
				// performs loop correction
				loopCorrector->correct(lastKeyframe, detectedLoopKeyframe, sim3Transform, duplicatedPointsIndices);
				countNewKeyframes = 0;
				// Loop optimisation
				globalBundler->bundleAdjustment(calibration, distortion);
				LOG_DEBUG("Global BA done");
				mapper->pruning();
			}
		};

		// instantiate and start tasks
		xpcf::DelegateTask taskCamImageCapture(fnCamImageCapture);
		xpcf::DelegateTask taskBootstrap(fnBootstrap);
		xpcf::DelegateTask taskDetection(fnDetection);
		xpcf::DelegateTask taskExtraction(fnExtraction);
		xpcf::DelegateTask taskTracking(fnTracking);
		xpcf::DelegateTask taskMapping(fnMapping);
		xpcf::DelegateTask taskLoopClosure(fnLoopClosure);

		taskCamImageCapture.start();
		taskBootstrap.start();
		taskDetection.start();
		taskExtraction.start();
		taskTracking.start();
		taskMapping.start();
		taskLoopClosure.start();

		// Start tracking
		clock_t start, end;
		start = clock();
		int count(0);
		while (!stop)
		{
			SRef<Image> displayImage;
			if (!m_dropBufferDisplay.tryPop(displayImage)) {
				xpcf::DelegateTask::yield();
				continue;
			}
			if (imageViewer->display(displayImage) == SolAR::FrameworkReturnCode::_STOP)
				stop = true;
			if (bootstrapOk) {
				if (!fnDisplay(framePoses))
					stop = true;
			}
			++count;
		}

		// Stop tasks
		taskCamImageCapture.stop();
		taskBootstrap.stop();
		taskDetection.stop();
		taskExtraction.stop();
		taskTracking.stop();
		taskMapping.stop();
		taskLoopClosure.stop();

		// display stats on frame rate
		end = clock();
		double duration = double(end - start) / CLOCKS_PER_SEC;
		printf("\n\nElasped time is %.2lf seconds.\n", duration);
		printf("Number of processed frame per second : %8.2f\n", count / duration);

		// run global BA before exit
		bundler->bundleAdjustment(calibration, distortion);
		mapper->pruning();
		LOG_INFO("Nb keyframes of map: {}", keyframesManager->getNbKeyframes());
		LOG_INFO("Nb cloud points of map: {}", pointCloudManager->getNbPoints());

		// visualize final map
		while (fnDisplay(framePoses)) {}

		// Save map
		mapper->saveToFile();
	}
	catch (xpcf::Exception e)
	{
		LOG_ERROR("The following exception has been catch : {}", e.what());
		return -1;
	}
	return 0;
}


/* boost random/subtract_with_carry.hpp header file
 *
 * Copyright Jens Maurer 2002
 * 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_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: subtract_with_carry.hpp 72951 2011-07-07 04:57:37Z steven_watanabe $
 *
 * Revision history
 *  2002-03-02  created
 */

#ifndef BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP
#define BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP

#include <boost/config/no_tr1/cmath.hpp>         // std::pow
#include <iostream>
#include <algorithm>     // std::equal
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/random/detail/generator_seed_seq.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

namespace detail {

struct subtract_with_carry_discard
{
    template<class Engine>
    static void apply(Engine& eng, boost::uintmax_t z)
    {
        typedef typename Engine::result_type IntType;
        const std::size_t short_lag = Engine::short_lag;
        const std::size_t long_lag = Engine::long_lag;
        std::size_t k = eng.k;
        IntType carry = eng.carry;
        if(k != 0) {
            // increment k until it becomes 0.
            if(k < short_lag) {
                std::size_t limit = (short_lag - k) < z?
                    short_lag : (k + static_cast<std::size_t>(z));
                for(std::size_t j = k; j < limit; ++j) {
                    carry = eng.do_update(j, j + long_lag - short_lag, carry);
                }
            }
            std::size_t limit = (long_lag - k) < z?
                long_lag : (k + static_cast<std::size_t>(z));
            std::size_t start = (k < short_lag ? short_lag : k);
            for(std::size_t j = start; j < limit; ++j) {
                carry = eng.do_update(j, j - short_lag, carry);
            }
        }

        k = ((z % long_lag) + k) % long_lag;

        if(k < z) {
            // main loop: update full blocks from k = 0 to long_lag
            for(std::size_t i = 0; i < (z - k) / long_lag; ++i) {
                for(std::size_t j = 0; j < short_lag; ++j) {
                    carry = eng.do_update(j, j + long_lag - short_lag, carry);
                }
                for(std::size_t j = short_lag; j < long_lag; ++j) {
                    carry = eng.do_update(j, j - short_lag, carry);
                }
            }

            // Update the last partial block
            std::size_t limit = short_lag < k? short_lag : k;
            for(std::size_t j = 0; j < limit; ++j) {
                carry = eng.do_update(j, j + long_lag - short_lag, carry);
            }
            for(std::size_t j = short_lag; j < k; ++j) {
                carry = eng.do_update(j, j - short_lag, carry);
            }
        }
        eng.carry = carry;
        eng.k = k;
    }
};

}

/**
 * Instantiations of @c subtract_with_carry_engine model a
 * \pseudo_random_number_generator.  The algorithm is
 * described in
 *
 *  @blockquote
 *  "A New Class of Random Number Generators", George
 *  Marsaglia and Arif Zaman, Annals of Applied Probability,
 *  Volume 1, Number 3 (1991), 462-480.
 *  @endblockquote
 */
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
class subtract_with_carry_engine
{
public:
    typedef IntType result_type;
    BOOST_STATIC_CONSTANT(std::size_t, word_size = w);
    BOOST_STATIC_CONSTANT(std::size_t, long_lag = r);
    BOOST_STATIC_CONSTANT(std::size_t, short_lag = s);
    BOOST_STATIC_CONSTANT(uint32_t, default_seed = 19780503u);

    // Required by the old Boost.Random concepts
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    // Backwards compatibility
    BOOST_STATIC_CONSTANT(result_type, modulus = (result_type(1) << w));

    BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_integer);

    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with the default seed.
     */
    subtract_with_carry_engine() { seed(); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_engine,
                                               IntType, value)
    { seed(value); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with values produced by @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(subtract_with_carry_engine,
                                             SeedSeq, seq)
    { seed(seq); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with values from a range.  first is updated to point
     * one past the last value consumed.  If there are not
     * enough elements in the range to fill the entire state of
     * the generator, throws @c std::invalid_argument.
     */
    template<class It> subtract_with_carry_engine(It& first, It last)
    { seed(first,last); }

    // compiler-generated copy ctor and assignment operator are fine

    /** Seeds the generator with the default seed. */
    void seed() { seed(default_seed); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_engine,
                                        IntType, value)
    {
        typedef linear_congruential_engine<uint32_t,40014,0,2147483563> gen_t;
        gen_t intgen(static_cast<boost::uint32_t>(value));
        detail::generator_seed_seq<gen_t> gen(intgen);
        seed(gen);
    }

    /** Seeds the generator with values produced by @c seq.generate(). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(subtract_with_carry, SeedSeq, seq)
    {
        detail::seed_array_int<w>(seq, x);
        carry = (x[long_lag-1] == 0);
        k = 0;
    }

    /**
     * Seeds the generator with values from a range.  Updates @c first to
     * point one past the last consumed value.  If the range does not
     * contain enough elements to fill the entire state of the generator,
     * throws @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_int<w>(first, last, x);
        carry = (x[long_lag-1] == 0);
        k = 0;
    }

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return 0; }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return boost::low_bits_mask_t<w>::sig_bits; }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        std::size_t short_index =
            (k < short_lag)?
                (k + long_lag - short_lag) :
                (k - short_lag);
        carry = do_update(k, short_index, carry);
        IntType result = x[k];
        ++k;
        if(k >= long_lag)
            k = 0;
        return result;
    }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        detail::subtract_with_carry_discard::apply(*this, z);
    }

    /** Fills a range with random values. */
    template<class It>
    void generate(It first, It last)
    { detail::generate_from_int(*this, first, last); }

    /** Writes a @c subtract_with_carry_engine to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, subtract_with_carry_engine, f)
    {
        for(unsigned int j = 0; j < f.long_lag; ++j)
            os << f.compute(j) << ' ';
        os << f.carry;
        return os;
    }

    /** Reads a @c subtract_with_carry_engine from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, subtract_with_carry_engine, f)
    {
        for(unsigned int j = 0; j < f.long_lag; ++j)
            is >> f.x[j] >> std::ws;
        is >> f.carry;
        f.k = 0;
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of values.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(subtract_with_carry_engine, x, y)
    {
        for(unsigned int j = 0; j < r; ++j)
            if(x.compute(j) != y.compute(j))
                return false;
        return true;
    }

    /**
     * Returns true if the two generators will produce different
     * sequences of values.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(subtract_with_carry_engine)

private:
    /// \cond show_private
    // returns x(i-r+index), where index is in 0..r-1
    IntType compute(unsigned int index) const
    {
        return x[(k+index) % long_lag];
    }

    friend struct detail::subtract_with_carry_discard;

    IntType do_update(std::size_t current, std::size_t short_index, IntType carry)
    {
        IntType delta;
        IntType temp = x[current] + carry;
        if (x[short_index] >= temp) {
            // x(n) >= 0
            delta =  x[short_index] - temp;
            carry = 0;
        } else {
            // x(n) < 0
            delta = modulus - temp + x[short_index];
            carry = 1;
        }
        x[current] = delta;
        return carry;
    }
    /// \endcond

    // state representation; next output (state) is x(i)
    //   x[0]  ... x[k] x[k+1] ... x[long_lag-1]     represents
    //  x(i-k) ... x(i) x(i+1) ... x(i-k+long_lag-1)
    // speed: base: 20-25 nsec
    // ranlux_4: 230 nsec, ranlux_7: 430 nsec, ranlux_14: 810 nsec
    // This state representation makes operator== and save/restore more
    // difficult, because we've already computed "too much" and thus
    // have to undo some steps to get at x(i-r) etc.

    // state representation: next output (state) is x(i)
    //   x[0]  ... x[k] x[k+1]          ... x[long_lag-1]     represents
    //  x(i-k) ... x(i) x(i-long_lag+1) ... x(i-k-1)
    // speed: base 28 nsec
    // ranlux_4: 370 nsec, ranlux_7: 688 nsec, ranlux_14: 1343 nsec
    IntType x[long_lag];
    std::size_t k;
    IntType carry;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const bool subtract_with_carry_engine<IntType, w, s, r>::has_fixed_range;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const IntType subtract_with_carry_engine<IntType, w, s, r>::modulus;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::word_size;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::long_lag;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::short_lag;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const uint32_t subtract_with_carry_engine<IntType, w, s, r>::default_seed;
#endif

// use a floating-point representation to produce values in [0..1)
/**
 * Instantiations of \subtract_with_carry_01_engine model a
 * \pseudo_random_number_generator.  The algorithm is
 * described in
 *
 *  @blockquote
 *  "A New Class of Random Number Generators", George
 *  Marsaglia and Arif Zaman, Annals of Applied Probability,
 *  Volume 1, Number 3 (1991), 462-480.
 *  @endblockquote
 */
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
class subtract_with_carry_01_engine
{
public:
    typedef RealType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    BOOST_STATIC_CONSTANT(std::size_t, word_size = w);
    BOOST_STATIC_CONSTANT(std::size_t, long_lag = r);
    BOOST_STATIC_CONSTANT(std::size_t, short_lag = s);
    BOOST_STATIC_CONSTANT(boost::uint32_t, default_seed = 19780503u);

    BOOST_STATIC_ASSERT(!std::numeric_limits<result_type>::is_integer);

    /** Creates a new \subtract_with_carry_01_engine using the default seed. */
    subtract_with_carry_01_engine() { init_modulus(); seed(); }
    /** Creates a new subtract_with_carry_01_engine and seeds it with value. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_01_engine,
                                               boost::uint32_t, value)
    { init_modulus(); seed(value); }
    /**
     * Creates a new \subtract_with_carry_01_engine and seeds with with values
     * produced by seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(subtract_with_carry_01_engine,
                                             SeedSeq, seq)
    { init_modulus(); seed(seq); }
    /**
     * Creates a new \subtract_with_carry_01_engine and seeds it with values
     * from a range.  Advances first to point one past the last consumed
     * value.  If the range does not contain enough elements to fill the
     * entire state, throws @c std::invalid_argument.
     */
    template<class It> subtract_with_carry_01_engine(It& first, It last)
    { init_modulus(); seed(first,last); }

private:
    /// \cond show_private
    void init_modulus()
    {
#ifndef BOOST_NO_STDC_NAMESPACE
        // allow for Koenig lookup
        using std::pow;
#endif
        _modulus = pow(RealType(2), RealType(word_size));
    }
    /// \endcond

public:
    // compiler-generated copy ctor and assignment operator are fine

    /** Seeds the generator with the default seed. */
    void seed() { seed(default_seed); }

    /** Seeds the generator with @c value. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_01_engine,
                                        boost::uint32_t, value)
    {
        typedef linear_congruential_engine<uint32_t, 40014, 0, 2147483563> gen_t;
        gen_t intgen(value);
        detail::generator_seed_seq<gen_t> gen(intgen);
        seed(gen);
    }

    /** Seeds the generator with values produced by @c seq.generate(). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(subtract_with_carry_01_engine,
                                      SeedSeq, seq)
    {
        detail::seed_array_real<w>(seq, x);
        carry = (x[long_lag-1] ? 0 : 1 / _modulus);
        k = 0;
    }

    /**
     * Seeds the generator with values from a range.  Updates first to
     * point one past the last consumed element.  If there are not
     * enough elements in the range to fill the entire state, throws
     * @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_real<w>(first, last, x);
        carry = (x[long_lag-1] ? 0 : 1 / _modulus);
        k = 0;
    }

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return result_type(0); }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return result_type(1); }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        std::size_t short_index =
            (k < short_lag) ?
                (k + long_lag - short_lag) :
                (k - short_lag);
        carry = do_update(k, short_index, carry);
        RealType result = x[k];
        ++k;
        if(k >= long_lag)
            k = 0;
        return result;
    }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    { detail::subtract_with_carry_discard::apply(*this, z); }

    /** Fills a range with random values. */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_real(*this, first, last); }

    /** Writes a \subtract_with_carry_01_engine to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, subtract_with_carry_01_engine, f)
    {
        std::ios_base::fmtflags oldflags =
            os.flags(os.dec | os.fixed | os.left);
        for(unsigned int j = 0; j < f.long_lag; ++j)
            os << (f.compute(j) * f._modulus) << ' ';
        os << (f.carry * f._modulus);
        os.flags(oldflags);
        return os;
    }

    /** Reads a \subtract_with_carry_01_engine from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, subtract_with_carry_01_engine, f)
    {
        RealType value;
        for(unsigned int j = 0; j < long_lag; ++j) {
            is >> value >> std::ws;
            f.x[j] = value / f._modulus;
        }
        is >> value;
        f.carry = value / f._modulus;
        f.k = 0;
        return is;
    }

    /** Returns true if the two generators will produce identical sequences. */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(subtract_with_carry_01_engine, x, y)
    {
        for(unsigned int j = 0; j < r; ++j)
            if(x.compute(j) != y.compute(j))
                return false;
        return true;
    }

    /** Returns true if the two generators will produce different sequences. */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(subtract_with_carry_01_engine)

private:
    /// \cond show_private
    RealType compute(unsigned int index) const
    {
        return x[(k+index) % long_lag];
    }

    friend struct detail::subtract_with_carry_discard;

    RealType do_update(std::size_t current, std::size_t short_index, RealType carry)
    {
        RealType delta = x[short_index] - x[current] - carry;
        if(delta < 0) {
            delta += RealType(1);
            carry = RealType(1)/_modulus;
        } else {
            carry = 0;
        }
        x[current] = delta;
        return carry;
    }
    /// \endcond
    std::size_t k;
    RealType carry;
    RealType x[long_lag];
    RealType _modulus;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const bool subtract_with_carry_01_engine<RealType, w, s, r>::has_fixed_range;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::word_size;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::long_lag;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::short_lag;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const uint32_t subtract_with_carry_01_engine<RealType, w, s, r>::default_seed;
#endif

/// \cond show_deprecated

template<class IntType, IntType m, unsigned s, unsigned r, IntType v>
class subtract_with_carry :
    public subtract_with_carry_engine<IntType,
        boost::static_log2<m>::value, s, r>
{
    typedef subtract_with_carry_engine<IntType,
        boost::static_log2<m>::value, s, r> base_type;
public:
    subtract_with_carry() {}
    BOOST_RANDOM_DETAIL_GENERATOR_CONSTRUCTOR(subtract_with_carry, Gen, gen)
    { seed(gen); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry,
                                               IntType, val)
    { seed(val); }
    template<class It>
    subtract_with_carry(It& first, It last) : base_type(first, last) {}
    void seed() { base_type::seed(); }
    BOOST_RANDOM_DETAIL_GENERATOR_SEED(subtract_with_carry, Gen, gen)
    {
        detail::generator_seed_seq<Gen> seq(gen);
        base_type::seed(seq);
    }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry, IntType, val)
    { base_type::seed(val); }
    template<class It>
    void seed(It& first, It last) { base_type::seed(first, last); }
};

template<class RealType, int w, unsigned s, unsigned r, int v = 0>
class subtract_with_carry_01 :
    public subtract_with_carry_01_engine<RealType, w, s, r>
{
    typedef subtract_with_carry_01_engine<RealType, w, s, r> base_type;
public:
    subtract_with_carry_01() {}
    BOOST_RANDOM_DETAIL_GENERATOR_CONSTRUCTOR(subtract_with_carry_01, Gen, gen)
    { seed(gen); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_01,
                                               uint32_t, val)
    { seed(val); }
    template<class It>
    subtract_with_carry_01(It& first, It last) : base_type(first, last) {}
    void seed() { base_type::seed(); }
    BOOST_RANDOM_DETAIL_GENERATOR_SEED(subtract_with_carry_01, Gen, gen)
    {
        detail::generator_seed_seq<Gen> seq(gen);
        base_type::seed(seq);
    }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_01, uint32_t, val)
    { base_type::seed(val); }
    template<class It>
    void seed(It& first, It last) { base_type::seed(first, last); }
};

/// \endcond

namespace detail {

template<class Engine>
struct generator_bits;

template<class RealType, std::size_t w, std::size_t s, std::size_t r>
struct generator_bits<subtract_with_carry_01_engine<RealType, w, s, r> > {
    static std::size_t value() { return w; }
};

template<class RealType, int w, unsigned s, unsigned r, int v>
struct generator_bits<subtract_with_carry_01<RealType, w, s, r, v> > {
    static std::size_t value() { return w; }
};

}

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP


/*
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2020, INRIA
 */

#include "eigenpy/eigenpy.hpp"
#include "eigenpy/version.hpp"
#include "eigenpy/geometry.hpp"

#include "eigenpy/computation-info.hpp"

#include "eigenpy/solvers/solvers.hpp"
#include "eigenpy/solvers/preconditioners.hpp"
#include "eigenpy/decompositions/decompositions.hpp"

#include "eigenpy/utils/is-approx.hpp"

#include <boost/python/scope.hpp>

using namespace eigenpy;

BOOST_PYTHON_MODULE(eigenpy)
{
  namespace bp = boost::python;
  enableEigenPy();

  bp::scope().attr("__version__") = eigenpy::printVersion();
  bp::scope().attr("__raw_version__") = bp::str(EIGENPY_VERSION);
  bp::def("checkVersionAtLeast",&eigenpy::checkVersionAtLeast,
          bp::args("major_version","minor_version","patch_version"),
          "Checks if the current version of EigenPy is at least the version provided by the input arguments.");

  exposeAngleAxis();
  exposeQuaternion();
  exposeGeometryConversion();

  exposeComputationInfo();

  {
    bp::scope solvers = boost::python::class_<SolversScope>("solvers");
    exposeSolvers();
    exposePreconditioners();

    register_symbolic_link_to_registered_type<Eigen::ComputationInfo>();
  }

  {
    using namespace Eigen;

    bp::def("is_approx",(bool (*)(const Eigen::MatrixBase<MatrixXd> &, const Eigen::MatrixBase<MatrixXd> &, const double &))&is_approx<MatrixXd,MatrixXd>,
            bp::args("A","B","prec"),
            "Returns True if A is approximately equal to B, within the precision determined by prec.");

    bp::def("is_approx",(bool (*)(const Eigen::MatrixBase<MatrixXd> &, const Eigen::MatrixBase<MatrixXd> &))&is_approx<MatrixXd,MatrixXd>,
            bp::args("A","B"),
            "Returns True if A is approximately equal to B.");
  }

  exposeDecompositions();
}