#include <glad/glad.h>
#include <GLFW/glfw3.h>
#define STB_IMAGE_IMPLEMENTATION
#include <stb_image.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <learnopengl/shader.h>

#include <iostream>
#include <math.h>
#include <Eigen/Eigen>
#include <vector>
#include <numeric>
#include <omp.h>
#include <time.h>
#include <random>

using namespace Eigen;
using namespace std;

#define PI 3.1415926535

// settings
const unsigned int SCR_WIDTH = 800;
const unsigned int SCR_HEIGHT = 800;
const unsigned int SCENE_SHRINK = 60;

float clamp(float x, float xmin, float xmax){
    if (x < xmin) return xmin;
    if (x > xmax) return xmax;
    return x;
}

float getRand(float rmin, float rmax){
    float num = (float)rand()/RAND_MAX;
    return num*(rmax-rmin)+rmin;
}

void drawCircle(float posx, float posy, float radius, int dim, Shader goalShader){
    goalShader.use();
    glm::mat4 transform;
    glm::vec3 trans_vec(0.0f, -1.0f, 0.0f);
    glm::vec3 shrink_vec(tanf(PI/dim)/tanf(PI/6)*radius/sqrt(3), radius/sqrt(3), 1.0f);
    for (int i = 0; i < dim; ++i){
        transform = glm::mat4(1.0f);
        transform = glm::translate(transform, glm::vec3(posx, posy, 0.0f));
        transform = glm::rotate(transform, (float)(2.0*PI*i/dim), glm::vec3(0.0f, 0.0f, 1.0f));
        transform = glm::scale(transform, shrink_vec);
        transform = glm::translate(transform, trans_vec);
        goalShader.setMat4("transform", transform);
        goalShader.setInt("goalInd", 1);
        glDrawArrays(GL_TRIANGLES, 0, 3);
    }
}

bool collideTest(float x1, float y1, float r1, float x2, float y2, float r2){
    float dx = x1-x2, dy = y1-y2;
    float dist = dx * dx + dy * dy;
    return (sqrt(dist) < r1+r2);
}

// process all input: query GLFW whether relevant keys are pressed/released this frame and react accordingly
// ---------------------------------------------------------------------------------------------------------
void processInput(GLFWwindow *window, float& goalx, float& goaly, bool& goalNew)
{
    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
        glfwSetWindowShouldClose(window, true);
    if (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS){
        goalNew = true;
        double tempx, tempy;
        glfwGetCursorPos(window, &tempx, &tempy);
        goalx = SCENE_SHRINK * (tempx - SCR_WIDTH/2) * 2/SCR_WIDTH;
        goaly = - (SCENE_SHRINK * (tempy - SCR_HEIGHT/2) * 2/SCR_HEIGHT);
    }
}

// glfw: whenever the window size changed (by OS or user resize) this callback function executes
// ---------------------------------------------------------------------------------------------
void framebuffer_size_callback(GLFWwindow* window, int width, int height)
{
    // make sure the viewport matches the new window dimensions; note that width and
    // height will be significantly larger than specified on retina displays.
    glViewport(0, 0, width, height);
}

void two_layer_model(VectorXf& param, VectorXf& in_data, VectorXf& out_data, int hidden_size){
    // in -> hidden
    int in_size = in_data.size();
    Map<MatrixXf> w1(&param(0), hidden_size, in_size);
    Map<VectorXf> b1(&param(w1.size()), hidden_size);
    VectorXf hidden_layer = w1 * in_data + b1;
    for (int i = 0; i < hidden_layer.size(); ++i){
        hidden_layer(i) = (hidden_layer(i) >= 0) ? hidden_layer(i) : 0.1*hidden_layer(i); // leaky relu
    }
    // hidden -> out
    int out_size = out_data.size();
    Map<MatrixXf> w2(&param(w1.size()+b1.size()), out_size, hidden_size);
    Map<VectorXf> b2(&param(w1.size()+b1.size()+w2.size()), out_size);
    out_data = w2 * hidden_layer + b2;
}

void run_model(VectorXf& param, vector<float>& init_state, vector<vector<float>>& actions, vector<vector<float>>& states, float dt, float vmax, float omax, int hidden_size){
    int steps = actions.size();
    if (steps != states.size()-1 || steps == 0){
        cout<<"run_model: actions.size != states.size or =0"<<endl;
        return;
    }
    int in_size = init_state.size();
    VectorXf in_data = VectorXf::Zero(in_size);
    for (int i = 0; i < in_size; ++i)
        in_data(i) = init_state[i];
    int out_size = actions[0].size();
    VectorXf out_data = VectorXf::Zero(out_size);
    states[0] = vector<float>(init_state.begin(), init_state.begin()+3);
    for (int i = 0; i < steps; ++i){
        two_layer_model(param, in_data, out_data, hidden_size);
        actions[i][0] = clamp(out_data(0), -vmax, vmax); // velocity
        actions[i][1] = clamp(out_data(1), -omax, omax); // omega

        // update state
        states[i+1][0] = states[i][0] + actions[i][0]*dt*cos(states[i][2]); // xnew = x + v*dt*cos(theta)
        states[i+1][1] = states[i][1] + actions[i][0]*dt*sin(states[i][2]); // ynew = y + v*dt*cos(theta)
        states[i+1][2] = states[i][2] + actions[i][1]*dt; // anglenew = angle + omega*dt
        for (int j = 0; j < 3; ++j){
            in_data(j) = states[i+1][j];
        }
    }
}

float reward(VectorXf& param, vector<float>& init_state, int steps, float time_total, float vmax, float omax, int hidden_size){
    vector<vector<float>> actions(steps, vector<float>(2, 0));
    vector<vector<float>> states(steps+1, vector<float>(4, 0));
    run_model(param, init_state, actions, states, time_total/steps, vmax, omax, hidden_size);
    float reward = 0.0, final_dist = 0.0;
    for (int i = 1; i <= steps; ++i){
        float dx = states[i][0] - init_state[3];
        float dy = states[i][1] - init_state[4];
        float dist = sqrt(dx*dx+dy*dy);
        reward -= (dist+powf(actions[i-1][1], 2));
        if (collideTest(states[i][0], states[i][1], 1, init_state[5], init_state[6], init_state[7])){
            reward -= 10000;
        }
        if (i == steps) final_dist = dist;
    }
    if (final_dist < 20){
        reward += 1000;
    }
    if (final_dist < 10 && abs(actions[steps-1][0] < 5)){
        reward += 10000;
    }
    return reward;
}

void cem(VectorXf& mu, VectorXf& th, vector<float>& init_state, int steps, float time_total, float vmax, float omax, int hidden_size){
    int batch_size = 50;
    int iterations = 150;
    float elite_frac = 0.5;
    float noise_factor = 8.0;

    int elite_n = elite_frac * batch_size;

    int p_size = mu.size();
    std::default_random_engine generator;
    for (int i = 0; i < iterations; ++i){
        MatrixXf params(p_size, batch_size);
        vector<float> score(batch_size);
        # pragma omp parallel for
        for (int j = 0; j < batch_size; ++j){
            VectorXf param = VectorXf::Zero(p_size);
            for (int k = 0; k < p_size; ++k){
                normal_distribution<float> distribution(mu(k), th(k));
                param(k) = clamp(distribution(generator), mu(k)-3*th(k), mu(k)+3*th(k));
            }
            params.block(0, j, p_size, 1) = param;
            score[j] = reward(param, init_state, steps, time_total, vmax, omax, hidden_size);
        }
        // sort
        vector<int> indices(batch_size);
        iota(indices.begin(), indices.end(), 0);
        stable_sort(indices.begin(), indices.end(), [&score](int i1, int i2){return score[i1]>score[i2];});
        // recompute mu and theta
        MatrixXf paramsCur(p_size, elite_n);
        float reward = 0.0;
        # pragma omp parallel for
        for (int j = 0; j < elite_n; ++j){
            paramsCur.block(0, j, p_size, 1) = params.block(0, indices[j], p_size, 1);
            reward += score[indices[j]]/elite_n;
        }
        mu = paramsCur.rowwise().mean();
        # pragma omp parallel for
        for (int j = 0; j < p_size; ++j){
            th(j) = (paramsCur.block(j, 0, 1, elite_n) - mu(j) * RowVectorXf::Ones(elite_n)).array().square().sum()/(elite_n - 1);
            th(j) = sqrt(th(j)) + noise_factor/(i+1);
        }
        // if (i%5 == 0){
        //     cout<<"iter:"<<i<<"; reward_mean:"<<reward<<"; mu_mean:"<<mu.mean()<<"; th_mean:"<<th.mean()<<endl;
        // }
    }
}

int main()
{
    srand((unsigned)time(NULL));
    // glfw: initialize and configure
    // ------------------------------
    glfwInit();
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

    // glfw window creation
    // --------------------
    GLFWwindow* window = glfwCreateWindow(SCR_WIDTH, SCR_HEIGHT, "simple car model", NULL, NULL);
    if (window == NULL)
    {
        std::cout << "Failed to create GLFW window" << std::endl;
        glfwTerminate();
        return -1;
    }
    glfwMakeContextCurrent(window);
    glfwSetFramebufferSizeCallback(window, framebuffer_size_callback);

    // glad: load all OpenGL function pointers
    // ---------------------------------------
    if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress))
    {
        std::cout << "Failed to initialize GLAD" << std::endl;
        return -1;
    }

    // build and compile our shader zprogram
    // ------------------------------------
    Shader ourShader("D:\\InUseRepos\\MLMiniProject\\src\\shaders\\shader.vs", "D:\\InUseRepos\\MLMiniProject\\src\\shaders\\shader.fs");
    Shader goalShader("D:\\InUseRepos\\MLMiniProject\\src\\shaders\\gshader.vs", "D:\\InUseRepos\\MLMiniProject\\src\\shaders\\shader.fs");

    // set up vertex data (and buffer(s)) and configure vertex attributes
    // ------------------------------------------------------------------
    float vertices[] = {
        // positions           // texture coords
         0.0f,  1.0f, 0.0f,    1.0f, 1.0f, 0.0f,
         -0.866f,  -0.5f, 0.0f,    0.0f, 1.0f, 1.0f,
         0.866f, -0.5f, 0.0f,    1.0f, 0.0f, 1.0f,
    };
    unsigned int VBO, VAO;
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &VBO);

    glBindVertexArray(VAO);

    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    // position attribute
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);
    // texture coord attribute
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void*)(3 * sizeof(float)));
    glEnableVertexAttribArray(1);

    // generate random initial states
    vector<float> init_state = {-50.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 10.0};
    // init_state[5] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    // init_state[6] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    // init_state[7] = getRand(5, 10);
    // while (true){
    //     init_state[0] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    //     init_state[1] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    //     if (!collideTest(init_state[0], init_state[1], 1, init_state[5], init_state[6], init_state[7])) break;
    // }
    // while (true){
    //     init_state[4] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    //     init_state[5] = getRand(-(float)SCENE_SHRINK, (float)SCENE_SHRINK);
    //     if (!collideTest(init_state[4], init_state[5], 1, init_state[5], init_state[6], init_state[7])) break;
    // }
    int in_size = 8, hidden_size = 8, out_size = 2;
    float time_total = 4.0;
    int steps = 1000;
    float vmax = 80.0, omax = 2.0;
    VectorXf mu = VectorXf::Zero((in_size+1)*hidden_size+(hidden_size+1)*out_size);
    VectorXf th = VectorXf::Ones((in_size+1)*hidden_size+(hidden_size+1)*out_size);

    clock_t startT, endT;
    startT = clock();
    cem(mu, th, init_state, steps, time_total, vmax, omax, hidden_size);
    endT = clock();
    cout<<"run cem using "<<(double)(endT-startT)/CLOCKS_PER_SEC<<"s"<<endl;

    vector<vector<float>> actions(steps, vector<float>(out_size, 0));
    vector<vector<float>> states(steps+1, vector<float>(4, 0));
    run_model(mu, init_state, actions, states, time_total/steps, vmax, omax, hidden_size);

    int frame = 0;

    float nextGoalX = 0.0, nextGoalY = 0.0;
    bool nextGoalFlag = false;

    // render loop
    // -----------
    while (!glfwWindowShouldClose(window))
    {
        // processInput
        // ------------
        processInput(window, nextGoalX, nextGoalY, nextGoalFlag);

        // computation
        // -----------
        if (nextGoalFlag && frame == steps+1){
            for (int i = 0; i < 4; ++i){
                init_state[i] = states[frame-1][i];
            }
            init_state[3] = nextGoalX;
            init_state[4] = nextGoalY;
            mu = VectorXf::Zero((in_size+1)*hidden_size+(hidden_size+1)*out_size);
            th = VectorXf::Ones((in_size+1)*hidden_size+(hidden_size+1)*out_size);
            cem(mu, th, init_state, steps, time_total, vmax, omax, hidden_size);
            run_model(mu, init_state, actions, states, time_total/steps, vmax, omax, hidden_size);
            frame = 0;
            nextGoalFlag = false;
        }
        frame = (frame == steps) ? frame:frame+1;

        // render
        // ------
        glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);
        glBindVertexArray(VAO);
        // draw current goal
        goalShader.use();
        glm::mat4 transform = glm::mat4(1.0f);
        transform = glm::translate(transform, glm::vec3(init_state[3]/SCENE_SHRINK, init_state[4]/SCENE_SHRINK, 0.0f));
        transform = glm::scale(transform, glm::vec3(1.0/SCENE_SHRINK, 1.0/SCENE_SHRINK, 1.0));
        goalShader.setMat4("transform", transform);
        goalShader.setInt("goalInd", 0);
        glDrawArrays(GL_TRIANGLES, 0, 3);
        // draw next goal
        if (nextGoalFlag){
            transform = glm::mat4(1.0f);
            transform = glm::translate(transform, glm::vec3(nextGoalX/SCENE_SHRINK, nextGoalY/SCENE_SHRINK, 0.0f));
            transform = glm::scale(transform, glm::vec3(1.0/SCENE_SHRINK, 1.0/SCENE_SHRINK, 1.0));
            goalShader.setMat4("transform", transform);
            goalShader.setInt("goalInd", 2);
            glDrawArrays(GL_TRIANGLES, 0, 3);
        }
        // draw obstacle
        drawCircle(init_state[5]/SCENE_SHRINK, init_state[6]/SCENE_SHRINK, init_state[7]/SCENE_SHRINK, 10, goalShader);
        // draw agent
        transform = glm::mat4(1.0f);
        transform = glm::translate(transform, glm::vec3(states[frame][0]/SCENE_SHRINK, states[frame][1]/SCENE_SHRINK, 0.0f));
        transform = glm::rotate(transform, states[frame][2], glm::vec3(0.0f, 0.0f, 1.0f));
        transform = glm::scale(transform, glm::vec3(1.0/SCENE_SHRINK, 1.0/SCENE_SHRINK, 1.0));
        ourShader.use();
        ourShader.setMat4("transform", transform);
        glDrawArrays(GL_TRIANGLES, 0, 3);

        // glfw: swap buffers and poll IO events (keys pressed/released, mouse moved etc.)
        // -------------------------------------------------------------------------------
        if (nextGoalFlag && frame == steps) frame++;
        glfwSwapBuffers(window);
        glfwPollEvents();
    }

    // optional: de-allocate all resources once they've outlived their purpose:
    // ------------------------------------------------------------------------
    glDeleteVertexArrays(1, &VAO);
    glDeleteBuffers(1, &VBO);

    // glfw: terminate, clearing all previously allocated GLFW resources.
    // ------------------------------------------------------------------
    glfwTerminate();
    return 0;
}


#include <SFML\Graphics.hpp>
#include <functional>
#include <memory>
#include <boost/math/special_functions/round.hpp>
#include <iostream>
#include <cmath>
#include "ResourceManager.h"
#include "Number.h"

namespace GUI{

Number::Number(TextureManager &textureManager, unsigned int num) :
        mOnesSprite(),
        mTensSprite(),
        mTenthsSprite(),
        mHundredthsSprite(),
        mDecimalSprite(){

    int numTens = num / 10;
    int numOnes = num % 10;

    mOnesSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    if (numTens > 0)
        mTensSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    mOnesSprite.setTextureRect(getRectFromNum(numOnes));
    if (numTens > 0)
        mTensSprite.setTextureRect(getRectFromNum(numTens));

    sf::FloatRect onesBounds = mOnesSprite.getGlobalBounds();
    sf::FloatRect tensBounds = mTensSprite.getGlobalBounds();
    mOnesSprite.setOrigin(onesBounds.width / 2.f, onesBounds.height / 2.f);
    mTensSprite.setOrigin(tensBounds.width / 2.f, tensBounds.height / 2.f);

    if (mTensSprite.getTexture()){
        mTensSprite.move(-16.f, 0.f);
        mOnesSprite.move(16.f, 0.f);
    }
}

Number::Number(TextureManager &textureManager, float num) :
        mOnesSprite(),
        mTensSprite(),
        mTenthsSprite(),
        mHundredthsSprite(),
        mDecimalSprite(){

    int numInt = static_cast<int>(std::floor(num));
    int numTens = numInt / 10;
    int numOnes = numInt % 10;

    float decRemainder = boost::math::round(100.f * fmod(num, std::floor(num))) / 100.f;
    int decimal = static_cast<int>(100 * decRemainder);
    int numTenths = decimal / 10;
    int numHundredths = decimal % 10;

    mOnesSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    if (numTens > 0)
        mTensSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    mOnesSprite.setTextureRect(getRectFromNum(numOnes));
    if (numTens > 0)
        mTensSprite.setTextureRect(getRectFromNum(numTens));
    mHundredthsSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    mHundredthsSprite.setTextureRect(getRectFromNum(numHundredths));
    mTenthsSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    mTenthsSprite.setTextureRect(getRectFromNum(numTenths));
    mDecimalSprite.setTexture(textureManager.get(TextureID::HUDSpriteSheet));
    mDecimalSprite.setTextureRect(sf::IntRect(168, 0, 8, 38));

    sf::FloatRect onesBounds = mOnesSprite.getGlobalBounds();
    sf::FloatRect tensBounds = mTensSprite.getGlobalBounds();
    sf::FloatRect hundredthsBounds = mHundredthsSprite.getGlobalBounds();
    sf::FloatRect tenthsBounds = mTenthsSprite.getGlobalBounds();
    sf::FloatRect decimalBounds = mDecimalSprite.getGlobalBounds();
    mOnesSprite.setOrigin(onesBounds.width / 2.f, onesBounds.height / 2.f);
    mTensSprite.setOrigin(tensBounds.width / 2.f, tensBounds.height / 2.f);
    mHundredthsSprite.setOrigin(hundredthsBounds.width / 2.f, hundredthsBounds.height / 2.f);
    mTenthsSprite.setOrigin(tenthsBounds.width / 2.f, tenthsBounds.height / 2.f);
    mDecimalSprite.setOrigin(decimalBounds.width / 2.f, decimalBounds.height / 2.f);

    if (mTensSprite.getTexture()){
        mTensSprite.move(-52.f, 0.f);
    }
    mOnesSprite.move(-20.f, 0.f);
    mTenthsSprite.move(20.f, 0.f);
    mHundredthsSprite.move(52.f, 0.f);
}

bool Number::isSelectable(){
    return false;
}

void Number::handleEvent(const sf::Event &event){}

void Number::draw(sf::RenderTarget &target, sf::RenderStates states) const{

    states.transform *= getTransform();
    target.draw(mTensSprite, states);
    target.draw(mOnesSprite, states);
    target.draw(mTenthsSprite, states);
    target.draw(mHundredthsSprite, states);
    target.draw(mDecimalSprite, states);
}

sf::IntRect Number::getRectFromNum(unsigned int num){

    switch(num){
    case 0:
        return sf::IntRect(230, 0, 30, 38);
    case 1:
        return sf::IntRect(196, 41, 26, 37);
    case 2:
        return sf::IntRect(55, 98, 32, 38);
    case 3:
        return sf::IntRect(239, 80, 28, 38);
    case 4:
        return sf::IntRect(238, 122, 29, 38);
    case 5:
        return sf::IntRect(238, 162, 28, 38);
    case 6:
        return sf::IntRect(230, 40, 30, 38);
    case 7:
        return sf::IntRect(226, 206, 32, 39);
    case 8:
        return sf::IntRect(192, 206, 32, 40);
    case 9:
        return sf::IntRect(196, 0, 32, 39);
    default:
        return sf::IntRect(230, 0, 30, 38);
    }
}
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Roland Lichters
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <qlo/qladdindefines.hpp>
#include <qlo/credit.hpp>
#include <qlo/enumerations/factories/termstructuresfactory.hpp>

#include <ql/instruments/stock.hpp>
#include <ql/quote.hpp>
#include <ql/currencies/europe.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/termstructures/credit/interpolatedhazardratecurve.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/credit/piecewisedefaultcurve.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/pricingengines/credit/midpointcdsengine.hpp>

#include <ql/experimental/credit/riskybond.hpp>
#include <ql/experimental/credit/syntheticcdo.hpp>
#include <ql/experimental/credit/midpointcdoengine.hpp>
#include <ql/experimental/credit/nthtodefault.hpp>
#include <ql/experimental/credit/integralntdengine.hpp>

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/make_shared.hpp>

#include <ql/settings.hpp>

using boost::algorithm::to_upper_copy;

namespace QuantLibAddin {

    Issuer::Issuer(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const boost::shared_ptr<QuantLib::DefaultProbabilityTermStructure>& dfts,
            const boost::shared_ptr<QuantLib::DefaultEventSet>& evtSet,
            bool permanent
        )
        : ObjectHandler::LibraryObject<QuantLib::Issuer>(properties, permanent) {

        std::vector<QuantLib::Issuer::key_curve_pair> curves(1, std::make_pair(
            QuantLib::NorthAmericaCorpDefaultKey(QuantLib::EURCurrency(),
                                                     QuantLib::SeniorSec,
                                                     QuantLib::Period(),
                                                     1. // amount threshold
                                                     ),
            QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>(dfts)
        ));
        libraryObject_ = boost::shared_ptr<QuantLib::Issuer>(new QuantLib::Issuer(curves, *evtSet));
    }

    DefaultEventSet::DefaultEventSet(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::string& eventType,
            const QuantLib::Date& eventDate,
            const QuantLib::Currency& cur,
            QuantLib::Seniority sen,
            const QuantLib::Date& settlementDate,
            QuantLib::Real settledRecovery,
            bool permanent
        )
    : ObjectHandler::LibraryObject<QuantLib::DefaultEventSet>(properties, permanent) {
        // if no match return empty set
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultEventSet>(new QuantLib::DefaultEventSet());

        // only one recovery parsed by now; bankruptcy events need the whole
        //  set or they fail to construct.
        std::map<QuantLib::Seniority, QuantLib::Real> rrs;
        rrs.insert(std::pair<QuantLib::Seniority, QuantLib::Real>(sen, settledRecovery));
        if(eventType==std::string("FailureToPayEvent")) {
            libraryObject_->insert(boost::shared_ptr<QuantLib::FailureToPayEvent> (
                new QuantLib::FailureToPayEvent(eventDate, cur, sen, 1.e7,
                //implSettlemt,
                settlementDate,
                rrs)));
        }else if(eventType==std::string("BankruptcyEvent")){
            libraryObject_->insert(boost::shared_ptr<QuantLib::BankruptcyEvent> (
                new QuantLib::BankruptcyEvent(eventDate, cur, sen,
                settlementDate,
                rrs)));
        }
    }

    /* Code essentially copied from QuantLibAddIn::SimpleQuote.
         Same considerations mentioned there apply. Here the
         Seniority of the quote is considered a fixed
         property, not a market value.
    */
    RecoveryRateQuote::RecoveryRateQuote(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            QuantLib::Seniority sen,
            QuantLib::Real value,
            bool permanent) : Quote(properties, permanent) {
        libraryObject_ = recoveryQuote_ = boost::shared_ptr<QuantLib::RecoveryRateQuote>(
            new QuantLib::RecoveryRateQuote(value, sen));
    }

   // CreditDefaultSwap::CreditDefaultSwap(
   //           const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
   //           QuantLib::Protection::Side side,
   //           QuantLib::Real notional,
   //           QuantLib::Rate upfront,
   //           QuantLib::Rate spread,
   //           const boost::shared_ptr<QuantLib::Schedule>& schedule,
   //           QuantLib::BusinessDayConvention paymentConvention,
   //           const QuantLib::DayCounter& dayCounter,
   //           bool settlesAccrual,
   //           bool paysAtDefaultTime,
   //           const QuantLib::Date& protectionStart,
   //           const QuantLib::Date& upfrontDate,
   //           bool permanent)
   //     : Instrument(properties, permanent) {
			//// dirty way to decide if this is constructed through a run only version
			//if(upfrontDate == QuantLib::Null<QuantLib::Date>() && upfront == 0.) {
			//	libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
			//				new QuantLib::CreditDefaultSwap(side,
			//												notional,
			//												spread,
			//												*schedule,
			//												paymentConvention,
			//												dayCounter,
			//												settlesAccrual,
			//												paysAtDefaultTime,
			//												protectionStart,
			//												boost::shared_ptr<QuantLib::Claim>()));
			//}else{
			//	libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
			//				new QuantLib::CreditDefaultSwap(side,
			//												notional,
			//												upfront,
			//												spread,
			//												*schedule,
			//												paymentConvention,
			//												dayCounter,
			//												settlesAccrual,
			//												paysAtDefaultTime,
			//												protectionStart,
			//												upfrontDate,
			//												boost::shared_ptr<QuantLib::Claim>()));
   //         }
   // }
   //
    MidPointCdsEngine::MidPointCdsEngine(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>& defaultTS,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool permanent)
        : PricingEngine(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::PricingEngine>(new
              QuantLib::MidPointCdsEngine(defaultTS, recoveryRate, yieldTS));
    }

    SpreadCdsHelper::SpreadCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {

        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::SpreadCdsHelper(quote,
						 period,
						 settlementDays,
						 calendar,
						 frequency,
						 paymentConvention,
						 rule,
						 dayCounter,
						 recoveryRate,
						 yieldTS,
						 settlesAccrual,
						 paysAtDefaultTime));
    }

    UpfrontCdsHelper::UpfrontCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            QuantLib::Rate runningSpread,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            QuantLib::Natural upfrontSettlementDays,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::UpfrontCdsHelper(quote,
                                          runningSpread,
                                          period,
                                          settlementDays,
                                          calendar,
                                          frequency,
                                          paymentConvention,
                                          rule,
                                          dayCounter,
                                          recoveryRate,
                                          yieldTS,
                                          upfrontSettlementDays,
                                          settlesAccrual,
                                          paysAtDefaultTime));
    }

    HazardRateCurve::HazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::vector<QuantLib::Date>& dates,
            const std::vector<QuantLib::Rate>& hazardRates,
            const QuantLib::DayCounter& dayCounter,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        QL_REQUIRE(dates.size() == hazardRates.size(),
                   "vector sizes differ");
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(
        new QuantLib::InterpolatedHazardRateCurve<QuantLib::BackwardFlat>(
 				 dates, hazardRates, dayCounter));
    }

    PiecewiseHazardRateCurve::PiecewiseHazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::vector<boost::shared_ptr<QuantLib::DefaultProbabilityHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            const QuantLib::Calendar& calendar,
            const std::string& interpolator,
            QuantLib::Real accuracy,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {

        if(interpolator == std::string("LINEAR")){
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::Linear>(
                            0,
                            calendar,
                            helpers,
                            dayCounter));
        }else if(interpolator == std::string("BACKWARDFLAT")) {
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::BackwardFlat>(
                            0,
                            calendar,
                            helpers,
                            dayCounter));
        }else{
            QL_FAIL("Unrecognised interpolator");
        }

        libraryObject_->enableExtrapolation();
    }

    // ptr type check here would correspond to template spez in the
    //   subscribers factory solution.
    const std::vector<QuantLib::Date>& PiecewiseHazardRateCurve::dates() const {
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
        boost::shared_ptr<flat_curve> ptrBF =
            boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
        if(ptrBF) return ptrBF->dates();
        boost::shared_ptr<lin_curve> ptrLIN =
            boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
        if(ptrLIN) return ptrLIN->dates();
        QL_FAIL("Unable to cast default probability term structure.");
    }

    const std::vector<QuantLib::Real>& PiecewiseHazardRateCurve::data() const {
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
        boost::shared_ptr<flat_curve> ptrBF =
            boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
        if(ptrBF) return ptrBF->data();
        boost::shared_ptr<lin_curve> ptrLIN =
            boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
        if(ptrLIN) return ptrLIN->data();
        QL_FAIL("Unable to cast default probability term structure.");
        }

    PiecewiseFlatForwardCurve::PiecewiseFlatForwardCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Date& referenceDate,
            const std::vector<boost::shared_ptr<QuantLib::RateHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real accuracy,
            bool permanent)
        : YieldTermStructure(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
               QuantLib::PiecewiseYieldCurve<QuantLib::Discount,QuantLib::LogLinear>(referenceDate, helpers, dayCounter));
    }

    RiskyFixedBond::RiskyFixedBond(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        std::string name,
        QuantLib::Currency ccy,
        QuantLib::Real recoveryRate,
        QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure> defaultTS,
        const boost::shared_ptr<QuantLib::Schedule>& schedule,
        QuantLib::Real rate,
        QuantLib::DayCounter dayCounter,
        QuantLib::BusinessDayConvention paymentConvention,
        QuantLib::Real notional,
        QuantLib::Handle<QuantLib::YieldTermStructure> yieldTS,
        QuantLib::Date npvDate, // unused by now
        bool permanent)
    : Instrument(properties, permanent) {

        std::vector<QuantLib::Real> notionals(1,notional);

        libraryObject_ = boost::shared_ptr<QuantLib::RiskyFixedBond>(
            new QuantLib::RiskyFixedBond(
                    name,ccy,recoveryRate,defaultTS,*schedule,rate,dayCounter,
                    paymentConvention,notionals,yieldTS///, npvDate
                                       ));

    }

    SyntheticCDO::SyntheticCDO(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const boost::shared_ptr<QuantLib::Basket>& bskt,
        QuantLib::Protection::Side side,
        const boost::shared_ptr<QuantLib::Schedule>& schedule,
        QuantLib::Rate upfront,
        QuantLib::Rate spread,
        const QuantLib::DayCounter& dayCounter,
        QuantLib::BusinessDayConvention paymentConvention,
        bool permanent)
    : Instrument(properties, permanent) {
		libraryObject_ = boost::make_shared<QuantLib::SyntheticCDO>(
            bskt,side,*schedule,upfront,spread,dayCounter,paymentConvention);
    }

    MidPointCDOEngine::MidPointCDOEngine(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const QuantLib::Handle<QuantLib::YieldTermStructure>& discTS,
        bool permanent)
    : PricingEngine(properties, permanent) {
		libraryObject_ =
            boost::make_shared<QuantLib::MidPointCDOEngine>(discTS);
    }

    NthToDefault::NthToDefault(
          const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
          const boost::shared_ptr<QuantLib::Basket>& bskt,
          QuantLib::Size order,
          QuantLib::Protection::Side side,
          const boost::shared_ptr<QuantLib::Schedule>& schedule,
          QuantLib::Rate upfront,
          QuantLib::Rate spread,
          const QuantLib::DayCounter& dayCounter,
          QuantLib::Real notional,
          bool paysAccrual,
          bool permanent)
    : Instrument(properties, permanent) {
		libraryObject_ = boost::make_shared<QuantLib::NthToDefault>(
            bskt,order,side,*schedule,upfront,spread,dayCounter,
            notional,paysAccrual);
    }

    IntegralNtdEngine::IntegralNtdEngine(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const QuantLib::Period& step,
        const QuantLib::Handle<QuantLib::YieldTermStructure>& discTS,
        bool permanent)
    : PricingEngine(properties, permanent) {
		libraryObject_ =
            boost::make_shared<QuantLib::IntegralNtdEngine>(step, discTS);
    }

}


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

#include <algorithm>

#if defined(_OPENMP)
#include <omp.h>
#endif

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

#include "luxrays/core/geometry/bbox.h"
#include "slg/samplers/sobol.h"
#include "slg/lights/strategies/dlscacheimpl/dlscacheimpl.h"
#include "slg/lights/strategies/dlscacheimpl/dlscoctree.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// DLSCOctree
//------------------------------------------------------------------------------

DLSCOctree::DLSCOctree(const BBox &bbox, const float r, const float normAngle, const u_int md) :
	worldBBox(bbox), maxDepth(md), entryRadius(r), entryRadius2(r * r),
	entryNormalCosAngle(cosf(Radians(normAngle))) {
	worldBBox.Expand(MachineEpsilon::E(worldBBox));
}

DLSCOctree::~DLSCOctree() {
}

void DLSCOctree::Add(DLSCacheEntry *cacheEntry) {
	const Vector entryRadiusVector(entryRadius, entryRadius, entryRadius);
	const BBox entryBBox(cacheEntry->p - entryRadiusVector, cacheEntry->p + entryRadiusVector);

	AddImpl(&root, worldBBox, cacheEntry, entryBBox, DistanceSquared(entryBBox.pMin,  entryBBox.pMax));
}

const DLSCacheEntry *DLSCOctree::GetEntry(const Point &p, const Normal &n,
		const bool isVolume) const {
	return GetEntryImpl(&root, worldBBox, p, n, isVolume);
}

void DLSCOctree::GetAllNearEntries(vector<DLSCacheEntry *> &entries,
		const Point &p, const Normal &n,
		const bool isVolume,
		const float radius) const {
	const Vector radiusVector(radius, radius, radius);
	const BBox bbox(p - radiusVector, p + radiusVector);

	return GetAllNearEntriesImpl(entries, &root, worldBBox,
			p, n, isVolume,
			bbox, radius * radius);
}

BBox DLSCOctree::ChildNodeBBox(u_int child, const BBox &nodeBBox,
	const Point &pMid) const {
	BBox childBound;

	childBound.pMin.x = (child & 0x4) ? pMid.x : nodeBBox.pMin.x;
	childBound.pMax.x = (child & 0x4) ? nodeBBox.pMax.x : pMid.x;
	childBound.pMin.y = (child & 0x2) ? pMid.y : nodeBBox.pMin.y;
	childBound.pMax.y = (child & 0x2) ? nodeBBox.pMax.y : pMid.y;
	childBound.pMin.z = (child & 0x1) ? pMid.z : nodeBBox.pMin.z;
	childBound.pMax.z = (child & 0x1) ? nodeBBox.pMax.z : pMid.z;

	return childBound;
}

void DLSCOctree::AddImpl(DLSCOctreeNode *node, const BBox &nodeBBox,
	DLSCacheEntry *entry, const BBox &entryBBox,
	const float entryBBoxDiagonal2, const u_int depth) {
	// Check if I have to store the entry in this node
	if ((depth == maxDepth) ||
			DistanceSquared(nodeBBox.pMin, nodeBBox.pMax) < entryBBoxDiagonal2) {
		node->entries.push_back(entry);
		return;
	}

	// Determine which children the item overlaps
	const Point pMid = .5 * (nodeBBox.pMin + nodeBBox.pMax);

	const bool x[2] = {
		entryBBox.pMin.x <= pMid.x,
		entryBBox.pMax.x > pMid.x
	};
	const bool y[2] = {
		entryBBox.pMin.y <= pMid.y,
		entryBBox.pMax.y > pMid.y
	};
	const bool z[2] = {
		entryBBox.pMin.z <= pMid.z,
		entryBBox.pMax.z > pMid.z
	};

	const bool overlap[8] = {
		bool(x[0] & y[0] & z[0]),
		bool(x[0] & y[0] & z[1]),
		bool(x[0] & y[1] & z[0]),
		bool(x[0] & y[1] & z[1]),
		bool(x[1] & y[0] & z[0]),
		bool(x[1] & y[0] & z[1]),
		bool(x[1] & y[1] & z[0]),
		bool(x[1] & y[1] & z[1])
	};

	for (u_int child = 0; child < 8; ++child) {
		if (!overlap[child])
			continue;

		// Allocated the child node if required
		if (!node->children[child])
			node->children[child] = new DLSCOctreeNode();

		// Add the entry to each overlapping child
		const BBox childBBox = ChildNodeBBox(child, nodeBBox, pMid);
		AddImpl(node->children[child], childBBox,
				entry, entryBBox, entryBBoxDiagonal2, depth + 1);
	}
}

const DLSCacheEntry *DLSCOctree::GetEntryImpl(const DLSCOctreeNode *node, const BBox &nodeBBox,
	const Point &p, const Normal &n, const bool isVolume) const {
	// Check if I'm inside the node bounding box
	if (!nodeBBox.Inside(p))
		return NULL;

	// Check every entry in this node
	for (auto entry : node->entries) {
		if ((DistanceSquared(p, entry->p) <= entryRadius2) &&
				(isVolume == entry->isVolume) &&
				(isVolume || (Dot(n, entry->n) >= entryNormalCosAngle))) {
			// I have found a valid entry
			return entry;
		}
	}

	// Check the children too
	const Point pMid = .5 * (nodeBBox.pMin + nodeBBox.pMax);
	for (u_int child = 0; child < 8; ++child) {
		if (node->children[child]) {
			const BBox childBBox = ChildNodeBBox(child, nodeBBox, pMid);

			const DLSCacheEntry *entry = GetEntryImpl(node->children[child], childBBox,
					p, n, isVolume);
			if (entry) {
				// I have found a valid entry
				return entry;
			}
		}
	}

	return NULL;
}

void DLSCOctree::GetAllNearEntriesImpl(vector<DLSCacheEntry *> &entries,
		const DLSCOctreeNode *node, const BBox &nodeBBox,
		const Point &p, const Normal &n,
		const bool isVolume,
		const BBox areaBBox,
		const float areaRadius2) const {
	// Check if I overlap the node bounding box
	if (!nodeBBox.Overlaps(areaBBox))
		return;

	// Check every entry in this node
	for (auto entry : node->entries) {
		if ((DistanceSquared(p, entry->p) <= areaRadius2) &&
				(isVolume == entry->isVolume) &&
				// I relax the condition of normal (just check the sign and not
				// use neighbors parameter) in order to to merge more neighbors
				// and avoid problems on the edge of object with interpolated
				// normals
				(isVolume || (Dot(n, entry->n) > 0.f))) {
			// I have found a valid entry but I avoid to insert duplicates
			if (find(entries.begin(), entries.end(), entry) == entries.end())
				entries.push_back(entry);
		}
	}

	// Check the children too
	const Point pMid = .5 * (nodeBBox.pMin + nodeBBox.pMax);
	for (u_int child = 0; child < 8; ++child) {
		if (node->children[child]) {
			const BBox childBBox = ChildNodeBBox(child, nodeBBox, pMid);

			GetAllNearEntriesImpl(entries, node->children[child], childBBox,
					p, n, isVolume,
					areaBBox, areaRadius2);
		}
	}
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE DynamicSliceCreation
#include <boost/test/unit_test.hpp>
#include <cassert>
#include <iostream>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <popops/DynamicSlice.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <vector>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_support;

int testDim(unsigned sliceDim) {
  auto device = createTestDeviceFullSize(TEST_TARGET);
  Target target = device.getTarget();

  std::cout << "Creating graph" << std::endl;
  Graph graph(target);
  popops::addCodelets(graph);

  Sequence seq;
  const size_t DIM = 812;
  std::vector<size_t> inputShape = {15, DIM, 512};
  std::vector<size_t> sliceDims = {sliceDim};
  std::vector<size_t> sliceSizes = {1};
  auto sliceIndices = graph.addVariable(UNSIGNED_INT, {1});
  graph.setTileMapping(sliceIndices, 0);

  auto input = popops::createSliceableTensor(graph, HALF, inputShape, sliceDims,
                                             sliceSizes, 0, "input");
  // There should be no more than a single region per tile
  BOOST_CHECK(input.slice(0, 1, sliceDim).getVarRegions().size() <=
              target.getNumTiles());

  {
    auto start = std::chrono::high_resolution_clock::now();
    auto sliced = popops::dynamicSlice(graph, input, sliceIndices, sliceDims,
                                       sliceSizes, seq, "dyn_slice");
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    BOOST_TEST_MESSAGE("Time for dynamicSlice " +
                       std::to_string(elapsed.count()) + " s");
  }

  {
    std::vector<size_t> slice_shape = inputShape;
    slice_shape[sliceDim] = 1;
    auto update = popops::createSliceableTensor(
        graph, HALF, slice_shape, sliceDims, sliceSizes, 0, "update");
    auto start = std::chrono::high_resolution_clock::now();
    popops::dynamicUpdate(graph, input, update, sliceIndices, sliceDims,
                          sliceSizes, seq, "dyn_update_slice");
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    BOOST_TEST_MESSAGE("Time for dynamicUpdate " +
                       std::to_string(elapsed.count()) + " s");
  }

  // flat input as host-writable to ensure it is always live
  graph.createHostWrite("in", input, true);

  OptionFlags options{{"showVarStorage", "true"}};
  // Actually create the engine just to check that memory has not exploded
  Engine eng(graph, seq);
  auto &graphProfile = eng.getGraphProfile();
  bool verbose = false;
  if (verbose)
    poplar::printProfileSummary(std::cerr, graphProfile, {}, options);
  auto tile0Memory = graphProfile["memory"]["byTile"]["total"][0].asUint();
  BOOST_TEST_FRAMEWORK_MESSAGE("blah");
  BOOST_TEST_MESSAGE("Tile0 memory = " + std::to_string(tile0Memory));
  BOOST_CHECK(tile0Memory <= input.numElements() + 10240);

  return 0;
}

BOOST_AUTO_TEST_CASE(Dim0,
                     *boost::unit_test::precondition(enableIfIpuModel())) {
  testDim(0);
}

BOOST_AUTO_TEST_CASE(Dim1,
                     *boost::unit_test::precondition(enableIfIpuModel())) {
  testDim(1);
}

BOOST_AUTO_TEST_CASE(Dim2,
                     *boost::unit_test::precondition(enableIfIpuModel())) {
  testDim(2);
}


#include <math.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <iostream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "faceDetection.h"
#include "util.h"

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

	cv::VideoCapture cap(0);

	FaceFeatures *face_features = new FaceFeatures();
	FaceData *face_data = new FaceData();
	FacePose *face_pose = new FacePose();

	cv::Mat frame, frame_clr;

	dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
	dlib::shape_predictor pose_model;

	dlib::deserialize("./res/shape_predictor_68_face_landmarks.dat") >> pose_model;

	while(1) {
		try {
			cap>>frame;
			cv::flip(frame, frame, 1);
			frame.copyTo(frame_clr);
			cv::cvtColor(frame, frame, CV_BGR2GRAY);

			dlib::cv_image<unsigned char> cimg_gray(frame);
			dlib::cv_image<dlib::bgr_pixel> cimg_clr(frame_clr);

			std::vector<dlib::rectangle> faces = detector(cimg_gray);

			std::vector<dlib::full_object_detection> shapes;
			for (unsigned long i = 0; i < faces.size(); ++i)
				shapes.push_back(pose_model(cimg_gray, faces[i]));

			if(faces.size()) {

				dlib::full_object_detection shape = shapes[0];

				face_features->assign(cv::Point(0,0),
					get_mid_point(cv::Point(shape.part(42).x(), shape.part(42).y()),
						cv::Point(shape.part(45).x(), shape.part(45).y())),
					get_mid_point(cv::Point(shape.part(36).x(), shape.part(36).y()),
						cv::Point(shape.part(39).x(), shape.part(39).y())),
					cv::Point(shape.part(30).x(), shape.part(30).y()),
					get_mid_point(cv::Point(shape.part(48).x(), shape.part(48).y()),
						cv::Point(shape.part(54).x(), shape.part(54).y())));

				face_data->assign(face_features);

				face_pose->assign(face_features, face_data);

				std::cout<<face_pose->normal[0]<<" "<<face_pose->normal[1]<<" "<<face_pose->normal[2]<<"\n";
			}
			else {
				std::cout<<"Zero faces"<<std::endl;
			}
		}
		catch(std::exception& e) {
			break;
		}
	}

	return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2022 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2022 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2022 Ilia Shirobokov <i.shirobokov@nil.foundation>
// Copyright (c) 2022 Alisa Cherniaeva <a.cherniaeva@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE placeholder_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/pallas.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/pallas.hpp>
#include <nil/crypto3/algebra/random_element.hpp>

#include <nil/crypto3/math/algorithms/unity_root.hpp>
#include <nil/crypto3/math/polynomial/lagrange_interpolation.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>
#include <nil/crypto3/hash/sha2.hpp>
#include <nil/crypto3/hash/keccak.hpp>

#include <nil/crypto3/zk/snark/systems/plonk/placeholder/prover.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/verifier.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/permutation_argument.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/gates_argument.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/preprocessor.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/detail/placeholder_policy.hpp>
#include <nil/crypto3/zk/snark/arithmetization/plonk/constraint_system.hpp>
#include <nil/crypto3/zk/snark/arithmetization/plonk/gate.hpp>
#include <nil/crypto3/zk/transcript/fiat_shamir.hpp>
#include <nil/crypto3/zk/commitments/polynomial/fri.hpp>

#include "circuits.hpp"

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::zk::snark;

template<typename fri_type, typename FieldType>
typename fri_type::params_type create_fri_params(std::size_t degree_log) {
    typename fri_type::params_type params;
    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};

    constexpr std::size_t expand_factor = 4;

    std::size_t r = degree_log - 1;

    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> domain_set =
        zk::commitments::detail::calculate_domain_set<FieldType>(degree_log + expand_factor, r);

    params.r = r;
    params.D = domain_set;
    params.q = q;
    params.max_degree = (1 << degree_log) - 1;

    return params;
}

BOOST_AUTO_TEST_SUITE(placeholder_prover_test_suite)

// using curve_type = algebra::curves::bls12<381>;
using curve_type = algebra::curves::pallas;
using FieldType = typename curve_type::base_field_type;

// lpc params
constexpr static const std::size_t m = 2;

constexpr static const std::size_t table_rows_log = 4;
constexpr static const std::size_t table_rows = 1 << table_rows_log;
constexpr static const std::size_t permutation_size = 4;
constexpr static const std::size_t usable_rows = (1 << table_rows_log) - 3;

struct placeholder_test_params {
    using merkle_hash_type = hashes::keccak_1600<512>;
    using transcript_hash_type = hashes::keccak_1600<512>;

    constexpr static const std::size_t witness_columns = 3;
    constexpr static const std::size_t public_input_columns = 1;
    constexpr static const std::size_t constant_columns = 0;
    constexpr static const std::size_t selector_columns = 2;

    using arithmetization_params =
        plonk_arithmetization_params<witness_columns, public_input_columns, constant_columns, selector_columns>;

    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t r = table_rows_log - 1;
    constexpr static const std::size_t m = 2;
};

struct placeholder_test_params_lookups {
    using merkle_hash_type = hashes::keccak_1600<512>;
    using transcript_hash_type = hashes::keccak_1600<512>;

    constexpr static const std::size_t witness_columns = 3;
    constexpr static const std::size_t public_input_columns = 0;
    constexpr static const std::size_t constant_columns = 3;
    constexpr static const std::size_t selector_columns = 1;

    using arithmetization_params =
        plonk_arithmetization_params<witness_columns, public_input_columns, constant_columns, selector_columns>;

    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t r = table_rows_log - 1;
    constexpr static const std::size_t m = 2;
};

constexpr static const std::size_t table_columns =
    placeholder_test_params::witness_columns + placeholder_test_params::public_input_columns;

typedef commitments::fri<FieldType, placeholder_test_params::merkle_hash_type,
                         placeholder_test_params::transcript_hash_type, m>
    fri_type;

typedef placeholder_params<FieldType, typename placeholder_test_params::arithmetization_params> circuit_2_params;
typedef placeholder_params<FieldType, typename placeholder_test_params_lookups::arithmetization_params>
    circuit_3_params;

BOOST_AUTO_TEST_CASE(placeholder_split_polynomial_test) {

    math::polynomial<typename FieldType::value_type> f = {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1};
    std::size_t expected_size = 4;
    std::size_t max_degree = 3;

    std::vector<math::polynomial<typename FieldType::value_type>> f_splitted =
        zk::snark::detail::split_polynomial<FieldType>(f, max_degree);

    BOOST_CHECK(f_splitted.size() == expected_size);

    typename FieldType::value_type y = algebra::random_element<FieldType>();

    typename FieldType::value_type f_at_y = f.evaluate(y);
    typename FieldType::value_type f_splitted_at_y = FieldType::value_type::zero();
    for (std::size_t i = 0; i < f_splitted.size(); i++) {
        f_splitted_at_y = f_splitted_at_y + f_splitted[i].evaluate(y) * y.pow((max_degree + 1) * i);
    }

    BOOST_CHECK(f_at_y == f_splitted_at_y);
}

BOOST_AUTO_TEST_CASE(placeholder_permutation_polynomials_test) {

    circuit_description<FieldType, circuit_2_params, table_rows_log, permutation_size> circuit =
        circuit_test_2<FieldType>();

    constexpr std::size_t argument_size = 3;

    using policy_type = zk::snark::detail::placeholder_policy<FieldType, circuit_2_params>;

    typedef commitments::list_polynomial_commitment<FieldType,
        circuit_2_params::batched_commitment_params_type> lpc_type;

    typename fri_type::params_type fri_params = create_fri_params<fri_type, FieldType>(table_rows_log);

    plonk_table_description<FieldType, typename circuit_2_params::arithmetization_params> desc;

    desc.rows_amount = table_rows;
    desc.usable_rows_amount = usable_rows;

    typename policy_type::constraint_system_type constraint_system(circuit.gates, circuit.copy_constraints,
                                                                   circuit.lookup_gates);
    typename policy_type::variable_assignment_type assignments = circuit.table;

    std::vector<std::size_t> columns_with_copy_constraints = {0, 1, 2, 3};

    typename placeholder_public_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_public_data =
        placeholder_public_preprocessor<FieldType, circuit_2_params>::process(
            constraint_system, assignments.public_table(), desc,
            fri_params, columns_with_copy_constraints.size());

    typename placeholder_private_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_private_data =
        placeholder_private_preprocessor<FieldType, circuit_2_params>::process(constraint_system,
                                                                               assignments.private_table(), desc);

    auto polynomial_table = plonk_polynomial_table<FieldType, typename placeholder_test_params::arithmetization_params>(
        preprocessed_private_data.private_polynomial_table, preprocessed_public_data.public_polynomial_table);

    std::shared_ptr<math::evaluation_domain<FieldType>> domain = preprocessed_public_data.common_data.basic_domain;
    typename FieldType::value_type id_res = FieldType::value_type::one();
    typename FieldType::value_type sigma_res = FieldType::value_type::one();
    for (std::size_t i = 0; i < table_rows; i++) {
        for (auto &identity_polynomial : preprocessed_public_data.identity_polynomials) {
            id_res = id_res * identity_polynomial.evaluate(domain->get_domain_element(i));
        }

        for (auto &permutation_polynomial : preprocessed_public_data.permutation_polynomials) {
            sigma_res = sigma_res * permutation_polynomial.evaluate(domain->get_domain_element(i));
        }
    }
    BOOST_CHECK_MESSAGE(id_res == sigma_res, "Simple check");

    typename FieldType::value_type beta = algebra::random_element<FieldType>();
    typename FieldType::value_type gamma = algebra::random_element<FieldType>();

    id_res = FieldType::value_type::one();
    sigma_res = FieldType::value_type::one();

    for (std::size_t i = 0; i < table_rows; i++) {
        for (std::size_t j = 0; j < preprocessed_public_data.identity_polynomials.size(); j++) {
            id_res = id_res *
                     (polynomial_table[j].evaluate(domain->get_domain_element(i)) +
                      beta * preprocessed_public_data.identity_polynomials[j].evaluate(domain->get_domain_element(i)) +
                      gamma);
        }

        for (std::size_t j = 0; j < preprocessed_public_data.permutation_polynomials.size(); j++) {
            sigma_res =
                sigma_res *
                (polynomial_table[j].evaluate(domain->get_domain_element(i)) +
                 beta * preprocessed_public_data.permutation_polynomials[j].evaluate(domain->get_domain_element(i)) +
                 gamma);
        }
    }
    BOOST_CHECK_MESSAGE(id_res == sigma_res, "Complex check");
}

BOOST_AUTO_TEST_CASE(placeholder_permutation_argument_test) {

    circuit_description<FieldType, circuit_2_params, table_rows_log, permutation_size> circuit =
        circuit_test_2<FieldType>();

    constexpr std::size_t argument_size = 3;

    using policy_type = zk::snark::detail::placeholder_policy<FieldType, circuit_2_params>;

    typedef commitments::list_polynomial_commitment<FieldType,
        circuit_2_params::batched_commitment_params_type> lpc_type;

    typename fri_type::params_type fri_params = create_fri_params<fri_type, FieldType>(table_rows_log);

    plonk_table_description<FieldType, typename circuit_2_params::arithmetization_params> desc;

    desc.rows_amount = table_rows;
    desc.usable_rows_amount = usable_rows;

    typename policy_type::constraint_system_type constraint_system(circuit.gates, circuit.copy_constraints,
                                                                   circuit.lookup_gates);
    typename policy_type::variable_assignment_type assignments = circuit.table;

    std::vector<std::size_t> columns_with_copy_constraints = {0, 1, 2, 3};

    typename placeholder_public_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_public_data =
        placeholder_public_preprocessor<FieldType, circuit_2_params>::process(
            constraint_system, assignments.public_table(), desc,
            fri_params, columns_with_copy_constraints.size());

    typename placeholder_private_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_private_data =
        placeholder_private_preprocessor<FieldType, circuit_2_params>::process(constraint_system,
                                                                               assignments.private_table(), desc);

    auto polynomial_table = plonk_polynomial_table<FieldType, typename placeholder_test_params::arithmetization_params>(
        preprocessed_private_data.private_polynomial_table, preprocessed_public_data.public_polynomial_table);

    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params::transcript_hash_type> prover_transcript(
        init_blob);
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params::transcript_hash_type> verifier_transcript(
        init_blob);

    typename placeholder_permutation_argument<FieldType, circuit_2_params>::prover_result_type
        prover_res = placeholder_permutation_argument<FieldType, circuit_2_params>::prove_eval(
            constraint_system, preprocessed_public_data, desc, polynomial_table,
            fri_params, prover_transcript);

    // Challenge phase
    typename FieldType::value_type y = algebra::random_element<FieldType>();
    std::vector<typename FieldType::value_type> f_at_y(permutation_size);
    for (int i = 0; i < permutation_size; i++) {
        f_at_y[i] = polynomial_table[i].evaluate(y);
    }

    typename FieldType::value_type v_p_at_y = prover_res.permutation_polynomial.evaluate(y);
    typename FieldType::value_type v_p_at_y_shifted = prover_res.permutation_polynomial.evaluate(circuit.omega * y);

    std::array<typename FieldType::value_type, 3> verifier_res =
        placeholder_permutation_argument<FieldType, circuit_2_params>::verify_eval(
            preprocessed_public_data, y, f_at_y, v_p_at_y, v_p_at_y_shifted,
            prover_res.permutation_poly_precommitment.root(), verifier_transcript);

    typename FieldType::value_type verifier_next_challenge = verifier_transcript.template challenge<FieldType>();
    typename FieldType::value_type prover_next_challenge = prover_transcript.template challenge<FieldType>();
    BOOST_CHECK(verifier_next_challenge == prover_next_challenge);

    for (int i = 0; i < argument_size; i++) {
        BOOST_CHECK(prover_res.F[i].evaluate(y) == verifier_res[i]);
    }
}

BOOST_AUTO_TEST_CASE(placeholder_lookup_argument_test) {

    circuit_description<FieldType, circuit_3_params, table_rows_log, 0> circuit = circuit_test_3<FieldType>();

    constexpr std::size_t argument_size = 5;

    using policy_type = zk::snark::detail::placeholder_policy<FieldType, circuit_3_params>;

    typedef commitments::list_polynomial_commitment<FieldType,
        circuit_3_params::batched_commitment_params_type> lpc_type;

    typename fri_type::params_type fri_params = create_fri_params<fri_type, FieldType>(table_rows_log);

    plonk_table_description<FieldType, typename circuit_3_params::arithmetization_params> desc;

    desc.rows_amount = table_rows;
    desc.usable_rows_amount = usable_rows;

    typename policy_type::constraint_system_type constraint_system(circuit.gates, circuit.copy_constraints,
                                                                   circuit.lookup_gates);
    typename policy_type::variable_assignment_type assignments = circuit.table;

    typename placeholder_public_preprocessor<FieldType, circuit_3_params>::preprocessed_data_type
        preprocessed_public_data =
        placeholder_public_preprocessor<FieldType, circuit_3_params>::process(
            constraint_system, assignments.public_table(), desc, fri_params, 0);

    typename placeholder_private_preprocessor<FieldType, circuit_3_params>::preprocessed_data_type
        preprocessed_private_data =
        placeholder_private_preprocessor<FieldType, circuit_3_params>::process(constraint_system,
                                                                               assignments.private_table(), desc);

    auto polynomial_table =
        plonk_polynomial_table<FieldType, typename placeholder_test_params_lookups::arithmetization_params>(
            preprocessed_private_data.private_polynomial_table, preprocessed_public_data.public_polynomial_table);

    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params_lookups::transcript_hash_type>
        prover_transcript(init_blob);
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params_lookups::transcript_hash_type>
        verifier_transcript(init_blob);

    typename placeholder_lookup_argument<FieldType, lpc_type, circuit_3_params>::prover_lookup_result prover_res =
        placeholder_lookup_argument<FieldType, lpc_type, circuit_3_params>::prove_eval(
            constraint_system, preprocessed_public_data, assignments,
            fri_params, prover_transcript);

    // Challenge phase
    typename FieldType::value_type y = algebra::random_element<FieldType>();
    typename policy_type::evaluation_map columns_at_y;
    for (std::size_t i = 0; i < placeholder_test_params::witness_columns; i++) {

        std::size_t i_global_index = i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::witness);
            columns_at_y[key] = polynomial_table.witness(i).evaluate(y * circuit.omega.pow(rotation));
        }
    }
    for (std::size_t i = 0; i < 0 + placeholder_test_params_lookups::constant_columns; i++) {

        std::size_t i_global_index = placeholder_test_params_lookups::witness_columns +
                                     placeholder_test_params_lookups::public_input_columns + i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::constant);

            columns_at_y[key] = polynomial_table.constant(i).evaluate(y * circuit.omega.pow(rotation));
        }
    }
    for (std::size_t i = 0; i < placeholder_test_params_lookups::selector_columns; i++) {

        std::size_t i_global_index = placeholder_test_params_lookups::witness_columns +
                                     placeholder_test_params_lookups::constant_columns +
                                     placeholder_test_params_lookups::public_input_columns + i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::selector);

            columns_at_y[key] = polynomial_table.selector(i).evaluate(y * circuit.omega.pow(rotation));
        }
    }

    typename FieldType::value_type input_at_y = prover_res.input_polynomial.evaluate(y);
    typename FieldType::value_type input_at_y_shifted =
        prover_res.input_polynomial.evaluate(circuit.omega.inversed() * y);

    typename FieldType::value_type value_at_y = prover_res.value_polynomial.evaluate(y);

    typename FieldType::value_type v_l_at_y = prover_res.V_L_polynomial.evaluate(y);
    typename FieldType::value_type v_l_at_y_shifted = prover_res.V_L_polynomial.evaluate(circuit.omega * y);

    std::array<typename FieldType::value_type, 5> verifier_res =
        placeholder_lookup_argument<FieldType, lpc_type, circuit_3_params>::verify_eval(
            preprocessed_public_data, constraint_system.lookup_gates(), y, columns_at_y, input_at_y, input_at_y_shifted,
            value_at_y, v_l_at_y, v_l_at_y_shifted, prover_res.input_precommitment.root(),
            prover_res.value_precommitment.root(), prover_res.V_L_precommitment.root(), verifier_transcript);

    typename FieldType::value_type verifier_next_challenge = verifier_transcript.template challenge<FieldType>();
    typename FieldType::value_type prover_next_challenge = prover_transcript.template challenge<FieldType>();
    BOOST_CHECK(verifier_next_challenge == prover_next_challenge);

    for (int i = 0; i < argument_size; i++) {
        BOOST_CHECK(prover_res.F[i].evaluate(y) == verifier_res[i]);
    }
}

BOOST_AUTO_TEST_CASE(placeholder_gate_argument_test) {

    circuit_description<FieldType, circuit_2_params, table_rows_log, permutation_size> circuit =
        circuit_test_2<FieldType>();

    using policy_type = zk::snark::detail::placeholder_policy<FieldType, circuit_2_params>;

    typedef commitments::list_polynomial_commitment<FieldType, circuit_2_params::commitment_params_type> lpc_type;

    typename fri_type::params_type fri_params = create_fri_params<fri_type, FieldType>(table_rows_log);

    plonk_table_description<FieldType, typename circuit_2_params::arithmetization_params> desc;

    desc.rows_amount = table_rows;
    desc.usable_rows_amount = usable_rows;

    typename policy_type::constraint_system_type constraint_system(circuit.gates, circuit.copy_constraints,
                                                                   circuit.lookup_gates);
    typename policy_type::variable_assignment_type assignments = circuit.table;

    std::vector<std::size_t> columns_with_copy_constraints = {0, 1, 2, 3};

    typename placeholder_public_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_public_data =
        placeholder_public_preprocessor<FieldType, circuit_2_params>::process(
            constraint_system, assignments.public_table(), desc,
            fri_params, columns_with_copy_constraints.size());

    typename placeholder_private_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_private_data =
        placeholder_private_preprocessor<FieldType, circuit_2_params>::process(constraint_system,
                                                                               assignments.private_table(), desc);

    auto polynomial_table = plonk_polynomial_table<FieldType, typename placeholder_test_params::arithmetization_params>(
        preprocessed_private_data.private_polynomial_table, preprocessed_public_data.public_polynomial_table);

    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params::transcript_hash_type> prover_transcript(
        init_blob);
    transcript::fiat_shamir_heuristic_sequential<placeholder_test_params::transcript_hash_type> verifier_transcript(
        init_blob);

    std::array<math::polynomial<typename FieldType::value_type>, 1> prover_res =
        placeholder_gates_argument<FieldType, circuit_2_params>::prove_eval(
            constraint_system, polynomial_table, preprocessed_public_data.common_data.basic_domain, prover_transcript);

    // Challenge phase
    typename FieldType::value_type y = algebra::random_element<FieldType>();
    typename FieldType::value_type omega = preprocessed_public_data.common_data.basic_domain->get_domain_element(1);

    typename policy_type::evaluation_map columns_at_y;
    for (std::size_t i = 0; i < placeholder_test_params::witness_columns; i++) {

        std::size_t i_global_index = i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::witness);
            columns_at_y[key] = polynomial_table.witness(i).evaluate(y * omega.pow(rotation));
        }
    }
    for (std::size_t i = 0; i < 0 + placeholder_test_params::public_input_columns; i++) {

        std::size_t i_global_index = placeholder_test_params::witness_columns + i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {

            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::public_input);

            columns_at_y[key] = polynomial_table.public_input(i).evaluate(y * omega.pow(rotation));
        }
    }
    for (std::size_t i = 0; i < 0 + placeholder_test_params::constant_columns; i++) {

        std::size_t i_global_index =
            placeholder_test_params::witness_columns + placeholder_test_params::public_input_columns + i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::constant);

            columns_at_y[key] = polynomial_table.constant(i).evaluate(y * omega.pow(rotation));
        }
    }
    for (std::size_t i = 0; i < placeholder_test_params::selector_columns; i++) {

        std::size_t i_global_index = placeholder_test_params::witness_columns +
                                     placeholder_test_params::constant_columns +
                                     placeholder_test_params::public_input_columns + i;

        for (int rotation : preprocessed_public_data.common_data.columns_rotations[i_global_index]) {
            auto key = std::make_tuple(i, rotation, plonk_variable<FieldType>::column_type::selector);

            columns_at_y[key] = polynomial_table.selector(i).evaluate(y * omega.pow(rotation));
        }
    }

    std::array<typename FieldType::value_type, 1> verifier_res =
        placeholder_gates_argument<FieldType, circuit_2_params>::verify_eval(constraint_system.gates(), columns_at_y, y,
                                                                             verifier_transcript);

    typename FieldType::value_type verifier_next_challenge = verifier_transcript.template challenge<FieldType>();
    typename FieldType::value_type prover_next_challenge = prover_transcript.template challenge<FieldType>();
    BOOST_CHECK(verifier_next_challenge == prover_next_challenge);

    BOOST_CHECK(prover_res[0].evaluate(y) == verifier_res[0]);
}

BOOST_AUTO_TEST_CASE(placeholder_prover_basic_test) {

    circuit_description<FieldType, circuit_2_params, table_rows_log, permutation_size> circuit =
        circuit_test_2<FieldType>();

    using policy_type = zk::snark::detail::placeholder_policy<FieldType, circuit_2_params>;

    typedef commitments::list_polynomial_commitment<FieldType,
        circuit_2_params::batched_commitment_params_type> lpc_type;

    typename fri_type::params_type fri_params = create_fri_params<fri_type, FieldType>(table_rows_log);

    plonk_table_description<FieldType, typename circuit_2_params::arithmetization_params> desc;

    desc.rows_amount = table_rows;
    desc.usable_rows_amount = usable_rows;

    typename policy_type::constraint_system_type constraint_system(circuit.gates, circuit.copy_constraints,
                                                                   circuit.lookup_gates);
    typename policy_type::variable_assignment_type assignments = circuit.table;

    std::vector<std::size_t> columns_with_copy_constraints = {0, 1, 2, 3};

    typename placeholder_public_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_public_data =
        placeholder_public_preprocessor<FieldType, circuit_2_params>::process(
            constraint_system, assignments.public_table(), desc,
            fri_params, columns_with_copy_constraints.size());

    typename placeholder_private_preprocessor<FieldType, circuit_2_params>::preprocessed_data_type
        preprocessed_private_data =
        placeholder_private_preprocessor<FieldType, circuit_2_params>::process(constraint_system,
                                                                               assignments.private_table(), desc);

    auto proof = placeholder_prover<FieldType, circuit_2_params>::process(
        preprocessed_public_data, preprocessed_private_data, desc, constraint_system, assignments, fri_params);

    bool verifier_res = placeholder_verifier<FieldType, circuit_2_params>::process(preprocessed_public_data, proof,
                                                                                constraint_system, fri_params);
    BOOST_CHECK(verifier_res);
}

BOOST_AUTO_TEST_SUITE_END()


#include <fstream>

#include <boost/version.hpp>

#include "ros/ros.h"
#include "ros/console.h"
#include "pointmatcher/PointMatcher.h"
#include "pointmatcher/Timer.h"

#include "pointmatcher_ros/point_cloud.h"
#include "pointmatcher_ros/transform.h"
#include "pointmatcher_ros/get_params_from_server.h"
#include "pointmatcher_ros/ros_logger.h"

#include "nav_msgs/Odometry.h"
#include "tf/transform_broadcaster.h"
#include "tf_conversions/tf_eigen.h"
#include "tf/transform_listener.h"

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>

#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>

#include "std_msgs/String.h"
#include "std_srvs/Empty.h"

using namespace std;
using namespace PointMatcherSupport;
using namespace visualization_msgs;
using namespace interactive_markers;

class MapMaintener
{
	typedef PointMatcher<float> PM;
	typedef PM::DataPoints DP;
	typedef PM::TransformationParameters TP;

	ros::NodeHandle& n;
	ros::NodeHandle& pn;

	message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub;
	message_filters::Subscriber<nav_msgs::Odometry> odom_sub;

	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
	message_filters::Synchronizer<MySyncPolicy> sync;

	ros::Subscriber scanSub;
	ros::Subscriber cloudSub;
	ros::Publisher mapPub;

	// FIXME which service do we need?
	//ros::ServiceServer getPointMapSrv;
	//ros::ServiceServer saveMapSrv;
	//ros::ServiceServer resetSrv;

	PM::ICP icp;

	PM::DataPointsFilters inputFilters;
	PM::DataPointsFilters mapPreFilters;
	PM::DataPointsFilters mapPostFilters;
	shared_ptr<PM::Transformation> transformation;

	PM::DataPoints mapPointCloud;

	// Parameters
	string vtkFinalMapName; //!< name of the final vtk map
	int inputQueueSize;
	const float size_x;
	const float size_y;
	const float size_z;

	// Interative markers
	boost::shared_ptr<InteractiveMarkerServer> server;
	MenuHandler menu_handler;
	bool mappingActive;
	bool singleScan;
	MenuHandler::EntryHandle h_single;
	MenuHandler::EntryHandle h_start;
	MenuHandler::EntryHandle h_pause;
	MenuHandler::EntryHandle h_stop;
	MenuHandler::EntryHandle h_save;

	// tf
	tf::TransformListener tfListener;

public:
	boost::mutex publishLock;
	PM::TransformationParameters TObjectToMap;
	// Parameters
	string objectFrame;
	string mapFrame;

public:
	MapMaintener(ros::NodeHandle& n, ros::NodeHandle& pn);
	~MapMaintener();

protected:
	void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom);
	void processCloud(DP cloud, const TP TScannerToMap);

	void makeMenuMarker( std::string name );
	void addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name);
	void update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );

	void singleScanCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void startMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void pauseMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void stopMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
	void saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );

	//bool getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res);
	//bool saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res);
	//bool reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
};

MapMaintener::MapMaintener(ros::NodeHandle& n, ros::NodeHandle& pn):
	n(n),
	pn(pn),
	cloud_sub(n, "cloud_in", 1),
	odom_sub(n, "icp_odom", 1),
	sync(MySyncPolicy(1), cloud_sub, odom_sub),
	transformation(PM::get().REG(Transformation).create("RigidTransformation")),
	vtkFinalMapName(getParam<string>("vtkFinalMapName", "finalMap_maintained.vtk")),
	size_x(getParam<double>("size_x", 1.0)),
	size_y(getParam<double>("size_y", 1.0)),
	size_z(getParam<double>("size_z", 1.0)),
	mappingActive(false),
	singleScan(false),
	tfListener(ros::Duration(30)),
	TObjectToMap(PM::TransformationParameters::Identity(4,4)),
	objectFrame(getParam<string>("object_frame", "map")),
	mapFrame(getParam<string>("map_frame", "map"))
{

	// load configs
	string configFileName;
	if (ros::param::get("~icpConfig", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			icp.loadFromYaml(ifs);
		}
		else
		{
			ROS_ERROR_STREAM("Cannot load ICP config from YAML file " << configFileName);
			icp.setDefault();
		}
	}
	else
	{
		ROS_INFO_STREAM("No ICP config file given, using default");
		icp.setDefault();
	}

	if (ros::param::get("~inputFiltersConfig", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			inputFilters = PM::DataPointsFilters(ifs);
		}
		else
		{
			ROS_ERROR_STREAM("Cannot load input filters config from YAML file " << configFileName);
		}
	}
	else
	{
		ROS_INFO_STREAM("No input filters config file given, not using these filters");
	}

	if (ros::param::get("~mapPreFiltersConfig", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			mapPreFilters = PM::DataPointsFilters(ifs);

			// Add the bounding box
			PM::Parameters params;
			params["removeInside"] = toParam(0);
			params["xMin"] = toParam(-size_x/2);
			params["xMax"] = toParam(size_x/2);
			params["yMin"] = toParam(-size_y/2);
			params["yMax"] = toParam(size_y/2);
			params["zMin"] = toParam(-size_z/2);
			params["zMax"] = toParam(size_z/2);

			shared_ptr<PM::DataPointsFilter> box =
				PM::get().DataPointsFilterRegistrar.create("BoundingBoxDataPointsFilter", params);
			mapPreFilters.push_back(box);

		}
		else
		{
			ROS_ERROR_STREAM("Cannot load map pre-filters config from YAML file " << configFileName);
		}
	}
	else
	{
		ROS_INFO_STREAM("No map pre-filters config file given, not using these filters");
	}

	if (ros::param::get("~mapPostFiltersConfig", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			mapPostFilters = PM::DataPointsFilters(ifs);
		}
		else
		{
			ROS_ERROR_STREAM("Cannot load map post-filters config from YAML file " << configFileName);
		}
	}
	else
	{
		ROS_INFO_STREAM("No map post-filters config file given, not using these filters");
	}

	// topics and services initialization
	sync.registerCallback(boost::bind(&MapMaintener::gotCloud, this, _1, _2));

	//cloudSub = n.subscribe("cloud_in", inputQueueSize, &MapMaintener::gotCloud, this);
	mapPub = n.advertise<sensor_msgs::PointCloud2>("object_map", 2, true);

	//getPointMapSrv = n.advertiseService("dynamic_point_map", &MapMaintener::getPointMap, this);
	//saveMapSrv = pn.advertiseService("save_map", &MapMaintener::saveMap, this);
	//resetSrv = pn.advertiseService("reset", &MapMaintener::reset, this);

	// Setup interactive maker
  server.reset( new InteractiveMarkerServer("MapCenter","", false) );

	h_single = menu_handler.insert( "Record single scan", boost::bind(&MapMaintener::singleScanCallback, this, _1));
	h_start = menu_handler.insert( "Start object mapping", boost::bind(&MapMaintener::startMapCallback, this, _1));
	h_pause = menu_handler.insert( "Pause object mapping", boost::bind(&MapMaintener::pauseMapCallback, this, _1));
  h_stop = menu_handler.insert( "Stop/reset object mapping", boost::bind(&MapMaintener::stopMapCallback, this, _1));
	menu_handler.setVisible(h_stop, false);
	h_save = menu_handler.insert( "Save object to VTK", boost::bind(&MapMaintener::saveMapCallback, this, _1));

  makeMenuMarker( "object_menu" );
  menu_handler.apply( *server, "object_menu" );
	menu_handler.reApply( *server);
  server->applyChanges();

}

MapMaintener::~MapMaintener()
{
	server.reset();

	// save point cloud
	if (mapPointCloud.features.cols())
	{
		mapPointCloud.save(vtkFinalMapName);
	}
}

// Callback
void MapMaintener::gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom)
{
	// IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)

	if(mappingActive || singleScan)
	{
		// Convert pointcloud2 to DataPoint
		DP cloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(*cloudMsgIn));
		// Convert odometry msg to TransformationParameters
		TP TScannerToMap = 	PointMatcher_ros::odomMsgToEigenMatrix<float>(*odom);
		processCloud(cloud, TScannerToMap);
	}

	singleScan = false;
}

// Point cloud processing
void MapMaintener::processCloud(DP newPointCloud, const TP TScannerToMap)
{
	string reason;
	timer t;

	// Convert point cloud
	const size_t goodCount(newPointCloud.features.cols());
	if (goodCount == 0)
	{
		ROS_ERROR("I found no good points in the cloud");
		return;
	}

	ROS_INFO("Processing new point cloud");
	{
		timer t; // Print how long take the algo

		// Apply filters to incoming cloud, in scanner coordinates
		inputFilters.apply(newPointCloud);

		ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
	}

	// Correct new points using ICP result and move them in their own frame
	//cout << "TObjectToMap: " << endl << TObjectToMap << endl;
	//cout << "TScannerToMap: " << endl << TScannerToMap << endl;
	//cout << "concat: " << endl << TObjectToMap.inverse() * TScannerToMap << endl;
	newPointCloud = transformation->compute(newPointCloud, transformation->correctParameters(TObjectToMap.inverse() * TScannerToMap));

	// Preparation of cloud for inclusion in map
	mapPreFilters.apply(newPointCloud);

	// FIXME put that as parameter
	if(newPointCloud.features.cols() < 400)
		return;

	// Generate first map
	if(!mapPointCloud.features.rows())
	{
		mapPointCloud = newPointCloud;
		return;
	}

	// Check dimension
	if (newPointCloud.features.rows() != mapPointCloud.features.rows())
	{
		ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud.features.rows()-1 << " while map is " << mapPointCloud.features.rows()-1);
		return;
	}

	PM::TransformationParameters Tcorr;
	try
	{
		Tcorr = icp(newPointCloud, mapPointCloud);
		TObjectToMap = TObjectToMap * Tcorr.inverse();
	}
	catch (PM::ConvergenceError error)
	{
		ROS_WARN_STREAM("ICP failed to converge: " << error.what());
		return;
	}

	const double estimatedOverlap = icp.errorMinimizer->getOverlap();
	if(estimatedOverlap < 0.40)
	{
		ROS_WARN_STREAM("Estimated overlap too small: " << estimatedOverlap);
		return;
	}

	ROS_DEBUG_STREAM("Tcorr:\n" << Tcorr);

	cout << "Tcorr: " << endl << Tcorr << endl;
	newPointCloud = transformation->compute(newPointCloud, Tcorr);
	// Merge point clouds to map
	mapPointCloud.concatenate(newPointCloud);

	// Map maintenance
	mapPostFilters.apply(mapPointCloud);

	ROS_INFO_STREAM("New map available (" << mapPointCloud.features.cols() << " pts), update took " << t.elapsed() << " [s]");

	// Publish map point cloud
	// FIXME this crash when used without descriptor
	if (mapPub.getNumSubscribers())
		mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(mapPointCloud, objectFrame, ros::Time::now()));

	ROS_INFO_STREAM("Total map merging: " << t.elapsed() << " [s]");

	//ros::Rate r(2);
	//r.sleep();
}

// SERVICES definitions
//bool MapMaintener::getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res)
//{
//	if (!mapPointCloud)
//		return false;
//
//	res.map = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, ros::Time::now());
//	return true;
//}
//
//bool MapMaintener::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
//{
//	if (!mapPointCloud)
//		return false;
//
//	try
//	{
//		mapPointCloud->save(req.filename.data);
//	}
//	catch (const std::runtime_error& e)
//	{
//		return false;
//	}
//
//	return true;
//}
//
//bool MapMaintener::reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
//{
//	// note: no need for locking as we do ros::spin(), to update if we go for multi-threading
//	TOdomToMap = PM::TransformationParameters::Identity(4,4);
//	icp.clearMap();
//	return true;
//}

//================================================
// Markers
void MapMaintener::makeMenuMarker( std::string name )
{
	InteractiveMarker int_marker;

	// Header
	// TODO: adapt that for real system
	int_marker.header.frame_id = mapFrame;
	int_marker.pose.position.x = 0.0;
	int_marker.pose.position.y = 0.0;
	int_marker.pose.position.z = 0.0;

	int_marker.scale = 1;

 	// Information
	int_marker.name = name;
	//int_marker.description = "Move to the zone of interest";

	// Create 6 DoF control axis
	addRotAndTransCtrl(int_marker, 1, 1, 0, 0, "x");
	addRotAndTransCtrl(int_marker, 1, 0, 1, 0, "z");
	addRotAndTransCtrl(int_marker, 1, 0, 0, 1, "y");

	// Create a gray box to support a menu
	Marker grayBox;
  grayBox.type = Marker::CUBE;
  grayBox.scale.x = size_x;
  grayBox.scale.y = size_y;
  grayBox.scale.z = size_z;
  grayBox.color.r = 0.5;
  grayBox.color.g = 0.5;
  grayBox.color.b = 0.5;
  grayBox.color.a = 0.5;

	InteractiveMarkerControl control;
	control = InteractiveMarkerControl();
  control.markers.push_back(grayBox);
  control.interaction_mode = InteractiveMarkerControl::BUTTON;
	int_marker.controls.push_back(control);
	int_marker.scale = std::max(std::max(size_x, size_y), size_z);

  server->insert( int_marker );
	server->setCallback(int_marker.name, boost::bind(&MapMaintener::update_tf, this, _1), visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
}

void MapMaintener::addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name)
{
	InteractiveMarkerControl control;
	control.orientation.w = w;
	control.orientation.x = x;
	control.orientation.y = y;
	control.orientation.z = z;
	//control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
	control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
	control.name = "rotate_" + name;

  int_marker.controls.push_back( control );

}

// Marker callback
void MapMaintener::update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	//InteractiveMarker int_marker;
	//server->get("object_menu", int_marker);

	if(mapPointCloud.features.cols() == 0)
	{
		nav_msgs::Odometry odom;
		odom.pose.pose = feedback->pose;
		publishLock.lock();
		TObjectToMap = 	PointMatcher_ros::odomMsgToEigenMatrix<float>(odom);
		publishLock.unlock();

		//int_marker.description = "Move to the zone of interest";
		menu_handler.reApply( *server);
		server->applyChanges();
	}
	else
	{
		//int_marker.description = "Reset the map to apply change";
		menu_handler.reApply( *server);
		server->applyChanges();
	}
}

void MapMaintener::singleScanCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
		singleScan = true;
}

void MapMaintener::startMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	menu_handler.setVisible(h_start, false);
	menu_handler.setVisible(h_pause, true);
	menu_handler.setVisible(h_stop, true);
	menu_handler.reApply( *server);
  server->applyChanges();
	mappingActive = true;
}

void MapMaintener::pauseMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	menu_handler.setVisible(h_start, true);
	menu_handler.setVisible(h_pause, false);
	menu_handler.setVisible(h_stop, true);
	menu_handler.reApply( *server);
  server->applyChanges();
	mappingActive = false;
}

void MapMaintener::stopMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	menu_handler.setVisible(h_start, true);
	menu_handler.setVisible(h_pause, false);
	menu_handler.setVisible(h_stop, false);
	menu_handler.reApply( *server);
  server->applyChanges();
	mappingActive = false;

	mapPointCloud = PM::DataPoints();
	update_tf(feedback);
}

void MapMaintener::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	// save point cloud
	if (mapPointCloud.features.cols())
	{
		mapPointCloud.save(vtkFinalMapName);
	}
}

// Main function supporting the MapMaintener class
int main(int argc, char **argv)
{
	ros::init(argc, argv, "map_maintainer");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	MapMaintener map(n, pn);

	tf::TransformBroadcaster tfBroadcaster;

	ros::Rate r(50);
	while(ros::ok())
	{
		map.publishLock.lock();
		tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(map.TObjectToMap,  map.mapFrame, map.objectFrame, ros::Time::now()));
		map.publishLock.unlock();
		ros::spinOnce();
		r.sleep();
	}

	return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_CONTAINER_GRID_DETAIL_POS_ITERATOR_BASE_HPP_INCLUDED
#define FCPPT_CONTAINER_GRID_DETAIL_POS_ITERATOR_BASE_HPP_INCLUDED

#include <fcppt/container/grid/pos_fwd.hpp>
#include <fcppt/container/grid/pos_iterator_fwd.hpp>
#include <fcppt/container/grid/size_type.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <iterator>
#include <type_traits>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace container
{
namespace grid
{
namespace detail
{

template<
	typename SizeType,
	fcppt::container::grid::size_type Size
>
using pos_iterator_base
=
boost::iterator_facade<
	fcppt::container::grid::pos_iterator<
		SizeType,
		Size
	>,
	fcppt::container::grid::pos<
		SizeType,
		Size
	>,
	std::forward_iterator_tag,
	fcppt::container::grid::pos<
		SizeType,
		Size
	>,
	typename
	std::make_signed<
		SizeType
	>::type
>;

}
}
}
}

#endif


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

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

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

#include <boost/simd/function/definition/is_ngt.hpp>
#include <boost/simd/arch/common/scalar/function/is_not_greater.hpp>

#endif