/**
* This file is part of DSO.
*
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO 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 DSO. If not, see <http://www.gnu.org/licenses/>.
*/

/*
 * KFBuffer.cpp
 *
 *  Created on: Jan 7, 2014
 *      Author: engelj
 */

#include "FullSystem/FullSystem.h"

#include "stdio.h"
#include "util/globalFuncs.h"
#include <Eigen/LU>
#include <algorithm>
#include "IOWrapper/ImageDisplay.h"
#include "util/globalCalib.h"
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include "FullSystem/PixelSelector.h"
#include "FullSystem/PixelSelector2.h"
#include "FullSystem/ResidualProjections.h"
#include "FullSystem/ImmaturePoint.h"

#include "FullSystem/CoarseTracker.h"
#include "FullSystem/CoarseInitializer.h"

#include "OptimizationBackend/EnergyFunctional.h"
#include "OptimizationBackend/EnergyFunctionalStructs.h"

#include "IOWrapper/Output3DWrapper.h"

#include <cmath>

//#include <monodepth/monodepth.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <string>

#include <chrono>

namespace dso
{
int FrameHessian::instanceCounter=0;
int PointHessian::instanceCounter=0;
int CalibHessian::instanceCounter=0;
int frameNumber = 0;
std::string depthPrefix = "";
float depthMapScaleFactor = 1;

FullSystem::FullSystem(std::string prefix,float scaleFactor) {

    int retstat = 0;
    if (setting_logStuff) {

        retstat += system("rm -rf logs");
        retstat += system("mkdir logs");

        retstat += system("rm -rf mats");
        retstat += system("mkdir mats");
        eigenAllLog->precision(10);

        eigenPLog = new std::ofstream();
        eigenPLog->open("logs/eigenPLog.txt", std::ios::trunc | std::ios::out);
        eigenPLog->precision(10);

        eigenALog = new std::ofstream();
        eigenALog->open("logs/eigenALog.txt", std::ios::trunc | std::ios::out);
        eigenALog->precision(10);

        DiagonalLog = new std::ofstream();
        DiagonalLog->open("logs/diagonal.txt", std::ios::trunc | std::ios::out);
        DiagonalLog->precision(10);

        variancesLog = new std::ofstream();
        variancesLog->open("logs/variancesLog.txt", std::ios::trunc | std::ios::out);
        variancesLog->precision(10);

        nullspacesLog = new std::ofstream();
        nullspacesLog->open("logs/nullspacesLog.txt", std::ios::trunc | std::ios::out);
        nullspacesLog->precision(10);
    } else {
        nullspacesLog = 0;
        variancesLog = 0;
        DiagonalLog = 0;
        eigenALog = 0;
        eigenPLog = 0;
        eigenAllLog = 0;
        numsLog = 0;
        calibLog = 0;
    }

    assert(retstat != 293847);

    selectionMap = new float[wG[0] * hG[0]];

    coarseDistanceMap = new CoarseDistanceMap(wG[0], hG[0]);
    coarseTracker = new CoarseTracker(wG[0], hG[0]);
    coarseTracker_forNewKF = new CoarseTracker(wG[0], hG[0]);
    coarseInitializer = new CoarseInitializer(wG[0], hG[0]);
    pixelSelector = new PixelSelector(wG[0], hG[0]);

    statistics_lastNumOptIts = 0;
    statistics_numDroppedPoints = 0;
    statistics_numActivatedPoints = 0;
    statistics_numCreatedPoints = 0;
    statistics_numForceDroppedResBwd = 0;
    statistics_numForceDroppedResFwd = 0;
    statistics_numMargResFwd = 0;
    statistics_numMargResBwd = 0;

    lastCoarseRMSE.setConstant(100);

    currentMinActDist = 2;
    initialized = false;

    ef = new EnergyFunctional();
    ef->red = &this->treadReduce;

    isLost = false;
    initFailed = false;

    needNewKFAfter = -1;

    linearizeOperation = true;
    runMapping = true;
    mappingThread = boost::thread(&FullSystem::mappingLoop, this);
    lastRefStopID = 0;

    minIdJetVisDebug = -1;
    maxIdJetVisDebug = -1;
    minIdJetVisTracker = -1;
    maxIdJetVisTracker = -1;

	depthPrefix = prefix;
	depthMapScaleFactor = scaleFactor;
	// depthPredictor = new monodepth::MonoDepth(wG[0], hG[0], path_cnn);
	//	depthPredictor = new monodepth::MonoDepth(wG[0], hG[0],
	//                                  "/home/hide/catkin_ws/src/CNN-SVO/rpg_svo/monodepth-cpp/model/model_city2kitti.pb");
}
FullSystem::~FullSystem()
{
	blockUntilMappingIsFinished();

	if(setting_logStuff)
	{
		calibLog->close(); delete calibLog;
		numsLog->close(); delete numsLog;
		coarseTrackingLog->close(); delete coarseTrackingLog;
		//errorsLog->close(); delete errorsLog;
		eigenAllLog->close(); delete eigenAllLog;
		eigenPLog->close(); delete eigenPLog;
		eigenALog->close(); delete eigenALog;
		DiagonalLog->close(); delete DiagonalLog;
		variancesLog->close(); delete variancesLog;
		nullspacesLog->close(); delete nullspacesLog;
	}

	delete[] selectionMap;

	for(FrameShell* s : allFrameHistory)
		delete s;
	for(FrameHessian* fh : unmappedTrackedFrames)
		delete fh;

	delete coarseDistanceMap;
	delete coarseTracker;
	delete coarseTracker_forNewKF;
	delete coarseInitializer;
	delete pixelSelector;
	delete ef;
	// delete depthPredictor;
}

void FullSystem::setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH)
{

}

void FullSystem::setGammaFunction(float* BInv)
{
	if(BInv==0) return;

	// copy BInv.
	memcpy(Hcalib.Binv, BInv, sizeof(float)*256);

	// invert.
	for(int i=1;i<255;i++)
	{
		// find val, such that Binv[val] = i.
		// I dont care about speed for this, so do it the stupid way.

		for(int s=1;s<255;s++)
		{
			if(BInv[s] <= i && BInv[s+1] >= i)
			{
				Hcalib.B[i] = s+(i - BInv[s]) / (BInv[s+1]-BInv[s]);
				break;
			}
		}
	}
	Hcalib.B[0] = 0;
	Hcalib.B[255] = 255;
}

void FullSystem::printResult(std::string file)
{
	boost::unique_lock<boost::mutex> lock(trackMutex);
	boost::unique_lock<boost::mutex> crlock(shellPoseMutex);

	std::ofstream myfile;
	myfile.open (file.c_str());
	myfile << std::setprecision(15);

	for(FrameShell* s : allFrameHistory)
	{
		if(!s->poseValid) continue;

		if(setting_onlyLogKFPoses && s->marginalizedAt == s->id) continue;

		myfile << s->timestamp <<
			" " << s->camToWorld.translation().transpose()<<
			" " << s->camToWorld.so3().unit_quaternion().x()<<
			" " << s->camToWorld.so3().unit_quaternion().y()<<
			" " << s->camToWorld.so3().unit_quaternion().z()<<
			" " << s->camToWorld.so3().unit_quaternion().w() << "\n";
	}
	myfile.close();
}

Vec4 FullSystem::trackNewCoarse(FrameHessian* fh) {

	assert(allFrameHistory.size() > 0);
	// set pose initialization.

	for (IOWrap::Output3DWrapper *ow : outputWrapper)
		ow->pushLiveFrame(fh);

	FrameHessian *lastF = coarseTracker->lastRef;

	AffLight aff_last_2_l = AffLight(0, 0);

	std::vector<SE3, Eigen::aligned_allocator<SE3>> lastF_2_fh_tries;
	if (allFrameHistory.size() == 2){
		initializeFromInitializerCNN(fh);

	lastF_2_fh_tries.push_back(SE3(Eigen::Matrix<double, 3, 3>::Identity(), Eigen::Matrix<double, 3, 1>::Zero()));
	for (float rotDelta = 0.02; rotDelta < 0.05; rotDelta = rotDelta + 0.02) {
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, 0, 0), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, rotDelta, 0), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, 0, rotDelta), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, -rotDelta, 0, 0), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, -rotDelta, 0), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, 0, -rotDelta), Vec3(0, 0, 0)));            // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, rotDelta, 0), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, rotDelta, rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, 0, rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, -rotDelta, rotDelta, 0), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, -rotDelta, rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, -rotDelta, 0, rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, -rotDelta, 0), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, rotDelta, -rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, 0, -rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, -rotDelta, -rotDelta, 0), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, 0, -rotDelta, -rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, -rotDelta, 0, -rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, -rotDelta, -rotDelta, -rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, -rotDelta, -rotDelta, rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, -rotDelta, rotDelta, -rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, -rotDelta, rotDelta, rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, rotDelta, -rotDelta, -rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, rotDelta, -rotDelta, rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(SE3(Sophus::Quaterniond(1, rotDelta, rotDelta, -rotDelta),
									   Vec3(0, 0, 0)));    // assume constant motion.
		lastF_2_fh_tries.push_back(
				SE3(Sophus::Quaterniond(1, rotDelta, rotDelta, rotDelta), Vec3(0, 0, 0)));    // assume constant motion.
	}

	coarseTracker->makeK(&Hcalib);
	coarseTracker->setCTRefForFirstFrame(frameHessians);

	lastF = coarseTracker->lastRef;
	}
	else
	{
		FrameShell* slast = allFrameHistory[allFrameHistory.size()-2];
		FrameShell* sprelast = allFrameHistory[allFrameHistory.size()-3];
		SE3 slast_2_sprelast;
		SE3 lastF_2_slast;
		{	// lock on global pose consistency!
			boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
			slast_2_sprelast = sprelast->camToWorld.inverse() * slast->camToWorld;
			lastF_2_slast = slast->camToWorld.inverse() * lastF->shell->camToWorld;
			aff_last_2_l = slast->aff_g2l;
		}
		SE3 fh_2_slast = slast_2_sprelast;// assumed to be the same as fh_2_slast.

		// get last delta-movement.
		lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast);	// assume constant motion.
		lastF_2_fh_tries.push_back(fh_2_slast.inverse() * fh_2_slast.inverse() * lastF_2_slast);	// assume double motion (frame skipped)
		lastF_2_fh_tries.push_back(SE3::exp(fh_2_slast.log()*0.5).inverse() * lastF_2_slast); // assume half motion.
		lastF_2_fh_tries.push_back(lastF_2_slast); // assume zero motion.
		lastF_2_fh_tries.push_back(SE3()); // assume zero motion FROM KF.

		// just try a TON of different initializations (all rotations). In the end,
		// if they don't work they will only be tried on the coarsest level, which is super fast anyway.
		// also, if tracking rails here we loose, so we really, really want to avoid that.
		for(float rotDelta=0.02; rotDelta < 0.05; rotDelta++)
		{
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,0), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,0), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,rotDelta), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,0), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,0), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,-rotDelta), Vec3(0,0,0)));			// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,0), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,0), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,0), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,0), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,-rotDelta), Vec3(0,0,0)));	// assume constant motion.
			lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,rotDelta), Vec3(0,0,0)));	// assume constant motion.
		}

		if(!slast->poseValid || !sprelast->poseValid || !lastF->shell->poseValid)
		{
			lastF_2_fh_tries.clear();
			lastF_2_fh_tries.push_back(SE3());
		}
	}

	Vec3 flowVecs = Vec3(100,100,100);
	SE3 lastF_2_fh = SE3();
	AffLight aff_g2l = AffLight(0,0);

	// as long as maxResForImmediateAccept is not reached, I'll continue through the options.
	// I'll keep track of the so-far best achieved residual for each level in achievedRes.
	// If on a coarse level, tracking is WORSE than achievedRes, we will not continue to save time.

	Vec5 achievedRes = Vec5::Constant(NAN);
	bool haveOneGood = false;
	int tryIterations=0;
	for(unsigned int i=0;i<lastF_2_fh_tries.size();i++)
	{
		AffLight aff_g2l_this = aff_last_2_l;
		SE3 lastF_2_fh_this = lastF_2_fh_tries[i];
		bool trackingIsGood = coarseTracker->trackNewestCoarse(
				fh, lastF_2_fh_this, aff_g2l_this,
				pyrLevelsUsed-1,
				achievedRes);	// in each level has to be at least as good as the last try.
		tryIterations++;

		if(i != 0)
		{
			printf("RE-TRACK ATTEMPT %d with initOption %d and start-lvl %d (ab %f %f): %f %f %f %f %f -> %f %f %f %f %f \n",
					i,
					i, pyrLevelsUsed-1,
					aff_g2l_this.a,aff_g2l_this.b,
					achievedRes[0],
					achievedRes[1],
					achievedRes[2],
					achievedRes[3],
					achievedRes[4],
					coarseTracker->lastResiduals[0],
					coarseTracker->lastResiduals[1],
					coarseTracker->lastResiduals[2],
					coarseTracker->lastResiduals[3],
					coarseTracker->lastResiduals[4]);
		}

		// do we have a new winner?
		if(trackingIsGood && std::isfinite((float)coarseTracker->lastResiduals[0]) && !(coarseTracker->lastResiduals[0] >=  achievedRes[0]))
		{
			//printf("take over. minRes %f -> %f!\n", achievedRes[0], coarseTracker->lastResiduals[0]);
			flowVecs = coarseTracker->lastFlowIndicators;
			aff_g2l = aff_g2l_this;
			lastF_2_fh = lastF_2_fh_this;
			haveOneGood = true;
		}

		// take over achieved res (always).
		if(haveOneGood)
		{
			for(int i=0;i<5;i++)
			{
				if(!std::isfinite((float)achievedRes[i]) || achievedRes[i] > coarseTracker->lastResiduals[i])	// take over if achievedRes is either bigger or NAN.
					achievedRes[i] = coarseTracker->lastResiduals[i];
			}
		}

        if(haveOneGood &&  achievedRes[0] < lastCoarseRMSE[0]*setting_reTrackThreshold)
            break;

	}

	if(!haveOneGood)
	{
        printf("BIG ERROR! tracking failed entirely. Take predictred pose and hope we may somehow recover.\n");
		flowVecs = Vec3(0,0,0);
		aff_g2l = aff_last_2_l;
		lastF_2_fh = lastF_2_fh_tries[0];
	}

	lastCoarseRMSE = achievedRes;

	// no lock required, as fh is not used anywhere yet.
	fh->shell->camToTrackingRef = lastF_2_fh.inverse();
	fh->shell->trackingRef = lastF->shell;
	fh->shell->aff_g2l = aff_g2l;
	fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;

	if(coarseTracker->firstCoarseRMSE < 0)
		coarseTracker->firstCoarseRMSE = achievedRes[0];

    if(!setting_debugout_runquiet)
        printf("Coarse Tracker tracked ab = %f %f (exp %f). Res %f!\n", aff_g2l.a, aff_g2l.b, fh->ab_exposure, achievedRes[0]);

	if(setting_logStuff)
	{
		(*coarseTrackingLog) << std::setprecision(16)
						<< fh->shell->id << " "
						<< fh->shell->timestamp << " "
						<< fh->ab_exposure << " "
						<< fh->shell->camToWorld.log().transpose() << " "
						<< aff_g2l.a << " "
						<< aff_g2l.b << " "
						<< achievedRes[0] << " "
						<< tryIterations << "\n";
	}

	return Vec4(achievedRes[0], flowVecs[0], flowVecs[1], flowVecs[2]);
}

void FullSystem::traceNewCoarse(FrameHessian* fh)
{
	boost::unique_lock<boost::mutex> lock(mapMutex);

	int trace_total=0, trace_good=0, trace_oob=0, trace_out=0, trace_skip=0, trace_badcondition=0, trace_uninitialized=0;

	Mat33f K = Mat33f::Identity();
	K(0,0) = Hcalib.fxl();
	K(1,1) = Hcalib.fyl();
	K(0,2) = Hcalib.cxl();
	K(1,2) = Hcalib.cyl();

	for(FrameHessian* host : frameHessians)		// go through all active frames
	{

		SE3 hostToNew = fh->PRE_worldToCam * host->PRE_camToWorld;
		Mat33f KRKi = K * hostToNew.rotationMatrix().cast<float>() * K.inverse();
		Vec3f Kt = K * hostToNew.translation().cast<float>();

		Vec2f aff = AffLight::fromToVecExposure(host->ab_exposure, fh->ab_exposure, host->aff_g2l(), fh->aff_g2l()).cast<float>();

		for(ImmaturePoint* ph : host->immaturePoints)
		{
			ph->traceOn(fh, KRKi, Kt, aff, &Hcalib, false );
//			std::cout<<"idepth "<<ph->idepth_min<<" "<<ph->idepth_max<<std::endl;

			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD) trace_good++;
			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION) trace_badcondition++;
			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB) trace_oob++;
			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER) trace_out++;
			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED) trace_skip++;
			if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED) trace_uninitialized++;
			trace_total++;
		}
	}
//	printf("ADD: TRACE: %'d points. %'d (%.0f%%) good. %'d (%.0f%%) skip. %'d (%.0f%%) badcond. %'d (%.0f%%) oob. %'d (%.0f%%) out. %'d (%.0f%%) uninit.\n",
//			trace_total,
//			trace_good, 100*trace_good/(float)trace_total,
//			trace_skip, 100*trace_skip/(float)trace_total,
//			trace_badcondition, 100*trace_badcondition/(float)trace_total,
//			trace_oob, 100*trace_oob/(float)trace_total,
//			trace_out, 100*trace_out/(float)trace_total,
//			trace_uninitialized, 100*trace_uninitialized/(float)trace_total);
}

void FullSystem::activatePointsMT_Reductor(
		std::vector<PointHessian*>* optimized,
		std::vector<ImmaturePoint*>* toOptimize,
		int min, int max, Vec10* stats, int tid)
{
	ImmaturePointTemporaryResidual* tr = new ImmaturePointTemporaryResidual[frameHessians.size()];
	for(int k=min;k<max;k++)
	{
		(*optimized)[k] = optimizeImmaturePoint((*toOptimize)[k],1,tr);
	}
	delete[] tr;
}

void FullSystem::activatePointsMT()
{

	if(ef->nPoints < setting_desiredPointDensity*0.66)
		currentMinActDist -= 0.8;
	if(ef->nPoints < setting_desiredPointDensity*0.8)
		currentMinActDist -= 0.5;
	else if(ef->nPoints < setting_desiredPointDensity*0.9)
		currentMinActDist -= 0.2;
	else if(ef->nPoints < setting_desiredPointDensity)
		currentMinActDist -= 0.1;

	if(ef->nPoints > setting_desiredPointDensity*1.5)
		currentMinActDist += 0.8;
	if(ef->nPoints > setting_desiredPointDensity*1.3)
		currentMinActDist += 0.5;
	if(ef->nPoints > setting_desiredPointDensity*1.15)
		currentMinActDist += 0.2;
	if(ef->nPoints > setting_desiredPointDensity)
		currentMinActDist += 0.1;

	if(currentMinActDist < 0) currentMinActDist = 0;
	if(currentMinActDist > 4) currentMinActDist = 4;

    if(!setting_debugout_runquiet)
        printf("SPARSITY:  MinActDist %f (need %d points, have %d points)!\n",
                currentMinActDist, (int)(setting_desiredPointDensity), ef->nPoints);

	FrameHessian* newestHs = frameHessians.back();

	// make dist map.
	coarseDistanceMap->makeK(&Hcalib);
	coarseDistanceMap->makeDistanceMap(frameHessians, newestHs);

	//coarseTracker->debugPlotDistMap("distMap");

	std::vector<ImmaturePoint*> toOptimize; toOptimize.reserve(20000);

	for(FrameHessian* host : frameHessians)		// go through all active frames
	{
		if(host == newestHs) continue;

		SE3 fhToNew = newestHs->PRE_worldToCam * host->PRE_camToWorld;
		Mat33f KRKi = (coarseDistanceMap->K[1] * fhToNew.rotationMatrix().cast<float>() * coarseDistanceMap->Ki[0]);
		Vec3f Kt = (coarseDistanceMap->K[1] * fhToNew.translation().cast<float>());

		for(unsigned int i=0;i<host->immaturePoints.size();i+=1)
		{
			ImmaturePoint* ph = host->immaturePoints[i];
			ph->idxInImmaturePoints = i;

			// delete points that have never been traced successfully, or that are outlier on the last trace.
			if(!std::isfinite(ph->idepth_max) || ph->lastTraceStatus == IPS_OUTLIER)
			{
//				immature_invalid_deleted++;
				// remove point.
				delete ph;
				host->immaturePoints[i]=0;
				continue;
			}

			// can activate only if this is true.
			bool canActivate = (ph->lastTraceStatus == IPS_GOOD
					|| ph->lastTraceStatus == IPS_SKIPPED
					|| ph->lastTraceStatus == IPS_BADCONDITION
					|| ph->lastTraceStatus == IPS_OOB )
							&& ph->lastTracePixelInterval < 8
							&& ph->quality > setting_minTraceQuality
							&& (ph->idepth_max+ph->idepth_min) > 0;

			// if I cannot activate the point, skip it. Maybe also delete it.
			if(!canActivate)
			{
				// if point will be out afterwards, delete it instead.
				if(ph->host->flaggedForMarginalization || ph->lastTraceStatus == IPS_OOB)
				{
//					immature_notReady_deleted++;
					delete ph;
					host->immaturePoints[i]=0;
				}
//				immature_notReady_skipped++;
				continue;
			}

			// see if we need to activate point due to distance map.
			Vec3f ptp = KRKi * Vec3f(ph->u, ph->v, 1) + Kt*(0.5f*(ph->idepth_max+ph->idepth_min));
			int u = ptp[0] / ptp[2] + 0.5f;
			int v = ptp[1] / ptp[2] + 0.5f;

			if((u > 0 && v > 0 && u < wG[1] && v < hG[1]))
			{

				float dist = coarseDistanceMap->fwdWarpedIDDistFinal[u+wG[1]*v] + (ptp[0]-floorf((float)(ptp[0])));

				if(dist>=currentMinActDist* ph->my_type)
				{
					coarseDistanceMap->addIntoDistFinal(u,v);
					toOptimize.push_back(ph);
				}
			}
			else
			{
				delete ph;
				host->immaturePoints[i]=0;
			}
		}
	}

//	printf("ACTIVATE: %d. (del %d, notReady %d, marg %d, good %d, marg-skip %d)\n",
//			(int)toOptimize.size(), immature_deleted, immature_notReady, immature_needMarg, immature_want, immature_margskip);

	std::vector<PointHessian*> optimized; optimized.resize(toOptimize.size());

	if(multiThreading)
		treadReduce.reduce(boost::bind(&FullSystem::activatePointsMT_Reductor, this, &optimized, &toOptimize, _1, _2, _3, _4), 0, toOptimize.size(), 50);

	else
		activatePointsMT_Reductor(&optimized, &toOptimize, 0, toOptimize.size(), 0, 0);

	for(unsigned k=0;k<toOptimize.size();k++)
	{
		PointHessian* newpoint = optimized[k];
		ImmaturePoint* ph = toOptimize[k];

		if(newpoint != 0 && newpoint != (PointHessian*)((long)(-1)))
		{
			newpoint->host->immaturePoints[ph->idxInImmaturePoints]=0;
			newpoint->host->pointHessians.push_back(newpoint);
			ef->insertPoint(newpoint);
			for(PointFrameResidual* r : newpoint->residuals)
				ef->insertResidual(r);
			assert(newpoint->efPoint != 0);
			delete ph;
		}
		else if(newpoint == (PointHessian*)((long)(-1)) || ph->lastTraceStatus==IPS_OOB)
		{
			delete ph;
			ph->host->immaturePoints[ph->idxInImmaturePoints]=0;
		}
		else
		{
			assert(newpoint == 0 || newpoint == (PointHessian*)((long)(-1)));
		}
	}

	for(FrameHessian* host : frameHessians)
	{
		for(int i=0;i<(int)host->immaturePoints.size();i++)
		{
			if(host->immaturePoints[i]==0)
			{
				host->immaturePoints[i] = host->immaturePoints.back();
				host->immaturePoints.pop_back();
				i--;
			}
		}
	}

}

void FullSystem::activatePointsOldFirst()
{
	assert(false);
}

void FullSystem::flagPointsForRemoval()
{
	assert(EFIndicesValid);

	std::vector<FrameHessian*> fhsToKeepPoints;
	std::vector<FrameHessian*> fhsToMargPoints;

	//if(setting_margPointVisWindow>0)
	{
		for(int i=((int)frameHessians.size())-1;i>=0 && i >= ((int)frameHessians.size());i--)
			if(!frameHessians[i]->flaggedForMarginalization) fhsToKeepPoints.push_back(frameHessians[i]);

		for(int i=0; i< (int)frameHessians.size();i++)
			if(frameHessians[i]->flaggedForMarginalization) fhsToMargPoints.push_back(frameHessians[i]);
	}

	//ef->setAdjointsF();
	//ef->setDeltaF(&Hcalib);
	int flag_oob=0, flag_in=0, flag_inin=0, flag_nores=0;

	for(FrameHessian* host : frameHessians)		// go through all active frames
	{
		for(unsigned int i=0;i<host->pointHessians.size();i++)
		{
			PointHessian* ph = host->pointHessians[i];
			if(ph==0) continue;

			if(ph->idepth_scaled < 0 || ph->residuals.size()==0)
			{
				host->pointHessiansOut.push_back(ph);
				ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
				host->pointHessians[i]=0;
				flag_nores++;
			}
			else if(ph->isOOB(fhsToKeepPoints, fhsToMargPoints) || host->flaggedForMarginalization)
			{
				flag_oob++;
				if(ph->isInlierNew())
				{
					flag_in++;
					int ngoodRes=0;
					for(PointFrameResidual* r : ph->residuals)
					{
						r->resetOOB();
						r->linearize(&Hcalib);
						r->efResidual->isLinearized = false;
						r->applyRes(true);
						if(r->efResidual->isActive())
						{
							r->efResidual->fixLinearizationF(ef);
							ngoodRes++;
						}
					}
                    if(ph->idepth_hessian > setting_minIdepthH_marg)
					{
						flag_inin++;
						ph->efPoint->stateFlag = EFPointStatus::PS_MARGINALIZE;
						host->pointHessiansMarginalized.push_back(ph);
					}
					else
					{
						ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
						host->pointHessiansOut.push_back(ph);
					}

				}
				else
				{
					host->pointHessiansOut.push_back(ph);
					ph->efPoint->stateFlag = EFPointStatus::PS_DROP;

					//printf("drop point in frame %d (%d goodRes, %d activeRes)\n", ph->host->idx, ph->numGoodResiduals, (int)ph->residuals.size());
				}

				host->pointHessians[i]=0;
			}
		}

		for(int i=0;i<(int)host->pointHessians.size();i++)
		{
			if(host->pointHessians[i]==0)
			{
				host->pointHessians[i] = host->pointHessians.back();
				host->pointHessians.pop_back();
				i--;
			}
		}
	}

}

void FullSystem::addActiveFrame( ImageAndExposure* image, int id )
{

    if(isLost) return;
	boost::unique_lock<boost::mutex> lock(trackMutex);

	// =========================== add into allFrameHistory =========================
	FrameHessian* fh = new FrameHessian();
	FrameShell* shell = new FrameShell();
	shell->camToWorld = SE3(); 		// no lock required, as fh is not used anywhere yet.
	shell->aff_g2l = AffLight(0,0);
    shell->marginalizedAt = shell->id = allFrameHistory.size();
    shell->timestamp = image->timestamp;
    shell->incoming_id = id;
	fh->shell = shell;
	allFrameHistory.push_back(shell);

	// =========================== make Images / derivatives etc. =========================
	fh->ab_exposure = image->exposure_time;
    fh->makeImages(image->image, &Hcalib);

	if(!initialized)
	{
		// use initializer!
		if(coarseInitializer->frameID<0)	// first frame set. fh is kept by coarseInitializer.
		{
			coarseInitializer->setFirst(&Hcalib, fh, getDepthMap(fh,frameNumber));
			frameNumber = frameNumber + 1;
			initialized=true;
		}

		return;
	}
	else	// do front-end operation.
	{
		// =========================== SWAP tracking reference?. =========================
		if(coarseTracker_forNewKF->refFrameID > coarseTracker->refFrameID)
		{
			boost::unique_lock<boost::mutex> crlock(coarseTrackerSwapMutex);
			CoarseTracker* tmp = coarseTracker; coarseTracker=coarseTracker_forNewKF; coarseTracker_forNewKF=tmp;
		}

		Vec4 tres = trackNewCoarse(fh);
		if(!std::isfinite((double)tres[0]) || !std::isfinite((double)tres[1]) || !std::isfinite((double)tres[2]) || !std::isfinite((double)tres[3]))
        {
            printf("Initial Tracking failed: LOST!\n");
			isLost=true;
            return;
        }

		bool needToMakeKF = false;
		if(setting_keyframesPerSecond > 0)
		{
			needToMakeKF = allFrameHistory.size()== 1 ||
					(fh->shell->timestamp - allKeyFramesHistory.back()->timestamp) > 0.95f/setting_keyframesPerSecond;
		}
		else
		{
			Vec2 refToFh=AffLight::fromToVecExposure(coarseTracker->lastRef->ab_exposure, fh->ab_exposure,
					coarseTracker->lastRef_aff_g2l, fh->shell->aff_g2l);

			// BRIGHTNESS CHECK
			needToMakeKF = allFrameHistory.size()== 1 ||
					setting_kfGlobalWeight*setting_maxShiftWeightT *  sqrtf((double)tres[1]) / (wG[0]+hG[0]) +
					setting_kfGlobalWeight*setting_maxShiftWeightR *  sqrtf((double)tres[2]) / (wG[0]+hG[0]) +
					setting_kfGlobalWeight*setting_maxShiftWeightRT * sqrtf((double)tres[3]) / (wG[0]+hG[0]) +
					setting_kfGlobalWeight*setting_maxAffineWeight * fabs(logf((float)refToFh[0])) > 1 ||
					2*coarseTracker->firstCoarseRMSE < tres[0];

		}

        for(IOWrap::Output3DWrapper* ow : outputWrapper)
            ow->publishCamPose(fh->shell, &Hcalib);

		lock.unlock();
		deliverTrackedFrame(fh, needToMakeKF);
		return;
	}
}
void FullSystem::deliverTrackedFrame(FrameHessian* fh, bool needKF)
{

	if(linearizeOperation)
	{
		if(goStepByStep && lastRefStopID != coarseTracker->refFrameID)
		{
			MinimalImageF3 img(wG[0], hG[0], fh->dI);
			IOWrap::displayImage("frameToTrack", &img);
			while(true)
			{
				char k=IOWrap::waitKey(0);
				if(k==' ') break;
				handleKey( k );
			}
			lastRefStopID = coarseTracker->refFrameID;
		}
		else handleKey( IOWrap::waitKey(1) );

		if(needKF) makeKeyFrame(fh);
		else makeNonKeyFrame(fh);
	}
	else
	{
		boost::unique_lock<boost::mutex> lock(trackMapSyncMutex);
		unmappedTrackedFrames.push_back(fh);
		if(needKF) needNewKFAfter=fh->shell->trackingRef->id;
		trackedFrameSignal.notify_all();

		while(coarseTracker_forNewKF->refFrameID == -1 && coarseTracker->refFrameID == -1 )
		{
			mappedFrameSignal.wait(lock);
		}

		lock.unlock();
	}
}

void FullSystem::mappingLoop()
{
	boost::unique_lock<boost::mutex> lock(trackMapSyncMutex);

	while(runMapping)
	{
		while(unmappedTrackedFrames.size()==0)
		{
			trackedFrameSignal.wait(lock);
			if(!runMapping) return;
		}

		FrameHessian* fh = unmappedTrackedFrames.front();
		unmappedTrackedFrames.pop_front();

		// guaranteed to make a KF for the very first two tracked frames.
		if(allKeyFramesHistory.size() <= 2)
		{
			lock.unlock();
			makeKeyFrame(fh);
			lock.lock();
			mappedFrameSignal.notify_all();
			continue;
		}

		if(unmappedTrackedFrames.size() > 3)
			needToKetchupMapping=true;

		if(unmappedTrackedFrames.size() > 0) // if there are other frames to tracke, do that first.
		{
			lock.unlock();
			makeNonKeyFrame(fh);
			lock.lock();

			if(needToKetchupMapping && unmappedTrackedFrames.size() > 0)
			{
				FrameHessian* fh = unmappedTrackedFrames.front();
				unmappedTrackedFrames.pop_front();
				{
					boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
					assert(fh->shell->trackingRef != 0);
					fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
					fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l);
				}
				delete fh;
			}

		}
		else
		{
			if(setting_realTimeMaxKF || needNewKFAfter >= frameHessians.back()->shell->id)
			{
				lock.unlock();
				makeKeyFrame(fh);
				needToKetchupMapping=false;
				lock.lock();
			}
			else
			{
				lock.unlock();
				makeNonKeyFrame(fh);
				lock.lock();
			}
		}
		mappedFrameSignal.notify_all();
	}
	printf("MAPPING FINISHED!\n");
}

void FullSystem::blockUntilMappingIsFinished()
{
	boost::unique_lock<boost::mutex> lock(trackMapSyncMutex);
	runMapping = false;
	trackedFrameSignal.notify_all();
	lock.unlock();

	mappingThread.join();

}

void FullSystem::makeNonKeyFrame( FrameHessian* fh)
{
	// needs to be set by mapping thread. no lock required since we are in mapping thread.
	{
		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
		assert(fh->shell->trackingRef != 0);
		fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
		fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l);
	}

	traceNewCoarse(fh);
	delete fh;
}

void FullSystem::makeKeyFrame( FrameHessian* fh)
{
	// needs to be set by mapping thread
	{
		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
		assert(fh->shell->trackingRef != 0);
		fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
		fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l);
	}

	traceNewCoarse(fh);

	boost::unique_lock<boost::mutex> lock(mapMutex);

	// =========================== Flag Frames to be Marginalized. =========================
	flagFramesForMarginalization(fh);

	// =========================== add New Frame to Hessian Struct. =========================
	fh->idx = frameHessians.size();
	frameHessians.push_back(fh);
	fh->frameID = allKeyFramesHistory.size();
	allKeyFramesHistory.push_back(fh->shell);
	ef->insertFrame(fh, &Hcalib);

	setPrecalcValues();

	// =========================== add new residuals for old points =========================
	int numFwdResAdde=0;
	for(FrameHessian* fh1 : frameHessians)		// go through all active frames
	{
		if(fh1 == fh) continue;
		for(PointHessian* ph : fh1->pointHessians)
		{
			PointFrameResidual* r = new PointFrameResidual(ph, fh1, fh);
			r->setState(ResState::IN);
			ph->residuals.push_back(r);
			ef->insertResidual(r);
			ph->lastResiduals[1] = ph->lastResiduals[0];
			ph->lastResiduals[0] = std::pair<PointFrameResidual*, ResState>(r, ResState::IN);
			numFwdResAdde+=1;
		}
	}

	// =========================== Activate Points (& flag for marginalization). =========================
	activatePointsMT();
	ef->makeIDX();

	// =========================== OPTIMIZE ALL =========================

	fh->frameEnergyTH = frameHessians.back()->frameEnergyTH;
	float rmse = optimize(setting_maxOptIterations);

	// =========================== Figure Out if INITIALIZATION FAILED =========================
	if(allKeyFramesHistory.size() <= 4)
	{
		if(allKeyFramesHistory.size()==2 && rmse > 20*benchmark_initializerSlackFactor)
		{
			printf("I THINK INITIALIZATINO FAILED! Resetting.\n");
			initFailed=true;
		}
		if(allKeyFramesHistory.size()==3 && rmse > 13*benchmark_initializerSlackFactor)
		{
			printf("I THINK INITIALIZATINO FAILED! Resetting.\n");
			initFailed=true;
		}
		if(allKeyFramesHistory.size()==4 && rmse > 9*benchmark_initializerSlackFactor)
		{
			printf("I THINK INITIALIZATINO FAILED! Resetting.\n");
			initFailed=true;
		}
	}

    if(isLost) return;

	// =========================== REMOVE OUTLIER =========================
	removeOutliers();

	{
		boost::unique_lock<boost::mutex> crlock(coarseTrackerSwapMutex);
		coarseTracker_forNewKF->makeK(&Hcalib);
		coarseTracker_forNewKF->setCoarseTrackingRef(frameHessians);

        coarseTracker_forNewKF->debugPlotIDepthMap(&minIdJetVisTracker, &maxIdJetVisTracker, outputWrapper);
        coarseTracker_forNewKF->debugPlotIDepthMapFloat(outputWrapper);
	}

	debugPlot("post Optimize");

	// =========================== (Activate-)Marginalize Points =========================
	flagPointsForRemoval();
	ef->dropPointsF();
	getNullspaces(
			ef->lastNullspaces_pose,
			ef->lastNullspaces_scale,
			ef->lastNullspaces_affA,
			ef->lastNullspaces_affB);
	ef->marginalizePointsF();

	// =========================== add new Immature points & new residuals =========================
	makeNewTraces(fh, 0);

    for(IOWrap::Output3DWrapper* ow : outputWrapper)
    {
        ow->publishGraph(ef->connectivityMap);
        ow->publishKeyframes(frameHessians, false, &Hcalib);
    }

	// =========================== Marginalize Frames =========================

	for(unsigned int i=0;i<frameHessians.size();i++)
		if(frameHessians[i]->flaggedForMarginalization)
			{marginalizeFrame(frameHessians[i]); i=0;}

	printLogLine();
    //printEigenValLine();

}

void FullSystem::initializeFromInitializer(FrameHessian* newFrame)
{
	boost::unique_lock<boost::mutex> lock(mapMutex);

	// add firstframe.
	FrameHessian* firstFrame = coarseInitializer->firstFrame;
	firstFrame->idx = frameHessians.size();
	frameHessians.push_back(firstFrame);
	firstFrame->frameID = allKeyFramesHistory.size();
	allKeyFramesHistory.push_back(firstFrame->shell);
	ef->insertFrame(firstFrame, &Hcalib);
	setPrecalcValues();

	//int numPointsTotal = makePixelStatus(firstFrame->dI, selectionMap, wG[0], hG[0], setting_desiredDensity);
	//int numPointsTotal = pixelSelector->makeMaps(firstFrame->dIp, selectionMap,setting_desiredDensity);

	firstFrame->pointHessians.reserve(wG[0]*hG[0]*0.2f);
	firstFrame->pointHessiansMarginalized.reserve(wG[0]*hG[0]*0.2f);
	firstFrame->pointHessiansOut.reserve(wG[0]*hG[0]*0.2f);

	float sumID=1e-5, numID=1e-5;
	for(int i=0;i<coarseInitializer->numPoints[0];i++)
	{
		sumID += coarseInitializer->points[0][i].iR;
		numID++;
	}
	float rescaleFactor = 1 / (sumID / numID);

	// randomly sub-select the points I need.
	float keepPercentage = setting_desiredPointDensity / coarseInitializer->numPoints[0];

    if(!setting_debugout_runquiet)
        printf("Initialization: keep %.1f%% (need %d, have %d)!\n", 100*keepPercentage,
                (int)(setting_desiredPointDensity), coarseInitializer->numPoints[0] );

	for(int i=0;i<coarseInitializer->numPoints[0];i++)
	{
		if(rand()/(float)RAND_MAX > keepPercentage) continue;

		Pnt* point = coarseInitializer->points[0]+i;
		ImmaturePoint* pt = new ImmaturePoint(point->u+0.5f,point->v+0.5f,firstFrame,point->my_type, &Hcalib);

		if(!std::isfinite(pt->energyTH)) { delete pt; continue; }
		pt->idepth_max=pt->idepth_min=1;
		PointHessian* ph = new PointHessian(pt, &Hcalib);
		delete pt;
		if(!std::isfinite(ph->energyTH)) {delete ph; continue;}

		ph->setIdepthScaled(point->iR*rescaleFactor);
		ph->setIdepthZero(ph->idepth);
		ph->hasDepthPrior=true;
		ph->setPointStatus(PointHessian::ACTIVE);

		firstFrame->pointHessians.push_back(ph);
		ef->insertPoint(ph);
	}

	SE3 firstToNew = coarseInitializer->thisToNext;
	firstToNew.translation() /= rescaleFactor;

	// really no lock required, as we are initializing.
	{
		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
		firstFrame->shell->camToWorld = SE3();
		firstFrame->shell->aff_g2l = AffLight(0,0);
		firstFrame->setEvalPT_scaled(firstFrame->shell->camToWorld.inverse(),firstFrame->shell->aff_g2l);
		firstFrame->shell->trackingRef=0;
		firstFrame->shell->camToTrackingRef = SE3();

		newFrame->shell->camToWorld = firstToNew.inverse();
		newFrame->shell->aff_g2l = AffLight(0,0);
		newFrame->setEvalPT_scaled(newFrame->shell->camToWorld.inverse(),newFrame->shell->aff_g2l);
		newFrame->shell->trackingRef = firstFrame->shell;
		newFrame->shell->camToTrackingRef = firstToNew.inverse();

	}

	initialized=true;
	printf("INITIALIZE FROM INITIALIZER (%d pts)!\n", (int)firstFrame->pointHessians.size());
}

void FullSystem::initializeFromInitializerCNN(FrameHessian* newFrame)
{
	boost::unique_lock<boost::mutex> lock(mapMutex);

	// add firstframe.
	FrameHessian* firstFrame = coarseInitializer->firstFrame;
	firstFrame->idx = frameHessians.size();
	frameHessians.push_back(firstFrame);
	firstFrame->frameID = allKeyFramesHistory.size();
	allKeyFramesHistory.push_back(firstFrame->shell);
	ef->insertFrame(firstFrame, &Hcalib);
	setPrecalcValues();

	firstFrame->pointHessians.reserve(wG[0]*hG[0]*0.2f);
	firstFrame->pointHessiansMarginalized.reserve(wG[0]*hG[0]*0.2f);
	firstFrame->pointHessiansOut.reserve(wG[0]*hG[0]*0.2f);

	float sumID=1e-5, numID=1e-5;
	for(int i=0;i<coarseInitializer->numPoints[0];i++)
	{
		sumID += coarseInitializer->points[0][i].iR;
		numID++;
	}

	// randomly sub-select the points I need.
	float keepPercentage = setting_desiredPointDensity / coarseInitializer->numPoints[0];

	if(!setting_debugout_runquiet)
		printf("Initialization: keep %.1f%% (need %d, have %d)!\n", 100*keepPercentage,
			   (int)(setting_desiredPointDensity), coarseInitializer->numPoints[0] );

	cv::Mat depth = getDepthMap(firstFrame,frameNumber);
	frameNumber = frameNumber + 1;

	float* depthmap_ptr = (float*) depth.data;

	for(int i=0;i<coarseInitializer->numPoints[0];i++)
	{
		if(rand()/(float)RAND_MAX > keepPercentage) continue;

		Pnt* point = coarseInitializer->points[0]+i;
		ImmaturePoint* pt = new ImmaturePoint(point->u+0.5f,point->v+0.5f,firstFrame,point->my_type, &Hcalib);

		float depth = *(depthmap_ptr + int((point->v*wG[0]+point->u+0.5f)));
		float idepth=1.0/depth;
		float var = 1.0/(6*depth);
		pt->idepth_max=idepth;
		pt->idepth_min=idepth;
		if(pt->idepth_min<0) pt->idepth_min=0;

		PointHessian* ph = new PointHessian(pt, &Hcalib);
		delete pt;
		if(!std::isfinite(ph->energyTH)) {delete ph; continue;}

		ph->setIdepthScaled(idepth);
		ph->setIdepthZero(idepth);
		ph->hasDepthPrior=true;
		ph->setPointStatus(PointHessian::ACTIVE);

		firstFrame->pointHessians.push_back(ph);
		ef->insertPoint(ph);
	}

	SE3 firstToNew = coarseInitializer->thisToNext;

	// really no lock required, as we are initializing.
	{
		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
		firstFrame->shell->camToWorld = SE3();
		firstFrame->shell->aff_g2l = AffLight(0,0);
		firstFrame->setEvalPT_scaled(firstFrame->shell->camToWorld.inverse(),firstFrame->shell->aff_g2l);
		firstFrame->shell->trackingRef=0;
		firstFrame->shell->camToTrackingRef = SE3();

		newFrame->shell->camToWorld = firstToNew.inverse();
		newFrame->shell->aff_g2l = AffLight(0,0);
		newFrame->setEvalPT_scaled(newFrame->shell->camToWorld.inverse(),newFrame->shell->aff_g2l);
		newFrame->shell->trackingRef = firstFrame->shell;
		newFrame->shell->camToTrackingRef = firstToNew.inverse();

	}

	initialized=true;
	printf("INITIALIZE FROM INITIALIZER (%d pts)!\n", (int)firstFrame->pointHessians.size());
}

	void FullSystem::makeNewTraces(FrameHessian* newFrame, float* gtDepth)
{
	pixelSelector->allowFast = true;
	//int numPointsTotal = makePixelStatus(newFrame->dI, selectionMap, wG[0], hG[0], setting_desiredDensity);
	int numPointsTotal = pixelSelector->makeMaps(newFrame, selectionMap,setting_desiredImmatureDensity);

	newFrame->pointHessians.reserve(numPointsTotal*1.2f);
	//fh->pointHessiansInactive.reserve(numPointsTotal*1.2f);
	newFrame->pointHessiansMarginalized.reserve(numPointsTotal*1.2f);
	newFrame->pointHessiansOut.reserve(numPointsTotal*1.2f);

	cv::Mat depth = getDepthMap(newFrame,frameNumber);
	frameNumber = frameNumber + 1;
    for (IOWrap::Output3DWrapper *ow : outputWrapper)
        ow->pushCNNImage(depth);

	float* depthmap_ptr = (float*) depth.data;

	for(int y=patternPadding+1;y<hG[0]-patternPadding-2;y++)
		for(int x=patternPadding+1;x<wG[0]-patternPadding-2;x++)
		{
			int i = x+y*wG[0];
			if(selectionMap[i]==0) continue;

			ImmaturePoint* impt = new ImmaturePoint(x,y,newFrame, selectionMap[i], &Hcalib);
			impt->idepth_max=1./(*(depthmap_ptr+i));
			impt->idepth_min=1./(*(depthmap_ptr+i));
			if(impt->idepth_min<0) impt->idepth_min=0;

			if(!std::isfinite(impt->energyTH)) delete impt;
			else newFrame->immaturePoints.push_back(impt);
		}
	//printf("MADE %d IMMATURE POINTS!\n", (int)newFrame->immaturePoints.size());

}

void FullSystem::setPrecalcValues()
{
	for(FrameHessian* fh : frameHessians)
	{
		fh->targetPrecalc.resize(frameHessians.size());
		for(unsigned int i=0;i<frameHessians.size();i++)
			fh->targetPrecalc[i].set(fh, frameHessians[i], &Hcalib);
	}

	ef->setDeltaF(&Hcalib);
}

void FullSystem::printLogLine()
{
	if(frameHessians.size()==0) return;

    if(!setting_debugout_runquiet)
        printf("LOG %d: %.3f fine. Res: %d A, %d L, %d M; (%'d / %'d) forceDrop. a=%f, b=%f. Window %d (%d)\n",
                allKeyFramesHistory.back()->id,
                statistics_lastFineTrackRMSE,
                ef->resInA,
                ef->resInL,
                ef->resInM,
                (int)statistics_numForceDroppedResFwd,
                (int)statistics_numForceDroppedResBwd,
                allKeyFramesHistory.back()->aff_g2l.a,
                allKeyFramesHistory.back()->aff_g2l.b,
                frameHessians.back()->shell->id - frameHessians.front()->shell->id,
                (int)frameHessians.size());

	if(!setting_logStuff) return;

	if(numsLog != 0)
	{
		(*numsLog) << allKeyFramesHistory.back()->id << " "  <<
				statistics_lastFineTrackRMSE << " "  <<
				(int)statistics_numCreatedPoints << " "  <<
				(int)statistics_numActivatedPoints << " "  <<
				(int)statistics_numDroppedPoints << " "  <<
				(int)statistics_lastNumOptIts << " "  <<
				ef->resInA << " "  <<
				ef->resInL << " "  <<
				ef->resInM << " "  <<
				statistics_numMargResFwd << " "  <<
				statistics_numMargResBwd << " "  <<
				statistics_numForceDroppedResFwd << " "  <<
				statistics_numForceDroppedResBwd << " "  <<
				frameHessians.back()->aff_g2l().a << " "  <<
				frameHessians.back()->aff_g2l().b << " "  <<
				frameHessians.back()->shell->id - frameHessians.front()->shell->id << " "  <<
				(int)frameHessians.size() << " "  << "\n";
		numsLog->flush();
	}

}

cv::Mat FullSystem::getDepthMap(FrameHessian* fh, int n){
	cv::Mat image(hG[0], wG[0], CV_8UC1);
    unsigned char *ptr = (unsigned char*)image.ptr();
	for(int y = 0; y < image.rows; ++y)
		for(int x = 0; x < image.cols; ++x)
            ptr[y*wG[0]+x]=fh->dI[y*wG[0]+x][0];

	std::string depthMapName = depthPrefix;
	int num = n;
	for (int p=0; p<5; p++) {
		num = num / 10;
		if (num == 0)
			depthMapName = depthMapName + "0";
	}
	depthMapName = depthMapName + std::to_string(n) + "_depth.png";
	//std::cout << depthMapName << std::endl;

	cv::Mat depthOriginal = cv::imread(depthMapName,cv::IMREAD_UNCHANGED);
	double minOriginal,maxOriginal;
	cv::minMaxLoc(depthOriginal,&minOriginal,&maxOriginal);

	cv::Mat depthFloat;

	depthOriginal.convertTo(depthFloat,CV_32FC1);

	double minFloat,maxFloat;
	cv::minMaxLoc(depthFloat,&minFloat,&maxFloat);

	//depthFloat = depthFloat / depthMapScaleFactor;

	/*
	if (depthMapScaleFactor == 14.5) {
	    depthFloat = 1.0 / (depthFloat + 0.0001);
		depthFloat = depthFloat * depthMapScaleFactor;

		//double minLoc,maxLoc;
		//cv::minMaxLoc(depthFloat,&minLoc,&maxLoc);
		//depthFloat = 1 * (depthFloat - minLoc) / (maxLoc - minLoc);
		//depthFloat = 10 * depthFloat;
	}
	*/

	// to find min != 0
	float minNotZero = 1;
	if (minOriginal == 0) {
		for(int y = 0; y < depthFloat.rows; ++y) {
			for(int x = 0; x < depthFloat.cols; ++x) {
				if (depthFloat.at<float>(y,x) < minNotZero && depthFloat.at<float>(y,x) != 0)
					minNotZero = depthFloat.at<float>(y,x);
			}
		}

		float lastNotZero=-1;
		for(int y = depthFloat.rows-1; y >= 0; --y) {
			for(int x = 0; x < depthFloat.cols; ++x) {
				if (depthFloat.at<float>(y,x) == 0) {
					depthFloat.at<float>(y,x) = lastNotZero == -1 ? minNotZero : lastNotZero;
					/*
					// ANGOLI
					if (x==0 && y==0)
						depth.at<float>(0,0) = (depth.at<float>(0,1) + depth.at<float>(1,0)) / 2.0;
					else if (x == depth.cols -1 && y == 0)
						depth.at<float>(0,depth.cols-1) = (depth.at<float>(0,depth.cols-2) + depth.at<float>(1,depth.cols-1)) / 2.0;
					else if (x == 0 && y == depth.rows - 1)
						depth.at<float>(depth.rows-1,0) = (depth.at<float>(depth.rows-2,0) + depth.at<float>(depth.rows-1,1)) / 2.0;
					else if (x == depth.cols -1 && y == depth.rows - 1)
						depth.at<float>(depth.rows-1,depth.cols-1) = (depth.at<float>(depth.rows-1,depth.cols-2) + depth.at<float>(depth.rows-2,depth.cols-1)) / 2.0;

					// CONTORNI (NO ANGOLI)
					else if (x == 0)
						depth.at<float>(y,0) = (depth.at<float>(y-1,0) + depth.at<float>(y,1) + depth.at<float>(y+1,0)) / 3.0;
					else if (y == 0)
						depth.at<float>(0,x) = (depth.at<float>(0,x-1) + depth.at<float>(1,x) + depth.at<float>(0,x+1)) / 3.0;
					else if (x == depth.cols-1)
						depth.at<float>(y,depth.cols-1) = (depth.at<float>(y-1,depth.cols-1) + depth.at<float>(y,depth.cols-2) + depth.at<float>(y+1,depth.cols-1)) / 3.0;
					else if (y == depth.rows-1)
						depth.at<float>(depth.rows-1,x) = (depth.at<float>(depth.rows-1,x-1) + depth.at<float>(depth.rows-2,x) + depth.at<float>(depth.rows-1,x+1)) / 3.0;

					// TUTTO IL RESTO
					else
						depth.at<float>(y,x) = (depth.at<float>(y,x-1) + depth.at<float>(y-1,x) + depth.at<float>(y,x+1) + depth.at<float>(y+1,x)) / 4.0;

					if (depth.at<float>(y,x) == 0) {
						depth.at<float>(y,x) = lastNotZero == -1 ? minNotZero : lastNotZero;
						std::cout << "Zero --> x=" << x << "\ty=" << y << "\tvalue=" << depth.at<float>(y,x) << std::endl;
					}
					*/
				} else
					lastNotZero = depthFloat.at<float>(y,x);
			}
		}
	}

	double minFinal,maxFinal;
	cv::minMaxLoc(depthFloat,&minFinal,&maxFinal);

	// /*
	std::cout << "Originale:  " << minOriginal << " , " << maxOriginal ;
	std::cout << "\tFloat: " << minFloat << " , " << maxFloat;
	std::cout	<< "\tFinal: " << minFinal << " , " << maxFinal << (minOriginal == 0 ? "\tC'era uno zero..." : "\tNo zeri.") << std::endl;
	// */

	// USE TO CONVERT FROM DISPARITY TO DEPTH
	// depth = 0.3128f / (depth + 0.00001f);

	/*
	if (n == 0) {
		cv::Mat depthToWrite;
		depthFloat.convertTo(depthToWrite,CV_8UC3,255.0);
		cv::imwrite("/media/sf_Ubuntu/provaDepth.png",depthToWrite);
	}
	*/

	if ((n+1) % 50 == 0)
		std::cout << n+1 << " depth read!" << std::endl;
	return depthFloat;
}

}


#include <boost/heap/binomial_heap.hpp>
#include <iostream>

using namespace boost::heap;

int main()
{
  binomial_heap<int> bh;
  auto handle = bh.push(2);
  bh.push(3);
  bh.push(1);

  bh.update(handle, 4);

  std::cout << bh.top() << '\n';
}


#ifdef _MSC_VER
#define _CRT_SECURE_NO_WARNINGS
#endif
#include <boost/multiprecision/cpp_int.hpp>

class Image
{
public:
	using uns = unsigned;
	using str = std::string;

	void get_info()
	{
		for (uns i = 0; i < N; ++i) {
			std::cout << pixels[i].get_color() << std::endl;
		}
	}
private:
	class Pixel
	{
	public:
		enum color
		{
			black, white, red, green, blue
		};
		Pixel() : r(0), g(0), b(0)
		{

		}
		Pixel(uns r, uns g, uns b)
		{
			this->r = r;
			this->g = g;
			this->b = b;
		}
		color check_color(uns r, uns g, uns b)
		{
			uns breaking_values[][4] = {
				  0,   0,   0, color::black,
				  0,   0, 255, color::blue,
				  0, 255,   0, color::green,
				255,   0,   0, color::red,
				255, 255, 255, color::white
			};
			if ((r >= 0) && (g >= 0) && (b >= 0)) {
				for (uns i = 0; i < 5; ++i) {
					if (
							(r == breaking_values[i][0]) &&
							(g == breaking_values[i][1]) &&
							(b == breaking_values[i][2])
						) {
						return static_cast<color>(breaking_values[i][3]);
					}
				}
			}
		}
		str get_color()
		{
			std::vector<str> colors = {
				"black",
				"white",
				"red",
				"green",
				"blue"
			};
			return colors[check_color(this->r, this->g, this->b)];
		}
	private:
		uns r;
		uns g;
		uns b;
	};

	static const uns N = 5;
	Pixel pixels[N]
	{
		Pixel(  0,  0,    0),
		Pixel(255,   0,   0),
		Pixel(255, 255,	255),
		Pixel(  0, 255,   0),
		Pixel(255,   0,   0)
	};
};

int main()
{
	Image img;
	img.get_info();
	system("pause");
}


#include <boost/numeric/ublas/operation_sparse.hpp>


#define BOOST_TEST_MODULE pcraster model_engine runtimeenv
#include <boost/test/unit_test.hpp>
#include "geo_rasterspace.h"
#include "calc_runtimeenv.h"
#include "calc_spatial.h"

BOOST_AUTO_TEST_CASE(testPopField)
{
  using namespace calc;

  RunTimeEnv      rte(geo::RasterSpace(2,2));
  REAL4 zData[4]= { 1,1,1,1};
  Spatial *sp=new Spatial(VS_S,zData,4);

  BOOST_CHECK(!sp->readOnlyReference());

  sp->setReadOnlyReference(true);
  rte.pushField(sp);

  Field *f= rte.popField();
  BOOST_CHECK(f==sp);
  BOOST_CHECK(f->readOnlyReference());

  BOOST_CHECK(f->vs() == VS_S);
  BOOST_CHECK(f->src_f()[0] == 1);
  BOOST_CHECK(f->src_f()[3] == 1);

  deleteAlways(sp);
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_WORLD_ROADGRAPH_HPP_
#define MODULES_WORLD_ROADGRAPH_HPP_

#include <vector>
#include <string>
#include <ostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include "modules/world/opendrive/opendrive.hpp"

namespace modules {
namespace world {
namespace map {

using namespace modules::world::opendrive;
using namespace boost;

struct LaneVertex {
  RoadId road_id;
  LaneId global_lane_id;
  LaneId get_global_line_id() { return global_lane_id; }
  LanePtr get_lane() { return lane; }
  LanePtr lane;
  LaneVertex() : road_id(0), global_lane_id(0), lane(NULL) {}
  LaneVertex(int road_id_in, int global_lane_id_in, LanePtr lane_in) : road_id(road_id_in), global_lane_id(global_lane_id_in), lane(lane_in) {}
};

enum LaneEdgeType {
  SUCCESSOR_EDGE = 0,
  INNER_NEIGHBOR_EDGE = 1,
  OUTER_NEIGHBOR_EDGE = 2
};

struct LaneEdge {
  LaneEdgeType edge_type;
  float weight; //! @todo tobias: for shortest path calculation: a very basic implementation!
  LaneEdgeType get_edge_type() { return edge_type; }
  LaneEdge() : edge_type(SUCCESSOR_EDGE), weight(1) {}
  LaneEdge(LaneEdgeType edge_type_in) : edge_type(edge_type_in), weight(edge_type_in==SUCCESSOR_EDGE?1:10) {}
};

typedef boost::adjacency_list<vecS, vecS, directedS, LaneVertex, LaneEdge> LaneGraph;
typedef boost::graph_traits<LaneGraph>::vertex_descriptor vertex_t;
typedef boost::graph_traits<LaneGraph>::edge_descriptor edge_t;

class Roadgraph {
 public:
  Roadgraph() {}

  LaneId add_lane(const RoadId& road_id, const LanePtr& laneptr) {
    LaneVertex lane = LaneVertex(road_id, laneptr->get_id(), laneptr);
    boost::add_vertex(lane, g_);
    return laneptr->get_id();
  }

  bool add_inner_neighbor(const LaneId& inner_id, const LaneId& outer_id) {
    return add_edge_of_type( outer_id, inner_id, INNER_NEIGHBOR_EDGE);
  }

  bool add_outer_neighbor(const LaneId& inner_id, const LaneId& outer_id) {
    return add_edge_of_type( inner_id, outer_id, OUTER_NEIGHBOR_EDGE);
  }

  bool add_successor(const LaneId& prev, const LaneId& succ) {
    return add_edge_of_type( prev, succ, SUCCESSOR_EDGE);
  }

  std::vector<LaneId> get_successor_lanes(const LaneId& lane_id)
  {
    std::pair<vertex_t,bool> lane_vertex_pair = get_vertex_by_lane_id(lane_id);
    boost::graph_traits<LaneGraph>::out_edge_iterator i, end;
    std::vector<LaneId> successor_lanes;
    for (boost::tie(i, end) = boost::out_edges(lane_vertex_pair.first, g_); i != end; ++i) {
      if(g_[*i].edge_type == SUCCESSOR_EDGE) {
        vertex_t target = boost::target(*i,g_);
        successor_lanes.push_back(g_[target].global_lane_id);
      }
    }
    return successor_lanes;
  }

  std::vector<LaneId> find_path(const LaneId& startid, const LaneId& goalid) {
    std::vector<LaneId> path;

    std::pair<vertex_t, bool> start_vertex = get_vertex_by_lane_id(startid);
    std::pair<vertex_t, bool> goal_vertex = get_vertex_by_lane_id(goalid);

    if (start_vertex.second && goal_vertex.second)
    {
      std::vector<vertex_t> p(boost::num_vertices(g_));
      std::vector<int> d(boost::num_vertices(g_));
      boost::property_map<LaneGraph, float LaneEdge::*>::type weightmap = boost::get(&LaneEdge::weight, g_);

      boost::dijkstra_shortest_paths(g_, start_vertex.first,
                                     predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g_)))
                                         .distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g_)))
                                         .weight_map(weightmap));

      // get shortest path from predecessor map
      int stop_the_loop = boost::num_vertices(g_);
      int idx = 0;
      boost::graph_traits< LaneGraph >::vertex_descriptor current = goal_vertex.first;
      while(current!=start_vertex.first && idx < stop_the_loop) {
        path.push_back(g_[current].global_lane_id);
        current=p[current];
        ++idx;
      }
      path.push_back(g_[start_vertex.first].global_lane_id);
      std::reverse(path.begin(), path.end());

      //for (auto &p : path) {
      //    std::cout << p << " ";
      //}
      //std::cout << std::endl;
    }

    return path;
  }

  //std::vector<LaneId> find_path(const LaneId& start, const RoadId& goal)
  //{
  //  std::vector<LaneId> path;
  //  return path;
  //}

  LanePtr get_laneptr(const LaneId& id){
    std::pair<vertex_t,bool> lane_vertex_pair = get_vertex_by_lane_id(id);
    if(lane_vertex_pair.second) {
      return g_[lane_vertex_pair.first].lane;
    } else {
      return NULL;
    }
  }

  //! LaneId of the neighboring lane and a flag if it exists or not
  std::pair<LaneId, bool> get_inner_neighbor(const LaneId& lane_id) {
    std::vector<std::pair<LaneId, bool> > neighbors =
     get_neighbor_from_edgetype(lane_id, INNER_NEIGHBOR_EDGE);
    std::pair<LaneId, bool> neighbor = neighbors.front(); //inner neighbor is unique
    return neighbor;
  }

  //! LaneId of the neighboring lane and a flag if it exists or not
  //! @note make sure to not call this function on the planview lane!
  std::pair<LaneId, bool> get_outer_neighbor(const LaneId& lane_id) {
    std::vector<std::pair<LaneId, bool> > tmp = get_neighbor_from_edgetype(lane_id, OUTER_NEIGHBOR_EDGE);
    return tmp.front(); // for a non planview edge the outer neighbor is unique
  }

  //! LaneId of the neighboring lane and a flag if it exists or not, usecase: lane_id is the planview
  //! @param lane_id queried lane id
  //! @param from the query answer return the lane id that is not but_not
  std::pair<LaneId, bool> get_outer_neighbor_but_not(const LaneId& lane_id, const LaneId& but_not) {
    std::vector<std::pair<LaneId, bool> > tmp = get_neighbor_from_edgetype(lane_id, OUTER_NEIGHBOR_EDGE);
    for (auto &t : tmp) {
      if (t.first != but_not) {
        return t;
      }
    }
    return std::make_pair<LaneId, bool>(0, false); //error case
  }

  bool has_lane(const LaneId& lane_id){
    boost::graph_traits<LaneGraph>::vertex_iterator i, end;
    for (boost::tie(i, end) = boost::vertices(g_); i != end; ++i) {
      if (g_[*i].global_lane_id == lane_id) {
        return true;
      }
    }
    return false;
  }

  void print_graph(const char* filename) {
    std::ofstream dotfile(filename);
    print_graph(dotfile);
    dotfile.close();
  }

  void print_graph(std::ofstream& dotfile){
    	write_graphviz(dotfile, g_
		, make_vertex_label_writer_graph(boost::get(&LaneVertex::road_id, g_), boost::get(&LaneVertex::global_lane_id, g_), boost::get(&LaneVertex::lane, g_))
		, make_edge_label_writer_text(get(&LaneEdge::edge_type, g_))
    );
  }

  LaneGraph get_lane_graph() const {
    return g_;
  }

  LaneVertex get_vertex(vertex_t v_des) {
    return g_[v_des];
  }

  std::vector<vertex_t> get_vertices() {
    std::vector<vertex_t> vertex_descriptors;
    boost::graph_traits<LaneGraph>::vertex_iterator i, end;
    for (boost::tie(i, end) = boost::vertices(g_); i != end; ++i) {
      vertex_descriptors.push_back(*i);
    }
    return vertex_descriptors;
  }

  LaneEdge get_edge(edge_t e_des) {
    return g_[e_des];
  }

  std::vector<vertex_t> get_next_vertices(vertex_t current_vertex) {
    std::vector<vertex_t> vertex_descriptors;
    boost::graph_traits<LaneGraph>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = boost::out_edges(current_vertex, g_); ei != ei_end; ei++) {
      vertex_t target = boost::target(*ei, g_);
      vertex_descriptors.push_back(target);
    }
    return vertex_descriptors;
  }

  std::vector<edge_t> get_out_edges(vertex_t current_vertex) {
    std::vector<edge_t> edge_descriptors;
    boost::graph_traits<LaneGraph>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = boost::out_edges(current_vertex, g_); ei != ei_end; ei++) {
      vertex_t source = boost::source(*ei, g_);
      vertex_t target = boost::target(*ei, g_);
      auto edge = boost::edge(source, target, g_);
      edge_descriptors.push_back(edge.first);
    }
    return edge_descriptors;
  }

  std::vector<edge_t> get_edges() {
    std::vector<edge_t> edge_descriptors;
    boost::graph_traits<LaneGraph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = boost::edges(g_); ei != ei_end; ei++) {
      vertex_t source = boost::source(*ei, g_);
      vertex_t target = boost::target(*ei, g_);
      auto edge = boost::edge(source, target, g_);
      edge_descriptors.push_back(edge.first);
    }
    return edge_descriptors;
  }

  edge_t get_edge_descr(vertex_t from, vertex_t to) {
    return boost::edge(from, to, g_).first;
  }

  std::pair<vertex_t,bool> get_vertex_by_lane_id(const LaneId& lane_id) {
    std::pair<vertex_t, bool> retval;
    retval.second = false;
    boost::graph_traits<LaneGraph>::vertex_iterator i, end;
    for (boost::tie(i, end) = boost::vertices(g_); i != end; ++i) {
      if (g_[*i].global_lane_id == lane_id) {
        retval.first = *i;
        retval.second = true;
        break;
      }
    }
    return retval;
  }

 private:
  LaneGraph g_;

  bool add_edge_of_type(const LaneId& source_id, const LaneId& target_id, const LaneEdgeType& edgetype) {
    LaneEdge edge = LaneEdge(edgetype);
    std::pair<vertex_t,bool> source_lane = get_vertex_by_lane_id(source_id);
    std::pair<vertex_t,bool> target_lane = get_vertex_by_lane_id(target_id);
    if(source_lane.second && target_lane.second) {
      boost::add_edge(source_lane.first, target_lane.first, edge, g_);
      return true;
    } else {
      return false;
    }
  }

  std::vector<std::pair<LaneId, bool> > get_neighbor_from_edgetype(const LaneId& lane_id, const LaneEdgeType edge_type) { //! we can have two outer neighbors
    std::vector<std::pair<LaneId, bool> > retval;
    std::pair<vertex_t, bool> lane_vertex_pair = get_vertex_by_lane_id(lane_id);
    boost::graph_traits<LaneGraph>::out_edge_iterator i, end;
    for (boost::tie(i, end) = boost::out_edges(lane_vertex_pair.first, g_); i != end; ++i) {
      if(g_[*i].edge_type == edge_type) {
        vertex_t target = boost::target(*i,g_);
        retval.push_back(std::make_pair(g_[target].global_lane_id, true));
      }
    }
    if (retval.empty()) {
      retval.push_back(std::make_pair(0, false));
    }
    return retval;
  }

  template <class RoadIdMap, class LaneIdMap, class LaneMap>
  class my_vertex_writer_graph {
  public:
    my_vertex_writer_graph(RoadIdMap r, LaneIdMap l, LaneMap lanemap) : rm(r), lm(l), lanemap_(lanemap) {}
    template <class LaneVertex>
    void operator()(std::ostream &out, const LaneVertex& s) const {
      out << "[" <<
        "label=\"" << "road_id=" << rm[s] << " lane_id=" << lm[s] << " lane_pos=" << lanemap_[s]->get_lane_position() <<
        "\"]";
    }
  private:
    RoadIdMap rm;
    LaneIdMap lm;
    LaneMap lanemap_;
  };

  template <class RoadIdMap, class LaneIdMap, class LaneMap>
  inline my_vertex_writer_graph<RoadIdMap, LaneIdMap, LaneMap>
  make_vertex_label_writer_graph(RoadIdMap r, LaneIdMap l, LaneMap lm) {
    return my_vertex_writer_graph<RoadIdMap, LaneIdMap, LaneMap> (r, l, lm);
  }

  template <class TypeMap>
  class my_edge_writer_text {
  public:
    my_edge_writer_text(TypeMap t) : tm(t) {}
    template <class LaneEdge>
    void operator()(std::ostream &out, const LaneEdge& e) const {
      const char* color = tm[e]==0?"red":"blue";
      out << "[label=\"" << tm[e] << "\"" <<  "color=\"" << color << "\"" << "]";
    }
  private:
    TypeMap tm;
  };

  template <class TypeMap>
  inline my_edge_writer_text<TypeMap>
  make_edge_label_writer_text(TypeMap t) {
    return my_edge_writer_text<TypeMap> (t);
  }

};

typedef std::shared_ptr<Roadgraph> RoadgraphPtr;

}  // namespace map
}  // namespace world
}  // namespace modules

#endif  // MODULES_WORLD_ROADGRAPH_HPP_


#include <Core/SettingsFields.h>

#include <Core/Field.h>
#include <Common/getNumberOfPhysicalCPUCores.h>
#include <Common/FieldVisitorConvertToNumber.h>
#include <Common/logger_useful.h>
#include <IO/ReadHelpers.h>
#include <IO/ReadBufferFromString.h>
#include <IO/WriteHelpers.h>
#include <boost/algorithm/string/predicate.hpp>

#include <cmath>

namespace DB
{
namespace ErrorCodes
{
    extern const int SIZE_OF_FIXED_STRING_DOESNT_MATCH;
    extern const int CANNOT_PARSE_BOOL;
    extern const int CANNOT_PARSE_NUMBER;
}

namespace
{
    template <typename T>
    T stringToNumber(const String & str)
    {
        if constexpr (std::is_same_v<T, bool>)
        {
            if (str == "0")
                return false;
            if (str == "1")
                return true;
            if (boost::iequals(str, "false"))
                return false;
            if (boost::iequals(str, "true"))
                return true;
            throw Exception("Cannot parse bool from string '" + str + "'", ErrorCodes::CANNOT_PARSE_BOOL);
        }
        else
            return parseWithSizeSuffix<T>(str);
    }

    template <typename T>
    T fieldToNumber(const Field & f)
    {
        if (f.getType() == Field::Types::String)
            return stringToNumber<T>(f.get<const String &>());
        else
            return applyVisitor(FieldVisitorConvertToNumber<T>(), f);
    }
}

template <typename T>
SettingFieldNumber<T>::SettingFieldNumber(const Field & f) : SettingFieldNumber(fieldToNumber<T>(f))
{
}

template <typename T>
SettingFieldNumber<T> & SettingFieldNumber<T>::operator=(const Field & f)
{
    *this = fieldToNumber<T>(f);
    return *this;
}

template <typename T>
String SettingFieldNumber<T>::toString() const
{
    return ::DB::toString(value);
}

template <typename T>
void SettingFieldNumber<T>::parseFromString(const String & str)
{
    *this = stringToNumber<T>(str);
}

template <typename T>
void SettingFieldNumber<T>::writeBinary(WriteBuffer & out) const
{
    if constexpr (std::is_integral_v<T> && is_unsigned_v<T>)
        writeVarUInt(static_cast<UInt64>(value), out);
    else if constexpr (std::is_integral_v<T> && is_signed_v<T>)
        writeVarInt(static_cast<Int64>(value), out);
    else
    {
        static_assert(std::is_floating_point_v<T>);
        writeStringBinary(::DB::toString(value), out);
    }
}

template <typename T>
void SettingFieldNumber<T>::readBinary(ReadBuffer & in)
{
    if constexpr (std::is_integral_v<T> && is_unsigned_v<T>)
    {
        UInt64 x;
        readVarUInt(x, in);
        *this = static_cast<T>(x);
    }
    else if constexpr (std::is_integral_v<T> && is_signed_v<T>)
    {
        Int64 x;
        readVarInt(x, in);
        *this = static_cast<T>(value);
    }
    else
    {
        static_assert(std::is_floating_point_v<T>);
        String str;
        readStringBinary(str, in);
        *this = ::DB::parseFromString<T>(str);
    }
}

template struct SettingFieldNumber<UInt64>;
template struct SettingFieldNumber<Int64>;
template struct SettingFieldNumber<float>;
template struct SettingFieldNumber<bool>;

namespace
{
    UInt64 stringToMaxThreads(const String & str)
    {
        if (startsWith(str, "auto"))
            return 0;
        return parseFromString<UInt64>(str);
    }

    UInt64 fieldToMaxThreads(const Field & f)
    {
        if (f.getType() == Field::Types::String)
            return stringToMaxThreads(f.get<const String &>());
        else
            return applyVisitor(FieldVisitorConvertToNumber<UInt64>(), f);
    }
}

SettingFieldMaxThreads::SettingFieldMaxThreads(const Field & f) : SettingFieldMaxThreads(fieldToMaxThreads(f))
{
}

SettingFieldMaxThreads & SettingFieldMaxThreads::operator=(const Field & f)
{
    *this = fieldToMaxThreads(f);
    return *this;
}

String SettingFieldMaxThreads::toString() const
{
    if (is_auto)
        return "'auto(" + ::DB::toString(value) + ")'";
    else
        return ::DB::toString(value);
}

void SettingFieldMaxThreads::parseFromString(const String & str)
{
    *this = stringToMaxThreads(str);
}

void SettingFieldMaxThreads::writeBinary(WriteBuffer & out) const
{
    writeVarUInt(is_auto ? 0 : value, out);
}

void SettingFieldMaxThreads::readBinary(ReadBuffer & in)
{
    UInt64 x = 0;
    readVarUInt(x, in);
    *this = x;
}

UInt64 SettingFieldMaxThreads::getAuto()
{
    return getNumberOfPhysicalCPUCores();
}

namespace
{
    Poco::Timespan::TimeDiff float64AsSecondsToTimespan(Float64 d)
    {
        if (d != 0.0 && !std::isnormal(d))
            throw Exception(
                ErrorCodes::CANNOT_PARSE_NUMBER, "A setting's value in seconds must be a normal floating point number or zero. Got {}", d);
        return static_cast<Poco::Timespan::TimeDiff>(d * 1000000);
    }

}

template <>
SettingFieldSeconds::SettingFieldTimespan(const Field & f) : SettingFieldTimespan(float64AsSecondsToTimespan(fieldToNumber<Float64>(f)))
{
}

template <>
SettingFieldMilliseconds::SettingFieldTimespan(const Field & f) : SettingFieldTimespan(fieldToNumber<UInt64>(f))
{
}

template <>
SettingFieldTimespan<SettingFieldTimespanUnit::Second> & SettingFieldSeconds::operator=(const Field & f)
{
    *this = Poco::Timespan{float64AsSecondsToTimespan(fieldToNumber<Float64>(f))};
    return *this;
}

template <>
SettingFieldTimespan<SettingFieldTimespanUnit::Millisecond> & SettingFieldMilliseconds::operator=(const Field & f)
{
    *this = fieldToNumber<UInt64>(f);
    return *this;
}

template <>
String SettingFieldSeconds::toString() const
{
    return ::DB::toString(static_cast<Float64>(value.totalMicroseconds()) / microseconds_per_unit);
}

template <>
String SettingFieldMilliseconds::toString() const
{
    return ::DB::toString(operator UInt64());
}

template <>
SettingFieldSeconds::operator Field() const
{
    return static_cast<Float64>(value.totalMicroseconds()) / microseconds_per_unit;
}

template <>
SettingFieldMilliseconds::operator Field() const
{
    return operator UInt64();
}

template <>
void SettingFieldSeconds::parseFromString(const String & str)
{
    Float64 n = parse<Float64>(str.data(), str.size());
    *this = Poco::Timespan{static_cast<Poco::Timespan::TimeDiff>(n * microseconds_per_unit)};
}

template <>
void SettingFieldMilliseconds::parseFromString(const String & str)
{
    *this = stringToNumber<UInt64>(str);
}

template <SettingFieldTimespanUnit unit_>
void SettingFieldTimespan<unit_>::writeBinary(WriteBuffer & out) const
{
    /// Note that this returns an UInt64 (for both seconds and milliseconds units) for compatibility reasons as the value
    /// for seconds used to be a integer (now a Float64)
    /// This method is only used to communicate with clients or servers older than DBMS_MIN_REVISION_WITH_SETTINGS_SERIALIZED_AS_STRINGS
    /// in which the value was passed as binary (as a UInt64)
    /// Later versions pass the setting values as String (using toString() and parseFromString()) and there passing "1.2" will
    /// lead to `1` on releases with integer seconds or `1.2` on more recent releases
    /// See https://github.com/ClickHouse/ClickHouse/issues/36940 for more details
    auto num_units = operator UInt64();
    writeVarUInt(num_units, out);
}

template <SettingFieldTimespanUnit unit_>
void SettingFieldTimespan<unit_>::readBinary(ReadBuffer & in)
{
    UInt64 num_units = 0;
    readVarUInt(num_units, in);
    *this = num_units;
}

template struct SettingFieldTimespan<SettingFieldTimespanUnit::Second>;
template struct SettingFieldTimespan<SettingFieldTimespanUnit::Millisecond>;

void SettingFieldString::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(value, out);
}

void SettingFieldString::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = std::move(str);
}

namespace
{
    char stringToChar(const String & str)
    {
        if (str.size() > 1)
            throw Exception("A setting's value string has to be an exactly one character long", ErrorCodes::SIZE_OF_FIXED_STRING_DOESNT_MATCH);
        if (str.empty())
            return '\0';
        return str[0];
    }

    char fieldToChar(const Field & f)
    {
        return stringToChar(f.safeGet<const String &>());
    }
}

SettingFieldChar::SettingFieldChar(const Field & f) : SettingFieldChar(fieldToChar(f))
{
}

SettingFieldChar & SettingFieldChar::operator =(const Field & f)
{
    *this = fieldToChar(f);
    return *this;
}

void SettingFieldChar::parseFromString(const String & str)
{
    *this = stringToChar(str);
}

void SettingFieldChar::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(toString(), out);
}

void SettingFieldChar::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = stringToChar(str);
}

void SettingFieldURI::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(value.toString(), out);
}

void SettingFieldURI::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = Poco::URI{str};
}

void SettingFieldEnumHelpers::writeBinary(const std::string_view & str, WriteBuffer & out)
{
    writeStringBinary(str, out);
}

String SettingFieldEnumHelpers::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    return str;
}

String SettingFieldCustom::toString() const
{
    return value.dump();
}

void SettingFieldCustom::parseFromString(const String & str)
{
    *this = Field::restoreFromDump(str);
}

void SettingFieldCustom::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(toString(), out);
}

void SettingFieldCustom::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    parseFromString(str);
}

}


/*
 * OctomapConverterTest.cpp
 *
 *  Created on: May 1, 2017
 *      Author: Jeff Delmerico
 *	 Institute: University of Zürich, Robotics and Perception Group
 */

// Grid map
#include <grid_map_core/GridMap.hpp>
#include <grid_map_octomap/GridMapOctomapConverter.hpp>

// Octomap
#include <octomap/octomap.h>
#include <octomap/math/Utils.h>

// Gtest
#include <gtest/gtest.h>

// Eigen
#include <Eigen/Core>

using namespace grid_map;

TEST(OctomapConversion, convertOctomapToGridMap)
{
  // Generate Octomap (a 1m sphere centered at (1,1,1))
  // Adapted from octomap unit_tests.cpp
  octomap::OcTree octomap(0.05);
  octomap::Pointcloud* measurement = new octomap::Pointcloud();

  octomap::point3d origin(1.0f, 1.0f, 1.0f);
  octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);

  for (int i = 0; i < 360; i++) {
    for (int j = 0; j < 360; j++) {
      octomap::point3d p = origin + point_on_surface;
      measurement->push_back(p);
      point_on_surface.rotate_IP(0, 0, DEG2RAD(1.));
    }
    point_on_surface.rotate_IP(0, DEG2RAD(1.), 0);
  }
  octomap.insertPointCloud(*measurement, origin);

  // Convert to grid map.
  GridMap gridMap;
  bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridMap);
  ASSERT_TRUE(res);

  // Check map info.
  Length length = gridMap.getLength();
  Vector3 octo_length;
  octomap.getMetricSize(octo_length.x(), octo_length.y(), octo_length.z());
  EXPECT_FLOAT_EQ(octo_length.x(), length.x());
  EXPECT_FLOAT_EQ(octo_length.y(), length.y());
  EXPECT_FLOAT_EQ(octomap.getResolution(), gridMap.getResolution());
  EXPECT_FLOAT_EQ(2.0 + 0.5 * octomap.getResolution(), gridMap.atPosition("elevation", Position(1.0, 1.0)));
}

TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox)
{
  // Repeat test with bounding box

  // Generate Octomap (a 1m sphere centered at (1,1,1))
  // Adapted from Octomap unit_tests.cpp
  octomap::OcTree octomap(0.05);
  octomap::Pointcloud* measurement = new octomap::Pointcloud();

  octomap::point3d origin(1.0f, 1.0f, 1.0f);
  octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);

  for (int i = 0; i < 360; i++) {
    for (int j = 0; j < 360; j++) {
      octomap::point3d p = origin + point_on_surface;
      measurement->push_back(p);
      point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
    }
    point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
  }
  octomap.insertPointCloud(*measurement, origin);

  // Convert to grid map.
  GridMap gridmap;
  grid_map::Position3 minpt(0.5, 0.5, 0.0);
  grid_map::Position3 maxpt(2.0, 2.0, 1.0);
  bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridmap, &minpt, &maxpt);
  ASSERT_TRUE(res);
  Length length = gridmap.getLength();
  EXPECT_FLOAT_EQ(1.5, length.x());
  EXPECT_FLOAT_EQ(1.5, length.y());
  EXPECT_FLOAT_EQ(octomap.getResolution(), gridmap.getResolution());
  EXPECT_FLOAT_EQ(0.5 * octomap.getResolution(), gridmap.atPosition("elevation", Position(1.0, 1.0)));
}


#ifndef BOOST_QVM_GEN_MAT_ASSIGN4_HPP_INCLUDED
#define BOOST_QVM_GEN_MAT_ASSIGN4_HPP_INCLUDED

// Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.

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

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/config.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/mat_traits.hpp>

namespace boost { namespace qvm {

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
assign( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(a,mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(a,mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(a,mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(a,mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(a,mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(a,mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(a,mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(a,mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(a,mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(a,mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(a,mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(a,mat_traits<B>::template read_element<3,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::assign;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct assign_mm_defined;

    template <>
    struct
    assign_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    A &>::type
assign( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(a,mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(a,mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(a,mat_traits<B>::template read_element<3,0>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::assign;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct assign_mm_defined;

    template <>
    struct
    assign_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
assign( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<B>::template read_element<0,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::assign;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct assign_mm_defined;

    template <>
    struct
    assign_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

} }

#endif


/*
 * ray.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/vec2.h>
#include <scitbx/vec3.h>
#include <dials/model/data/ray.h>

namespace dials { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::vec2;
  using scitbx::vec3;

  static vec3<double> get_s1(const Ray &ray) {
    return ray.s1;
  }

  static void set_s1(Ray &ray, const vec3<double> &s1) {
    ray.s1 = s1;
  }

  void export_ray() {
    class_<Ray>("Ray")
      .add_property("s1", get_s1, set_s1)
      .def_readwrite("angle", &Ray::angle)
      .def_readwrite("entering", &Ray::entering);
  }

}}}  // namespace dials::model::boost_python


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing testz capabilities

    Returns True is only if all bits set in x are not set in y

    This is a convenient alias of @ref is_included_c
  **/
  const boost::dispatch::functor<tag::testz_> testz = {};
} }
#endif

#include <boost/simd/function/scalar/is_included_c.hpp>
#include <boost/simd/function/simd/testz.hpp>

#endif


#ifndef BOOST_METAPARSE_GETTING_STARTED_6_HPP
#define BOOST_METAPARSE_GETTING_STARTED_6_HPP

// Automatically generated header file

// Definitions before section 5.2.4
#include "5_2_4.hpp"

// Definitions of section 5.2.4
#include <boost/metaparse/foldl_start_with_parser.hpp>

using exp_parser11 =
 build_parser<
   foldl_start_with_parser<
     sequence<plus_token, int_token>, /* apply this parser repeatedly */
     int_token, /* use this parser to get the initial value */
     boost::mpl::quote2<sum_items> /* use this function to add a new value to the summary */
   >
 >;

// query:
//    exp_parser11::apply<BOOST_METAPARSE_STRING("1 + 2 + 3 + 4")>::type

#endif


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

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T.Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
    @ingroup group-boolean
    Function object implementing sel capabilities

    If cond is @ref True returns t else returns f

    This is a convenient alias of @ref if_else
  **/
  const boost::dispatch::functor<tag::sel_> sel = {};
} }
#endif

#include <boost/simd/function/scalar/sel.hpp>
#include <boost/simd/function/simd/sel.hpp>

#endif


/* Copyright (c) 2014, Stefan.Eilemann@epfl.ch
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * - Neither the name of Eyescale Software GmbH nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <vmmlib/frustum.hpp>
#include <vmmlib/frustum_culler.hpp>

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

static void _testCull( const vmml::frustum_culler< float >& fc )
{
    const vmml::vector< 4, float > sphereIn( 0.f, 0.f, -10.f, 1.f );
    const vmml::vector< 4, float > sphereOut( 0.f, 0.f, 0.f, .5f );
    const vmml::vector< 4, float > sphereBorder( 0.f, 0.f, -1.f, 1.f );

    BOOST_CHECK_EQUAL( fc.test_sphere( sphereIn ), vmml::VISIBILITY_FULL );
    BOOST_CHECK_EQUAL( fc.test_sphere( sphereOut ), vmml::VISIBILITY_NONE );
    BOOST_CHECK_EQUAL( fc.test_sphere( sphereBorder ), vmml::VISIBILITY_PARTIAL );

    const vmml::vector< 2, float > xy( -1.f, 1.f );
    const vmml::vector< 2, float > zIn( -2.f, -4.f );
    const vmml::vector< 2, float > zOut( 0.f, -.5f );
    const vmml::vector< 2, float > zBorder( -.5f, -1.5f );

    BOOST_CHECK_EQUAL( fc.test_aabb( xy, xy, zIn ), vmml::VISIBILITY_FULL );
    BOOST_CHECK_EQUAL( fc.test_aabb( xy, xy, zOut ), vmml::VISIBILITY_NONE );
    BOOST_CHECK_EQUAL( fc.test_aabb( xy, xy, zBorder ), vmml::VISIBILITY_PARTIAL );
}

BOOST_AUTO_TEST_CASE(frustum_base)
{
    const vmml::frustum< float > frustum( -1.f, 1., -1.f, 1., 1.f, 100.f );
    const vmml::matrix< 4, 4, float > mvp = frustum.compute_matrix();

    vmml::frustum_culler< float > fc;
    fc.setup( mvp );
    _testCull( fc );

    //   e_____f
    //  /     /|
    // | a b | |
    // | c d |/h
    //  -----
    const vmml::vector< 3, float > a( -1.f,  1.f, -1.f );
    const vmml::vector< 3, float > b(  1.f,  1.f, -1.f );
    const vmml::vector< 3, float > c( -1.f, -1.f, -1.f );
    const vmml::vector< 3, float > d(  1.f, -1.f, -1.f );
    const vmml::vector< 3, float > e( -100.f,  100.f, -100.f );
    const vmml::vector< 3, float > f(  100.f,  100.f, -100.f );
    const vmml::vector< 3, float > g( -100.f, -100.f, -100.f );
    const vmml::vector< 3, float > h(  100.f, -100.f, -100.f );

    fc.setup( a, b, c, d, e, f, g, h );
    _testCull( fc );
}


#ifndef ODESOLVER_HPP_INCLUDED
#define ODESOLVER_HPP_INCLUDED

#include <petscts.h>
#include <string>
#include <cmath>
#include <vector>
#include <algorithm>
#include <assert.h>
#include <boost/circular_buffer.hpp>
#include "integratorContextEx.hpp"
#include "genFuncs.hpp"

using namespace std;
/*
 * Provides a set of algorithms to solve a system of ODEs of
 * the form y' = f(t,y) using EXPLICIT time stepping.
 *
 * SOLVER TYPE      ALGORITHM
 *  FEuler        forward Euler
 *  RK32          explicit Runge-Kutta (2,3)
 *  RK43          explicit Runge-Kutta (3,4)
 *
 * To obtain solutions at user-specified times, use FEuler and call setStepSize
 * in the routine f(t,y).
 *
 * y is represented as an array of one or more Vecs (PETSc data type).
 *
 * At minimum, the user must specify:
 *     QUANTITY               FUNCTION
 *  max number of steps      constructor
 *  solver type              constructor
 *  initial conditions       setInitialConds     Note: this array will be modified during integration
 *  initial step size        constructor
 *  step size alg            constructor (this is only used by the adaptive time-stepping algorithm)
 *  f(t,y)                   object passed to integrate must have member function d_dt(PetscScalar, PetscScalar*,PetscScalar*)
 *  final time               constructor and setTimeRange
 *  timeMonitor              object passed to integrate must have member function timeMonitor
 *
 *
 * Optional fields that can also be specified:
 *     QUANTITY               FUNCTION
 *  tolerance                setTolerance
 *  maximum step size        setStepSize
 *  minimum step size        setTimeStepBounds
 *  initial step size        setTimeStepBounds
 *
 * Once the odeSolver context is set, call integrate() to perform
 * the integration.
 *
 * y(t = final_time) is stored in the initial conditions array. Summary output
 * information is provided by viewSolver. Users can obtain information at
 * each time step within a user-defined monitor function.
 *
 */

class OdeSolver
{
public:

  PetscReal          _initT,_finalT,_currT,_deltaT;
  PetscReal          _newDeltaT; // stores future deltaT for access by outside classes, primarily for checkpointing
  PetscInt           _maxNumSteps,_stepCount;
  map<string,Vec>    _var,_dvar; // integration variable and rate
  vector<string>     _errInds; // which keys of _var to use for error control
  vector<double>     _scale; // scale factor for entries in _errInds
  double             _runTime;
  string             _controlType;
  string             _normType;

  // for PID error control
  boost::circular_buffer<double> _errA;
  map<string,Vec> _y2,_y3,_y4;

  OdeSolver(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  virtual ~OdeSolver() {};

  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setInitialStepCount(const PetscReal stepCount);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setToleranceType(const string normType); // type of norm used for error control

  virtual PetscErrorCode setTolerance(const PetscReal tol) = 0;
  virtual PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT) = 0;
  virtual PetscErrorCode setInitialConds(map<string,Vec>& var){return 1;};
  virtual PetscErrorCode setErrInds(vector<string>& errInds) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) = 0;
  virtual PetscErrorCode view() = 0;
  virtual PetscErrorCode integrate(IntegratorContextEx *obj) = 0;
};

// FEuler is a derived class from OdeSolver
class FEuler : public OdeSolver
{
public:
  FEuler(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~FEuler();
  PetscErrorCode view();

  PetscErrorCode setTolerance(const PetscReal tol){return 0;};
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT){ return 0;};
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds) {return 0;};
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) {return 0;};
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

// Runge-kutta time-stepping, 3rd-order
// Based on algorithm from Hairer et al.
class RK32 : public OdeSolver
{
public:

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _totTol; // total tolerance, might be atol, or rtol, or a combination of both
  PetscReal   _kappa,_ord; // safety factor in step size determinance, order of accuracy of method
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;

  PetscReal   _totErr;

  map<string,Vec> _k1,_f1,_k2,_f2,_y2,_y3;

  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();

  // constructor and destructor
  RK32(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK32();

  // member functions of this class
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

// Based on "ARK4(3)6L[2]SA-ERK" algorithm from Kennedy and Carpenter (2003):
// "Additive Runge-Kutta schemes for convection-diffusion-reaction equations"
// Runge-Kutta time-stepping, 4th order
// Note: Has matching IMEX equivalent
class RK43 : public OdeSolver
{
public:

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _atol,_rtol;
  PetscReal   _totTol;
  PetscReal   _kappa,_ord;
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;
  PetscReal   _totErr;

  map<string,Vec> _k1,_k2,_k3,_k4,_k5,_k6,_y3,_y4;
  map<string,Vec> _f1,_f2,_f3,_f4,_f5,_f6;

  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();

  // constructor and destructor
  RK43(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK43();

  // various member functions
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& var);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextEx *obj);
};

#endif