#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Utils/OBJLoader.h"
#include "Demos/Visualization/Visualization.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

#define _USE_MATH_DEFINES

#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void initParameters();
void timeStep();
void buildModel();
void createBunnyRodModel();
void render();
void reset();

DemoBase *base;

// bunny rod scene
const int numberOfBodies = 10;
const Real width = static_cast<Real>(1.0);
const Real height = static_cast<Real>(0.1);
const Real depth = static_cast<Real>(0.1);
const Real youngsModulus = static_cast<Real>(209e9);
const Real torsionModulus = static_cast<Real>(79e9);
const Real density = 7800.;

const Real bunnyDensity = 500.;

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

    base = new DemoBase();
    base->init(argc, argv, "Stretch-bending-twisting demo");

    SimulationModel *model = new SimulationModel();
    model->init();
    Simulation::getCurrent()->setModel(model);

    buildModel();

    initParameters();

    Simulation::getCurrent()->setSimulationMethodChangedCallback([&]()
                                                                 {
                                                                     reset();
                                                                     initParameters();
                                                                     base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep());
                                                                 });

    // OpenGL
    MiniGL::setClientIdleFunc(50, timeStep);
    MiniGL::setKeyFunc(0, 'r', reset);
    MiniGL::setClientSceneFunc(render);
    MiniGL::setViewport(40.0, 0.1f, 500.0, Vector3r(5.0, 10.0, 30.0), Vector3r(5.0, 0.0, 0.0));

    glutMainLoop();

    Utilities::Timing::printAverageTimes();
    Utilities::Timing::printTimeSums();

    delete Simulation::getCurrent();
    delete base;
    delete model;

    return 0;
}

void initParameters()
{
    TwRemoveAllVars(MiniGL::getTweakBar());
    TweakBarParameters::cleanup();

    MiniGL::initTweakBarParameters();

    TweakBarParameters::createParameterGUI();
    TweakBarParameters::createParameterObjectGUI(base);
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent());
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getModel());
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getTimeStep());
}

void reset()
{
    Utilities::Timing::printAverageTimes();
    Utilities::Timing::reset();

    Simulation::getCurrent()->reset();
    base->getSelectedParticles().clear();

    Simulation::getCurrent()->getModel()->cleanup();
    buildModel();
}

void timeStep()
{
    const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
    if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
        base->setValue(DemoBase::PAUSE, true);

    if (base->getValue<bool>(DemoBase::PAUSE))
        return;

    // Simulation code
    SimulationModel *model = Simulation::getCurrent()->getModel();
    const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
    for (unsigned int i = 0; i < numSteps; i++)
    {
        START_TIMING("SimStep");
        Simulation::getCurrent()->getTimeStep()->step(*model);
        STOP_TIMING_AVG;
    }
}

void buildModel()
{
    TimeManager::getCurrent()->setTimeStepSize(static_cast<Real>(0.01));

    createBunnyRodModel();
}

void render()
{
    base->render();
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
    std::vector<OBJLoader::Vec3f> x;
    std::vector<OBJLoader::Vec3f> normals;
    std::vector<OBJLoader::Vec2f> texCoords;
    std::vector<MeshFaceIndices> faces;
    OBJLoader::Vec3f s = {(float) scale[0], (float) scale[1], (float) scale[2]};
    OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);

    mesh.release();
    const unsigned int nPoints = (unsigned int) x.size();
    const unsigned int nFaces = (unsigned int) faces.size();
    const unsigned int nTexCoords = (unsigned int) texCoords.size();
    mesh.initMesh(nPoints, nFaces * 2, nFaces);
    vd.reserve(nPoints);
    for (unsigned int i = 0; i < nPoints; i++)
    {
        vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
    }
    for (unsigned int i = 0; i < nTexCoords; i++)
    {
        mesh.addUV(texCoords[i][0], texCoords[i][1]);
    }
    for (unsigned int i = 0; i < nFaces; i++)
    {
        // Reduce the indices by one
        int posIndices[3];
        int texIndices[3];
        for (int j = 0; j < 3; j++)
        {
            posIndices[j] = faces[i].posIndices[j] - 1;
            if (nTexCoords > 0)
            {
                texIndices[j] = faces[i].texIndices[j] - 1;
                mesh.addUVIndex(texIndices[j]);
            }
        }

        mesh.addFace(&posIndices[0]);
    }
    mesh.buildNeighbors();

    mesh.updateNormals(vd, 0);
    mesh.updateVertexNormals(vd);

    LOG_INFO << "Number of triangles: " << nFaces;
    LOG_INFO << "Number of vertices: " << nPoints;
}

/** Create the bunny rod body model
*/
void createBunnyRodModel()
{
    SimulationModel *model = Simulation::getCurrent()->getModel();
    SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
    SimulationModel::ConstraintVector &constraints = model->getConstraints();

    string fileName = FileSystem::normalizePath(base->getDataPath() + "/models/cube.obj");
    IndexedFaceMesh mesh;
    VertexData vd;
    loadObj(fileName, vd, mesh, Vector3r(height, width, depth));

    string fileName2 = FileSystem::normalizePath(base->getDataPath() + "/models/bunny_10k.obj");
    IndexedFaceMesh mesh2;
    VertexData vd2;
    loadObj(fileName2, vd2, mesh2, Vector3r(2.0, 2.0, 2.0));

    rb.resize(numberOfBodies);
    for (unsigned int i = 0; i < numberOfBodies - 1; i++)
    {
        rb[i] = new RigidBody();

        Real mass(static_cast<Real>(0.25 * M_PI) * width * height * depth * density);

        const Real Iy = static_cast<Real>(1. / 12.) * mass * (static_cast<Real>(3.) * (static_cast<Real>(0.25) * height * height) + width * width);
        const Real Iz = static_cast<Real>(1. / 12.) * mass * (static_cast<Real>(3.) * (static_cast<Real>(0.25) * depth * depth) + width * width);
        const Real Ix = static_cast<Real>(0.25) * mass * (static_cast<Real>(0.25) * (height * height + depth * depth));
        Vector3r inertia(Iy, Ix, Iz); // rod axis along y-axis

        Real angle(static_cast<Real>(M_PI_2));
        Vector3r axis(0., 0., 1.);
        AngleAxisr angleAxis(angle, axis);
        Quaternionr rotation(angleAxis);

        rb[i]->initBody(mass, Vector3r((Real) i * width, 0.0, 0.0), inertia, rotation, vd, mesh);
    }
    // Make first body static
    rb[0]->setMass(0.0);

    // bunny
    const Quaternionr q(AngleAxisr(static_cast<Real>(1.0 / 6.0 * M_PI), Vector3r(0.0, 0.0, 1.0)));
    const Vector3r t(static_cast<Real>(0.411) + (static_cast<Real>(numberOfBodies - 1.0)) * width, -static_cast<Real>(1.776), static_cast<Real>(0.356));
    rb[numberOfBodies - 1] = new RigidBody();
    rb[numberOfBodies - 1]->initBody(bunnyDensity, t, q, vd2, mesh2);

    constraints.reserve(numberOfBodies - 1);
    for (unsigned int i = 0; i < numberOfBodies - 2; i++)
    {
        model->addStretchBendingTwistingConstraint(i, i + 1, Vector3r(static_cast<Real>(i) * width + static_cast<Real>(0.5) * width, 0.0, 0.0), 0.25 * (height + depth),
                                                   width, youngsModulus, torsionModulus);
    }
    unsigned int i(numberOfBodies - 2);
    model->addBallJoint(i, i + 1, Vector3r(static_cast<Real>(i) * width + static_cast<Real>(0.5) * width, 0.0, 0.0));

    Simulation::getCurrent()->getTimeStep()->setValue(TimeStepController::MAX_ITERATIONS, 200);
}


#include <Eigen/Eigen>
#include <atomic>
#include <cassert>
#include <cmath>
#include <deque>
#include <functional>
#include <iostream>
#include <mutex>
#include <vector>

#include <vtkActor.h>
#include <vtkCallbackCommand.h>
#include <vtkCamera.h>
#include <vtkCellArray.h>
#include <vtkDoubleArray.h>
#include <vtkImageActor.h>
#include <vtkImageChangeInformation.h>
#include <vtkImageData.h>
#include <vtkImageMapToColors.h>
#include <vtkImageMapper.h>
#include <vtkImageMapper3D.h>
#include <vtkImageProperty.h>
#include <vtkInteractorStyleTrackballCamera.h>
#include <vtkLookupTable.h>
#include <vtkMath.h>
#include <vtkMatrix4x4.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPointSource.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkVertexGlyphFilter.h>

#include "colormaps.h"
#include "ouster/lidar_scan.h"
#include "ouster/os1_util.h"
#include "ouster/viz.h"

namespace ouster {
namespace viz {

using Points = Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor>;

/**
 * Helper function to create vtk lookup tables for the specific color palettes
 **/
vtkSmartPointer<vtkLookupTable> palette_gen(const float c[palette_n][3]) {
    auto lut = vtkSmartPointer<vtkLookupTable>::New();
    lut->SetNumberOfTableValues(palette_n);
    lut->Build();
    for (int i = 0; i < palette_n; i++) {
        lut->SetTableValue(i, c[i][0], c[i][1], c[i][2]);
    }
    return lut;
}

const std::vector<std::pair<std::string, vtkSmartPointer<vtkLookupTable>>>
    palettes = {
        {"Parula", palette_gen(parula)}, {"Viridis", palette_gen(viridis)},
        {"Magma", palette_gen(magma)},   {"Rainbow", palette_gen(rainbow)},
        {"Autumn", palette_gen(autumn)}, {"Grey", palette_gen(grey)}};

/**
 * Specify what quantity to color in the point cloud visualization
 **/
enum ColorMode { COLOR_Z, COLOR_INTENSITY, COLOR_ZINTENSITY, COLOR_RANGE };

const std::vector<std::pair<std::string, ColorMode>> color_modes = {
    {"Z", COLOR_Z},
    {"INTENSITY", COLOR_INTENSITY},
    {"Z+INTENSITY", COLOR_ZINTENSITY},
    {"RANGE", COLOR_RANGE}};

/**
 * Visualizer options set through user controls
 **/
struct VisualizerConfig {
    int c_palette;     // cloud color palette
    int point_size;    // cloud point size
    int color_mode;    // cloud coloring mode
    bool cycle_range;  // cycle range palette
    bool parallel;     // use parallel projection

    int palette;       // image color palette
    bool image_noise;  // display noise image
    double intensity_scale;
    double range_scale;
    double noise_scale;

    int fraction_3d;  // percent of window displaying cloud
};

struct LidarScanBuffer {
    std::mutex ls_mtx;
    bool ls_dirty = true;  // false if the 'back' scan is new
    std::unique_ptr<ouster::LidarScan> back;
    std::unique_ptr<ouster::LidarScan> front;
};

/**
 * struct containing everything that is passed from init_viz to run_viz
 **/
struct VizHandle {
    VisualizerConfig config;
    LidarScanBuffer lsb;
    int W;
    int H;
    std::atomic_bool exit;
};

/**
 * Update data being displayed
 **/
void update(viz::VizHandle& vh, std::unique_ptr<ouster::LidarScan>& ls) {
    assert(ls->W * ls->H == vh.W * vh.H);

    std::unique_lock<std::mutex> ls_guard(vh.lsb.ls_mtx);
    ls.swap(vh.lsb.back);
    vh.lsb.ls_dirty = false;
    ls_guard.unlock();
}

/**
 * Applies filter for scaling the intensity scaling factor based on their
 * intensity
 **/
void color_intensity(Eigen::Ref<Eigen::ArrayXd> key_eigen,
                     const VisualizerConfig& config) {
    key_eigen *= config.intensity_scale;
    key_eigen = key_eigen.max(0.0).sqrt();
}

/**
 * Applies filter for scaling the intensity scaling factor based on their range
 **/
void color_range(Eigen::Ref<Eigen::ArrayXd> range,
                 const VisualizerConfig& config) {
    range *= config.range_scale;
    if (config.cycle_range) {
        // range = 0.005 * range + 0.5 * (1.0 - (0.015 * M_PI * range).cos());
        range = 0.5 * (1.0 - (0.018 * M_PI * range).cos());
    } else {
        range = (range * 0.02).min(1.0).max(0.0);
    }
}

void color_noise(Eigen::Ref<Eigen::ArrayXd> key_eigen,
                 const VisualizerConfig& config) {
    double noise_scale = config.image_noise ? config.noise_scale : 0.0;
    key_eigen *= noise_scale * 0.002;
    key_eigen = key_eigen.max(0.0).sqrt();
}

/**
 * Generates a point cloud from a lidar scan, by multiplying each pixel in the
 * lidar scan by a vector pointing radially outward
 **/
void lidar_scan_to_point_cloud(ouster::LidarScan& ls, Points& xyz) {
    const int n = ls.W * ls.H;
    xyz.resize(n, 3);
    xyz.col(0) = ls.x();
    xyz.col(1) = ls.y();
    xyz.col(2) = ls.z();
}

/**
 * Update scalars used to color points
 **/
void update_color_key(const VisualizerConfig& config, const Points& xyz,
                      Eigen::Ref<Eigen::ArrayXd> intensity,
                      Eigen::Ref<Eigen::ArrayXd> range,
                      std::vector<double>& color_key) {
    const int n = xyz.rows();
    color_key.resize(n);
    intensity.resize(n);
    Eigen::Map<Eigen::ArrayXd> key_eigen(color_key.data(), n);

    switch (color_modes[config.color_mode].second) {
        case COLOR_Z:
            key_eigen = ((1.5 + xyz.col(2)) * 0.1).abs().sqrt();
            break;
        case COLOR_INTENSITY:
            key_eigen = Eigen::Map<const Eigen::ArrayXd>(intensity.data(), n);
            color_intensity(key_eigen, config);
            break;
        case COLOR_ZINTENSITY:
            key_eigen = Eigen::Map<const Eigen::ArrayXd>(intensity.data(), n);
            color_intensity(key_eigen, config);
            key_eigen += ((1.5 + xyz.col(2)) * 0.05).abs().sqrt();
            break;
        case COLOR_RANGE:
            key_eigen = Eigen::Map<const Eigen::ArrayXd>(range.data(), n);
            color_range(key_eigen, config);
            break;
        default:
            cerr << "Invalid render mode" << endl;
    }
}

/**
 * Inserts lidar scan into frame so that it can be rendered
 **/
void update_images(
    ouster::LidarScan& ls,
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& arr,
    const std::vector<int>& px_offset, const VisualizerConfig& config) {
    using MapXXd = Eigen::Map<Eigen::ArrayXXd>;
    using MapXXdr = Eigen::Map<
        Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>;

    MapXXd r{ls.range().data(), ls.H, ls.W};
    MapXXd i{ls.intensity().data(), ls.H, ls.W};
    MapXXd n{ls.noise().data(), ls.H, ls.W};

    MapXXdr dst{arr.data(), 3 * ls.H, ls.W};

    // de-stagger and covert to row-major
    for (int u = 0; u < ls.H; u++) {
        const int ofs = px_offset[u];
        dst.row(1 * ls.H - u - 1) << r.row(u).tail(ls.W - ofs),
            r.row(u).head(ofs);
        dst.row(2 * ls.H - u - 1) << i.row(u).tail(ls.W - ofs),
            i.row(u).head(ofs);
        dst.row(3 * ls.H - u - 1) << n.row(u).tail(ls.W - ofs),
            n.row(u).head(ofs);
    }

    const int N = ls.W * ls.H;
    color_range(Eigen::Map<Eigen::ArrayXd>{arr.data(), N}, config);
    color_intensity(Eigen::Map<Eigen::ArrayXd>{arr.data() + N, N}, config);
    color_noise(Eigen::Map<Eigen::ArrayXd>{arr.data() + 2 * N, N}, config);
};

class KeyPressInteractorStyle : public vtkInteractorStyleTrackballCamera {
   public:
    static KeyPressInteractorStyle* New();
    vtkTypeMacro(KeyPressInteractorStyle, vtkInteractorStyleTrackballCamera);

    // suppress existing key bindings on char
    void OnChar() override {}

    void OnKeyPress() override {
        std::string key = this->Interactor->GetKeySym();

        if (key == "p") {
            std::cout << "Point size decreased" << std::endl;
            if (vh->config.point_size > 1) vh->config.point_size--;

        } else if (key == "o") {
            std::cout << "Point size increased" << std::endl;
            if (vh->config.point_size < 100) vh->config.point_size++;

        } else if (key == "m") {
            vh->config.color_mode =
                (vh->config.color_mode + 1) % color_modes.size();
            std::cout << "Cycling point cloud color mode: "
                      << color_modes[vh->config.color_mode].first << std::endl;

        } else if (key == "c") {
            vh->config.palette = (vh->config.palette + 1) % palettes.size();
            std::cout << "Cycling image color palette: "
                      << palettes[vh->config.palette].first << endl;

        } else if (key == "C") {
            vh->config.c_palette = (vh->config.c_palette + 1) % palettes.size();
            std::cout << "Cycling point cloud color palette: "
                      << palettes[vh->config.c_palette].first << std::endl;

        } else if (key == "v") {
            vh->config.cycle_range = !vh->config.cycle_range;
            std::cout << "Cycle range: "
                      << (vh->config.cycle_range ? "On" : "Off") << std::endl;

        } else if (key == "n") {
            vh->config.image_noise = !vh->config.image_noise;
            std::cout << "Noise image: "
                      << (vh->config.image_noise ? "On" : "Off") << std::endl;

        } else if (key == "0") {
            vh->config.parallel = !vh->config.parallel;
            std::cout << "Parallel projection: "
                      << (vh->config.parallel ? "On" : "Off") << std::endl;

        } else if (key == "d") {
            vh->config.fraction_3d += 5;
            if (vh->config.fraction_3d > 100) vh->config.fraction_3d = 0;
            std::cout << "Cloud pane: " << vh->config.fraction_3d << "%"
                      << std::endl;

        } else if (key == "r") {
            std::cout << "Camera reset" << std::endl;
            renderer->ResetCamera();
            renderer->ResetCameraClippingRange();
            camera->SetFocalPoint(0, 0, 0);

        } else {
            return;
        }

        // recognized a key, notify via callback
        config_updated();
    }

    VizHandle* vh;
    vtkSmartPointer<vtkCamera> camera;
    vtkSmartPointer<vtkRenderer> renderer;
    std::function<void()> config_updated = [] {};
};
vtkStandardNewMacro(KeyPressInteractorStyle);

vtkSmartPointer<vtkActor> init_cloud_actor(int n_points, Points& cloud,
                                           std::vector<double>& color_key) {
    cloud.resize(n_points, 3);
    color_key.resize(n_points);

    auto points = vtkSmartPointer<vtkPoints>::New();
    points->SetDataTypeToDouble();
    vtkDoubleArray::SafeDownCast(points->GetData())
        ->SetArray(cloud.data(), n_points * 3, 1);

    auto color = vtkSmartPointer<vtkDoubleArray>::New();
    color->SetName("DepthArray");
    color->SetArray(color_key.data(), n_points, 1);

    auto vtk_cloud = vtkSmartPointer<vtkPolyData>::New();
    vtk_cloud->SetPoints(points);
    vtk_cloud->GetPointData()->SetScalars(color);
    vtk_cloud->GetPointData()->SetActiveScalars("DepthArray");

    auto vfilter = vtkSmartPointer<vtkVertexGlyphFilter>::New();
    vfilter->AddInputData(vtk_cloud);
    vfilter->Update();

    auto mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
    mapper->SetInputConnection(vfilter->GetOutputPort());
    mapper->SetColorModeToDefault();
    mapper->SetScalarRange(0.0, 1.0);

    auto actor = vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);

    return actor;
}

void fill_viewport(vtkSmartPointer<vtkRenderer> renderer,
                   vtkSmartPointer<vtkImageData> image) {
    auto camera = renderer->GetActiveCamera();

    double* origin = image->GetOrigin();
    double* spacing = image->GetSpacing();
    int* extent = image->GetExtent();

    double xc = origin[0] + 0.5 * (extent[0] + extent[1]) * spacing[0];
    double yc = origin[1] + 0.5 * (extent[2] + extent[3]) * spacing[1];
    // double s = (extent[1] - extent[0] + 1) * spacing[0] * 0.5;
    double s = (extent[3] - extent[2] + 1) * spacing[1] * 0.5;
    double d = camera->GetDistance();

    camera->ParallelProjectionOn();
    camera->SetParallelScale(s);
    camera->SetFocalPoint(xc, yc, 0.0);
    camera->SetPosition(xc, yc, d);
    camera->SetViewUp(0, 1, 0);
}

/**
 * Run visualizer rendering loop
 **/
void run_viz(VizHandle& vh) {
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
        image_data{3 * vh.H, vh.W};
    Points pc_render{vh.H * vh.W, 3};
    std::vector<double> color_key{};
    color_key.resize(vh.H, vh.W);

    std::vector<int> px_offset = OS1::get_px_offset(vh.W);

    auto cloud_actor = init_cloud_actor(vh.W * vh.H, pc_render, color_key);

    auto image = vtkSmartPointer<vtkImageData>::New();
    image->SetDimensions(vh.W, 3 * vh.H, 1);
    image->AllocateScalars(VTK_DOUBLE, 1);
    vtkDoubleArray::SafeDownCast(image->GetPointData()->GetScalars())
        ->SetArray(image_data.data(), 3 * vh.H * vh.W, 1);

    auto image_color = vtkSmartPointer<vtkImageMapToColors>::New();
    image_color->SetLookupTable(palettes[vh.config.palette].second);
    image_color->SetInputData(image);

    // adjust aspect ratio
    auto image_change = vtkSmartPointer<vtkImageChangeInformation>::New();
    image_change->SetInputConnection(image_color->GetOutputPort());
    image_change->SetOutputSpacing(vh.H / 33.0, vh.W / 360.0, 1.0);
    image_change->Update();

    auto image_actor = vtkSmartPointer<vtkImageActor>::New();
    image_actor->GetMapper()->SetInputConnection(image_change->GetOutputPort());
    image_actor->GetProperty()->SetInterpolationTypeToNearest();

    auto renderer_2d = vtkSmartPointer<vtkRenderer>::New();
    renderer_2d->AddActor2D(image_actor);
    renderer_2d->InteractiveOff();

    auto renderer = vtkSmartPointer<vtkRenderer>::New();
    renderer->SetBackground(0, 0, 0);
    renderer->AddActor(cloud_actor);

    auto camera = renderer->GetActiveCamera();
    camera->SetPosition(0, 0, 150);
    camera->SetViewUp(1, 0, 0);

    auto render_window = vtkSmartPointer<vtkRenderWindow>::New();
    render_window->SetSize(1280, 720);
    render_window->AddRenderer(renderer);
    render_window->AddRenderer(renderer_2d);

    // fit image to viewport
    fill_viewport(renderer_2d, image_change->GetOutput());

    // vtk callback that calls a std::function in client data
    auto fn_cb = [](vtkObject*, long unsigned int, void* clientData, void*) {
        (*static_cast<std::function<void()>*>(clientData))();
    };

    vtkSmartPointer<vtkRenderWindowInteractor> render_window_interactor;

    // render callback
    std::function<void()> on_render = [&]() {

        // check if we're exiting
        if (vh.exit) render_window_interactor->ExitCallback();

        // swap in new frame
        {
            std::unique_lock<std::mutex> ls_guard(vh.lsb.ls_mtx);
            // already rendered this data
            if (vh.lsb.ls_dirty) return;
            vh.lsb.front.swap(vh.lsb.back);
            vh.lsb.ls_dirty = true;
        }

        // update data backing visualization: pc_render, color_key, image_data
        lidar_scan_to_point_cloud(*vh.lsb.front, pc_render);

        update_color_key(vh.config, pc_render, vh.lsb.front->intensity(),
                         vh.lsb.front->range(), color_key);

        update_images(*vh.lsb.front, image_data, px_offset, vh.config);

        // note: vtk 6 requires the first line, while >6 requires the second
        cloud_actor->GetMapper()->GetInput()->Modified();
        vtkPolyData::SafeDownCast(cloud_actor->GetMapper()->GetInput())
            ->GetPoints()
            ->GetData()
            ->Modified();

        image->Modified();

        render_window->Render();
    };

    auto timer_cb = vtkSmartPointer<vtkCallbackCommand>::New();
    timer_cb->SetCallback(fn_cb);
    timer_cb->SetClientData(&on_render);

    auto style = vtkSmartPointer<KeyPressInteractorStyle>::New();
    style->vh = &vh;
    style->camera = camera;
    style->renderer = renderer;
    style->SetCurrentRenderer(renderer);
    style->config_updated = [&]() {
        image_color->SetLookupTable(palettes[vh.config.palette].second);

        cloud_actor->GetProperty()->SetPointSize(vh.config.point_size);
        cloud_actor->GetMapper()->SetLookupTable(
            palettes[vh.config.c_palette].second);

        renderer_2d->SetViewport(0, vh.config.fraction_3d / 100.0, 1, 1);
        renderer->SetViewport(0, 0, 1, vh.config.fraction_3d / 100.0);

        if (camera->GetParallelProjection() != vh.config.parallel) {
            camera->SetParallelProjection(vh.config.parallel);
            renderer->ResetCamera();
            renderer->ResetCameraClippingRange();
            camera->SetFocalPoint(0, 0, 0);
        }

        render_window->Render();
    };

    render_window_interactor =
        vtkSmartPointer<vtkRenderWindowInteractor>::New();
    render_window_interactor->SetRenderWindow(render_window);
    render_window_interactor->SetInteractorStyle(style);
    render_window_interactor->Initialize();
    render_window_interactor->AddObserver(vtkCommand::TimerEvent, timer_cb);
    render_window_interactor->CreateRepeatingTimer(60);

    style->config_updated();

    render_window_interactor->Start();
}

/**
 * Exit visualizer
 **/
void shutdown(VizHandle& vh) { vh.exit = true; }

/**
 * Initializes the visualizer based on user-given parameters
 **/
std::shared_ptr<VizHandle> init_viz(int W, int H) {
    auto vh = std::make_shared<VizHandle>();

    vh->config.fraction_3d = 70;

    vh->H = H;
    vh->W = W;
    vh->exit = false;

    vh->config.palette = 1;
    vh->config.c_palette = 1;
    vh->config.point_size = 2;

    vh->config.color_mode = 2;
    vh->config.cycle_range = false;
    vh->config.parallel = false;
    vh->config.image_noise = false;
    vh->config.intensity_scale = 0.002;
    vh->config.range_scale = 0.005;
    vh->config.noise_scale = 1.0;

    vh->lsb.back =
        std::unique_ptr<ouster::LidarScan>(new ouster::LidarScan(W, H));

    vh->lsb.front =
        std::unique_ptr<ouster::LidarScan>(new ouster::LidarScan(W, H));

    return vh;
}
}
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_SIMD_META_NATIVE_CARDINAL_HPP_INCLUDED
#define BOOST_SIMD_SDK_SIMD_META_NATIVE_CARDINAL_HPP_INCLUDED

#include <boost/mpl/size_t.hpp>
#include <boost/simd/sdk/simd/extensions.hpp>

namespace boost { namespace simd
{
  template<class T> struct logical;

  namespace meta
  {
    template<class T>
    struct  native_cardinal
          : boost::mpl::size_t< BOOST_SIMD_BYTES / sizeof(T) > {};

    template<class T>
    struct  native_cardinal< logical<T> >
          : boost::mpl::size_t< BOOST_SIMD_BYTES / sizeof(T) > {};
  }
} }

#endif


// ADMM
#include <ADMM.h>
#include <ADMMUtils.h>

// GTSAM
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>

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

#include <fstream>
#include <sstream>

bool isDebug = false;

using namespace std;
using namespace gtsam;

// Main script, takes a g2o file as input and the number of partitions, and does
// 1) (Edge) Partitioning of the graph using metis
// 2) Decentralized optimization using the Alternate Direction Method of Multipliers
int main(int argc, char* argv[])
{
  //////////////////////////////////////////////////////////////////////////////////////
  //Command line arguments
  //////////////////////////////////////////////////////////////////////////////////////
  size_t num_subgraphs; // input dir
  string input_g2o; // g2o file
  string output_dir; // output_dir

  // Optimizer Config
  double rho = 0.1; // penalty parameter in augmented lagrangian
  double mu = 10.0; // parameter used to decide whether to increase or not the rho (adaptive penalty)
  double tau = 2.0; // if we have to increase rho, we do rho <- rho * tau, otherwise rho <- rho / tau
  double min_p_res = 0.1f;
  double min_d_res = 0.1f;
  int maxIter = 1000; // max number of ADMM iterations
  int solver = 0; // GN
  bool isParallel = false; // if true ADMM runs in parallel mode, this is usually less stable
  bool adaptivePenalty = true;
  bool useFlaggedInitialization = false;
  bool computeSubgraphGradient = true;

  // Partitioning
  bool useMetisPartitioning = true;
  bool useLineGraph = true;
  bool orderSubgraphs = true;

  // Verbosity
  int verbosity = 0;

  try{
    // Parse program options
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("help", "Print help messages")
        ("input_g2o,i",po::value<string>(&input_g2o)->required(), "Input g2o file (required)")
        ("num_subgraphs,n", po::value<size_t>(&num_subgraphs)->required(), "Num Subgraphs (required)")
        ("result_dir,r", po::value<string>(&output_dir)->required(), "Results Directory (required)")
        ("rho_val", po::value<double>(&rho), "rho value (default: 0.1)")
        ("mu_val,mu", po::value<double>(&mu), "mu value (default: 10.0)")
        ("tau_val,tau", po::value<double>(&tau), "tau value (default: 2.0)")
        ("max_iter", po::value<int>(&maxIter), "Maximum number of iterations (default: 1000)")
        ("adaptive_penalty,adpt_pen", po::value<bool>(&adaptivePenalty), "To use adaptive penalty or not (default: true)")
        ("use_metis_partitioning", po::value<bool>(&useMetisPartitioning), "To use metis partiioning or not (default: true)")
        ("order_subgraphs", po::value<bool>(&orderSubgraphs), "To order subgraphs or not (default: true)")
        ("use_line_graph", po::value<bool>(&useLineGraph), "To use Line Graph or not (default: true)")
        ("use_flagged_init", po::value<bool>(&useFlaggedInitialization), "To use Flagged Initialization or not (default: true)")
        ("is_parallel", po::value<bool>(&isParallel), "To use parallel multiblock ADMM or not (default: false)")
        ("compute_subgraph_gradient", po::value<bool>(&computeSubgraphGradient), "To compute subgraph gradient  or not (default: true)")
        ("min_p_res", po::value<double>(&min_p_res), "min_p_res value (default: 0.01f)")
        ("min_d_res", po::value<double>(&min_d_res), "min_d_res value (default: 0.01f)")
        ("solver", po::value<int>(&solver), "Specify solver type (default: GN)")
        ("verbosity", po::value<int>(&verbosity), "Specify verbosity level (default: SILENT)");

    po::variables_map vm;
    try{
      po::store(po::parse_command_line(argc, argv, desc), vm); // can throw
      if ( vm.count("help")  ){ // --help option
        cout << "Partition and Run ADMM" << endl << desc << endl;
        return 0;
      }
      po::notify(vm); // throws on error, so do after help in case
    }
    catch(po::error& e){
      cerr << "ERROR: " << e.what() << endl << endl;
      cerr << desc << endl;
      return 1;
    }
  }
  catch(exception& e){
    cerr << "Unhandled Exception reached the top of main: "
         << e.what() << ", application will now exit" << endl;
    return 2;
  }

  // Create the output directory if it does not exist
  if( !(boost::filesystem::exists(output_dir))){
    boost::filesystem::create_directory(output_dir);
  }

  cout << "Running on " << input_g2o << endl;
  //////////////////////////////////////////////////////////////////////////////////////
  // Read centralized graph from g2o file
  //////////////////////////////////////////////////////////////////////////////////////
  GraphAndValues readGraph = readG2o(input_g2o);
  NonlinearFactorGraph graph = *(readGraph.first);
  Values initial = *(readGraph.second);

  // g2o file comes without prior - the following prior must be included after using Metis
  PriorFactor<Pose2> posePrior =
      PriorFactor<Pose2>(0, initial.at<Pose2>(0),
                         noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05,  5*M_PI/180)));

  //////////////////////////////////////////////////////////////////////////////////////
  // (Vertex) Partitioning using metis. This can be useful for GTSAM, but ADMM needs *edge* partitioning
  //////////////////////////////////////////////////////////////////////////////////////
  vector<NonlinearFactorGraph> sub_graphs;
  vector<Values> sub_initials;
  boost::tie(sub_graphs, sub_initials) = partitionGraph(graph,
                                                        initial,
                                                        posePrior,
                                                        num_subgraphs,
                                                        useMetisPartitioning,
                                                        useLineGraph, orderSubgraphs);

  //////////////////////////////////////////////////////////////////////////////////////
  /// Creating Separator vector
  //////////////////////////////////////////////////////////////////////////////////////
  vector<int> separators = createSeparators(sub_initials, initial);

  // Write down the G2o files
  for(size_t sub_id =0; sub_id < sub_initials.size(); sub_id++){
    string filepath = output_dir + "/" + boost::lexical_cast<string>(sub_id) + "_initial.g2o";
    writeG2o(sub_graphs[sub_id], sub_initials[sub_id], filepath);
  }

  // Write down the separators
  string separatorFile = output_dir + "/separators.txt";
  fstream separatorFileStream(separatorFile.c_str(), fstream::out);
  for(size_t i = 0; i < separators.size(); i += 3){
    separatorFileStream << separators[i] <<  "," << separators[i+1] <<  "," << separators[i+2] << endl;
  }
  separatorFileStream.close();

  //////////////////////////////////////////////////////////////////////////////////////
  /// ADMM
  //////////////////////////////////////////////////////////////////////////////////////
  // Construct ADMM
  ADMM admm(rho,
            mu,
            tau,
            maxIter,
            isParallel,
            adaptivePenalty,
            useFlaggedInitialization,
            computeSubgraphGradient,
            min_p_res,
            min_d_res,
            output_dir);

  admm.setSolver(ADMM::Solver(solver));
  admm.setVerbosity(ADMM::Verbosity(verbosity));

  // Load structures
  admm.load(sub_graphs, sub_initials, separators, graph);

  // Run ADMM
  pair<vector<Values>, vector<double> > primalDualResult;
  try{
    primalDualResult = admm.optimize();
  }
  catch (...){
    cout << "Trying with Odometry Partition" << endl;
    boost::tie(sub_graphs, sub_initials) = partitionGraph(graph,
                                                          initial,
                                                          posePrior,
                                                          num_subgraphs,
                                                          useMetisPartitioning,
                                                          useLineGraph, orderSubgraphs);
    vector<int> separators = createSeparators(sub_initials, initial);
    admm.load(sub_graphs, sub_initials, separators, graph);
    primalDualResult = admm.optimize();
  }

  vector<Values> sub_results = primalDualResult.first;

  //////////////////////////////////////////////////////////////////////////////////////
  /// Analysis
  //////////////////////////////////////////////////////////////////////////////////////
  logResults(sub_graphs, sub_results, graph, initial, posePrior, output_dir);

  return 0;
}


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

#define BOOST_CHRONO_HEADER_ONLY

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include "performance.hpp"
#include "table_helper.hpp"
#include <boost/random.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/multiprecision/cpp_int.hpp>

unsigned max_reps = 1000;

template <class T>
struct tester
{
   tester()
   {
      a.assign(500, T());
      for(int i = 0; i < 500; ++i)
      {
         b.push_back(generate_random(false));
         c.push_back(generate_random(false));
         small.push_back(generate_random(true));
      }
   }
   double test_add()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] + c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] - c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_add_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] + 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_subtract_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] - 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned k = 0; k < b.size(); ++k)
            a[k] = b[k] * c[k];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_multiply_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] * 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] / small[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_divide_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = b[i] / 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_gcd()
   {
      using boost::integer::gcd;
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for(unsigned i = 0; i < b.size(); ++i)
            a[i] = gcd(b[i], c[i]);
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }

   double test_inplace_add()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] += c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_subtract()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] -= c[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_add_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] += 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_subtract_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] -= 1;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_multiply()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned k = 0; k < b.size(); ++k)
            b[k] *= c[k];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_multiply_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] *= 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_divide()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            a[i] /= small[i];
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
   double test_inplace_divide_int()
   {
      stopwatch<boost::chrono::high_resolution_clock> w;
      for (unsigned repeats = 0; repeats < max_reps; ++repeats)
      {
         for (unsigned i = 0; i < b.size(); ++i)
            b[i] /= 3;
      }
      return boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count();
   }
private:
   T generate_random(bool issmall)
   {
      boost::uniform_int<> ui(2, issmall ? 5 : 40), ui2(1, 10000);
      std::size_t  len = ui(gen);
      std::vector<typename T::value_type> values;
      for (std::size_t i = 0; i < len; ++i)
      {
         values.push_back(static_cast<typename T::value_type>(ui2(gen)));
      }
      return T(values.begin(), values.end());
   }
   std::vector<T> a, b, c, small;
   static boost::random::mt19937 gen;
};

template <class N>
boost::random::mt19937 tester<N>::gen;

template <class Number>
void test(const char* type)
{
   std::cout << "Testing type: " << type << std::endl;
   tester<boost::math::tools::polynomial<Number> > t;
   int count = 500 * max_reps;
   std::string table_name = "Polynomial Arithmetic (" + compiler_name() + ", " + platform_name() + ")";
   //
   // Now the actual tests:
   //
   report_execution_time(t.test_add() / count, table_name, "operator +", type);
   report_execution_time(t.test_subtract() / count, table_name, "operator -", type);
   report_execution_time(t.test_multiply() / count, table_name, "operator *", type);
   report_execution_time(t.test_divide() / count, table_name, "operator /", type);
   report_execution_time(t.test_add_int() / count, table_name, "operator + (int)", type);
   report_execution_time(t.test_subtract_int() / count, table_name, "operator - (int)", type);
   report_execution_time(t.test_multiply_int() / count, table_name, "operator * (int)", type);
   report_execution_time(t.test_divide_int() / count, table_name, "operator / (int)", type);
   report_execution_time(t.test_inplace_add() / count, table_name, "operator +=", type);
   report_execution_time(t.test_inplace_subtract() / count, table_name, "operator -=", type);
   report_execution_time(t.test_inplace_multiply() / count, table_name, "operator *=", type);
   report_execution_time(t.test_inplace_divide() / count, table_name, "operator /=", type);
   report_execution_time(t.test_inplace_add_int() / count, table_name, "operator += (int)", type);
   report_execution_time(t.test_inplace_subtract_int() / count, table_name, "operator -= (int)", type);
   report_execution_time(t.test_inplace_multiply_int() / count, table_name, "operator *= (int)", type);
   report_execution_time(t.test_inplace_divide_int() / count, table_name, "operator /= (int)", type);
   //report_execution_time(t.test_gcd() / count, table_name, "gcd", type);
}

int main()
{
   test<boost::uint64_t>("boost::uint64_t");
   test<double>("double");
   max_reps = 100;
   test<boost::multiprecision::cpp_int>("cpp_int");
   return 0;
}


// Copyright (c) 2016, Daniel Pfeifer <daniel@pfeifer-mail.de>
//
// Permission to use, copy, modify, and/or distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
// ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
// ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
// OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

#include <semver/version_compare.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <cstring>

namespace semver {

static int vercmp(const char* str1, const char* str2)
{
  int ret = 0;
  const char *end1, *end2;

  while (*str1 && *str2) {
    while (*str1 && !std::isalnum(*str1)) {
      ++str1;
    }
    while (*str2 && !std::isalnum(*str2)) {
      ++str2;
    }

    if (std::isdigit(*str1) || std::isdigit(*str2)) {
      while (*str1 == '0' && std::isdigit(str1[1])) {
        ++str1;
      }
      while (*str2 == '0' && std::isdigit(str2[1])) {
        ++str2;
      }

      end1 = str1;
      end2 = str2;
      while (std::isdigit(*end1)) {
        ++end1;
      }
      while (std::isdigit(*end2)) {
        ++end2;
      }

      ret = (end1 - str1) - (end2 - str2);
      if (ret == 0) {
        ret = std::strncmp(str1, str2, end1 - str1);
      }
      if (ret != 0) {
        return ret > 0 ? 1 : -1;
      }

    } else {

      end1 = str1;
      end2 = str2;
      while (std::isalpha(*end1)) {
        end1++;
      }
      while (std::isalpha(*end2)) {
        end2++;
      }

      ret = (end1 - str1) - (end2 - str2);
      if (ret > 0) {
        ret = std::strncmp(str1, str2, end2 - str2);
        return ret >= 0 ? 1 : -1;
      }
      if (ret < 0) {
        ret = std::strncmp(str1, str2, end1 - str1);
        return ret <= 0 ? -1 : 1;
      }

      ret = std::strncmp(str1, str2, end1 - str1);
      if (ret) {
        return ret > 0 ? 1 : -1;
      }
    }

    str1 = end1;
    str2 = end2;
  }

  return *str1 ? 1 : *str2 ? -1 : 0;
}

bool version_compare(std::string const& a, std::string const& b, Operator op,
                     bool compare_branches)
{
  bool const a_is_branch = boost::starts_with(a, "dev-");
  bool const b_is_branch = boost::starts_with(b, "dev-");

  if (a_is_branch && b_is_branch) {
    return op == Operator::equal && vercmp(a.c_str(), b.c_str()) == 0;
  }

  // when branches are not comparable, we make sure dev branches never match
  // anything
  if (!compare_branches && (a_is_branch || b_is_branch)) {
    return false;
  }

  int const compare = vercmp(a.c_str(), b.c_str());
  switch (op) {
    case Operator::equal:
      return compare == 0;
    case Operator::less_than:
      return compare < 0;
    case Operator::less_equal:
      return compare <= 0;
    case Operator::greater_than:
      return compare > 0;
    case Operator::greater_equal:
      return compare >= 0;
    case Operator::not_equal:
      return compare != 0;
  }
}

} // namespace semver


// -*- mode: c++ -*-
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, JSK Lab
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/o2r other materials provided
 *     with the distribution.
 *   * Neither the name of the JSK Lab nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#define BOOST_PARAMETER_MAX_ARITY 7
#include "jsk_recognition_utils/geo_util.h"
#include "jsk_recognition_utils/pcl_conversion_util.h"
#include <algorithm>
#include <iterator>
#include <cfloat>
#include <pcl/conversions.h>
#include <boost/tuple/tuple_comparison.hpp>

#include <boost/foreach.hpp>
#include <boost/range/irange.hpp>
#include <boost/math/special_functions/round.hpp>
#include <jsk_topic_tools/log_utils.h>
#include <pcl/point_types.h>
#include <pcl/surface/processing.h>
#include "jsk_recognition_utils/sensor_model_utils.h"

// #define DEBUG_GEO_UTIL
namespace jsk_recognition_utils
{
  Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f& ex,
                                  const Eigen::Vector3f& ey,
                                  const Eigen::Vector3f& ez)
  {
    Eigen::Matrix3f rot;
    rot.col(0) = ex.normalized();
    rot.col(1) = ey.normalized();
    rot.col(2) = ez.normalized();
    return Eigen::Quaternionf(rot);
  }

}


// Difference from running average, with multiprocessing.

// Include standard headers.
#include <vector>
#include <sstream>

// Include 3rd party headers.
#include <boost/date_time.hpp>
#include <opencv2/opencv.hpp>
#include <bites.hpp>

// Include application headers.
#include "sherlock.hpp"

// Capture frames for given *duration* number of seconds,
// and push frames and alpha values onto their respective queues.
void capture(
    cv::VideoCapture* cap,
    const int& duration,
    bites::ConcurrentQueue <cv::Mat*>* captures,
    bites::ConcurrentQueue <float>* alphas
    )
{
    // Keep track of previous iteration's timestamp.
    boost::posix_time::ptime tstamp_prev;

    // Run the loop for designated amount of time.
    auto now = boost::posix_time::microsec_clock::universal_time();
    auto dur = boost::posix_time::seconds(duration);
    auto end = now + dur;
    while (end > boost::posix_time::microsec_clock::universal_time())
    {
        // Capture the snapshot.
        auto frame = new cv::Mat;
        *cap >> *frame;

        // Compute alpha value.
        auto alpha = sherlock::getAlpha(tstamp_prev, 1.0);

        // Push the frame and alpha value onto respective queues.
        captures->push(frame);
        alphas->push(alpha);
    }

    // Signal end-of-processing by pushing NULL value onto the queue.
    captures->push(NULL);
}

// Compute Difference from Average of the frames,
// taking into account previously computed alpha values,
// and push resulting diff frames onto the queue.
void diff_average(
    bites::ConcurrentQueue <cv::Mat*>* captures,
    bites::ConcurrentQueue <cv::Mat*>* diffs,
    bites::ConcurrentQueue <float>* alphas
    )
{
    // Monitor framerates for the given seconds past.
    std::vector<float> periods = { 1, 5, 10 };
    bites::RateTicker framerate (periods);

    // Maintain accumulation of differences.
    cv::Mat image_acc;

    // Pull from the queue while there are valid frames.
    cv::Mat* frame;
    captures->wait_and_pop(frame);
    while(frame){

        const auto RTYPE = CV_32FC3;

        // Initalize accumulation if so indicated.
        if (image_acc.empty()){
            image_acc = cv::Mat::zeros(frame->size(), frame->type());
            image_acc.convertTo(image_acc, RTYPE);
        }

        // Compute difference.
        cv::Mat converted;
        frame->convertTo(converted, RTYPE);
        auto diff = new cv::Mat;
        cv::absdiff(
            image_acc,
            converted,
            *diff
            );

        // Retrieve the alpha value.
        float alpha;
        alphas->wait_and_pop(alpha);

        // Accumulate the difference.
        cv::accumulateWeighted(
            converted,
            image_acc,
            alpha
            );

        // Convert the diff image.
        diff->convertTo(*diff, frame->type());

        // Original frame is no longer needed, so deallocate it.
        delete frame;

        // Write the processing framerate on top of the diff image.
        auto fps = framerate.tick();
        std::ostringstream line;
        line << std::fixed << std::setprecision(2);
        line << fps[0] << ", " << fps[1] << ", " << fps[2] << " (processing)";
        std::list<std::string> lines ({ line.str() });
        sherlock::writeOSD(*diff, lines, 0.04);

        // Push diff image onto queue.
        diffs->push(diff);

        // Retrieve the next frame from queue.
        captures->wait_and_pop(frame);
    }

    // Signal end-of-processing by pushing NULL value onto the queue.
    diffs->push(NULL);
}

// Display queued frames.
void display(
    bites::ConcurrentQueue <cv::Mat*>* diffs
    )
{
    // Create the output window.
    const char* title = "diff average 2";
    cv::namedWindow(title, CV_WINDOW_NORMAL);

    // Monitor framerates for the given seconds past.
    std::vector<float> periods = { 1, 5, 10 };
    bites::RateTicker framerate (periods);

    // Pull from the queue while there are valid matrices.
    cv::Mat* frame;
    diffs->wait_and_pop(frame);
    while(frame){

        // Write the display framerate on top of the image.
        auto fps = framerate.tick();
        std::ostringstream line;
        line << std::fixed << std::setprecision(2);
        line << fps[0] << ", " << fps[1] << ", " << fps[2] << " (display)";
        // Show the display framerate *below* the processing framerate
        // by first adding a "carriage return" (empty line) to the list.
        std::list<std::string> lines({ "", line.str() });
        sherlock::writeOSD(*frame, lines, 0.04);

        // Display the snapshot.
        cv::imshow(title, *frame);

        // Allow HighGUI to process event.
        cv::waitKey(1);

        // Deallocate the current image and retrieve the next,
        // while filtering out excess (intermediate) images in the queue.
        // If display hardware is not fast enough,
        // showing intermediate images introduces (incremental) lag.
        // Hence the "lossy filter" by dropping excess frames.
        cv::Mat* prev = frame;
        int count = 0;
        while(diffs->try_pop(frame))
        {
            // Since we popped the next image, we can delete the previous.
            delete prev;
            prev = frame;
            count++;
        }
        // If there weren't any successful try-pops above,
        // then do an absolute wait-and-pop here.
        if(count == 0)
        {
            diffs->wait_and_pop(frame);
            delete prev;
        }
    }
}

// Launch Capture and Diff threads, and display results to output window.
int main(int argc, char** argv)
{
    // Parse command-line arguments.
    int DEVICE, WIDTH, HEIGHT;
    float DURATION;
    std::istringstream(std::string(argv[1])) >> DEVICE;
    std::istringstream(std::string(argv[2])) >> WIDTH;
    std::istringstream(std::string(argv[3])) >> HEIGHT;
    std::istringstream(std::string(argv[4])) >> DURATION;

    // Create the OpenCV video capture object.
    cv::VideoCapture cap(DEVICE);
    cap.set(3, WIDTH);
    cap.set(4, HEIGHT);

    // Create the shared queues.
    bites::ConcurrentQueue <cv::Mat*> captures;
    bites::ConcurrentQueue <float> alphas;
    bites::ConcurrentQueue <cv::Mat*> diffs;

    // Start up the threads.
    std::thread capturer (capture, &cap, DURATION, &captures, &alphas);
    std::thread diff_averager (diff_average, &captures, &diffs, &alphas);
    std::thread displayer (display, &diffs);

    // Join the threads.
    capturer.join();
    diff_averager.join();
    displayer.join();
}


// Copyright (c) Open Enclave SDK contributors.
// Licensed under the MIT License.

#include <openenclave/host.h>
#include <stdio.h>
#include "load_balancer_u.h"

#include <sched.h>
#include <pthread.h>

#include "log.h"

#include <grpcpp/grpcpp.h>
#include <vector>
#include <mutex>
#include <thread>
#include <chrono>
#include <fstream>

#include <boost/math/special_functions/lambert_w.hpp>

#include "../../build/common/oram.grpc.pb.h"
#include "../../common/common.h"
#include "../../common/crypto.h"
#include "../../common/json.hpp"
#include "host.h"

#define LOOP_OPTION "-server-in-loop"

using grpc::Channel;
using grpc::ClientContext;
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::Status;
using oram::OramRequest;
using oram::OramRequests;
using oram::OramResponse;
using oram::OramResponses;
using oram::Oram;

using json = nlohmann::json;

using boost::math::lambert_w0;

using namespace std;
using namespace lb_types;

LBServerImpl *server;
LBBatchDispatcher *batchDispatcher;
vector<ClientReq *> reqList;
mutex reqListLock;
oe_enclave_t *enclave;
int num_suborams;

oe_enclave_t* create_enclave(const char* enclave_path)
{
    oe_enclave_t* enclave = NULL;

    debug_log::info("Host: Enclave library %s\n", enclave_path);
    oe_result_t result = oe_create_load_balancer_enclave(
        enclave_path,
        OE_ENCLAVE_TYPE_SGX,
        OE_ENCLAVE_FLAG_DEBUG,
        NULL,
        0,
        &enclave);

    if (result != OE_OK)
    {
        debug_log::info(
            "Host: oe_create_remoteattestation_enclave failed. %s",
            oe_result_str(result));
    }
    else
    {
        debug_log::info("Host: Enclave successfully created.\n");
    }
    return enclave;
}

void terminate_enclave(oe_enclave_t* enclave)
{
    oe_terminate_enclave(enclave);
    debug_log::info("Host: Enclave successfully terminated.\n");
}

LBServerImpl::LBServerImpl() : client_id_ctr_(0), caller_map_(), sec_param_(128) {}

LBServerImpl::~LBServerImpl() {
    server_->Shutdown();
    cq_->Shutdown();
}

void LBServerImpl::Run(std::string server_addr) {
    ServerBuilder builder;
    builder.AddListeningPort(server_addr, grpc::InsecureServerCredentials());
    builder.RegisterService(&service_);
    cq_ = builder.AddCompletionQueue();
    server_ = builder.BuildAndStart();
    std::cout << "Server listening on " << server_addr << std::endl;
    fflush(stdout);

    HandleRpcs();
}

int LBServerImpl::GetPaddedReqsPerSuboram(int incoming_reqs) {
    double mu = (double) incoming_reqs / (double) num_suborams;
    double alpha = log(((double) num_suborams) * pow(2.0, sec_param_));
    double rhs = alpha / (M_E * mu) - (1.0 / M_E);
    double epsilon = pow(M_E, lambert_w0(rhs) + 1.0) - 1.0;
    double result = mu * (1.0 + epsilon);
    return (int) ceil(result);
    // TODO: actually implement
    /*if (incoming_reqs == 1024)
        return 155;
    else if (incoming_reqs == 2048) {
        return 232;
    } else if (incoming_reqs == 4096) {
        return 366;
    } else if (incoming_reqs == 8192) {
        return 602;
    } else if (incoming_reqs == 16384) {
        return 1032;
    }
    return incoming_reqs;*/
}

void testGetPaddedReqsPerSuboram() {
    LBServerImpl lb;
    int save_num_suborams = num_suborams;
    num_suborams = 25;
    double res = lb.GetPaddedReqsPerSuboram(1024);
    printf("1024: got %f, needed 155\n", res);
    res = lb.GetPaddedReqsPerSuboram(2048);
    printf("2048: got %f, needed 232\n", res);
    res = lb.GetPaddedReqsPerSuboram(4096);
    printf("4096: got %f, needed 366\n", res);
    res = lb.GetPaddedReqsPerSuboram(8192);
    printf("8192: got %f, needed 602\n", res);
    res = lb.GetPaddedReqsPerSuboram(16384);
    printf("16384: got %f, needed 1032\n", res);
    num_suborams = save_num_suborams;
}

void LBServerImpl::ProcessBatch() {
    reqListLock.lock();
    if (reqList.size() == 0) {
        reqListLock.unlock();
        return;
    }
    debug_log::info("Processing batch\n");

    // TODO: actually have enclave process batch
    // Go through and process batch
    // TODO: deal with race condition with reqList
    int num_reqs = reqList.size();
    int pair_len = get_key_val_buf_sz();
    int reqs_per_suboram = GetPaddedReqsPerSuboram(num_reqs);
    if (reqs_per_suboram > num_reqs) {
        reqs_per_suboram = num_reqs;
    }
    int suboram_buf_len = get_key_val_buf_sz(reqs_per_suboram);
    int total_out_reqs = reqs_per_suboram * num_suborams;
    uint8_t **in_ct_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint8_t **in_iv_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint8_t **in_tag_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint8_t **out_ct_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **out_iv_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **out_tag_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **resp_ct_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **resp_iv_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **resp_tag_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
    uint8_t **client_ct_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint8_t **client_iv_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint8_t **client_tag_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
    uint32_t *in_client_id_arr = (uint32_t *)malloc(num_reqs * sizeof(uint32_t));
    set<uint32_t> client_id_set;

    debug_log::info("Batch: %d real_requests, %d total_requests, %d reqs_per_suboram\n", num_reqs, reqs_per_suboram*num_suborams, reqs_per_suboram);

    for (int i = 0; i < num_reqs; i++) {
        /*in_ct_arr[i] = (uint8_t *)malloc(pair_len);
        memcpy(in_ct_arr[i], reqList[i]->ct_, pair_len);
        in_iv_arr[i] = (uint8_t *)malloc(IV_LEN);
        memcpy(in_iv_arr[i], reqList[i]->iv_, IV_LEN);
        in_tag_arr[i] = (uint8_t *)malloc(TAG_LEN);
        memcpy(in_tag_arr[i], reqList[i]->tag_, TAG_LEN);*/
        in_ct_arr[i] = reqList[i]->ct_;
        in_iv_arr[i] = reqList[i]->iv_;
        in_tag_arr[i] = reqList[i]->tag_;
        in_client_id_arr[i] = reqList[i]->caller_->GetClientID();
        client_ct_arr[i] = (uint8_t *)malloc(pair_len);
        client_iv_arr[i] = (uint8_t *)malloc(IV_LEN);
        client_tag_arr[i] = (uint8_t *)malloc(TAG_LEN);
        client_id_set.insert(reqList[i]->caller_->GetClientID());
    }

    for (int i = 0; i < num_suborams; i++) {
        out_ct_arr[i] = (uint8_t *)malloc(suboram_buf_len);
        out_iv_arr[i] = (uint8_t *)malloc(IV_LEN);
        out_tag_arr[i] = (uint8_t *)malloc(TAG_LEN);
        resp_ct_arr[i] = (uint8_t *)malloc(suboram_buf_len);
        resp_iv_arr[i] = (uint8_t *)malloc(IV_LEN);
        resp_tag_arr[i] = (uint8_t *)malloc(TAG_LEN);
    }
    reqListLock.unlock();

    //debug_log::info("Going to create batch in enclave\n");
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    create_batch(enclave,in_ct_arr, in_iv_arr, in_tag_arr, in_client_id_arr, out_ct_arr,
        out_iv_arr, out_tag_arr, num_reqs, reqs_per_suboram
    );

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

    //debug_log::info("Sending requests to suborams\n");
    batchDispatcher->SendAllBatches(out_ct_arr, out_iv_arr, out_tag_arr, resp_ct_arr, resp_iv_arr, resp_tag_arr, reqs_per_suboram);

    chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
    //debug_log::info("Got responses from suborams, going to match them\n");
    match_responses_to_reqs(enclave, resp_ct_arr, resp_iv_arr, resp_tag_arr, in_client_id_arr, client_ct_arr, client_iv_arr, client_tag_arr);

    chrono::steady_clock::time_point t4 = chrono::steady_clock::now();
    cout << "total: " << std::chrono::duration_cast<std::chrono::microseconds>(t4-t1).count() << endl;
    cout << "\t create_batch: " << std::chrono::duration_cast<std::chrono::microseconds>(t2-t1).count() << endl;
    cout << "\t send_all_batches: " << std::chrono::duration_cast<std::chrono::microseconds>(t3-t2).count() << endl;
    cout << "\t match_responses: " << std::chrono::duration_cast<std::chrono::microseconds>(t4-t3).count() << endl;
    //debug_log::info("Matched responses from suborams\n");
    reqListLock.lock();
    caller_map_lock_.lock();
    for (int i = 0; i < num_reqs; i++) {
        caller_map_[in_client_id_arr[i]]->EnqueueResponse(client_ct_arr[i], client_iv_arr[i], client_tag_arr[i]);
        //caller_map_.erase(in_client_id_arr[i]);
    }
    set<uint32_t>::iterator it = client_id_set.begin();
    while (it != client_id_set.end()) {
        caller_map_[*it]->Respond();
        caller_map_.erase(*it);
        it++;
    }
    caller_map_lock_.unlock();
    reqList.erase(reqList.begin(), reqList.begin() + num_reqs);
    reqListLock.unlock();

    for (int i = 0; i < num_reqs; i++) {
        free(client_ct_arr[i]);
        free(client_iv_arr[i]);
        free(client_tag_arr[i]);
    }

    for (int i = 0; i < num_suborams; i++) {
        free(out_ct_arr[i]);
        free(out_iv_arr[i]);
        free(out_tag_arr[i]);
        free(resp_ct_arr[i]);
        free(resp_iv_arr[i]);
        free(resp_tag_arr[i]);
    }

    free(in_ct_arr);
    free(in_iv_arr);
    free(in_tag_arr);
    free(out_ct_arr);
    free(out_iv_arr);
    free(out_tag_arr);
    free(resp_ct_arr);
    free(resp_iv_arr);
    free(resp_tag_arr);
    free(client_ct_arr);
    free(client_iv_arr);
    free(client_tag_arr);
    free(in_client_id_arr);
}

uint32_t LBServerImpl::GetNextClientID() {
    client_id_ctr_lock_.lock();
    uint32_t next = client_id_ctr_++;
    client_id_ctr_lock_.unlock();
    return next;
}

void LBServerImpl::AddToCallerMap(uint32_t client_id, LBServerImpl::CallData *call_data) {
    caller_map_lock_.lock();
    caller_map_[client_id] = call_data;
    caller_map_lock_.unlock();
}

ClientReq::ClientReq(uint8_t *ct, uint8_t *iv, uint8_t *tag, LBServerImpl::CallData *caller)
    : caller_(caller) {
    ct_ = ct;
    iv_ = iv;
    tag_ = tag;
}

ClientReq::~ClientReq() {
    free(ct_);
    free(iv_);
    free(tag_);
}

LBServerImpl::CallData::CallData(Oram::AsyncService *service, ServerCompletionQueue *cq)
            : service_(service), cq_(cq), responder_(&ctx_), status_(CREATE) {
    Proceed();
}

uint32_t LBServerImpl::CallData::GetClientID() {
    return client_id_;
}

void LBServerImpl::CallData::Proceed() {
    if (status_ == CREATE) {
        status_ = PROCESS;
        service_->RequestAccessKey(&ctx_, &request_, &responder_, cq_, cq_,
                this);
    } else if (status_ == PROCESS) {
        //uint32_t new_client_id = server->GetNextClientID();
        server->AddToCallerMap(request_.client_id(), this);
        client_id_ = request_.client_id();
        new CallData(service_, cq_);
        // TODO: actual processing
        reqListLock.lock();
        for (int i = 0; i < request_.ct_size(); i++) {
            uint8_t *ct = (uint8_t *)malloc(get_key_val_buf_sz());
            uint8_t *iv = (uint8_t *)malloc(IV_LEN);
            uint8_t *tag = (uint8_t *)malloc(TAG_LEN);;
            memcpy(ct, request_.ct(i).c_str(), get_key_val_buf_sz());
            memcpy(iv, request_.iv(i).c_str(), IV_LEN);
            memcpy(tag, request_.tag(i).c_str(), TAG_LEN);
            ClientReq *creq = new ClientReq(ct, iv, tag, this);
            // TODO: need to create multiple client reqs
            reqList.push_back(creq);
        }
        reqListLock.unlock();
        lock_.lock();
        //reply_.set_key(request_.key());
        //reply_.set_data(request_.new_data());
        //status_ = FINISH;
        //responder_.Finish(reply_, Status::OK, this);
    } else {
        GPR_ASSERT(status_ == FINISH);
        delete this;
    }
}

void LBServerImpl::CallData::EnqueueResponse(uint8_t *ct, uint8_t *iv, uint8_t *tag) {
    reply_.add_ct(ct, get_key_val_buf_sz());
    reply_.add_iv(iv, IV_LEN);
    reply_.add_tag(tag, TAG_LEN);
}

void LBServerImpl::CallData::Respond() {
//void LBServerImpl::CallData::Respond(uint8_t *ct, uint8_t *iv, uint8_t *tag) {
    /*reply_.set_ct(ct, get_key_val_buf_sz());
    reply_.set_iv(iv, IV_LEN);
    reply_.set_tag(tag, TAG_LEN);*/
    // TODO: need to set repeated fields appropriately
    status_ = FINISH;
    responder_.Finish(reply_, Status::OK, this);
    lock_.unlock();
}

// can be run in multiple threads
void LBServerImpl::HandleRpcs() {
    new CallData(&service_, cq_.get());
    void *tag;
    bool ok;
    while (true) {
        GPR_ASSERT(cq_->Next(&tag, &ok));
        GPR_ASSERT(ok);
        static_cast<CallData*>(tag)->Proceed();
    }
}

LBDispatcher::LBDispatcher(shared_ptr<Channel> channel, uint32_t id)
    : stub_(Oram::NewStub(channel)), id_(id) {}

int LBDispatcher::SendBatch(uint8_t *in_ct, uint8_t *in_iv, uint8_t *in_tag, uint8_t *out_ct, uint8_t *out_iv, uint8_t *out_tag, int len, bool doCopy) {
    OramRequests reqs;
    OramResponses resps;
    ClientContext ctx;
    Status status;

    reqs.set_ct(in_ct, get_key_val_buf_sz(len));
    reqs.set_iv(in_iv, IV_LEN);
    reqs.set_tag(in_tag, TAG_LEN);
    reqs.set_len(len);
    reqs.set_balancer_id(id_);

    status = stub_->BatchedAccessKeys(&ctx, reqs, &resps);
    if (status.ok()) {
        if (doCopy) {
            memcpy(out_ct, resps.ct().c_str(), get_key_val_buf_sz(len));
            memcpy(out_iv, resps.iv().c_str(), IV_LEN);
            memcpy(out_tag, resps.tag().c_str(), TAG_LEN);
        }
        return OKAY;
    } else {
        debug_log::error("ERROR receiving responses from suboram\n");
    }
    return ERROR;
}

LBBatchDispatcher::LBBatchDispatcher(vector<vector<string>> suboramIPs, uint32_t id) {
    for (int i = 0; i < suboramIPs.size(); i++) {
        dispatchers.push_back(vector<LBDispatcher*>(suboramIPs[i].size()));
        for (int j = 0; j < suboramIPs[i].size(); j++) {
            dispatchers[i][j] = new LBDispatcher(grpc::CreateChannel(
                           suboramIPs[i][j], grpc::InsecureChannelCredentials()), id);
        }
    }
}

int LBBatchDispatcher::SendAllBatches(uint8_t **in_ct_arr, uint8_t **in_iv_arr, uint8_t **in_tag_arr, uint8_t **out_ct_arr, uint8_t **out_iv_arr, uint8_t **out_tag_arr, int batch_sz) {
    vector<thread*> threads;
    for (int i = 0; i < dispatchers.size(); i++) {
        for (int j = 0; j < dispatchers[i].size(); j++) {
            thread *t = new thread(&LBDispatcher::SendBatch, dispatchers[i][j],
                in_ct_arr[i], in_iv_arr[i], in_tag_arr[i],
                out_ct_arr[i], out_iv_arr[i], out_tag_arr[i],
                batch_sz, j == 0);
            threads.push_back(t);
        }

    }
    int len = threads.size();
    for (int i = 0; i < len; i++) {
        threads[i]->join();
    }
}

void batch_timer(int epoch_time) {
    struct sched_param param;
    param.sched_priority = 99;  // max
    //pthread_setschedparam(pthread_self(), SCHED_RR, &param);
    while(true) {
        chrono::steady_clock::time_point tStart = chrono::steady_clock::now();
        server->ProcessBatch();
        chrono::steady_clock::time_point tEnd = chrono::steady_clock::now();
        int msElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tEnd - tStart).count();
        if (epoch_time - msElapsed > 0) {
            this_thread::sleep_for(chrono::milliseconds(epoch_time - msElapsed));
        }
        server->ProcessBatch();
    }
}

int main(int argc, const char* argv[])
{
    debug_log::set_name("lb");
    oe_result_t result = OE_OK;
    int ret = 1;
    char* server_port = NULL;
    char* suboram_port = NULL;
    char* suboram_name = NULL; // TODO: SET THIS
    bool keep_server_up = false;
    //num_suborams = 1;
    //num_suborams = 25;
    //vector<string> suboramIPs {"127.0.0.1:12346"};
    ifstream config_stream(argv[2]);
    json config;
    config_stream >> config;
    //string config_filename(argv[2]);
    //Json::Value config = read_config(config_filename);
    vector<vector<string>> suboramIPs;
    int balancer_id = config[BALANCER_ID];
    for (int i = 0; i < config[SUBORAM_ADDRS].size(); i++) {
        suboramIPs.push_back(vector<string>(config[SUBORAM_ADDRS][i].size()));
        for (int j = 0; j < config[SUBORAM_ADDRS][i].size(); j++) {
            suboramIPs[i][j] = config[SUBORAM_ADDRS][i][j];
        }
    }
    num_suborams = suboramIPs.size();

    server = new LBServerImpl();

    thread t(batch_timer, config[EPOCH_MS]);

    debug_log::info("Host: Creating an tls client enclave\n");
    enclave = create_enclave(argv[1]);
    std::vector<std::thread> enclave_threads;
    if (enclave == NULL)
    {
        goto exit;
    }

    batchDispatcher = new LBBatchDispatcher(suboramIPs, balancer_id);
    init_load_balancer(enclave, suboramIPs.size(), config[NUM_BLOCKS], config[THREADS]);
    for (int i = 1; i < config[THREADS]; i++) {
        enclave_threads.push_back(spawn_enclave_thread_on_cpu(i, start_worker_loop, enclave, i));
    }
    debug_log::info("starting server\n");
    if (config[MODE] == SERVER) {
        server->Run("0.0.0.0:" + string(config[LISTENING_PORT]));
    } else if (config[MODE] == BENCH_MAKE_BATCH) {
        server->BenchCreateBatch(true);
    } else if (config[MODE] == BENCH_MATCH_RESPS) {
        server->BenchCreateBatch(false);
    }

    //t.join();

exit:

    debug_log::info("Host: Terminating enclaves\n");
    if (enclave)
        stop_workers(enclave);
        for (auto &et : enclave_threads) {
            et.join();
        }
        terminate_enclave(enclave);

    debug_log::info("Host:  %s \n", (ret == 0) ? "succeeded" : "failed");
    return ret;
}
// If want to be able to run this, need to pass ciphertexts to enclave not raw key-val
// pairs
void LBServerImpl::BenchCreateBatch(bool makeBatch) {
    uint32_t replay_counter = 1;

    unsigned char *comm_key = (unsigned char *)"01234567891234567891234567891234";
    ofstream make_batch_file("micro_balancer_make_batch_" + std::to_string(num_suborams) + ".dat");
    ofstream match_resps_file("micro_balancer_match_resps_" + std::to_string(num_suborams) + ".dat");

    for (int i = 6; i < 15; i++) {
        int num_reqs = 1 << i;

        int pair_len = get_key_val_buf_sz();
        int reqs_per_suboram = GetPaddedReqsPerSuboram(num_reqs);
        int suboram_buf_len = get_key_val_buf_sz(reqs_per_suboram);
        int total_out_reqs = reqs_per_suboram * num_suborams;

        uint8_t **in_ct_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint8_t **in_iv_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint8_t **in_tag_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint8_t **out_ct_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **out_iv_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **out_tag_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **resp_ct_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **resp_iv_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **resp_tag_arr = (uint8_t **)malloc(num_suborams * sizeof(uint8_t *));
        uint8_t **client_ct_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint8_t **client_iv_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint8_t **client_tag_arr = (uint8_t **)malloc(num_reqs * sizeof(uint8_t *));
        uint32_t *in_client_id_arr = (uint32_t *)malloc(num_reqs * sizeof(uint32_t));
        set<uint32_t> client_id_set;

        for (int j = 0; j < num_reqs; j++) {
            in_ct_arr[j] = (uint8_t *)malloc(pair_len);
            in_iv_arr[j] = (uint8_t *)malloc(IV_LEN);
            in_tag_arr[j] = (uint8_t *)malloc(TAG_LEN);
            encrypt_read_key(comm_key, in_ct_arr[j], in_iv_arr[j], in_tag_arr[j], 1, &replay_counter, false);
            in_client_id_arr[j] = 1;
            client_ct_arr[j] = (uint8_t *)malloc(pair_len);
            client_iv_arr[j] = (uint8_t *)malloc(IV_LEN);
            client_tag_arr[j] = (uint8_t *)malloc(TAG_LEN);
            client_id_set.insert(j);
        }

        for (int j = 0; j < num_suborams; j++) {
            out_ct_arr[j] = (uint8_t *)malloc(suboram_buf_len);
            out_iv_arr[j] = (uint8_t *)malloc(IV_LEN);
            out_tag_arr[j] = (uint8_t *)malloc(TAG_LEN);
            resp_ct_arr[j] = (uint8_t *)malloc(suboram_buf_len);
            resp_iv_arr[j] = (uint8_t *)malloc(IV_LEN);
            resp_tag_arr[j] = (uint8_t *)malloc(TAG_LEN);
            uint8_t **data_arr = (uint8_t **)malloc(reqs_per_suboram * sizeof(uint8_t *));
            uint32_t *key_arr = (uint32_t *)malloc(reqs_per_suboram * sizeof(uint32_t));
            for (int k = 0; k < reqs_per_suboram; k++) {
                data_arr[k] = (uint8_t *)malloc(BLOCK_LEN);
                key_arr[k] = 1;
            }
            encrypt_key_val_pairs(comm_key, resp_ct_arr[j], resp_iv_arr[j], resp_tag_arr[j], key_arr, data_arr, reqs_per_suboram, &replay_counter, false);
            for (int k = 0; k < reqs_per_suboram; k++) {
                free(data_arr[k]);
            }
            free(data_arr);
            free(key_arr);
        }

        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

        create_batch(enclave,
            in_ct_arr, in_iv_arr, in_tag_arr, in_client_id_arr, out_ct_arr,
            out_iv_arr, out_tag_arr, num_reqs, reqs_per_suboram
        );

        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

        match_responses_to_reqs(enclave, resp_ct_arr, resp_iv_arr, resp_tag_arr, in_client_id_arr, client_ct_arr, client_iv_arr, client_tag_arr);

        chrono::steady_clock::time_point t3 = chrono::steady_clock::now();

        replay_counter++;

        for (int j = 0; j < num_reqs; j++) {
            free(in_ct_arr[j]);
            free(in_iv_arr[j]);
            free(in_tag_arr[j]);
            free(client_ct_arr[j]);
            free(client_iv_arr[j]);
            free(client_tag_arr[j]);
        }
        for (int j = 0; j < num_suborams; j++) {
            free(out_ct_arr[j]);
            free(out_iv_arr[j]);
            free(out_tag_arr[j]);
            free(resp_ct_arr[j]);
            free(resp_iv_arr[j]);
            free(resp_tag_arr[j]);
        }
        free(in_ct_arr);
        free(in_iv_arr);
        free(in_tag_arr);
        free(client_ct_arr);
        free(client_iv_arr);
        free(client_tag_arr);
        free(in_client_id_arr);
        free(out_ct_arr);
        free(out_iv_arr);
        free(out_tag_arr);
        free(resp_ct_arr);
        free(resp_iv_arr);
        free(resp_tag_arr);

        if (makeBatch) {
            cout << num_suborams << " " << num_reqs << " " << std::chrono::duration_cast<std::chrono::microseconds>(t2-t1).count() << endl;
        } else {
            cout << num_suborams << " " << num_reqs << " " << std::chrono::duration_cast<std::chrono::microseconds>(t3-t2).count() << endl;
        }
        /*
        printf("Time to create batch for %d blocks: %f us\n", num_reqs, ((double)std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count()));
        make_batch_file << num_suborams << " " << num_reqs << " " << std::chrono::duration_cast<std::chrono::microseconds>(t2-t1).count() << endl;
        printf("Time to match up requests for %d blocks: %f us\n", num_reqs, ((double)std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2).count()));
        match_resps_file << num_suborams << " " << num_reqs << " " << std::chrono::duration_cast<std::chrono::microseconds>(t3-t2).count() << endl;*/
    }
    make_batch_file.close();
    match_resps_file.close();
}


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

#include "amount.h"
#include "policy/feerate.h"
#include "pos/rewards.h"
#include "test/test_ion.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(amount_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(MoneyRangeTest)
{
    BOOST_CHECK_EQUAL(MoneyRange(CAmount(-1)), false);
    BOOST_CHECK_EQUAL(MoneyRange(MAX_MONEY + CAmount(1)), false);
    BOOST_CHECK_EQUAL(MoneyRange(CAmount(1)), true);
}

BOOST_AUTO_TEST_CASE(GetFeeTest)
{
    CFeeRate feeRate, altFeeRate;

    feeRate = CFeeRate(0);
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), 0);

    feeRate = CFeeRate(1000);
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 121);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 999);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 1e3);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 9e3);

    feeRate = CFeeRate(-1000);
    // Must always just return -1 * arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), -1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), -121);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), -999);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), -1e3);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), -9e3);

    feeRate = CFeeRate(123);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), 1); // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 14);
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), 15);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 122);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 123);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 1107);

    feeRate = CFeeRate(-123);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), -1); // Special case: returns -1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), -1);

    // check alternate constructor
    feeRate = CFeeRate(1000);
    altFeeRate = CFeeRate(feeRate);
    BOOST_CHECK_EQUAL(feeRate.GetFee(100), altFeeRate.GetFee(100));

    // Check full constructor
    // default value
    BOOST_CHECK(CFeeRate(CAmount(-1), 1000) == CFeeRate(-1));
    BOOST_CHECK(CFeeRate(CAmount(0), 1000) == CFeeRate(0));
    BOOST_CHECK(CFeeRate(CAmount(1), 1000) == CFeeRate(1));
    // lost precision (can only resolve satoshis per kB)
    BOOST_CHECK(CFeeRate(CAmount(1), 1001) == CFeeRate(0));
    BOOST_CHECK(CFeeRate(CAmount(2), 1001) == CFeeRate(1));
    // some more integer checks
    BOOST_CHECK(CFeeRate(CAmount(26), 789) == CFeeRate(32));
    BOOST_CHECK(CFeeRate(CAmount(27), 789) == CFeeRate(34));
    // Maximum size in bytes, should not crash
    CFeeRate(MAX_MONEY, std::numeric_limits<size_t>::max() >> 1).GetFeePerK();
}

BOOST_AUTO_TEST_CASE(BinaryOperatorTest)
{
    CFeeRate a, b;
    a = CFeeRate(1);
    b = CFeeRate(2);
    BOOST_CHECK(a < b);
    BOOST_CHECK(b > a);
    BOOST_CHECK(a == a);
    BOOST_CHECK(a <= b);
    BOOST_CHECK(a <= a);
    BOOST_CHECK(b >= a);
    BOOST_CHECK(b >= b);
    // a should be 0.00000002 ION/kB now
    a += a;
    BOOST_CHECK(a == b);
}

BOOST_AUTO_TEST_CASE(ToStringTest)
{
    CFeeRate feeRate;
    feeRate = CFeeRate(1);
    BOOST_CHECK_EQUAL(feeRate.ToString(), "0.00000001 ION/kB");
}

BOOST_AUTO_TEST_CASE(RewardTest)
{
    uint256 grpData1;
    grpData1.SetHex("0006f0bfe02cb5baf8517503f3cc274f04f755837b00814574d33e9eb0f3b65a");
    CTokenGroupID tgID1(grpData1);

    uint256 grpData2;
    grpData2.SetHex("00bc4fd09e023474f0acdfe75cc2a3323899339d88e5eeef4b1fd64ec6ae9d6c");
    CTokenGroupID tgID2(grpData2);

    CReward reward1(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 200);

    CReward reward2a(CReward::RewardType::REWARD_COINBASE, 0.01 * COIN, tgID1, 200);
    CReward reward2b(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 200);
    CReward reward2c(CReward::RewardType::REWARD_COINBASE, 0.03 * COIN, tgID1, 200);

    CReward reward3a(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 100);
    CReward reward3b(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 200);
    CReward reward3c(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 300);

    CReward reward4a(CReward::RewardType::REWARD_COINBASE, 0.01 * COIN, tgID1, 100);
    CReward reward4b(CReward::RewardType::REWARD_COINBASE, 0.03 * COIN, tgID1, 100);
    CReward reward4c(CReward::RewardType::REWARD_COINBASE, 0.02 * COIN, tgID1, 100);
    reward4a.AddRewardAmounts(0, tgID2, 100);
    CReward reward1a = reward1;
    reward1a.AddRewardAmounts(0, tgID1, 100);
    reward4a.AddRewardAmounts(0.01 * COIN, tgID1, 200);
    reward1a.AddRewardAmounts(0.01 * COIN, tgID2, 301);
    reward4a.AddRewardAmounts(0.01 * COIN, tgID2, 200);
    reward4b.AddRewardAmounts(0, tgID2, 100);
    reward4c.AddRewardAmounts(0, tgID1, 100);

    BOOST_CHECK(reward1 > reward2a);
    BOOST_CHECK(reward1 == reward2b);
    BOOST_CHECK(reward1 < reward2c);

    BOOST_CHECK(reward1 > reward3a);
    BOOST_CHECK(reward1 == reward3b);
    BOOST_CHECK(reward1 < reward3c);

    BOOST_CHECK(reward1a <= reward4a);
    BOOST_CHECK(reward1 > reward4b);
    BOOST_CHECK(reward1 == reward4c);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by huangkun on 2020/9/15.
//

#include <fstream>
#include <thread>
#include <experimental/filesystem>

#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>

#include <opengv2/event_camera_calib/EventCalibIni.hpp>
#include <opengv2/event_camera_calib/CirclesEventFrame.hpp>
#include <opengv2/event_camera_calib/parameters.hpp>
#include <opengv2/event/EventContainer.hpp>
#include <opengv2/event/EventStream.hpp>
#include <opengv2/sensor/PinholeCamera.hpp>
#include <opengv2/frame/Bodyframe.hpp>
#include <opengv2/system/SystemBase.hpp>
#include <opengv2/viewer/PointLandmarkViewer.hpp>
#include <opengv2/event_camera_calib/EventCalibSpline.hpp>

using namespace opengv2;
using namespace std;

/**
 * @brief multi thread processing on different time span
 */
class MultiProcess {
public:
    explicit MultiProcess(std::vector<std::pair<double, double>> &timeBoundSet) : timeBoundSet_(
            std::move(timeBoundSet)) {}

    void process(EventContainer::Ptr eventContainer, CirclePatternParameters::Ptr circlePatternParameters,
                 TrackingBase::Ptr tracking, CirclesEventFrame::Params &params, double motionTimeStep,
                 double len, double frameGap, int frameEventNumThreshold) {
        while (true) {
            std::pair<double, double> timeBound;
            timeBoundSetMutex_.lock();
            if (!timeBoundSet_.empty()) {
                timeBound = timeBoundSet_.back();
                timeBoundSet_.pop_back();
                timeBoundSetMutex_.unlock();
            } else {
                timeBoundSetMutex_.unlock();
                return;
            }

            std::pair<double, double> duration(timeBound.first, timeBound.first + len);
            while (duration.second < timeBound.second) {
                vector<FrameBase::Ptr> frames;
                auto frame = std::make_shared<CirclesEventFrame>(eventContainer, duration,
                                                                 circlePatternParameters, params);
                int events_num = frame->eventsNum();

                if (frame->extractFeatures()) {
                    frames.push_back(frame);
                    Bodyframe::Ptr bf = make_shared<Bodyframe>(frames, (duration.first + duration.second) / 2);

                    if (tracking->process(bf)) {
                        duration.first = duration.second + frameGap;
                        duration.second = duration.first + len;

                        // debug
                        // cv::waitKey();
                    } else {
                        if (events_num > frameEventNumThreshold || (duration.second - duration.first) > 3 * len) {
                            duration.first += motionTimeStep;
                            duration.second = duration.first + len;
                        } else {
                            duration.second += motionTimeStep;
                        }
                    }

                } else {
                    if (events_num > frameEventNumThreshold || (duration.second - duration.first) > 3 * len) {
                        duration.first += motionTimeStep;
                        duration.second = duration.first + len;
                    } else {
                        duration.second += motionTimeStep;
                    }

                    /*// DEBUG: Set 'threadNum = 1' and uncomment this code to tune parameters
                    std::cout << events_num << " events." << std::endl;
                    if (frame->eventImage.empty())
                        continue;
                    cv::namedWindow("Debug_check: EventImage");
                    cv::imshow("Debug_check: EventImage", frame->eventImage);
                    //if (frame->clusterImage.empty())
                    //    continue;
                    //cv::namedWindow("Debug_check: clusterImage");
                    //cv::imshow("Debug_check: clusterImage", frame->clusterImage);
                    cv::waitKey();*/
                }
            }
        }
    }

protected:
    std::shared_mutex timeBoundSetMutex_;
    std::vector<std::pair<double, double>> timeBoundSet_;
};

int main(int argc, char **argv) {
    if (argc != 4) {
        cerr << endl
             << "Usage: ./unit_test_eventCameraCalib settingFilePath binFilePath SavePath"
             << endl;
        return 1;
    }
    cout.precision(8);

    //Check settings file
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "Failed to open settings file at: " << string(argv[1]) << endl;
        exit(-1);
    }

    /**************************************** Create system ********************************/
    auto calibrationSetting = make_shared<CalibrationSetting>(fsSettings);

    // map part
    MapBase::Ptr map = make_shared<MapBase>();
    // tracking part
    double motionTimeStep = fsSettings["MotionTimeStep"];
    EventCalibIni::Ptr tracking = make_shared<EventCalibIni>(map, calibrationSetting, motionTimeStep);
    // viewer part
    ViewerBase::Ptr viewer = make_shared<PointLandmarkViewer>(string(argv[1]), map, tracking);
    // Extrinsic parameters, to be calibrated
    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> Qsb;
    std::vector<Eigen::Vector3d> tsb;
    Qsb.emplace_back(1, 0, 0, 0);
    tsb.emplace_back(0, 0, 0);

    SystemBase SLAM(tracking, map, viewer, nullptr, Qsb, tsb, nullptr, nullptr, string(argv[1]));

    EventStream es((string(argv[2])));
    EventContainer::Ptr eventContainer = make_shared<EventContainer>();

    // create sensor
    int width = fsSettings["Camera.width"];
    int height = fsSettings["Camera.height"];
    Eigen::Vector2d cs(width, height);
    Eigen::Matrix3d K = Eigen::Matrix3d::Identity();
    Eigen::VectorXd distortion = Eigen::VectorXd::Zero(5);
    eventContainer->camera = make_shared<PinholeCamera>(cs, K, distortion);

    bool customEnd = !fsSettings["EndTime"].isNone();
    double startTime = fsSettings["StartTime"];
    double endTime;
    if (customEnd)
        endTime = fsSettings["EndTime"];
    while (!es.isEnd()) {
        if (customEnd && es.iterator()->timeStamp() >= endTime) {
            break;
        }
        if (es.iterator()->timeStamp() >= startTime) {
            eventContainer->container.emplace(es.iterator()->timeStamp(),
                                              Event_loc_pol(es.iterator()->location(), es.iterator()->polarity()));
        }
        es.iterator()++;
    }
    es.close();
    endTime = eventContainer->container.rbegin()->first;
    std::cout << "Events from " << startTime << " second to " << endTime << " second loaded." << std::endl;

    int frameEventNumThreshold = fsSettings["FrameEventNumThreshold"];
    double len = 3 * motionTimeStep;
    double frameGap = 5 * motionTimeStep;
    CirclesEventFrame::Params params(fsSettings);
    auto threadNum = std::thread::hardware_concurrency() - 2;
    auto pieceNum = 5 * threadNum;
    double step = (endTime - startTime) / pieceNum;
    std::vector<std::pair<double, double>> timeBoundSet;
    timeBoundSet.reserve(pieceNum);
    for (int k = 0; k < pieceNum; ++k) {
        timeBoundSet.emplace_back(endTime - step * (k + 1), endTime - step * k);
    }
    MultiProcess multiProcess(timeBoundSet);
    std::vector<std::thread> threads;
    threads.reserve(threadNum);
    for (int i = 0; i < threadNum; ++i) {
        threads.emplace_back(&MultiProcess::process, &multiProcess, eventContainer,
                             calibrationSetting->circlePatternParameters, SLAM.tracking,
                             std::ref(params), motionTimeStep, len, frameGap, frameEventNumThreshold);
    }
    for (auto &th: threads) {
        th.join();
    }

    SLAM.map->keyframeLockShared();
    std::cout << SLAM.map->keyframes().size() << " frames in Map." << std::endl;
    for (const auto &itr: SLAM.map->keyframes()) {
        auto cf = dynamic_cast<CirclesEventFrame *>(itr.second->frame(0).get());
        std::cout << "Frame " << itr.second->timeStamp() << " contain " << cf->eventsNum() << " events." << std::endl;
    }
    SLAM.map->keyframeUnlockShared();

    dynamic_cast<EventCalibIni *>(SLAM.tracking.get())->cvCalibration();

    std::cout << SLAM.map->frameNum() << " frames in Map after Initialization." << std::endl;

    bool useSO3 = false;
    bool reduceMap = false;
    fsSettings["useSO3"] >> useSO3;
    fsSettings["reduceMap"] >> reduceMap;
    EventCalibSpline eventCalibSpline(SLAM.map, eventContainer, useSO3, reduceMap, motionTimeStep,
                                      calibrationSetting->circlePatternParameters->circleRadius);
    SLAM.viewer->updateViewer();

    SLAM.saveKeyFrameTrajectoryTUM(0, string(argv[3]) + "/TrajectoryByEvent.txt");

    experimental::filesystem::remove_all(string(argv[3]) + "/image/");
    experimental::filesystem::create_directories(string(argv[3]) + "/image/");
    SLAM.map->keyframeLockShared();
    for (const auto &itr: SLAM.map->keyframes()) {
        auto cf = dynamic_cast<CirclesEventFrame *>(itr.second->frame(0).get());
        cv::imwrite(string(argv[3]) + "/image/" + std::to_string(itr.second->timeStamp()) + ".png",
                    cf->image());
        /*cv::imwrite(string(argv[3]) + "/image/" + std::to_string(itr.second->timeStamp()) + "_event.png",
                    cf->eventImage);
        cv::imwrite(string(argv[3]) + "/image/" + std::to_string(itr.second->timeStamp()) + "_extraction.png",
                    cf->circleExtractionImage);
        cv::imwrite(string(argv[3]) + "/image/" + std::to_string(itr.second->timeStamp()) + "_cluster.png",
                    cf->clusterImage);*/
    }
    SLAM.map->keyframeUnlockShared();

    std::cout << "press Enter to exit..." << std::endl;
    std::cin.ignore();
    SLAM.shutdown();
    return 0;
}


#pragma once
#include <string>
#include <memory>
#include <unordered_map>
#include <Eigen/Dense>
#include "NeuronScene.hpp"
#include "SwcLoader.hpp"
#include "SwcNode.hpp"
#include "../render/Scene.hpp"
#include "../render/object/SdfSphere.hpp"
#include "../render/object/SdfRoundCone.hpp"
#include "../render/material/Material.hpp"
#include "../render/material/MaterialDiffuse.hpp"

class NeuronScene : public Scene
{
public:
    void load(std::string filepath)
    {
        auto root = SwcLoader::load(filepath);
        if (!root)
            return;

        add_swc_tree(root, nullptr);
    };

private:
    std::unordered_map<std::string, std::shared_ptr<Material>> materials
    {
        {"White",   std::make_unique<MaterialDiffuse>(Eigen::Vector3f{ 0.0, 0.0, 0.0 })},
        {"Red",     std::make_unique<MaterialDiffuse>(Eigen::Vector3f{ 1.0, 0.0, 0.0 })},
        {"Green",   std::make_unique<MaterialDiffuse>(Eigen::Vector3f{ 0.0, 1.0, 0.0 })},
        {"Blue",    std::make_unique<MaterialDiffuse>(Eigen::Vector3f{ 0.0, 0.0, 1.0 })}
    };

    void add_swc_tree(const std::unique_ptr<SwcNode> &root, const std::unique_ptr<SwcNode> &parent)
    {
        if (parent)
            add(std::move(create_cone(root, parent)));
        else if (!root->child)
            add(std::move(create_root(root)));

        if (root->next)
            add_swc_tree(root->next, parent);

        if (root->child)
            add_swc_tree(root->child, root);
    };

    std::unique_ptr<Object> create_cone(const std::unique_ptr<SwcNode>& root, const std::unique_ptr<SwcNode>& parent)
    {
        auto roundCone = std::make_unique<SdfRoundCone>(
            Eigen::Vector3f(root->x, root->y, root->z),
            Eigen::Vector3f(parent->x, parent->y, parent->z),
            root->raidus,
            parent->raidus);
        roundCone->set_material(materials["Red"]);
        return roundCone;
    }

    std::unique_ptr<Object> create_root(const std::unique_ptr<SwcNode>& root)
    {
        auto sphere = std::make_unique<SdfSphere>(
            Eigen::Vector3f{ root->x, root->y, root->z },
            root->raidus);
        sphere->set_material(materials["Green"]);
        return sphere;
    }
};