#include "stdafx.h"

#include <OgreSubMesh.h>

#include <memory>
#include "MainLib/OgreFltk/MotionPanel.h"
#include "MainLib/OgreFltk/Mesh.h"
#include "MainLib/OgreFltk/FlLayout.h"
#include "MainLib/OgreFltk/framemoveobject.h"
#include "MainLib/OgreFltk/FltkRenderer.h"
#include "MainLib/OgreFltk/FlChoice.h"
#include "MainLib/OgreFltk/RE.h"
#include "MainLib/OgreFltk/pldprimskin.h"
#include "DeformableBody.h"
#include "BaseLib/baselib.h"
#include "BaseLib/math/Operator_NR.h"
#include "BaseLib/utility/scoped_ptr.h"
#include "femworld/IntegratedWorld.h"

#include <boost/smart_ptr.hpp>
#include <OgreSceneNode.h>
#include <OgreSceneManager.h>
#include <OgreEntity.h>

#include "cma/CMAwrap.h"
#include <OgrePrerequisites.h>
#include "MainLib/OgreFltk/objectList.h"
#include "MainLib/OgreFltk/FltkRenderer.h"
#include "MainLib/OgreFltk/Line3D.h"
#include "MainLib/OgreFltk/PointClouds.h"
#include <Fl/Fl_Double_Window.H>
#include "MainLib/OgreFltk/Fl_Hor_Slider2.h"
#include "MainLib/OgreFltk/FltkAddon.h"
#include "MainLib/OgreFltk/FltkMotionWindow.h"
#include "ClassificationLib/motion/TwoPosture.h"
#include "MainLib/OgreFltk/FlLayout.h"
#include "MainLib/OgreFltk/FltkScrollPanel.h"
#include "BaseLib/utility/operatorString.h"
#include "BaseLib/math/Operator.h"
//#include "../MainLib/ogrefltk/Motion.h"
#include <OgreMovableObject.h>
#include <OgreRenderable.h>
#include "MainLib/OgreFltk/MovableText.h"
#include "MainLib/OgreFltk/OgreMotionLoader.h"
//#include "saveZbuffer.h"
#include "MeanValueWeights.h"
#include "CompareHelper.h"

using namespace TetraMeshLoader;

double fitfun(vector3N const v3x, vector3N const v3y) { /* function "cigtab" */
    int i;
    double sum = 0;
//  for(i = 2; i < N; ++i)
//   sum += x[i]*x[i];
    return sum;
}

MeshDeformer* createHarmonicMeshDeformer();
MeshDeformer* createMeanValueMeshDeformer();

// defined in TestWin.cpp
void getVertices(OBJloader::Mesh const& mesh, vector3N & o);
void setVertices(OBJloader::Mesh & mesh, vector3N const& o);

class TetraDeformTestWin : public FlLayout, public FrameMoveObject, public FltkRenderer::Handler {

    TetraMesh mMesh;
    IntegratedWorld mSoftWorld;

    boost::shared_ptr<Ogre::SceneNode> mNode;

    MeshDeformer *mMeshDeformer;

    //MeshDeformer *mTargetMeshInControlVolume;
	//matrixn targetDeformToControlDeform;
	LinearDeformer *mControlPointsInFeatureMesh;
	vector3N m_vertices; // for the attachment CSTs
	vector3N m_vertices_orig; // for the attachment CSTs

    bool isLoadTet;
    BillboardLineList *line;
    BillboardLineList *ano_line;

    float min_selected_mesh;
    float max_selected_mesh;

    cmaes_t evo;
    ObjectList mObjectList;
    intmatrixn mLinks;

    CMAwrap *p_cma;

    vectorn vecn_start = vectorn(5, 0, 0, 0, 0, 0);
    vectorn vecn_dest = vectorn(5, 10, 12, 7, 4, 5);

    double *const *pop;

    //OBJloader::Mesh mObjTetra;
    OBJloader::MeshToEntity *mpTetraObjViewer;
    OBJloader::MeshToEntity *mpSurfaceObjViewer;
    OBJloader::MeshToEntity *mpTargetObjViewer;
    Ogre::Entity *mpTetraObjEntity;
    Ogre::Entity *mpSurfaceObjEntity;
    Ogre::Entity *mpTargetObjEntity;

public:

	int getNumSurfaceVertex() const {
		TString surfaceVertexFile = _meshName + ".sv";

		CTextFile file;
		if (!file.OpenReadFile(surfaceVertexFile)) {
			printf("Error! Faild to open %s\n", surfaceVertexFile.ptr());
			return NULL;
		}

		int result = atoi(file.GetToken());

		file.CloseFile();

		return result;
	} // hard-coded

    TetraDeformTestWin(int x, int y, int w, int h, MotionPanel &mp, FltkRenderer &renderer);

    ~TetraDeformTestWin(void);

	void tetToObj();
    // FltkRenderer::Handler
    virtual int handleRendererEvent(int ev);

	void loadTetMesh(const char* mesh)
	{
        bool bInit = true;

		if (!mNode) {
			//mNode.reset(RE::createSceneNode(RE::generateUniqueName()), (void(*)(Ogre::SceneNode* node))RE::removeEntity);
		}
		mMesh.loadMesh(mesh);
		/*
		matrix4 s_vec = matrix4();
		s_vec.setScaling(20, 20, 20);
		mMesh.transform(s_vec);
		*/

		mSoftWorld.addBody("testmesh", mMesh, 30);

		setLinearConstraint();

		redraw();
		isLoadTet = true;

		Ogre::SceneNode* pBg=RE::renderer().viewport().mScene->getSceneNode("BackgroundNode");
		pBg->setVisible(false);
	}

    MotionPanel &m_motionPanel;
    FltkRenderer &mRenderer;

    // layout callback
    virtual void onCallback(FlLayout::Widget const &w, Fl_Widget *pWidget, int userData);

    // FrameMoveObject
    virtual int FrameMove(float fElapsedTime);

    // Fl_Window
    virtual void show() {
        FlLayout::show();
        mRenderer.setHandler(this);
    }

    void redraw();

    void initializeCMA();

    void setLinearConstraint();

    void clearSelection();

    boolN mSelectedVertices;
    boolN mSelectedEdges;
    boolN mSelectedTetra;

    void drawSelection();

    OBJloader::Mesh *tet_to_obj;
    OBJloader::Mesh *mTargetMesh;
    OBJloader::Mesh *mTargetMesh_originalShape;
    OBJloader::Mesh mGrootHighMesh;

private:
	// For apply motion vector to Obj Mesh
	OBJloader::Mesh* UpdateMesh(OBJloader::Mesh* source);
	void ChangeSimilarity(double value);
	void ChangeError(double pcm, double rms);
	Fl_Widget* _maxWidget;
	Fl_Widget* _avgWidget;
	CompareHelper _compareHelper;

	TString _meshName; // File path + name without expansion
};

// FrameMoveObject
int TetraDeformTestWin::FrameMove(float fElapsedTime) {
	mObjectList.FrameMove(fElapsedTime);
    if (isLoadTet) {
        mSoftWorld.TimeStepping();
        redraw();
    }
    return 1;
}

TetraDeformTestWin::TetraDeformTestWin(int x, int y, int w, int h, MotionPanel &mp, FltkRenderer &renderer)
        : FlLayout(0, 0, w, h),
          m_motionPanel(mp),
          mRenderer(renderer),
		  mTargetMesh(NULL),
		  tet_to_obj(NULL),
          mSoftWorld(1.0 / 60.0, 0.95) {
    create("Button", "Load a mesh", "Load a mesh");
    //create("Button", "Load a embedded mesh", "Load a embedded mesh");
    create("Button", "Load a deformed mesh", "Load a deformed mesh");
    create("Value_Slider", "activation", "activation");
    slider(0)->range(0,0.03);
    slider(0)->value(0);

    create("Value_Slider", "min_select", "min_select");
    create("Value_Slider", "max_select", "max_select");

    create("Button", "save_current", "save_current");
    create("Button", "start_optimize", "start_optimize");

    //slider(0)->range(0,1);
    //slider(0)->value(0);

    create("Choice", "Option");
    menu(0)->size(2);
    menu(0)->item(0, "Option", 0);
    menu(0)->item(1, "Show mesh", 0, Hash("Show mesh"), FL_MENU_TOGGLE);
    menu(0)->item(1).set();
    menu(0)->value(0);

	_maxWidget = create("Box", "max", "Max Error : 0.0000");
	_avgWidget = create("Box", "avg", "Avg Error : 0.0000");
    updateLayout();
    renderer.ogreRenderer().addFrameMoveObject(this);
    isLoadTet = false;
    line = NULL;
    ano_line = NULL;

    p_cma = new CMAwrap(vecn_start, vecn_dest, 10, 2);
}

TetraDeformTestWin::~TetraDeformTestWin(void) {

	if(line)
	{
        RE::ogreRootSceneNode()->detachObject(line);
        RE::ogreRootSceneNode()->detachObject(ano_line);
        delete line;
	}
}

OBJloader::Mesh *tetraToObj(TetraMesh &srctet, const char *facefile, int surface_vtx_num ) {
    int n, m;
    vector3 x_tmp;
    CTextFile file;
    if (!file.OpenReadFile(facefile)) {
        printf("Error! Faild to open %s\n", facefile);
        return NULL;
    }

    OBJloader::Mesh *tetSurf_obj = new OBJloader::Mesh();

	//tetSurf_obj->resizeBuffer(OBJloader::Buffer::VERTEX, srctet.getNumNode());
	tetSurf_obj->resizeBuffer(OBJloader::Buffer::VERTEX, surface_vtx_num);
    for (int i = 0; i < surface_vtx_num; i++) {
        tetSurf_obj->getVertex(i) = srctet.nodePos(i);
    }

    TString token;
    TString currMode = "v";
    intvectorn typeCode;

    vector3N pos;

    int num_face = atoi(file.GetToken());
    int idon = atoi(file.GetToken());

    while (1) {
        char *t = file.GetToken();
        if (t == NULL)
            break;
        token = t;
        OBJloader::Face f;
//		int index=atoi(file.GetToken());
        int i = atoi(file.GetToken());
        int j = atoi(file.GetToken());
        int k = atoi(file.GetToken());
        int temp = atoi(file.GetToken());
        f.setIndex(i - 1, j - 1, k - 1, OBJloader::Buffer::VERTEX);

//		TString temp=file.GetToken();
//		if (temp.length()>0 && temp[0]>='0' && temp[0]<='9')
//		{
//			tetSurf_obj->m_arrayFace.push_back(f);
//			// quad mesh: triangulate
//			int l=atoi(temp);
//			f.setIndex(i-1,k-1, l-1, Buffer::VERTEX);
//		}
//		else
//			file.Undo();
        tetSurf_obj->addFace(f);
    }
    file.CloseFile();

	tetSurf_obj->saveObj("surface.obj", false, false);
    return tetSurf_obj;
}

void TetraDeformTestWin::onCallback(FlLayout::Widget const &w, Fl_Widget *pWidget, int userData) {
	if (w.mId =="Load a deformed mesh"){
        TString mesh=_meshName + "_deformed.obj"; //"deformed.mesh"; // target mesh file name.
        TString mesh_orig=_meshName + ".obj"; //"../mesh/Deformed_groot/iamgroot9k2.obj"; // corresponding undeformed mesh  (with the same topology).

        OBJloader::MeshToEntity::Option o;
        o.useTexCoord = false;
        o.useNormal = false;
        o.buildEdgeList = false;
		mTargetMesh=new OBJloader::Mesh();
		mTargetMesh->loadObj(mesh.ptr());
		mTargetMesh_originalShape=new OBJloader::Mesh();
		mTargetMesh_originalShape->loadObj(mesh_orig.ptr());

		if (0)
		{
			// calculate mapping from deformed mesh to control mesh
			/*
			mTargetMeshInControlVolume = createMeanValueMeshDeformer();
			Msg::verify(tet_to_obj, "Error! tet_to_obj first!");
			mTargetMeshInControlVolume ->calculateCorrespondence(*tet_to_obj, *mTargetMesh_originalShape);

			int nv=mTargetMesh_originalShape->numVertex();
			int ntv=getNumSurfaceVertex();
			for(int i=0; i<nv; i++)
			{
				vectorn& w=((LinearMeshDeformer*)mTargetMeshInControlVolume)->getWeights(i);
				std::cout<<i <<" : "<<w.size() <<","<<w.sum()<<std::endl;
			}

			// w11 w12 w13 ...
			// w21 w22 w23 ...
			// ...
			matrixn W(nv*3, ntv*3);
			W.setAllValue(0.0);
			for(int i=0; i<nv; i++)
			{
				vectorn& w=((LinearMeshDeformer*)mTargetMeshInControlVolume)->getWeights(i);
				for(int j=0; j<ntv; j++)
				{
					double wj=w(j);
					W(i*3, j*3)=wj;
					W(i*3+1, j*3+1)=wj;
					W(i*3+2, j*3+2)=wj;
				}
			}

			matrixn &invW=targetDeformToControlDeform;
			m::pseudoInverse(invW, W);
			std::cout<< invW.rows()<<","<<invW.cols()<<std::endl;
			*/
		}
		else
		{

			mControlPointsInFeatureMesh = createKNNIDWDeformer(10, 2.0, 0.01);
			getVertices(*tet_to_obj, m_vertices);
			m_vertices.resize(getNumSurfaceVertex());
			m_vertices_orig=m_vertices;

			/*
			//ASSERT(mTargetMesh_originalShape->numVertex()==mTargetMesh->numVertex());
			vector3N temp;
			getVertices(*mTargetMesh_originalShape, temp);
			temp.resize(mTargetMesh_originalShape, mTargetMesh->numVertex());

			*mTargetMesh_originalShape=*mTargetMesh;
			setVertices(*mTargetMesh_originalShape,temp);
			*/

			mControlPointsInFeatureMesh -> calculateCorrespondence(*mTargetMesh_originalShape, m_vertices);
		}

        mpTargetObjViewer = new OBJloader::MeshToEntity(*mTargetMesh, RE::generateUniqueName(), o);
        mpTargetObjEntity = mpTargetObjViewer->createEntity("targetmesh", "lightgrey_transparent");
		mpTargetObjEntity  ->setCastShadows(false);
		auto* node=RE::ogreRootSceneNode()->createChildSceneNode("TargetMeshNode");
		RE::moveEntity(node, vector3(200,0,0));
		node->attachObject(mpTargetObjEntity);

		_compareHelper.SetReferenceMesh(mTargetMesh);
	}
    else if (w.mId == "Load a mesh") {
        TString mesh;
        mesh = FlChooseFile("Choose mesh", "../mesh/", "*.tet");
		if (mesh == nullptr) {
			return;
		}
//		mesh=TString("../mesh/iamgroot101_surface.1by10.rearranged.tet");
		_meshName = mesh.subString(0, mesh.length()-4);
		std::cout<<mesh.ptr()<<std::endl;
        if (mesh.length())
			loadTetMesh(mesh.ptr());
		tetToObj();
    }
	else if (w.mId == "save_current") {
		mGrootHighMesh.saveObj(_meshName + ".mesh", false, false);
		printf("saved to deformed.mesh\n");
	}
	else if (w.mId == "Option") {
        if (userData == Hash("Show mesh")) {
            Fl_Choice *p = w.menu();
            if (p->menu()[p->value()].value()) {
            } else {
                    //mMeshViewer->getLastCreatedEntity()->setVisible(false);
            }
        }
    } else if (w.mId == "activation") {
        float v = w.slider()->value();
        for (int i = 0; i < mMesh.getNumTetra(); i++) {
            if (mSelectedTetra.size() && mSelectedTetra[i])
                mSoftWorld.setActivationLevel(i, v);
        }
    } else if (w.mId == "min_select") {
        min_selected_mesh = w.slider()->value();
    } else if (w.mId == "max_select") {
        max_selected_mesh = w.slider()->value();
    } else if (w.mId == "start_optimize") {

		/*
		vectorn targetDeform(mTargetMesh->numVertex()*3);
		for(int i=0; i<mTargetMesh->numVertex(); i++)
			targetDeform.setVec3(i*3, mTargetMesh->getVertex(i)-mTargetMesh_originalShape->getVertex(i));
		matrixn const&invW=targetDeformToControlDeform;
		vectorn controlDeform(invW.rows());

		Msg::verify(targetDeform.size()==invW.cols(), "a");
		Msg::verify(controlDeform.size()==invW.rows(), "b");
		controlDeform.column().mult(invW,targetDeform.column());
		*/
		Msg::verify(mTargetMesh, "load deformed first!");
		mControlPointsInFeatureMesh->transfer(*mTargetMesh, 1.0);

		/*
		vector3N temp;
		getVertices(*mTargetMesh, temp);
		mObjectList.registerObject("vertices2", "QuadListZ", "blueCircle", matView(temp),0);

		mObjectList.registerObject("vertices", "QuadListZ", "redCircle", matView(m_vertices),0);
		*/
		int surface_vtx_num = getNumSurfaceVertex();
		for (int i = 0; i < surface_vtx_num; i++) {
			double gain=1;
			//mSoftWorld.setAttachmentPoint(i, mMesh.nodePos(i)+gain*controlDeform.toVector3(i*3));
			mSoftWorld.setAttachmentPoint(i, m_vertices_orig(i)+gain*(m_vertices(i)-m_vertices_orig(i)));
		}

		printf("reset attachments\n");
		// todo :  compare버튼을 누르면
		// 2. mGrootHighMesh.getVertex()와 mTargetMesh->getVertex의 오차 비교
		/*
        while (!p_cma->testForTermination().empty()) {
            p_cma->samplePopulation();
            for (int i = 0; i < p_cma->numPopulation(); i++) {

            }
            p_cma->update();
        }
		*/
    }
}

void TetraDeformTestWin::redraw() {
    vector3N cst;
    mSoftWorld.getAttachmentCst(cst);
    vector3N elts;
    mSoftWorld.getCorotateFEMCst(elts);
	bool bNew=false;
	if(false){
		if (!line) 	{
			bNew=true;
			line = new BillboardLineList(RE::generateUniqueName(), elts.rows(), 0.4);
			ano_line = new BillboardLineList(RE::generateUniqueName(), elts.rows(), 0.4);
			line->setMaterialName("red");
		}
		//ano_line->setMaterialName("blue");
		for (int i = 0; i < elts.rows() - 1; i += 2) {
			if (i > min_selected_mesh * 10000 && i < max_selected_mesh * 10000)
				ano_line->line(i, elts.row(i), elts.row(i + 1));
			else
				line->line(i, elts.row(i), elts.row(i + 1));
		}
		if(bNew)
			//for test tetra obj
			RE::ogreRootSceneNode()->attachObject(line);
		//RE::ogreRootSceneNode()->attachObject(ano_line);
	}

	if ( mpTetraObjViewer && tet_to_obj)
	{
		int surface_vtx_num = getNumSurfaceVertex(); // -.-
		for (int i = 0; i < surface_vtx_num; i++)
		{
			tet_to_obj->getVertex(i)=mSoftWorld.getVertex(i);
			ASSERT(tet_to_obj->getVertex(i).x==tet_to_obj->getVertex(i).x);
		}

		mpTetraObjViewer->updatePositions();

		mMeshDeformer ->transfer(*tet_to_obj,1.0);
		mGrootHighMesh.calculateVertexNormal();
		mpSurfaceObjViewer->updatePositionsAndNormals();
	}

	double max, min, avg;
	if (_compareHelper.RMSE(mTargetMesh, &mGrootHighMesh, &max, &min, &avg) == 0) // Success
	{
		double pcm;
		_compareHelper.PointCloudMetricError(mTargetMesh, &mGrootHighMesh, &pcm);
		ChangeError(pcm, avg);
	}
}

FlLayout *createTetraDeformTestWin(int x, int y, int w, int h, MotionPanel &mp, FltkRenderer &renderer) {
    return new TetraDeformTestWin(x, y, w, h, mp, renderer);
}

void TetraDeformTestWin::clearSelection() {

    if (mSelectedVertices.size())
        mSelectedVertices.clearAll();
    if (mSelectedEdges.size())
        mSelectedEdges.clearAll();
}

void TetraDeformTestWin::drawSelection() {
    mObjectList.clear();
    for (int i = 0; i < mMesh.getNumNode(); i++) {
        if (mSelectedVertices[i]) {
            //RE::moveEntity(mObjectList.registerEntity(RE::generateUniqueName(), "sphere1010.mesh"), quater(1, 0, 0, 0),
            RE::moveEntity(mObjectList.registerEntityScheduled("sphere1010.mesh", 1.0), quater(1, 0, 0, 0),
                           mMesh.nodePos(i));
            double *getpos = new double[3];
            mMesh.nodePos(i).getValue(getpos);
            //printf("pos is %f %f %f \n",getpos[0],getpos[1],getpos[2]);
            //fflush(stdout);
        }
    }
}

inline intersectionTest::LineSegment Ray2LineSegment(Ray const &r) {
    return intersectionTest::LineSegment(r.origin(), r.getPoint(FLT_MAX));
}

void TetraDeformTestWin::setLinearConstraint() {
    for (int i = 0; i < mMesh.getNumTetra(); i++) {
        int musclestiffness = 4000000;
        vector3 fiber_direction(0, 1, 0);
        std::string muscle_name = "m" + std::to_string(i);
        mSoftWorld.addLinearMuscleConstraint(muscle_name.c_str(), musclestiffness, i, mMesh, fiber_direction);
    }
    int surface_vtx_num = getNumSurfaceVertex();
    for (int i = 0; i < surface_vtx_num; i++) {
        std::string muscle_name = "a" + std::to_string(i);
        int musclestiffness = 4000000;
		mSoftWorld.addAttachmentConstraint(muscle_name.c_str(), musclestiffness, i, mMesh.nodePos(i));
	}

    mSoftWorld.initSimulation();
}

#include <FL/Fl.H>
#include <Vision/realsense3.h>

int TetraDeformTestWin::handleRendererEvent(int ev) {
    static int pushed_x;
    static int pushed_y;
    static SelectionRectangle *mRect = NULL;

    switch (ev) {
        case FL_PUSH:
            pushed_x = Fl::event_x();
            pushed_y = Fl::event_y();
            if (Fl::event_state() & FL_SHIFT) {
                // marquee selection
                //mRenderer.viewLock();
                Ogre::SceneNode *pNode = RE::createSceneNode("MarqueeNode");
                mRect = new SelectionRectangle("Selection SelectionRectangle");
                pNode->attachObject(mRect);

                mRect->setCorners(TRect(pushed_x, pushed_y, pushed_x, pushed_y));
                RE::output("marqueeEvent", "push");
                return 1;
            }

            break;
//	case FL_MOVE:
        case FL_DRAG: {

        }
            break;
        case FL_RELEASE: {
            if (mRect) {
                // marquee selection
                mRect->setCorners(TRect(pushed_x, pushed_y, Fl::event_x(), Fl::event_y()));
                std::vector<Plane> vol;
                mRect->constructSelectionVolumn(mRenderer, vol);
                clearSelection();
                mSelectedVertices.resize(mMesh.getNumNode());
                mSelectedVertices.clearAll();
                mSelectedTetra.resize(mMesh.getNumTetra());
                mSelectedTetra.clearAll();

                for (int i = 0; i < mMesh.getNumNode(); i++) {
                    Sphere s(mMesh.nodePos(i), 0.001);
                    if (s.isInside(vol))
                        mSelectedVertices.setAt(i);
                }
                for (int i = 0; i < mMesh.getNumTetra(); i++) {
                    Sphere d(mMesh.calcTetraCenter(i), 0.001);
                    if (d.isInside(vol)) {
                        mSelectedTetra.setAt(i);
                    }
                }

                drawSelection();
                //setLinearConstraint();
                RE::removeEntity("MarqueeNode");
                mRect = NULL;
                return 1;
            } else {
                static vector3 pushed_cursorPos;
                int x = Fl::event_x();
                int y = Fl::event_y();

                if (x != pushed_x || y != pushed_y)
                    return 0;
                int button = Fl::event_button();
                bool bButton1 = Fl::event_state() & FL_BUTTON1;
                bool bButton2 = Fl::event_state() & FL_BUTTON2;
                bool bButton3 = Fl::event_state() & FL_BUTTON3;

                switch (ev) { // TODO: ev is always FL_RELEASE here...
                    case FL_PUSH:
                        RE::output("mouse event", "push %d %d %d", x, y, button & FL_BUTTON1);
                        break;
                    case FL_MOVE:
                        RE::output("mouse event", "move %d %d %d", x, y, button & FL_BUTTON1);
                        break;
                    case FL_DRAG:
                        RE::output("mouse event", "drag %d %d %d", x, y, button & FL_BUTTON1);
                        break;
                    case FL_RELEASE:
                        RE::output("mouse event", "release %d %d %d", x, y, button & FL_BUTTON1);
                        break;
                }
                RE::output("button", "%d", button);
                if ((ev == FL_PUSH || ev == FL_RELEASE) && button != FL_LEFT_MOUSE) break;
                Ray ray;
                mRenderer.screenToWorldRay(x, y, ray);

                //TString selectionMode = findMenu("selection mode")->text();
                TString selectionMode = "none";

                if (selectionMode == "edge") {
                    intersectionTest::LineSegment sx = Ray2LineSegment(ray);

                    m_real minDist = FLT_MAX;
                    int argMin = -1;
                    for (int i = 0; i < mLinks.rows(); i++) {
                        intersectionTest::LineSegment l(mMesh.nodePos(mLinks[i][0]), mMesh.nodePos(mLinks[i][1]));
                        m_real dist = l.minDist(sx);
                        if (dist < minDist) {
                            minDist = dist;
                            argMin = i;
                        }
                    }

                    clearSelection();
					/*
                    BillboardLineList *line = new BillboardLineList("edge", 1, 1);
                    line->setMaterialName("redToOrange");
                    line->line(0, mMesh.nodePos(mLinks[argMin][0]), mMesh.nodePos(mLinks[argMin][1]));
                    mObjectList.registerObject("link", line);
                    RE::moveEntity(mObjectList.registerEntity("sellink0", "sphere1010.mesh"), quater(1, 0, 0, 0),
                                   mMesh.nodePos(mLinks[argMin][0]));
                    RE::moveEntity(mObjectList.registerEntity("sellink1", "sphere1010.mesh"), quater(1, 0, 0, 0),
                                   mMesh.nodePos(mLinks[argMin][1]));
								   */
                }
				else if(selectionMode == "vertex")
				{
                    clearSelection();
                    for (int i = 0; i < mMesh.getNumNode(); i++) {
                        Sphere s(mMesh.nodePos(i), 3);
                        if (ray.intersects(s).first) {

                            RE::moveEntity(mObjectList.registerEntity(RE::generateUniqueName(), "sphere1010.mesh"),
                                           quater(1, 0, 0, 0), mMesh.nodePos(i));
                        }
                    }
                }
            }
        }
            break;
    }
    return 0;
}

OBJloader::Mesh* TetraDeformTestWin::UpdateMesh(OBJloader::Mesh* source) {
    auto mv = VisionProject::GetInstance()->GetMotionVector();

    for (int i = 0; i < mv.size(); ++i) {
		source->getVertex(mv[i].index) = vector3(mv[i].x, mv[i].y, mv[i].z);
    }

    return source;
}

void TetraDeformTestWin::ChangeError(double pcm, double rms) {
	char pcmString[32];
	char rmsString[32];
	sprintf(pcmString, "PCM Error : %.2lf mm", pcm);
	sprintf(rmsString, "Avg. Distance : %.2lf mm", rms);
	_maxWidget->copy_label(pcmString);
	_avgWidget->copy_label(rmsString);
	updateLayout();
}

void TetraDeformTestWin::tetToObj()
{
	//mMeshDeformer = createHarmonicMeshDeformer();
	mMeshDeformer = createMeanValueMeshDeformer();
	//mMeshDeformer = createKNNIDWMeshDeformer(30, 2.0);
	tet_to_obj = tetraToObj(mMesh, _meshName + ".face", getNumSurfaceVertex());  //"../mesh/iamgroot101_surface.1.rearranged.face"
	//mGrootHighMesh.loadObj("../mesh/Deformed_groot/iamgroot9k.obj"); // smaller mesh.
	mGrootHighMesh.loadObj(_meshName + ".obj"); // larger mesh.
	if(1)
	{
		// slightly scale down
		auto center=mGrootHighMesh.calcMeshCenter(); //?? 메시 센터가 이상하게 계산되는듯.
		matrix4 m;
		m.identity();
		m.leftMultTranslation(-center);
		m.leftMultScaling(0.99,0.99, 0.99);
		m.leftMultTranslation(center);
		mGrootHighMesh.transform(m);
	}

	// Load or save correspondence
	//auto dfn = "../mesh/iamgroot_high_hmd.def";
	auto dfn = "../mesh/iamgroot_high_mvn4.def";
	//if (!mMeshDeformer->loadCorrespondence(dfn, *tet_to_obj, mGrootHighMesh)) {
	if (1){ // mvn is fast enough.
		mMeshDeformer->calculateCorrespondence(*tet_to_obj, mGrootHighMesh);
		mMeshDeformer->saveCorrespondence(dfn);
	}

	//for test tetra obj
	//RE::ogreRootSceneNode()->detachObject(line);
	//RE::ogreRootSceneNode()->detachObject(ano_line);
	OBJloader::MeshToEntity::Option o;
	o.useTexCoord = false;
	o.useNormal = false;
	o.buildEdgeList = false;

	mpSurfaceObjViewer = new OBJloader::MeshToEntity(mGrootHighMesh, RE::generateUniqueName(), o);
	//mpSurfaceObjEntity = mpSurfaceObjViewer->createEntity("surfaceobj", "lambert6");
	mpSurfaceObjEntity = mpSurfaceObjViewer->createEntity("surfaceobj", "lightgrey_transparent");
	mpSurfaceObjEntity  ->setCastShadows(false);
	RE::ogreRootSceneNode()->attachObject(mpSurfaceObjEntity);

	if(1){
		mpTetraObjViewer = new OBJloader::MeshToEntity(*tet_to_obj, RE::generateUniqueName(), o);
		//mpTetraObjEntity = mpTetraObjViewer->createEntity("tettoobj", "lambert6");
		mpTetraObjEntity = mpTetraObjViewer->createEntity("tettoobj", "red_transparent"); // defined in color.material
		mpTetraObjEntity  ->setCastShadows(false);
		RE::ogreRootSceneNode()->attachObject(mpTetraObjEntity);
	}
	else
		mpTetraObjViewer=NULL;
}


//
// SPDX-License-Identifier: MIT
// Copyright (c) 2016-2020 Michael Purcaro, Henry Pratt, Jill Moore, Zhiping Weng
//

#pragma once

#define ARMA_64BIT_WORD
#include <armadillo>

namespace SCREEN {

  namespace bfs = boost::filesystem;
  namespace a = arma;

  class ZScore {

  private:
    void _read(const bfs::path&);
    void computeZScores(const a::vec&);
    void computeZScores(const a::vec&, const a::vec&);
    void _processbw(const bfs::path&, const bool);

  public:
    ZScore() {}
    ZScore(const bfs::path&, const bool);
    ZScore(const bfs::path&, const bfs::path&, const bool);
    ZScore(const std::vector<std::vector<std::string>>&,
	   const bfs::path&, const bool);

    void write(const std::string&, const bfs::path&);
    void write(const std::string&, const std::string&);
    void read(const bfs::path&);
    void qfilter(const double);

    std::vector<std::vector<std::string>> lines_;
    a::vec zscores_;
  };

  std::vector<float> _computeMaxZ(const std::vector<std::string>&, const std::vector<std::vector<std::string>>&,
				  std::vector<ZScore>&);

} // SCREEN


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

#include "kvservice/experiment.h"
#include "common/sys_logger.h"
#include "experiment.h"
#include <boost/format.hpp>

namespace scc {

    double IntToDouble(int integer) {
        if (integer == 100 || integer == 75 || integer == 50 || integer == 25)
            return ((double) integer) / 100.0;
        if (integer == 12) // 1/8
            return 12.5 / 100.0;
        if (integer == 6) // 1/16
            return 6.25 / 100.0;
        if (integer == 3) // 1/32
            return 3.125 / 100.0;
        if (integer == 1)
            return 1.5625 / 100.0; // 1/64
        fprintf(stderr, "Integer not valid %d\n", integer);
        assert(false);
    }

    std::string GetGlobalRandomKey(int totalNumItems) {
        return std::to_string(abs(random()) % totalNumItems);
    }

    std::string GetKey(Generator<uint64_t> *generator) {
        int key = generator->Next();
        return std::to_string(key);
    }

    std::string GetRandomKeyAtPartition(int partitionId, int numPartitions, int totalNumItems) {
        int key;

        do {
            key = abs(random()) % (totalNumItems * numPartitions);
        } while (key % numPartitions != partitionId);

        return std::to_string(key);
    }

    std::string GetKeyAtPartition(Generator<uint64_t> *generator, int partitionId, int numPartitions) {
        int rand, key;

        rand = generator->Next();
        key = rand * numPartitions + partitionId;

        return std::to_string(key);
    }

    std::string GetKeyAtPartitionAtReplica(Generator<uint64_t> *generator, int partitionId, int numPartitions,
                                           int replicaId, int numLocalKeysPerReplica) {
        int rand, key;
        rand = generator->Next();//rand-th element of the replicaId-th slice of a partition
        rand += (replicaId *numLocalKeysPerReplica); //Sum an offset to obtain the index of the desired key in the desired partition
        key = rand * numPartitions + partitionId; //Now compute the absolute value of the key, that takes into account the hashing function

        return std::to_string(key);
    }

    void experiment1(ThreadArg *arg) {
        try {

            srand(Utils::GetThreadId() * time(0));

            std::string setValue(arg->NumValueBytes, 'y');
            std::string getValue;

            // connect to kv server
            for (int i = 0; i < arg->NumPartitions; ++i) {
                arg->Clients.push_back(new PublicClientLB(arg->ServerNames[i], arg->ServerPorts[i]));
            }

            // notify readiness
            arg->OpReadyEvent.Set();

            // wait to start
            arg->OpStartEvent.WaitAndReset();

            vector<int> partition_ids;
            for (int i = 0; i < arg->NumPartitions; ++i) {
                partition_ids.push_back(i);
            }

            int offsetR = 0;
            int offsetW = 0;

            while (!(*arg->stopOperation)) {

                random_shuffle(partition_ids.begin(), partition_ids.end());

                //read from every partition once
                for (int p_idx = 0; p_idx < arg->NumPartitions; p_idx++) {

                    //randomly select a key (according to a predefined key distribution)
                    std::string opKey = GetKeyAtPartition(arg->generator, partition_ids.at(p_idx), arg->NumPartitions);

                    //record operation start time
                    PhysicalTimeSpec startTimeOfGet = Utils::GetCurrentClockTime();

                    //GET key
                    bool ret = arg->Clients[partition_ids.at(p_idx)]->Get(opKey, getValue);

                    if (!ret) {
                        fprintf(stdout, "Get %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    //record operation end time
                    auto endTimeGet = std::chrono::high_resolution_clock::now();
                    PhysicalTimeSpec endTimeOfGet = Utils::GetCurrentClockTime();
                    arg->NumOpsPerThread += 1;

                    //record operation latency
                    double duration = (endTimeOfGet - startTimeOfGet).toMilliSeconds();

#ifdef RESERVOIR_SAMPLING

                    if (arg->OpReadLatencies.size() < arg->ReservoirSamplingLimit) {

                        arg->OpReadLatencies.push_back(duration);
                    }
                    else {
                        offsetR++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetR);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpReadLatencies[position] = duration;
                        }
                    }
#else
                    arg->OpReadLatencies.push_back(duration);
#endif
                }

                //randomly select a key (according to a predefined key distribution)
                std::string opKey;
                opKey = GetKeyAtPartition(arg->generator, arg->LocalPartitionId, arg->NumPartitions);

                //record operation start time
                PhysicalTimeSpec startTimeOfSet = Utils::GetCurrentClockTime();

                //write to the local partition: SET key
                bool ret = arg->Clients[arg->LocalPartitionId]->Set(opKey, setValue);

                if (!ret) {
                    fprintf(stdout, "Set %s failed\n", opKey.c_str());
                    fflush(stdout);
                }

                //record operation end time
                PhysicalTimeSpec endTimeOfSet = Utils::GetCurrentClockTime();

                arg->NumOpsPerThread += 1;

                //record operation latency
                double duration = (endTimeOfSet - startTimeOfSet).toMilliSeconds();

#ifdef RESERVOIR_SAMPLING

                if (arg->OpWriteLatencies.size() < arg->ReservoirSamplingLimit) {
                    arg->OpWriteLatencies.push_back(duration);
                }
                else {
                    offsetW++;
                    int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetW);
                    if (position < arg->ReservoirSamplingLimit) {
                        arg->OpWriteLatencies[position] = duration;
                    }
                }
#else
                arg->OpWriteLatencies.push_back(duration);
#endif
            }

            // notify stopping
            arg->OpEndEvent.Set();
        } catch (
                SocketException &e
        ) {
            fprintf(stdout, "SocketException: %s\n", e.what());
            exit(1);
        } catch (...) {
            fprintf(stdout, "Exception while executing Experiment 1. ");
            exit(1);
        }
    }

    void experiment2(ThreadArg *arg) {
        //Round Robin R/W ration
        try {
            srand(Utils::GetThreadId() * time(0));

            // connect to kv server
            std::string setValue(arg->NumValueBytes, 'y');
            std::string getValue;

            std::this_thread::sleep_for(std::chrono::milliseconds(rand() % 50 + 1));
            arg->Client = new PublicClientLB(arg->ServerNames[arg->servingPartition],
                                             arg->ServerPorts[arg->servingPartition]);

            arg->Client->setTotalNumKeyInKVStore(arg->TotalNumItems);

            // notify readiness
            arg->OpReadyEvent.Set();

            // wait to start
            arg->OpStartEvent.WaitAndReset();

            int offsetR = 0;
            int offsetW = 0;
            int numCycles = 0;

            while (!(*arg->stopOperation)) {

                int pId = rand() % arg->NumPartitions;

                // READ OPERATIONS
                for (int rCount = 0; rCount < arg->ReadRatio; rCount++) {
                    //randomly select a key (according to a predefined key distribution)
                    std::this_thread::sleep_for(std::chrono::milliseconds(arg->thinkTime));
                    std::string opKey = GetKeyAtPartition(arg->generator, pId, arg->NumPartitions);

                    //record operation start time
                    PhysicalTimeSpec startTimeOfGet = Utils::GetCurrentClockTime();

                    //GET key
                    bool ret = arg->Client->Get(opKey, getValue);

                    if (!ret) {
                        fprintf(stdout, "Get %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    PhysicalTimeSpec endTimeOfGet = Utils::GetCurrentClockTime();
                    arg->NumOpsPerThread += 1;

                    //record operation latency
                    double duration = (endTimeOfGet - startTimeOfGet).toMilliSeconds();

                    arg->TotalNumReadOps += 1;
                    arg->ReadLatenciesSum += duration;

                    if (arg->MaxReadLatency < duration) {
                        arg->MaxReadLatency = duration;
                    }

                    if (arg->MinReadLatency > duration) {
                        arg->MinReadLatency = duration;
                    }

#ifdef RESERVOIR_SAMPLING

                    if (arg->OpReadLatencies.size() < arg->ReservoirSamplingLimit) {

                        arg->OpReadLatencies.push_back(duration);
                    }
                    else {
                        offsetR++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetR);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpReadLatencies[position] = duration;
                        }
                    }
#else
                    arg->OpReadLatencies.push_back(duration);
#endif
                    pId = (pId + 1) % arg->NumPartitions;

                }

                // WRITE OPERATIONS
                pId = rand() % arg->NumPartitions;
                for (int wCount = 0; wCount < arg->WriteRatio; wCount++) {
                    //randomly select a key (according to a predefined key distribution)
                    std::string opKey;
#ifdef THINK_TIME_BEFORE_WRITE
                    std::this_thread::sleep_for(std::chrono::milliseconds(arg->thinkTime));
#endif
                    opKey = GetKeyAtPartition(arg->generator, pId, arg->NumPartitions);
                    //record operation start time
                    PhysicalTimeSpec startTimeOfSet = Utils::GetCurrentClockTime();

                    //write to the local partition: SET key
                    bool ret = arg->Client->Set(opKey, setValue);

                    if (!ret) {
                        fprintf(stdout, "Set %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    //record operation end time
                    PhysicalTimeSpec endTimeOfSet = Utils::GetCurrentClockTime();
                    arg->NumOpsPerThread += 1;

#ifdef ENABLE_CLIENT_RESET
                    if (arg->enableClientReset) {
                        if (arg->clientResetNumber == numCycles) {
                            arg->Client->resetSession();
                            numCycles = 0;
                        }
                    }
#endif

                    //record operation latency
                    double duration = (endTimeOfSet - startTimeOfSet).toMilliSeconds();

                    arg->TotalNumWriteOps += 1;
                    arg->WriteLatenciesSum += duration;

                    if (arg->MaxWriteLatency < duration) {
                        arg->MaxWriteLatency = duration;
                    }

                    if (arg->MinWriteLatency > duration) {
                        arg->MinWriteLatency = duration;
                    }

#ifdef RESERVOIR_SAMPLING

                    if (arg->OpWriteLatencies.size() < arg->ReservoirSamplingLimit) {
                        arg->OpWriteLatencies.push_back(duration);
                    }
                    else {
                        offsetW++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetW);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpWriteLatencies[position] = duration;
                        }
                    }
#else
                    arg->OpWriteLatencies.push_back(duration);

#endif
                    pId = (pId + 1) % arg->NumPartitions;
                }
            }

            arg->numHotBlockKeys = 0;

            // notify stopping
            arg->OpEndEvent.Set();
        } catch (SocketException &e) {
            fprintf(stdout, "SocketException: %s\n", e.what());
            exit(1);
        } catch (...) {
            fprintf(stdout, "Exception while executing Experiment 1. ");
            exit(1);
        }
    }

    static int _partitionsToReadFrom = -1;

    void experimentTx(ThreadArg *arg) {

        try {
            srand(Utils::GetThreadId() * time(0));
            std::this_thread::sleep_for(std::chrono::milliseconds(10 * arg->ThreadId));

            // connect to kv server
            PublicClientLB client(arg->ServerNames[arg->servingPartition], arg->ServerPorts[arg->servingPartition]);

            client.setTotalNumKeyInKVStore(arg->TotalNumItems);

            // notify readiness
            arg->OpReadyEvent.Set();

            // wait to start
            arg->OpStartEvent.WaitAndReset();

            int p_idx = 0;
            vector<int> partition_ids;
            for (int i = 0; i < arg->NumPartitions; ++i) {
                partition_ids.push_back(i);
            }

            srand(time(0));
            random_shuffle(partition_ids.begin(), partition_ids.end());

            std::vector<std::string> txGetKeys;
            std::vector<std::string> txGetValues;

            const int partitionsToReadFrom = (int) (IntToDouble(arg->ReadRatio) *
                                                    ((double) partition_ids.size())) > 0 ?
                                             (int) ((IntToDouble(arg->ReadRatio) * ((double) partition_ids.size())))
                                                                                : 1;
            const int numXactPerIteration = arg->NumTxReadItems;

            fprintf(stdout, "PartitionsToReadFrom %d\n", partitionsToReadFrom);
            fprintf(stdout, "NumXactPerXact %d\n", numXactPerIteration);

            if (arg->ThreadId == 0)
                _partitionsToReadFrom = partitionsToReadFrom;

            int pId;
            bool ret;

            while (!(*arg->stopOperation)) {
                for (int j = 0; j < numXactPerIteration; j++) {
                    // ====== do some transactions ========

                    txGetKeys.clear();
                    txGetValues.clear();

                    pId = rand() % arg->NumPartitions;

                    // READ OPERATIONS

                    for (int rCount = 0; rCount < partitionsToReadFrom; rCount++) {
                        std::string opKey = GetKeyAtPartition(arg->generator, pId, arg->NumPartitions);
                        txGetKeys.push_back(opKey);
                        pId = (pId + 1) % arg->NumPartitions;
                    }

                    //record operation start time
                    PhysicalTimeSpec startTime = Utils::GetCurrentClockTime();
                    std::this_thread::sleep_for(std::chrono::milliseconds(arg->thinkTime));
                    ret = client.TxGet(txGetKeys, txGetValues);
                    if (!ret) {
                        fprintf(stdout, "TxGet failed\n");
                    }

                    PhysicalTimeSpec endTime = Utils::GetCurrentClockTime();
                    double duration = (endTime - startTime).toMilliSeconds();

                    arg->NumOpsPerThread += 1;
                    arg->TotalNumReadOps += 1;

                    arg->ReadLatenciesSum += duration;

                    if (arg->MaxReadLatency < duration) {
                        arg->MaxReadLatency = duration;
                    }

                    if (arg->MinReadLatency > duration) {
                        arg->MinReadLatency = duration;
                    }

#ifdef RESERVOIR_SAMPLING

                    if (arg->OpReadLatencies.size() < arg->ReservoirSamplingLimit) {

                            arg->OpReadLatencies.push_back(duration);
                        }
                        else {
                            offsetR++;
                            int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetR);
                            if (position < arg->ReservoirSamplingLimit) {
                                arg->OpReadLatencies[position] = duration;
                            }
                        }
#else
                    arg->OpReadLatencies.push_back(duration);
#endif
                }

                // ====== do a write ========

                std::string setValue(arg->NumValueBytes, 'y');
                std::string opKey;
                std::string getValue;

                pId = rand() % arg->NumPartitions;
                for (int wCount = 0; wCount < arg->WriteRatio; wCount++) {

                    opKey = GetKeyAtPartition(arg->generator, pId, arg->NumPartitions);

                    //record operation start time
                    PhysicalTimeSpec startTimeOfSet = Utils::GetCurrentClockTime();

                    //write to the local partition: SET key
                    std::this_thread::sleep_for(std::chrono::milliseconds(arg->thinkTime));
                    ret = client.Set(opKey, setValue);

                    if (!ret) {
                        fprintf(stdout, "Set %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    //record operation end time
                    PhysicalTimeSpec endTimeOfSet = Utils::GetCurrentClockTime();

                    //record operation latency
                    double durationWrite = (endTimeOfSet - startTimeOfSet).toMilliSeconds();

                    arg->TotalNumWriteOps += 1;
                    arg->NumOpsPerThread += 1;
                    arg->WriteLatenciesSum += durationWrite;

                    if (arg->MaxWriteLatency < durationWrite) {
                        arg->MaxWriteLatency = durationWrite;
                    }

                    if (arg->MinWriteLatency > durationWrite) {
                        arg->MinWriteLatency = durationWrite;
                    }
#ifdef RESERVOIR_SAMPLING
                    if (arg->OpWriteLatencies.size() < arg->ReservoirSamplingLimit) {
                        arg->OpWriteLatencies.push_back(durationWrite);
                    }
                    else {
                        offsetW++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetW);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpWriteLatencies[position] = durationWrite;
                        }
                    }
#else
                    arg->OpWriteLatencies.push_back(durationWrite);

#endif
                    pId = (pId + 1) % arg->NumPartitions;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(arg->thinkTime));

            } // end while

            arg->numHotBlockKeys = 0;

            // notify stopping
            arg->OpEndEvent.Set();

        } catch (SocketException &e) {
            fprintf(stdout, "SocketException: %s\n", e.what());
            exit(1);
        }
    }

    void experiment4(ThreadArg *arg) {

        //Round Robin R/W ration
        try {

            srand(Utils::GetThreadId() * time(0));

            // connect to kv server
            std::string setValue(arg->NumValueBytes, 'y');
            std::string getValue;

            arg->Client = new PublicClientLB(arg->ServerNames[arg->servingPartition],
                                             arg->ServerPorts[arg->servingPartition]);

            arg->Client->setTotalNumKeyInKVStore(arg->TotalNumItems);

            // notify readiness
            arg->OpReadyEvent.Set();

            // wait to start
            arg->OpStartEvent.WaitAndReset();

            int offsetR = 0;
            int offsetW = 0;
            int numCycles = 0;

            /////////// GENERATE LOCALITY AWARE DATA STRUCTURES /////////////
            std::vector<int> dc_vector;
            int local = arg->locality; //Percentage or gets issued towards local data
            fflush(stdout);
            int local_replica_id = arg->Client->getLocalReplicaId();

            int otherReplicas = arg->Client->getNumReplicas() - 1;
            assert((100 - local) % otherReplicas == 0); //We want nice numbers :)

            int otherReplicasBuckets =
                    (100 - local) / otherReplicas; //Percentage of get towards a specific remote partition

            //Set the local entries
            for (int i = 0; i < local; i++) {
                dc_vector.push_back(local_replica_id);
            }
            int remote_index = local;
            //Set the remote entries
            for (int i = 0; i < arg->Client->getNumReplicas(); i++) {
                if (i != local_replica_id) {
                    for (int j = 0; j < otherReplicasBuckets; j++) {
                        dc_vector.push_back(i);
                        remote_index++;
                    }
                }
            }

            //Make sure the vector is differently shuffled across threads
            for (int i = 0; i < arg->ThreadId * (local_replica_id + 1); i++) {
                std::random_shuffle(dc_vector.begin(), dc_vector.end());
            }

            //Drawing uniformly from dc_generator you have a local% probability of hitting the local DC
            //The remainder is equally split among remote DCs
            UniformGenerator dc_generator(0, 99);

            int next_dc;
            //Number of keys belonging to a replica per partition. Needed to compute the offset to obtain the per-partition
            //key id starting from the per-DC key id within that partition.
            int local_keys_per_replca = (arg->TotalNumItems / arg->NumPartitions) / arg->NumReplicas;

            /////////// START THE TEST /////////////

            while (!(*arg->stopOperation)) {

                int pId = rand() % arg->NumPartitions;

                // READ OPERATIONS
                for (int rCount = 0; rCount < arg->ReadRatio; rCount++) {
                    //randomly select a key (according to a predefined key distribution)
                    next_dc = dc_vector[dc_generator.Next()];
                    std::string opKey = GetKeyAtPartitionAtReplica(arg->generator, pId, arg->NumPartitions, next_dc,
                                                                   local_keys_per_replca);

                    //record operation start time
                    PhysicalTimeSpec startTimeOfGet = Utils::GetCurrentClockTime();

                    //GET key
                    fflush(stdout);
                    bool ret = arg->Client->Get(opKey, getValue);

                    if (!ret) {
                        fprintf(stdout, "Get %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    PhysicalTimeSpec endTimeOfGet = Utils::GetCurrentClockTime();
                    arg->NumOpsPerThread += 1;

                    //record operation latency
                    double duration = (endTimeOfGet - startTimeOfGet).toMilliSeconds();

                    arg->TotalNumReadOps += 1;
                    arg->ReadLatenciesSum += duration;

                    if (arg->MaxReadLatency < duration) {
                        arg->MaxReadLatency = duration;
                    }

                    if (arg->MinReadLatency > duration) {
                        arg->MinReadLatency = duration;
                    }

#ifdef RESERVOIR_SAMPLING
                    if (arg->OpReadLatencies.size() < arg->ReservoirSamplingLimit) {

                        arg->OpReadLatencies.push_back(duration);
                    }
                    else {
                        offsetR++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetR);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpReadLatencies[position] = duration;
                        }
                    }
#else
                    arg->OpReadLatencies.push_back(duration);
#endif
                    pId = (pId + 1) % arg->NumPartitions;

                }

                // WRITE OPERATIONS
                pId = rand() % arg->NumPartitions;
                for (int wCount = 0; wCount < arg->WriteRatio; wCount++) {
                    //randomly select a key (according to a predefined key distribution)

                    //Writes are always local
                    std::string opKey = GetKeyAtPartitionAtReplica(arg->generator, pId,
                                                                   arg->NumPartitions,
                                                                   local_replica_id, local_keys_per_replca);
                    //record operation start time
                    PhysicalTimeSpec startTimeOfSet = Utils::GetCurrentClockTime();

                    //write to the local partition: SET key
                    bool ret = arg->Client->Set(opKey, setValue);

                    if (!ret) {
                        fprintf(stdout, "Set %s failed\n", opKey.c_str());
                        fflush(stdout);
                    }

                    //record operation end time
                    PhysicalTimeSpec endTimeOfSet = Utils::GetCurrentClockTime();

                    arg->NumOpsPerThread += 1;

#ifdef ENABLE_CLIENT_RESET
                    if (arg->enableClientReset) {
                        if (arg->clientResetNumber == numCycles) {
                            arg->Client->resetSession();
                            numCycles = 0;
                        }
                    }
#endif

                    //record operation latency
                    double duration = (endTimeOfSet - startTimeOfSet).toMilliSeconds();

                    arg->TotalNumWriteOps += 1;
                    arg->WriteLatenciesSum += duration;

                    if (arg->MaxWriteLatency < duration) {
                        arg->MaxWriteLatency = duration;
                    }

                    if (arg->MinWriteLatency > duration) {
                        arg->MinWriteLatency = duration;
                    }

#ifdef RESERVOIR_SAMPLING

                    if (arg->OpWriteLatencies.size() < arg->ReservoirSamplingLimit) {
                        arg->OpWriteLatencies.push_back(duration);
                    }
                    else {
                        offsetW++;
                        int position = abs(random()) % (arg->ReservoirSamplingLimit + offsetW);
                        if (position < arg->ReservoirSamplingLimit) {
                            arg->OpWriteLatencies[position] = duration;
                        }
                    }
#else
                    arg->OpWriteLatencies.push_back(duration);

#endif
                    pId = (pId + 1) % arg->NumPartitions;
                }

            }

            arg->numHotBlockKeys = 0;

            // notify stopping
            arg->OpEndEvent.Set();
        } catch (SocketException &e) {
            fprintf(stdout, "SocketException: %s\n", e.what());
            exit(1);
        } catch (...) {
            fprintf(stdout, "Exception while executing Experiment 1. ");
            exit(1);
        }
    }

    Experiment::Experiment(char *argv[]) {

        this->stopOperation = false;

        this->config.RequestsDistribution = argv[2];
        this->config.RequestsDistributionParameter = atof(argv[3]);
        this->config.ReadRatio = atoi(argv[4]);
        this->config.WriteRatio = atoi(argv[5]);
        this->config.GroupServerName = argv[6];
        this->config.GroupServerPort = atoi(argv[7]);
        GroupClient groupClient(config.GroupServerName, config.GroupServerPort);
        allPartitions = groupClient.GetRegisteredPartitions();
        this->config.NumPartitions = allPartitions.size();
        this->config.NumReplicasPerPartition = allPartitions[0].size();
        this->config.TotalNumItems = atoi(argv[8]);
        this->config.NumValueBytes = atoi(argv[9]);
//        this->config.NumOpsPerThread = atoi(argv[10]);
        this->config.NumOpsPerThread = 0;
        this->config.TotalNumOps = 0;
        this->config.NumThreads = atoi(argv[11]);

        this->config.ServingPartitionId = atoi(argv[12]);
        this->config.ServingReplicaId = atoi(argv[13]);
        this->config.clientServerName = argv[14];
        this->config.latencyOutputFileName = argv[15];
        this->config.reservoirSampling = (strcmp(argv[16], "true") == 0);
        this->config.reservoirSamplingLimit = atoi(argv[17]);
        this->config.experimentDuration = atof(argv[18]);
        this->config.experimentType = atoi(argv[19]);
        this->config.clientResetNumber = atoi(argv[20]);
        this->config.enableClientReset = (strcmp(argv[21], "true") == 0);
        this->config.numTxReadItems = atoi(argv[22]);
        fprintf(stdout, "numTxReadItems %d\n", this->config.numTxReadItems);
        this->config.locality = atoi(argv[23]);
        this->config.thinkTime = atoi(argv[24]);

        printExperimentParameters();
    }

    void Experiment::printExperimentParameters() {
        fprintf(stdout, "##EXPERIMENT\n");
        fprintf(stdout, "##EXPERIMENT_TYPE %d \n", config.experimentType);
        fprintf(stdout, "##REQUEST_DISTRIBUTION %s \n", config.RequestsDistribution.c_str());
        fprintf(stdout, "##REQUEST_DISTRIBUTION_PARAM %lf \n", config.RequestsDistributionParameter);
        fprintf(stdout, "##GROUPSERVER_NAME %s \n", config.GroupServerName.c_str());
        fprintf(stdout, "##GROUPSERVER_PORT %d \n", config.GroupServerPort);
        fprintf(stdout, "##NUM_PARTITIONS %d \n", config.NumPartitions);
        fprintf(stdout, "##NUM_REPLICAS_PER_PARTITION %d \n", config.NumReplicasPerPartition);
        fprintf(stdout, "##TOTAL_NUM_ITEMS %d \n", config.TotalNumItems);
        fprintf(stdout, "##NUM_VALUE_BYTES %d \n", config.NumValueBytes);
        fprintf(stdout, "##NUM_OPS_PER_THERAD %d \n", config.NumOpsPerThread);
        fprintf(stdout, "##NUM_THREADS %d \n", config.NumThreads);
        fprintf(stdout, "##SERVING_PARTITION_ID %d \n", config.ServingPartitionId);
        fprintf(stdout, "##SERVING_REPLICA %d \n", config.ServingReplicaId);
        fprintf(stdout, "##CLIENT_SERVER_NAME %s \n", config.clientServerName.c_str());
        fprintf(stdout, "##LATENCY OUTPUT FILE NAME %s \n", config.latencyOutputFileName.c_str());
        fprintf(stdout, "##RESERVOIR SAMPLING %d \n", config.reservoirSampling);
        fprintf(stdout, "##RESERVOIR SAMPLING LIMIT %d \n", config.reservoirSamplingLimit);
        fprintf(stdout, "##EXPERIMENT DURATION %d \n", config.experimentDuration);
        fprintf(stdout, "##NUM_TxReadItems %d \n", config.numTxReadItems);
        fprintf(stdout, "##LOCALITY %d\n", config.locality);
        fprintf(stdout, "##THINK_TIME %d\n", config.thinkTime);

        fflush(stdout);
    }

    void Experiment::buildThreadArguments() {
        for (int i = 0; i < config.NumThreads; i++) {
            ThreadArg *arg = new ThreadArg;
            arg->ThreadId = i;

            int partitionIndex = config.ServingPartitionId;
            arg->LocalPartitionId = partitionIndex;
            int replicaIndex = config.ServingReplicaId;

            for (int k = 0; k < config.NumPartitions; ++k) {
                DBPartition p = allPartitions[k][replicaIndex];
                arg->ServerNames.push_back(p.Name);
                arg->ServerPorts.push_back(p.PublicPort);
            }

            arg->NumPartitions = config.NumPartitions;
            arg->NumReplicas = config.NumReplicasPerPartition;
            arg->TotalNumItems = config.TotalNumItems;

            arg->NumOpsPerThread = 0;
            arg->NumValueBytes = config.NumValueBytes;
            arg->ReadRatio = config.ReadRatio;
            arg->WriteRatio = config.WriteRatio;
            arg->ReservoirSampling = config.reservoirSampling;
            arg->ReservoirSamplingLimit = config.reservoirSamplingLimit;
            arg->stopOperation = &(this->stopOperation);

            arg->servingPartition = config.ServingPartitionId;
            arg->servingReplica = config.ServingReplicaId;
            arg->ReadRatio = config.ReadRatio;
            arg->WriteRatio = config.WriteRatio;
            arg->ReadLatenciesSum = 0;
            arg->WriteLatenciesSum = 0;
            arg->TotalNumReadOps = 0;
            arg->TotalNumWriteOps = 0;
            arg->MinReadLatency = 100000000;
            arg->MaxReadLatency = 0;
            arg->MinWriteLatency = 100000000;
            arg->MaxWriteLatency = 0;
            arg->numHotBlockKeys = 0;
            arg->clientResetNumber = config.clientResetNumber;
            arg->enableClientReset = config.enableClientReset;
            arg->NumTxReadItems = config.numTxReadItems;
            arg->RequestsDistribution = config.RequestsDistribution;
            arg->locality = config.locality;
            arg->thinkTime = config.thinkTime;
            initializeThreadRequestDistribution(arg);

            threadArgs.push_back(arg);
            opReadyEvents.push_back(&arg->OpReadyEvent);
            opEndEvents.push_back(&arg->OpEndEvent);
        }
    }

    void Experiment::initializeThreadRequestDistribution(ThreadArg *t) {

        int numItems;
        if (config.experimentType == 4)
            numItems = (config.TotalNumItems / config.NumPartitions) / config.NumReplicasPerPartition - 1;
        else
            numItems = config.TotalNumItems / config.NumPartitions - 1;

        if (t->RequestsDistribution == "uniform") {
            srand(Utils::GetThreadId() * time(0));
            std::this_thread::sleep_for(std::chrono::milliseconds(10 * rand() % 10));
            t->generator = new UniformGenerator(numItems);

        } else if (t->RequestsDistribution == "zipfian") {
            t->generator = new ZipfianGenerator(t->ThreadId, 0, numItems,
                                                config.RequestsDistributionParameter);

        } else if (t->RequestsDistribution == "scrambledZipfian") {
            t->generator = new ScrambledZipfianGenerator(numItems);
        } else if (config.RequestsDistribution == "skewLatest") {
            CounterGenerator insert_key_sequence_(0);
            insert_key_sequence_.Set(numItems + 1);
            t->generator = new SkewedLatestGenerator(t->ThreadId, insert_key_sequence_);
        } else if (t->RequestsDistribution == "hotSpot") {
            t->generator = new HotSpotGenerator(numItems,
                                                config.RequestsDistributionParameter, 0.01);
        } else {
            printf("Non existing distribution.");
        }
    }

    void Experiment::launchBenchmarkingClientThreads() {
        void (*func)(ThreadArg *arg);
        switch (config.experimentType) {
            case 1:
                func = &experiment1;
                break;
            case 2:
                func = &experiment2;
                break;
            case 3:
                func = &experimentTx;
                break;
            case 4:
                func = &experiment4;
                break;
            default:
                fprintf(stdout,
                        "[ERROR]: Unknown experiment type. The default experiment, experiment 1 will be executed.\n");
                func = &experiment1;
                break;
        }

        for (int i = 0; i < config.NumThreads; i++) {
            std::thread *t = new std::thread(func, this->threadArgs[i]);
            t->detach();
            threads.push_back(t);
        }

        fprintf(stdout, "[INFO]: Launched all threads.\n");

    }

    void Experiment::writeResults(FILE *&stream, Results results) {
        std::string p = std::to_string(results.NumPartitions);
        std::string r = std::to_string(results.NumReplicas);
        char separator = ';';

        std::string shortForm = (
                boost::format("*%s " //0
                                      "%d " //1
                                      "%.5lf " //3
                                      "%.5lf " //4
                                      "%.5lf " //5
                                      "%.5lf " //6
                                      "%.5lf " //7
                                      "%.5lf " //8
                                      "%.5lf " //9
                                      "%.5lf " //10

                                      "%.5lf " //11
                                      "%.5lf " //12
                                      "%.5lf " //13
                                      "%.5lf " //14
                                      "%.5lf " //15
                                      "%.5lf " //16
                                      "%.5lf " //17

                                      "%d " //18
                                      "%d " //19
                                      "%d " //20
                                      "%d " //21
                                      "%.5f " //22
                                      "%.5f\n\n") //23
                % config.clientServerName //0
                % config.ServingReplicaId //1
                % results.OpThroughput //3
                % results.OpGetLatencyStats.average //4
                % results.OpGetLatencyStats.median //5
                % results.OpGetLatencyStats._90Percentile //6
                % results.OpGetLatencyStats._95Percentile //7
                % results.OpGetLatencyStats._99Percentile //8
                % results.OpGetLatencyStats.min //9
                % results.OpGetLatencyStats.max //10

                % results.OpSetLatencyStats.average //11
                % results.OpSetLatencyStats.median //12
                % results.OpSetLatencyStats._90Percentile //13
                % results.OpSetLatencyStats._95Percentile //14
                % results.OpSetLatencyStats._99Percentile //15
                % results.OpSetLatencyStats.min //16
                % results.OpSetLatencyStats.max //17

                % results.NumPartitions //18
                % results.NumReplicas //19
                % results.NumThreadsPerReplica //20
                % results.NumOpsPerThread //21
                % results.TotalOps //22
                % results.TotalTime).str(); //23

        replace(shortForm.begin(), shortForm.end(), ' ', separator);

        cout << shortForm << endl;
        fprintf(stream, "Finished %.5lf operations in %.5lf milliseconds\n", results.TotalOps,
                results.TotalTime);
        fprintf(stream, "Operation throughput is %.5lf ops/ms\n", results.OpThroughput);
        fprintf(stream, "Operation average read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats.average);
        fprintf(stream, "Operation median read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats.median);
        fprintf(stream, "Operation 75-percentile read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats._75Percentile);
        fprintf(stream, "Operation 90-percentile read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats._90Percentile);
        fprintf(stream, "Operation 95-percentile read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats._95Percentile);
        fprintf(stream, "Operation 99-percentile read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats._99Percentile);
        fprintf(stream, "Operation minimum observed read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats.min);
        fprintf(stream, "Operation maximum observed read latency is %.5lf milliseconds\n",
                results.OpGetLatencyStats.max);
        fprintf(stream, "Operation read latency variance is %.5lf milliseconds\n",
                results.OpGetLatencyStats.variance);
        fprintf(stream, "Operation read latency standard deviation is %.5lf milliseconds\n",
                results.OpGetLatencyStats.standardDeviation);

        fprintf(stream, "Operation average write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats.average);
        fprintf(stream, "Operation median write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats.median);
        fprintf(stream, "Operation 75-percentile write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats._75Percentile);
        fprintf(stream, "Operation 90-percentile write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats._90Percentile);
        fprintf(stream, "Operation 95-percentile write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats._95Percentile);
        fprintf(stream, "Operation 99-percentile write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats._99Percentile);
        fprintf(stream, "Operation minimum observed write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats.min);
        fprintf(stream, "Operation maximum observed write latency is %.5lf milliseconds\n",
                results.OpSetLatencyStats.max);
        fprintf(stream, "Operation write latency variance is %.5lf milliseconds\n",
                results.OpSetLatencyStats.variance);
        fprintf(stream, "Operation write latency standard deviation is %.5lf milliseconds\n",
                results.OpSetLatencyStats.standardDeviation);
        fprintf(stream, "Number of hot blocked keys is %d \n",
                results.NumHotBlockedKeys);

#ifdef WRITE_LATENCY_DISTRIBUTION

        std::string fileName = config.latencyOutputFileName + "_read_latency.txt";

        fprintf(stream, "\n[INFO] Writing read latencies to %s_read_latency.txt \n",
                config.latencyOutputFileName.c_str());
        writeLatencyDistribution(fileName, results.readLatencies);

        fileName = config.latencyOutputFileName + "_write_latency.txt";
        fprintf(stream, "\n[INFO] Writing write latencies to %s_write_latency.txt \n",
                config.latencyOutputFileName.c_str());

        writeLatencyDistribution(fileName, results.writeLatencies);
#endif

#ifdef WAIT_REPLICATION
        fprintf(stream, "Wait on replication is true\n");
#else
        fprintf(stream, "Wait on replication is false\n");
#endif

#ifdef DEP_VECTORS
        fprintf(stream, "DEP_VECTORS is true\n");
#else
        fprintf(stream, "DEP_VECTORS is false\n");
#endif
        fprintf(stream, "PARTITIONS_TO_READ_FROM %d\n", _partitionsToReadFrom);

        fprintf(stream, "NUM_READS %f\n", results.TotalReadOps);
        fprintf(stream, "NUM_WRITES %f\n", results.TotalWriteOps);
        fprintf(stream, "sum_read_latency %f\n", results.OpGetLatencyStats.average * results.TotalReadOps);
        fprintf(stream, "sum_write_latency %f\n", results.OpSetLatencyStats.average * results.TotalWriteOps);
    }

    void Experiment::writeLatencyDistribution(std::string fileName, vector<double> vector) {

        std::sort(vector.begin(), vector.end());

        FILE *stream;
        stream = fopen(fileName.c_str(), "w");

        for (int i = 0; i < vector.size(); i++) {
            fprintf(stream, "%.5lf;", vector[i]);
        }
        fprintf(stream, "\n\n");

        fclose(stream);
    }

    void calculateLatencyMeasurements(std::vector<double> opLatencies, std::vector<double> &latencies,
                                      double &totalLatency,
                                      double &minLatency, double &maxLatency) {
        latencies.insert(latencies.end(), opLatencies.begin(), opLatencies.end());

        totalLatency += std::accumulate(opLatencies.begin(),
                                        opLatencies.end(), 0.0);

        double min = *(std::min_element(std::begin(opLatencies),
                                        std::end(opLatencies)));

        if (minLatency > min) {
            minLatency = min;
        }

        double max = *(std::max_element(std::begin(opLatencies),
                                        std::end(opLatencies)));
        if (maxLatency < max) {
            maxLatency = max;
        }

    }

    void Experiment::calculateMedian_Percentiles(std::vector<double> vect, Statistics &stat) {

        std::sort(vect.begin(), vect.end());

        stat.median = vect.at((int) std::round(vect.size() * 0.50));

        stat._75Percentile = vect.at((int) std::round(vect.size() * 0.75) - 1);

        stat._90Percentile = vect.at((int) std::round(vect.size() * 0.90) - 1);

        stat._95Percentile = vect.at((int) std::round(vect.size() * 0.95) - 1);

        stat._99Percentile = vect.at((int) std::round(vect.size() * 0.99) - 1);

    }

    void Experiment::calculateVariation_StandardDeviation(Statistics &stat, std::vector<double> vect) {

        stat.variance = 0;

        for (int i = 0; i < vect.size(); i++) {
            stat.variance += std::pow(stat.average - vect[i], 2);
        }

        stat.variance = stat.variance / vect.size();
        stat.standardDeviation = std::sqrt(stat.variance);

    }

    void Experiment::calculatePerformanceMeasures(double duration) {

        SLOG("Calculating performance measurements!");

        // calculate perf numbers
        double totalOps = config.TotalNumOps;
        double totalTime = duration;
        double opRate = totalOps / totalTime;
        double totalReadLatency = 0.0;
        double totalWriteLatency = 0.0;
        double totalTxLatency = 0.0;

        double totalRead = 0.0;
        double totalWrite = 0.0;

        double minReadLatency = threadArgs[0]->MinReadLatency;
        double minWriteLatency = threadArgs[0]->MinWriteLatency;
        double maxReadLatency = threadArgs[0]->MaxReadLatency;
        double maxWriteLatency = threadArgs[0]->MaxWriteLatency;

        Results results;

        results.OpGetLatencyStats.average = 0;
        results.OpGetLatencyStats.max = 0;
        results.OpGetLatencyStats.min = 100000000;
        results.NumHotBlockedKeys = 0;

        results.OpSetLatencyStats.average = 0;
        results.OpSetLatencyStats.max = 0;
        results.OpSetLatencyStats.min = 100000000;

        for (int i = 0; i < config.NumThreads; i++) {

//#ifdef MEASURE_STATISTICS
            calculateLatencyMeasurements(threadArgs[i]->OpReadLatencies, results.readLatencies,
                                         totalReadLatency,
                                         minReadLatency, maxReadLatency);
            calculateLatencyMeasurements(threadArgs[i]->OpWriteLatencies, results.writeLatencies,
                                         totalWriteLatency,
                                         minWriteLatency, maxWriteLatency);
//#endif

            results.OpGetLatencyStats.average += (threadArgs[i]->ReadLatenciesSum);
            totalRead += threadArgs[i]->TotalNumReadOps;
            results.OpGetLatencyStats.max = max(results.OpGetLatencyStats.max, threadArgs[i]->MaxReadLatency);
            results.OpGetLatencyStats.min = min(results.OpGetLatencyStats.min, threadArgs[i]->MinReadLatency);

            results.OpSetLatencyStats.average += (threadArgs[i]->WriteLatenciesSum);
            totalWrite += threadArgs[i]->TotalNumWriteOps;
            results.OpSetLatencyStats.max = max(results.OpSetLatencyStats.max, threadArgs[i]->MaxWriteLatency);
            results.OpSetLatencyStats.min = min(results.OpSetLatencyStats.min, threadArgs[i]->MinWriteLatency);

            results.NumHotBlockedKeys += threadArgs[i]->numHotBlockKeys;
        }

        results.OpGetLatencyStats.average /= totalRead;
        results.OpSetLatencyStats.average /= totalWrite;
        results.TotalReadOps = totalRead;
        results.TotalWriteOps = totalWrite;
        results.NumOpsPerThread = config.NumOpsPerThread;
        results.NumPartitions = config.NumPartitions;
        results.NumReplicas = config.NumReplicasPerPartition;
        results.OpThroughput = opRate;
        results.NumThreadsPerReplica = config.NumThreads;
        results.TotalOps = totalOps;
        results.TotalTime = totalTime;

//#ifdef MEASURE_STATISTICS
        calculateMedian_Percentiles(results.readLatencies, results.OpGetLatencyStats);
        calculateMedian_Percentiles(results.writeLatencies, results.OpSetLatencyStats);
        calculateVariation_StandardDeviation(results.OpGetLatencyStats, results.readLatencies);
        calculateVariation_StandardDeviation(results.OpSetLatencyStats, results.writeLatencies);
//#endif

        writeResults(stdout, results);
    }

    void Experiment::freeResources() {

        for (int i = 0; i < config.NumThreads; i++) {
            delete threadArgs[i];
            delete threads[i];
        }
    }

    void Experiment::runExperiment() {

        buildThreadArguments();

        // launch benchmarking client threads
        launchBenchmarkingClientThreads();
        // wait for all threads ready to run
        WaitHandle::WaitAll(opReadyEvents);

        // record start time
        PhysicalTimeSpec startTimeExp = Utils::GetCurrentClockTime();

        // signal threads to start
        for (int i = 0; i < config.NumThreads; i++) {
            threadArgs[i]->OpStartEvent.Set();
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(this->config.experimentDuration));
        this->stopOperation = true;
        fprintf(stdout, "Stopping the experiment. Waiting for the clients to finish.\n");
        fflush(stdout);
        // wait for all threads finishing
        WaitHandle::WaitAll(opEndEvents);

        // record end time
        PhysicalTimeSpec endTimeExp = Utils::GetCurrentClockTime();
        double duration = (endTimeExp - startTimeExp).toMilliSeconds();
        fprintf(stdout, "[INFO] ALL CLIENT OPERATIONS ARE DONE\n");
        fflush(stdout);

        config.TotalNumOps = 0;
        config.NumOpsPerThread = 0;

        SLOG("Calculating performance statistics");
        for (int i = 0; i < config.NumThreads; i++) {
            config.TotalNumOps += threadArgs[i]->NumOpsPerThread;
            config.NumOpsPerThread += threadArgs[i]->NumOpsPerThread;
        }
        config.NumOpsPerThread /= config.NumThreads;

        calculatePerformanceMeasures(duration);

        // free resources
        freeResources();
        SLOG("Resources are FREE!");

    }

}


#include <dwl/WholeBodyState.h>

#define BOOST_TEST_MODULE DWL_TESTS
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

// Tolerance
double epsilon = 0.00001;

BOOST_AUTO_TEST_CASE(base_linear) // specify a test case for base linear states
{
	dwl::WholeBodyState ws;
	Eigen::Vector3d old_base_state;
	Eigen::Vector3d new_base_state;

	// Testing the base position
	old_base_state = Eigen::Vector3d(0.1, 0.2, 0.6);
	ws.setBasePosition(old_base_state);
	new_base_state = ws.getBasePosition();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	// Testing the base velocity
	old_base_state = Eigen::Vector3d(0.5, 0.7, 0.);
	ws.setBaseVelocity_W(old_base_state);
	new_base_state = ws.getBaseVelocity_W();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseVelocity_B();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseVelocity_H();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	// Testing the base acceleration
	old_base_state = Eigen::Vector3d(0.8, 0.9, -9.8);
	ws.setBaseAcceleration_W(old_base_state);
	new_base_state = ws.getBaseAcceleration_W();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAcceleration_B();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAcceleration_H();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);
}

BOOST_AUTO_TEST_CASE(base_angular) // specify a test case for base angular states
{
	dwl::WholeBodyState ws;
	Eigen::Vector3d old_base_state;
	Eigen::Vector3d new_base_state;
	Eigen::Vector3d rpy;
	Eigen::Quaterniond q;

	// Testing RPY
	ws.setBaseRPY(Eigen::Vector3d(0., M_PI_4, M_PI_4));
	ws.setBaseOrientation(ws.getBaseOrientation());
	rpy = ws.getBaseRPY();
	BOOST_CHECK_SMALL((double) rpy(0) - 0., epsilon);
	BOOST_CHECK_SMALL((double) rpy(1) - M_PI_4, epsilon);
	BOOST_CHECK_SMALL((double) rpy(2) - M_PI_4, epsilon);

	// Testing quaternion in the world frame
	ws.setBaseRPY(Eigen::Vector3d(0., 0., M_PI));
	q = ws.getBaseOrientation();
	BOOST_CHECK_SMALL(q.x() - 0., epsilon);
	BOOST_CHECK_SMALL(q.y() - 0., epsilon);
	BOOST_CHECK_SMALL(q.z() - 1., epsilon);
	BOOST_CHECK_SMALL(q.w() - 0., epsilon);

	// Testing another quaternion in the world frame
	ws.setBaseRPY(Eigen::Vector3d(0., M_PI_4, M_PI));
	q = ws.getBaseOrientation();
	BOOST_CHECK_SMALL(q.x() + 0.382683, epsilon);
	BOOST_CHECK_SMALL(q.y() - 0., epsilon);
	BOOST_CHECK_SMALL(q.z() - 0.92388, epsilon);
	BOOST_CHECK_SMALL(q.w() - 0., epsilon);

	// Testing the base angular velocity
	ws.setBaseRPY(Eigen::Vector3d(0., 0., 0.));
	old_base_state = Eigen::Vector3d(0.5, 0.7, 0.);
	ws.setBaseAngularVelocity_W(old_base_state);
	new_base_state = ws.getBaseAngularVelocity_W();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAngularVelocity_B();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAngularVelocity_H();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	// Testing the base acceleration
	old_base_state = Eigen::Vector3d(0.8, 0.9, -9.8);
	ws.setBaseAngularAcceleration_W(old_base_state);
	new_base_state = ws.getBaseAngularAcceleration_W();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAngularAcceleration_B();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);

	new_base_state = ws.getBaseAngularAcceleration_H();
	for (unsigned int i = 0; i < 2; i++)
		BOOST_CHECK_SMALL((double) (new_base_state(i) - old_base_state(i)), epsilon);
}

BOOST_AUTO_TEST_CASE(base_joint) // specify a test case for joint states
{
	dwl::WholeBodyState ws(12);
	unsigned int n_joints;
	unsigned int idx;
	double joint_value;
	Eigen::Vector2d old_joint_state;
	Eigen::Vector2d new_joint_state;

	n_joints = ws.getJointDoF();
	BOOST_CHECK(n_joints == 12);

	ws.setJointDoF(2);
	n_joints = ws.getJointDoF();
	BOOST_CHECK(n_joints == 2);

	// Testing joint position
	idx = 1;
	old_joint_state = Eigen::Vector2d(0.1, 0.5);
	ws.setJointPosition(old_joint_state);
	joint_value = ws.getJointPosition(idx);
	new_joint_state = ws.getJointPosition();
	BOOST_CHECK_SMALL(joint_value - (double) old_joint_state(idx), epsilon);
	for (unsigned int i = 0; i < old_joint_state.size(); i++)
		BOOST_CHECK_SMALL((double) (new_joint_state(i) - old_joint_state(i)), epsilon);

	// Testing joint velocity
	old_joint_state = Eigen::Vector2d(0.7, 0.8);
	ws.setJointVelocity(old_joint_state);
	joint_value = ws.getJointVelocity(idx);
	new_joint_state = ws.getJointVelocity();
	BOOST_CHECK_SMALL(joint_value - (double) old_joint_state(idx), epsilon);
	for (unsigned int i = 0; i < old_joint_state.size(); i++)
		BOOST_CHECK_SMALL((double) (new_joint_state(i) - old_joint_state(i)), epsilon);

	// Testing joint acceleration
	old_joint_state = Eigen::Vector2d(1.7, 2.8);
	ws.setJointAcceleration(old_joint_state);
	joint_value = ws.getJointAcceleration(idx);
	new_joint_state = ws.getJointAcceleration();
	BOOST_CHECK_SMALL(joint_value - (double) old_joint_state(idx), epsilon);
	for (unsigned int i = 0; i < old_joint_state.size(); i++)
		BOOST_CHECK_SMALL((double) (new_joint_state(i) - old_joint_state(i)), epsilon);

	// Testing joint effort
	old_joint_state = Eigen::Vector2d(17, 28);
	ws.setJointEffort(old_joint_state);
	joint_value = ws.getJointEffort(idx);
	new_joint_state = ws.getJointEffort();
	BOOST_CHECK_SMALL(joint_value - (double) old_joint_state(idx), epsilon);
	for (unsigned int i = 0; i < old_joint_state.size(); i++)
		BOOST_CHECK_SMALL((double) (new_joint_state(i) - old_joint_state(i)), epsilon);
}


#include <bacs/problem/resource/parse.hpp>

#include <boost/cast.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi.hpp>

#include <algorithm>
#include <string>

#include <cmath>

namespace bacs::problem::resource::parse {

namespace qi = boost::spirit::qi;

struct si_multiple : qi::symbols<char, double> {
  si_multiple() {
    add("da", 1e1);
    add("h", 1e2);
    add("k", 1e3);
    add("M", 1e6);
    add("G", 1e9);
    add("T", 1e12);
    add("P", 1e15);
    add("E", 1e18);
    add("Z", 1e21);
    add("Y", 1e24);
  }
};

struct si_submultiple : qi::symbols<char, double> {
  si_submultiple() {
    add("d", 1e-1);
    add("c", 1e-2);
    add("m", 1e-3);
    add("u", 1e-6);
    add("n", 1e-9);
    add("p", 1e-12);
    add("f", 1e-15);
    add("a", 1e-18);
    add("z", 1e-21);
    add("y", 1e-24);
  }
};

struct binary_multiple : qi::symbols<char, double> {
  binary_multiple() {
    constexpr double ten = 1 << 10;
    double mult = 1;
    add("Ki", mult *= ten);
    add("Mi", mult *= ten);
    add("Gi", mult *= ten);
    add("Ti", mult *= ten);
    add("Pi", mult *= ten);
    add("Ei", mult *= ten);
    add("Zi", mult *= ten);
    add("Yi", mult *= ten);
  }
};

template <typename Iterator>
struct time_parser : qi::grammar<Iterator, double()> {
  time_parser() : time_parser::base_type(start) {
    multiple = (si_multiple_[qi::_val = qi::_1] |
                si_submultiple_[qi::_val = qi::_1] | qi::eps[qi::_val = 1]) >>
               qi::lit('s');
    multiple_unit = multiple[qi::_val = qi::_1] | qi::eps[qi::_val = 1];
    start = (qi::double_ >> multiple_unit)[qi::_val = qi::_1 * qi::_2];
  }

  qi::rule<Iterator, double()> start, multiple, multiple_unit;
  si_multiple si_multiple_;
  si_submultiple si_submultiple_;
};

template <typename Iterator>
struct memory_parser : qi::grammar<Iterator, double()> {
  memory_parser() : memory_parser::base_type(start) {
    multiple = (binary_multiple_[qi::_val = qi::_1] | qi::eps[qi::_val = 1]) >>
               qi::lit('B');
    multiple_unit = multiple[qi::_val = qi::_1] | qi::eps[qi::_val = 1];
    start = (qi::double_ >> multiple_unit)[qi::_val = qi::_1 * qi::_2];
  }

  qi::rule<Iterator, double()> start, multiple, multiple_unit;
  binary_multiple binary_multiple_;
};

std::uint64_t time_millis(const std::string &time) {
  std::string::const_iterator begin = time.begin();
  double result;
  if (!qi::parse(begin, time.end(), time_parser<std::string::const_iterator>(),
                 result) ||
      begin != time.end()) {
    BOOST_THROW_EXCEPTION(time_error()
                          << time_error::value(time)
                          << time_error::pos(begin - time.begin()));
  }
  return boost::numeric_cast<std::uint64_t>(std::round(result * 1000));
}

std::uint64_t memory_bytes(const std::string &memory) {
  std::string::const_iterator begin = memory.begin();
  double result;
  if (!qi::parse(begin, memory.end(),
                 memory_parser<std::string::const_iterator>(), result) ||
      begin != memory.end()) {
    BOOST_THROW_EXCEPTION(memory_error()
                          << memory_error::value(memory)
                          << memory_error::pos(begin - memory.begin()));
  }
  return boost::numeric_cast<std::uint64_t>(std::round(result));
}

}  // namespace bacs::problem::resource::parse


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to 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)

// Qt World Mapper Example

// Qt is a well-known and often used platform independent windows library

// To build and run this example:
// 1) download (from http://qt.nokia.com), configure and make QT
// 2) if necessary, adapt Qt clause in include path (note there is a Qt property sheet)

#include <fstream>

#include <QtGui>
#include <QWidget>
#include <QObject>
#include <QPainter>

#include <boost/foreach.hpp>

#include <boost/geometry/geometry.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/multi/geometries/multi_geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

#include <boost/geometry/extensions/algorithms/selected.hpp>

// Adapt a QPointF such that it can be handled by Boost.Geometry
BOOST_GEOMETRY_REGISTER_POINT_2D_GET_SET(QPointF, double, cs::cartesian, x, y, setX, setY)

// Adapt a QPolygonF as well.
// A QPolygonF has no holes (interiors) so it is similar to a Boost.Geometry ring
BOOST_GEOMETRY_REGISTER_RING(QPolygonF)

typedef boost::geometry::model::d2::point_xy<double> point_2d;
typedef boost::geometry::model::multi_polygon
    <
        boost::geometry::model::polygon<point_2d>
    > country_type;

class WorldMapper : public QWidget
{
 public:
    WorldMapper(std::vector<country_type> const& countries, boost::geometry::model::box<point_2d> const& box)
        : m_countries(countries)
        , m_box(box)
    {
        setPalette(QPalette(QColor(200, 250, 250)));
        setAutoFillBackground(true);
    }

 protected:
    void paintEvent(QPaintEvent*)
    {
        map_transformer_type transformer(m_box, this->width(), this->height());

        QPainter painter(this);
        painter.setBrush(Qt::green);
        painter.setRenderHint(QPainter::Antialiasing);

        BOOST_FOREACH(country_type const& country, m_countries)
        {
            typedef boost::range_value<country_type>::type polygon_type;
            BOOST_FOREACH(polygon_type const& polygon, country)
            {
                typedef boost::geometry::ring_type<polygon_type>::type ring_type;
                ring_type const& ring = boost::geometry::exterior_ring(polygon);

                // This is the essention:
                // Directly transform from a multi_polygon (ring-type) to a QPolygonF
                QPolygonF qring;
                boost::geometry::transform(ring, qring, transformer);

                painter.drawPolygon(qring);
            }
        }
    }

 private:
    typedef boost::geometry::strategy::transform::map_transformer
        <
            double, 2, 2,
            true, true
        > map_transformer_type;

     std::vector<country_type> const& m_countries;
     boost::geometry::model::box<point_2d> const& m_box;
 };

class MapperWidget : public QWidget
{
 public:
     MapperWidget(std::vector<country_type> const& countries, boost::geometry::model::box<point_2d> const& box, QWidget *parent = 0)
         : QWidget(parent)
     {
         WorldMapper* mapper = new WorldMapper(countries, box);

         QPushButton *quit = new QPushButton(tr("Quit"));
         quit->setFont(QFont("Times", 18, QFont::Bold));
         connect(quit, SIGNAL(clicked()), qApp, SLOT(quit()));

         QVBoxLayout *layout = new QVBoxLayout;
         layout->addWidget(mapper);
         layout->addWidget(quit);
         setLayout(layout);
     }

};

// ----------------------------------------------------------------------------
// Read an ASCII file containing WKT's
// ----------------------------------------------------------------------------
template <typename Geometry, typename Box>
inline void read_wkt(std::string const& filename, std::vector<Geometry>& geometries, Box& box)
{
    std::ifstream cpp_file(filename.c_str());
    if (cpp_file.is_open())
    {
        while (! cpp_file.eof() )
        {
            std::string line;
            std::getline(cpp_file, line);
            if (! line.empty())
            {
                Geometry geometry;
                boost::geometry::read_wkt(line, geometry);
                geometries.push_back(geometry);
                boost::geometry::expand(box, boost::geometry::return_envelope<Box>(geometry));
            }
        }
    }
}

int main(int argc, char *argv[])
{
    std::vector<country_type> countries;
    boost::geometry::model::box<point_2d> box;
    boost::geometry::assign_inverse(box);
    read_wkt("../data/world.wkt", countries, box);

    QApplication app(argc, argv);
    MapperWidget widget(countries, box);
    widget.setWindowTitle("Boost.Geometry for Qt - Hello World!");
    widget.setGeometry(50, 50, 800, 500);
    widget.show();
    return app.exec();
}


#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include "grid_map.h"
#include "grid_mapper.h"

using namespace std;
using namespace pcl;
using namespace Eigen;
using namespace cv;

GridMap* g_map;
GridMapper* g_gmapper;

string POSE_GRAPH_SAVE_PATH = "../pose_graph/";

int main (int argc, char** argv){
    Eigen::Matrix4d T;
    T << 0.007941668, 0.00510531, 0.99995543, 0.0424947,
            -0.99955139, -0.02883817,   0.00808569,0.02858,
            0.028878166,  -0.9995710558,  0.004874000,-0.0019294,
            0, 0, 0, 1;
    Eigen::Vector3d tic = (T.block<3, 1>(0, 3));
    Eigen::Matrix3d qic = (T.block<3, 3>(0, 0));

    g_map = new GridMap ( 1500, 500, 750, 250, 0.05 );
    g_gmapper = new GridMapper (g_map, 0.6, 0.4, 0.5);

    FILE * pFile;
    string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt";
    printf("lode pose graph from: %s \n", file_path.c_str());
    printf("pose graph loading...\n");
    pFile = fopen (file_path.c_str(),"r");
    if (pFile == NULL)
    {
        printf("lode previous pose graph error: wrong previous pose graph path or no previous pose graph \n the system will start with new pose graph \n");
        return 1;
    }
    int index;
    double time_stamp;
    double VIO_Tx, VIO_Ty, VIO_Tz;
    double PG_Tx, PG_Ty, PG_Tz;
    double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz;
    double PG_Qw, PG_Qx, PG_Qy, PG_Qz;
    double loop_info_0, loop_info_1, loop_info_2, loop_info_3;
    double loop_info_4, loop_info_5, loop_info_6, loop_info_7;
    int loop_index;
    int keypoints_num,densepoints_num;
    Eigen::Matrix<double, 8, 1 > loop_info;
    int cnt = 0;
    while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d %d", &index, &time_stamp,
                  &VIO_Tx, &VIO_Ty, &VIO_Tz,
                  &PG_Tx, &PG_Ty, &PG_Tz,
                  &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz,
                  &PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz,
                  &loop_index,
                  &loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3,
                  &loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7,
                  &keypoints_num, &densepoints_num) != EOF)
    {
        cv::Mat image;
        std::string image_path, descriptor_path;

        Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz);
        Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz);
        Quaterniond VIO_Q;
        VIO_Q.w() = VIO_Qw;
        VIO_Q.x() = VIO_Qx;
        VIO_Q.y() = VIO_Qy;
        VIO_Q.z() = VIO_Qz;
        Quaterniond PG_Q;
        PG_Q.w() = PG_Qw;
        PG_Q.x() = PG_Qx;
        PG_Q.y() = PG_Qy;
        PG_Q.z() = PG_Qz;
        Matrix3d VIO_R, PG_R;
        VIO_R = VIO_Q.toRotationMatrix();
        PG_R = PG_Q.toRotationMatrix();
        Eigen::Matrix<double, 8, 1 > loop_info;
        loop_info << loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7;

        string densepoints_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_densepoints.txt";
        FILE *densepoints_file;
        densepoints_file = fopen(densepoints_path.c_str(), "r");
        vector<Eigen::Matrix<float ,6, 1>> points_rgbd;
        for (int i = 0; i < densepoints_num; i++){
            double p_x, p_y, p_z, p_r, p_g, p_b;
            Eigen::Matrix<float ,6, 1> point_rgbd;
            if(!fscanf(densepoints_file,"%lf %lf %lf %lf %lf %lf", &p_x, &p_y, &p_z, &p_r, &p_g, &p_b))
                printf(" fail to load pose graph \n");
            point_rgbd[0] = p_x;
            point_rgbd[1] = p_y;
            point_rgbd[2] = p_z;
            point_rgbd[3] = p_r;
            point_rgbd[4] = p_g;
            point_rgbd[5] = p_b;
            points_rgbd.push_back(point_rgbd);
        }
        fclose(densepoints_file);

        for(auto &point_rgbd : points_rgbd){
            Eigen::Vector3d point(point_rgbd[0], point_rgbd[1], point_rgbd[2]);
            Eigen::Vector3d pointWorld = PG_R * (qic * point + tic) + PG_T;
            if(pointWorld.z() > 0.5 || pointWorld.z() < 0.1)
                continue;
            pointWorld.z() = 0;
            g_gmapper->updateMap(pointWorld, PG_T);
        }
    }
    fclose (pFile);

    g_map->saveMap ( "map.png", "map.yaml" );

    return (0);

}