//
//  cnn.cpp
//  Jacobian
//
//  Created by David Freifeld
//

#include "bpnn.hpp"
#include "utils.hpp"

//#include <Eigen/unsupported/CXX11/Tensor>
#include "cnn.hpp"

#define LARGE_NUM 1000000 // Remove me.

#if (!RECKLESS)
#define checknan(x, loc) if(x==INFINITY || x==NAN || x == -INFINITY) throw ValueError("Detected NaN in operation", loc)
#else
#define checknan(x, loc)
#endif

// NOTE: Below three functions not mine, from https://compvisionlab.wordpress.com/2014/01/01/c-code-for-reading-mnist-data-set/
int ReverseInt (int i)
{
    unsigned char ch1, ch2, ch3, ch4;
    ch1=i&255;
    ch2=(i>>8)&255;
    ch3=(i>>16)&255;
    ch4=(i>>24)&255;
    return((int)ch1<<24)+((int)ch2<<16)+((int)ch3<<8)+ch4;
}

void ReadMNIST(int NumberOfImages, int DataOfAnImage,std::vector<std::vector<double>> &arr)
{
    arr.resize(NumberOfImages,std::vector<double>(DataOfAnImage));
    std::ifstream file ("./t10k-images-idx3-ubyte",std::ios::binary);
    if (file.is_open())
    {
        int magic_number=0;
        int number_of_images=0;
        int n_rows=0;
        int n_cols=0;
        file.read((char*)&magic_number,sizeof(magic_number));
        magic_number= ReverseInt(magic_number);
        file.read((char*)&number_of_images,sizeof(number_of_images));
        number_of_images= ReverseInt(number_of_images);
        file.read((char*)&n_rows,sizeof(n_rows));
        n_rows= ReverseInt(n_rows);
        file.read((char*)&n_cols,sizeof(n_cols));
        n_cols= ReverseInt(n_cols);
        for(int i=0;i<number_of_images;++i)
        {
            for(int r=0;r<n_rows;++r)
            {
                for(int c=0;c<n_cols;++c)
                {
                    unsigned char temp=0;
                    file.read((char*)&temp,sizeof(temp));
                    arr[i][(n_rows*r)+c]= (double)temp;
                }
            }
        }
    }
}

unsigned char* read_mnist_labels(std::string full_path, int number_of_labels) {
    auto reverseInt = [](int i) {
        unsigned char c1, c2, c3, c4;
        c1 = i & 255, c2 = (i >> 8) & 255, c3 = (i >> 16) & 255, c4 = (i >> 24) & 255;
        return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
    };

    typedef unsigned char uchar;

    std::ifstream file(full_path, std::ios::binary);

    if(file.is_open()) {
        int magic_number = 0;
        file.read((char *)&magic_number, sizeof(magic_number));
        magic_number = reverseInt(magic_number);

        if(magic_number != 2049) throw std::runtime_error("Invalid MNIST label file!");

        file.read((char *)&number_of_labels, sizeof(number_of_labels)), number_of_labels = reverseInt(number_of_labels);

        uchar* _dataset = new uchar[number_of_labels];
        for(int i = 0; i < number_of_labels; i++) {
            file.read((char*)&_dataset[i], 1);
        }
        return _dataset;
    } else {
        throw std::runtime_error("Unable to open file `" + full_path + "`!");
    }
}

ConvLayer::ConvLayer(int x, int y, int stride, int kern_x, int kern_y, int pad, std::function<float(float)> activ, std::function<float(float)> activ_deriv)
    :stride_len(stride), padding(pad), activation(activ), activation_deriv(activ_deriv)
{
    pad*=2;
    input = new Eigen::MatrixXf (x+pad,y+pad);
    dZ = new Eigen::MatrixXf (x+pad,y+pad);
    for (int i = 0; i < (x+pad)*(y+pad); i++) {
        (*input)((int)i / (y+pad),i%(y+pad)) = 0;
        (*dZ)((int)i / (y+pad),i%(y+pad)) = 0;
    }
    kernel = new Eigen::MatrixXf (kern_x, kern_y);
    for (int i = 0; i < kern_x*kern_y; i++) {
        (*kernel)((int)i / kern_y,i%kern_y) = (float) rand() / RAND_MAX;
    }
    output = new Eigen::MatrixXf ((x-kern_x+1+pad/stride_len), (y-kern_y+1+pad/stride_len));
    for (int i = 0; i < (x-kern_y+1+pad/stride_len)*(y-kern_x+1+pad/stride_len); i++) {
        (*output)((int)i / (y-kern_y+1+pad/stride_len),i%(y-kern_y+1+pad/stride_len)) = 0;
    }
    bias = 0;
};

void ConvLayer::convolute()
{
    for (int i = 0; i < (*input).rows(); i++) {
        for (int j = 0; j < (*input).cols(); j++) {
            (*dZ)(i, j) = activation_deriv((*input)(i, j));
            (*input)(i, j) = activation((*input)(i, j));
        }
    }
    for (int i = 0; i < output->rows(); i+=stride_len) {
        for (int j = 0; j < output->cols(); j+=stride_len) {
            (*output)(i, j) = (*kernel * (input->block(i, j, kernel->rows(), kernel->cols()))).sum();
        }
    }
    *output = (output->array() + bias).matrix();
}

void ConvLayer::set_input(Eigen::MatrixXf* matrix)
{
    input->block(padding, padding, matrix->rows(), matrix->cols()) = *matrix;
}

// Will eventually be different from ConvLayer
PoolingLayer::PoolingLayer(int x, int y, int stride, int kern_x, int kern_y, int pad)
    :stride_len(stride), padding(pad)
{
    input = new Eigen::MatrixXf (x+pad,y+pad);
    for (int i = 0; i < (x+pad)*(y+pad); i++) {
        (*input)((int)i / (y+pad),i%(y+pad)) = 0;
    }
    kernel = new Eigen::MatrixXf (kern_x, kern_y);
    for (int i = 0; i < kern_x*kern_y; i++) {
        (*kernel)((int)i / kern_y,i%kern_y) = (float) rand()/RAND_MAX;
    }
    output = new Eigen::MatrixXf (x-kern_x+1, y-kern_y+1);
    for (int i = 0; i < (x-kern_x+1)*(y-kern_y+1); i++) {
        (*output)((int)i / (y-kern_y+1),i%(y-kern_y+1)) = (float) rand()/RAND_MAX;
    }
};

void PoolingLayer::pool()
{
  // It doesn't look like anything better than O(n^4) is doable for this as kernel needs to go through matrix and you need to index kernel. LOOK INTO ME!!
    float maxnum = -LARGE_NUM;
    for (int i = 0; i < input->cols() - kernel->cols(); i+=stride_len) {
        for (int j = 0; j < input->rows() - kernel->rows(); j+=stride_len) {
            for (int k = 0; k < kernel->cols(); k++) {
                for (int l = 0; l < kernel->rows(); l++) {
                    if ((input->block(j, i, kernel->rows(), kernel->cols()))(l, k) > maxnum) {
                        maxnum = (input->block(j, i, kernel->rows(), kernel->cols()))(l, k);
                    }
                }
            }
        }
    }
}

ConvNet::ConvNet(const char* path, float learn_rate, float bias_rate, Regularization reg, float l, float ratio)
    :Network(path, 1, learn_rate, bias_rate, reg, l, ratio), preprocess_length{0}
{
    ReadMNIST(10000,784,data);
    data_labels = read_mnist_labels("./t10k-labels-idx1-ubyte",10000);
    labels = new Eigen::MatrixXf (1, 1);
}

void ConvNet::add_conv_layer(int x, int y, int stride, int kern_x, int kern_y, int pad, std::function<float(float)> activ, std::function<float(float)> activ_deriv)
{
    preprocess_length+=1;
    conv_layers.emplace_back(x,y,stride,kern_x, kern_y,pad,activ,activ_deriv);
}

// May make this inaccessible to user code and just have it called from add_conv_layer as pooling is basically always paired with conv.
void ConvNet::add_pool_layer(int x, int y, int stride, int kern_x, int kern_y, int pad)
{
    pool_layers.emplace_back(x,y,stride,kern_x,kern_y,pad);
}

void ConvNet::initialize()
{
    for (int i = 0; i < length-1; i++) {
        layers[i].init_weights(layers[i+1]);
    }
}

void ConvNet::next_batch()
{
    for (int i = 0; i < 784; i++) {
        (*conv_layers[0].input)(i/28, i%28) = data[batches][i];
    }
    (*labels)(0,0) = (float)(int)data_labels[batches];
}

void ConvNet::process()
{
    // Assumes pooling is immediately after any conv layer.
    for (int i = 0; i < preprocess_length-1; i++) {
        conv_layers[i].convolute();
        //   pool_layers[i].input = conv_layers[i].output;
        //   pool_layers[i].pool();
        conv_layers[i+1].input = conv_layers[i].output;
    }
    conv_layers[preprocess_length-1].convolute();
    //pool_layers[preprocess_length-1].input = conv_layers[preprocess_length-1].output;
    //pool_layers[preprocess_length-1].pool();
    //  std::cout << "Output:\n" << *pool_layers[preprocess_length-1].output << "\n\n";
    Eigen::Map<Eigen::RowVectorXf> flattened (conv_layers[preprocess_length-1].output->data(), conv_layers[preprocess_length-1].output->size());
    // std::cout << "Flattened:\n" << flattened << "\n\n";
    for (int i = 0; i < flattened.cols(); i++) {
        (*layers[0].contents)(0, i) = flattened[i];
    }
}

void ConvNet::set_label(Eigen::MatrixXf newlabels)
{
    *labels = newlabels;
}

void ConvNet::list_net()
{
    for (int i = 0; i < preprocess_length; i++) {
        std::cout << "-----------------------\nCONVOLUTIONAL LAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nStride: " << conv_layers[i].stride_len << "\nPadding: " << conv_layers[i].padding <<  "\n\n\u001b[31mINPUT:\x1B[0;37m\n" << *conv_layers[i].input << "\n\n\u001b[31mKERNEL:\x1B[0;37m\n" << *conv_layers[i].kernel << "\n\n\u001b[31mOUTPUT:\x1B[0;37m\n" << *conv_layers[i].output << "\n\n\u001b[31mBIAS:\x1B[0;37m\n" << conv_layers[i].bias << "\n\n\n";
        //std::cout << "-----------------------\nPOOLING LAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nStride: " << pool_layers[i].stride_len << "\nPadding: " << conv_layers[i].padding << "\n\n\u001b[31mINPUT:\x1B[0;37m\n" << *pool_layers[i].input << "\n\n\u001b[31mKERNEL:\x1B[0;37m\n-" << *pool_layers[i].kernel << "\n\n\u001b[31mOUTPUT:\x1B[0;37m\n" << *pool_layers[i].output << "\n\n\n";
    }
    std::cout << "-----------------------\nINPUT LAYER (LAYER 0)\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[0].activation_str << "\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[0].contents << "\n\n\u001b[31mWEIGHTS:\x1B[0;37m\n" << *layers[0].weights << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[0].bias << "\n\n\n";
    for (int i = 1; i < length-1; i++) {
        std::cout << "-----------------------\nLAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[i].activation_str << "\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[i].contents << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[i].bias << "\n\n\u001b[31mWEIGHTS:\x1B[0;37m\n" << *layers[i].weights << "\n\n\n";
    }
    std::cout << "-----------------------\nOUTPUT LAYER (LAYER " << length-1 << ")\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[length-1].activation_str <<"\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[length-1].contents << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[length-1].bias <<  "\n\n\n";
}

void ConvNet::backpropagate()
{
    list_net();
    char a;
    std::cin >> a;
    std::vector<Eigen::MatrixXf> gradients;
    gradients.push_back(Network::backpropagate());
    Eigen::Map<Eigen::MatrixXf> reshaped(gradients[gradients.size()-1].data(),
                           conv_layers.back().output->rows(),
                           conv_layers.back().output->cols());
    gradients[gradients.size()-1] = reshaped;
    std::vector<Eigen::MatrixXf> conv_deltas;
    for (int layer = conv_layers.size()-1; layer >= 0; layer--) {
        conv_deltas.emplace_back(conv_layers[layer].kernel->rows(),
                                 conv_layers[layer].kernel->cols());
        std::cout << conv_layers[layer].input->cols() << " " << gradients.back().cols() << "\n";
        for (int i = 0; i < conv_layers[layer].input->rows() - gradients.back().rows() + 1; i++) {
            for (int j = 0; j < conv_layers[layer].input->cols() - gradients.back().cols() + 1; j++) {
                conv_deltas[conv_deltas.size()-1](i, j) = (gradients.back() * conv_layers[layer].input->block(i, j, gradients.back().rows(), gradients.back().cols())).sum();
            }
        }
        *conv_layers[layer].kernel -= conv_deltas.back();

        Eigen::MatrixXf flipped_kernel =
            Eigen::MatrixXf::Zero(conv_layers[layer].kernel->rows(), conv_layers[layer].kernel->cols());
        flipped_kernel = conv_layers[layer].kernel->transpose().colwise().reverse().transpose().colwise().reverse();
        Eigen::MatrixXf padded_grad = Eigen::MatrixXf::Zero(gradients.back().rows() + ((flipped_kernel.rows() - 1)*2), gradients.back().cols() + ((flipped_kernel.cols() - 1)*2));
        padded_grad.block(flipped_kernel.rows() - 1, flipped_kernel.cols() - 1, gradients.back().rows(), gradients.back().cols()) = gradients.back();
        Eigen::MatrixXf final_grad (padded_grad.rows() - flipped_kernel.rows() + 1, padded_grad.cols() - flipped_kernel.cols() + 1);
        for (int i = 0; i < padded_grad.rows() - flipped_kernel.rows() + 1; i++) {
            for (int j = 0; j < padded_grad.cols() - gradients.back().cols() + 1; j++) {
                final_grad(i, j) = (flipped_kernel * padded_grad.block(i, j, flipped_kernel.rows(), flipped_kernel.cols())).sum();
            }
        }
        gradients.push_back(final_grad.cwiseProduct(*conv_layers[layer].dZ));
    }
    list_net();
    assert(2<1);
}

void ConvNet::train()
{
    float cost_sum = 0;
    float acc_sum = 0;
    for (int i = 0; i <= 100; i++) {
        if (i != instances-batch_size) { // Don't try to advance batch on final batch.
            next_batch();
        }
        process();
        feedforward();
        backpropagate();
        cost_sum += cost();
        acc_sum += accuracy();
        batches++;
    }
    list_net();
    epoch_acc = 1.0/(100) * acc_sum;
    epoch_cost = 1.0/(100) * cost_sum;
    printf("Epoch %i complete - cost %f - acc %f\n", epochs, epoch_cost, epoch_acc);
    batches=0;
    decay();
    epochs++;
}

int main()
{
    ConvNet net ("../data_banknote_authentication.txt", 0.05, 0.01, L2, 0, 0.9);
    Eigen::MatrixXf labels (1,1);
    net.add_conv_layer(28, 28, 1, 9, 9, 0, lecun_tanh, lecun_tanh_deriv);
    //  net.add_pool_layer(20,20,1,6,6,0);
    net.add_conv_layer(20, 20, 1, 6, 6, 0, lecun_tanh, lecun_tanh_deriv);
    std::cout << net.conv_layers[net.conv_layers.size()-1].output->rows() << "\n";
    //net.add_pool_layer(10,10,1,2,2,0);
    net.add_layer(400, "sigmoid", sigmoid, sigmoid_deriv);
    net.add_layer(5, "lecun_tanh", lecun_tanh, lecun_tanh_deriv);
    net.add_layer(10, "resig", rectifier(sigmoid), rectifier(sigmoid_deriv));
    //  net.list_net();
    //  net.init_decay("step", 1, 2);
    net.initialize();
    //net.list_net();

    for (int i = 0; i < 1; i++) {
        net.train();
    }
    net.list_net();
}


#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <chrono>
#include "oful.h"
#include "contlinrep.h"
#include "utils.h"
#include "gzip.h"
#include "bandit.h"
#include "leader.h"
#include "adversarial_master.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

int PREC = 4;
int EVERY = 1;

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    srand (time(NULL));
    // rng.seed(10000); // warm it up
    int n_runs = 20, T = 1000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 0.2;
    double bonus_scale = 1;
    bool adaptive_ci = true;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    vec2double regrets(n_runs), pseudo_regrets(n_runs);

    ContToy1_phi1 lrep1 = ContToy1_phi1(noise_std, seeds[0]);
    ContToy1_phi2 lrep2 = ContToy1_phi2(noise_std, seeds[0]);

    std::cerr << "OFUL" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<std::vector<double>> localg(lrep1, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep1, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "$\\phi#1$_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "$\\phi#1$_pseudoregrets.csv.gz", EVERY, PREC);

    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<std::vector<double>> localg(lrep2, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep2, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "$\\phi#2$_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "$\\phi#2$_pseudoregrets.csv.gz", EVERY, PREC);

    std::vector<std::shared_ptr<ContRepresentation<std::vector<double>>>> lreps;

    auto tmp = std::make_shared<ContToy1_phi1>(lrep1);
    lreps.push_back(tmp);
    tmp = std::make_shared<ContToy1_phi2>(lrep2);
    lreps.push_back(tmp);

    //LEADER
    std::cerr << "LEADER" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        LEADER<std::vector<double>> localg(lreps, reg_val, noise_std, bonus_scale, delta/lreps.size(), adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(*lreps[0], localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
        // delete localg;
    }
    save_vector_csv_gzip(regrets, "\\algo_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "\\algo_pseudoregrets.csv.gz", EVERY, PREC);

    //EXP4.IX
    std::cerr << "EXP4" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        std::vector<std::shared_ptr<Algo<std::vector<double>>>> base_algs;
        for(auto& ll : lreps)
        {
            base_algs.push_back(
                std::make_shared<OFUL<std::vector<double>>>(
                    OFUL<std::vector<double>>(*tmp, reg_val,noise_std,bonus_scale,delta,adaptive_ci)
                )
            );
        }
        double exp4_gamma = sqrt(2*log(base_algs.size())/(4*T));
        double exp4_lr = 2*exp4_gamma;
        EXP4dotIX<std::vector<double>> localg(base_algs, exp4_lr, exp4_gamma, seeds[i]);
        ContBanditProblem<std::vector<double>> prb(*lreps[0], localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "\\expfour_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "\\expfour_pseudoregrets.csv.gz", EVERY, PREC);

    return 0;
}


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/zernike.h>
#include <scitbx/math/zernike_moments.h>

namespace scitbx { namespace math {
namespace {

//
//
  struct grid_wrapper
  {
    typedef grid  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("sphere_grid", no_init)
        .def( init<
                   int const&,
                   int const&
                  >
             ((
                arg("np"),
                arg("n_max")
             ))
            )
        .def("get_ss", &w_t::get_ss)
        .def("get_ss", &w_t::get_all_ss)
        .def("clean_space", &w_t::clean_space)
        .def("construct_space_sum",&w_t::construct_space_sum)
        .def("construct_space_sum_via_list",&w_t::construct_space_sum_via_list)
        .def("construct_space_sum_via_list",&w_t::construct_space_sum_via_list_only)
        .def("unit_sphere_index", &w_t::unit_sphere_index)
        .def("occupied_sites", &w_t::occupied_sites)
      ;
    }
  };

//
//

  struct moments_wrapper
  {
    typedef moments  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("zernike_moments", no_init)
        .def( init<
                   grid<double>,
                   int const&
                  >
             ((
                arg("grid"),
                arg("nmax")
             ))
            )
        .def("moments", &w_t::all_moments)
        .def("get_moment",&w_t::get_moment)
        .def("calc_moments",&w_t::calc_moments)
        .def("update_ss",&w_t::update_ss)
        .def("fnn",&w_t::fnn)
        .def("fnl",&w_t::fnl)
        .def("fnnl",&w_t::fnnl)
      ;
    }
  };

//
//
  struct voxel_wrapper
  {
    typedef voxel < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("sphere_voxel", no_init)
        .def( init<
                   int const&,
                   int const&,
                   bool const&,
                   bool const&,
                   double const&,
                   double const&,
                   double const&,
                   scitbx::af::const_ref< scitbx::vec3<double> >,
                   scitbx::af::const_ref< double >
                  >
             ((
                arg("np"),
                arg("splat_range"),
                arg("uniform"),
                arg("fixed_dx"),
                arg("external_rmax"),
                arg("fraction"),
                arg("dx"),
                arg("xyz"),
                arg("density")
             ))
            )
        .def("value", &w_t::get_value)
        .def("rmax", &w_t::rmax)
        .def("rg", &w_t::rg)
        .def("map", &w_t::map)
        .def("xyz", &w_t::xyz)
        .def("rotate", &w_t::rotate)
        .def("np",&w_t::np)
        .def("weight_sum",&w_t::weight_sum)
        .def("occupied_sites", &w_t::occupied_sites)
        .def("status", &w_t::print_status)
        .def("border", &w_t::border)
      ;
    }
  };

} //namespace <anonymous>

namespace boost_python {

  void wrap_zernike_mom()
  {
    voxel_wrapper::wrap();
    moments_wrapper::wrap();
    grid_wrapper::wrap();
  }

}}}


#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>

namespace gtsam
{
  class LidarPlaneFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarPlaneFactor2;

  public:
    LidarPlaneFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarPlaneFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1w).dot(uw)
              = (T2*p2 - T1*p1).dot(R1*u)
    dplane_err/dT1 = (T2*p2 - T1*p1)'* d(R1*u)/dT1 - (R1*u)'*d(T1*p1)/dT1
    dplane_err/dT2 = (R1*u)'*d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix&> H1 = boost::none,
                         boost::optional<Matrix&> H2 = boost::none) const
    {
      const auto &rotation1 = pose1.rotation().matrix();
      const auto &rotation2 = pose2.rotation().matrix();
      const auto p12w = pose2.transformFrom(p2_) - pose1.transformFrom(p1_);
      const auto uw = rotation1 * u_;
      if (H1)
        *H1 = p12w.transpose() * rotation1 * (Matrix36() << skewSymmetric(-u_[0], -u_[1], -u_[2]), Z_3x3).finished()
            - uw.transpose() * rotation1 * (Matrix36() << skewSymmetric(-p1_[0], -p1_[1], -p1_[2]), I_3x3).finished();
      if (H2)
        *H2 = uw.transpose() * rotation2 * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return Vector1(p12w.dot(uw));
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarPlaneFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Plane Point: " << p1_.transpose() << "\n"
           << "  Plane norm Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;
  }; // class LidarPlaneFactor2

  class LidarPlaneFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarPlaneFactor1;

  public:
    LidarPlaneFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarPlaneFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1w).dot(uw)
              = (T2*p2 - T1*p1).dot(R1*u)
    dplane_err/dT1 = (T2*p2 - T1*p1)'* d(R1*u)/dT1 - (R1*u)'*d(T1*p1)/dT1
    dplane_err/dT2 = (R1*u)'*d(T2*p2)/dT2
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix&> H = boost::none) const
    {
      const auto &rotation = pose.rotation();
      if (H)
        *H = u_.transpose() * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return Vector1((pose.transformFrom(p2_) - p1_).dot(u_));
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarPlaneFactor1 on (" << keyFormatter(key()) <<  ")\n"
           << "  Plane Point: " << p1_.transpose() << "\n"
           << "  Plane norm Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;
  }; // class LidarPlaneFactor1

} // namespace gtsam


#include "SignedDistance.h"
#include "PointTriangleDistance.h"
#include <boost/assert.hpp>

using namespace MeshSdf;

SignedDistance::SignedDistance(Mesh mesh)
	: mesh(std::forward<Mesh>(mesh))
	, meshadj(this->mesh.tris, static_cast<int>(this->mesh.verts.size()))
	, usdist(this->mesh.verts, this->mesh.tris)
{
}

double SignedDistance::operator()(double x, double y, double z) const
{
	auto udinfo = usdist(x, y, z, 0);

	auto pnormal = PseudoNormal(udinfo.triEntityNearest, udinfo.triNearest);
	auto qc = Vec3{ x,y,z } - udinfo.triPtNearest;
	auto sign = boost::math::sign(qc.Dot(pnormal));

	return sign * udinfo.udist;
}

Vec3 SignedDistance::PseudoNormal_Vertex(int vnearest) const
{
	Vec3 pnormal{ 0.0, 0.0, 0.0 };

	auto trisadj = meshadj.Vert2Tris(vnearest);
	BOOST_ASSERT_MSG(!trisadj.empty(), "no incident triangles");

	for (auto itriadj : trisadj)
	{
		auto const& triadj = mesh.tris.at(itriadj);

		// find other 2 vertices of tri
		auto edgevert0 = -1;
		auto edgevert1 = -1;

		if (vnearest == triadj[0])
		{
			edgevert0 = triadj[1];
			edgevert1 = triadj[2];
		}
		else if (vnearest == triadj[1])
		{
			edgevert0 = triadj[0];
			edgevert1 = triadj[2];
		}
		else if (vnearest == triadj[2])
		{
			edgevert0 = triadj[0];
			edgevert1 = triadj[1];
		}
		else
		{
			BOOST_ASSERT_MSG(false, "no nearest vertex");
		}

		// weigh triangle normal by incident angle at nearest vertex
		auto edge1 = mesh.verts.at(edgevert0) - mesh.verts.at(vnearest);
		auto edge2 = mesh.verts.at(edgevert1) - mesh.verts.at(vnearest);

		auto incidentAngle = acos(edge1.Dot(edge2) / (edge1.Norm()*edge2.Norm()));
		pnormal += incidentAngle * mesh.triNormals.at(itriadj);
	}

	return pnormal.Normalized();
}

Vec3 SignedDistance::PseudoNormal_Edge(NearestTriEntity ent, int tri) const
{
	auto const& triadj = mesh.tris.at(tri);

	// find edge vertices
	auto nearestvert0 = -1;
	auto nearestvert1 = -1;

	if (ent == NearestTriEntity::Edge0)
	{
		nearestvert0 = triadj[0];
		nearestvert1 = triadj[1];
	}
	else if (ent == NearestTriEntity::Edge1)
	{
		nearestvert0 = triadj[1];
		nearestvert1 = triadj[2];
	}
	else if (ent == NearestTriEntity::Edge2)
	{
		nearestvert0 = triadj[2];
		nearestvert1 = triadj[0];
	}

	// equal weight for triangle normal of both adjacent triangles
	auto adjtris = meshadj.Edge2Tris(nearestvert0, nearestvert1);

	BOOST_ASSERT_MSG(adjtris.size() == 1 || adjtris.size() == 2,
		"invalid number of incident triangles");

	Vec3 pnormal{ 0.0, 0.0 };

	for (auto adjtri : adjtris)
	{
		pnormal += mesh.triNormals.at(adjtri);
	}

	return pnormal.Normalized();
}

Vec3 SignedDistance::PseudoNormal(NearestTriEntity ent, int tri) const
{
	switch (ent)
	{
		case NearestTriEntity::Vert0: return PseudoNormal_Vertex(mesh.tris.at(tri)[0]);
		case NearestTriEntity::Vert1: return PseudoNormal_Vertex(mesh.tris.at(tri)[1]);
		case NearestTriEntity::Vert2: return PseudoNormal_Vertex(mesh.tris.at(tri)[2]);
		case NearestTriEntity::Edge0:
		case NearestTriEntity::Edge1:
		case NearestTriEntity::Edge2: return PseudoNormal_Edge(ent, tri);
		case NearestTriEntity::Face: return mesh.triNormals.at(tri);
		default: throw std::runtime_error{"unknown entity"};
	}

	return {};
}


/*

  Código con grafo ya establecido (se encuentra en el main)
  Se realizan y se calcula el tiempo de insertar un vertice y una arista, eliminar un vertice y una arista, DFS, BFS, Prim, Kruskal, Dijkstra y Floyd Warshall
  Equipo: Sebastián Medina  y Carlos de la Garza

*/

#include <iostream>
#include <deque>
#include <iterator>
#include <vector>
#include <time.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/exterior_property.hpp>

using namespace std;
using namespace boost;

struct node{
    float id;
};

typedef property<edge_weight_t, float> EdgeWeightProperty;
typedef adjacency_list<listS,vecS,directedS,node,EdgeWeightProperty> DirectedGraph;
typedef property_map<DirectedGraph, edge_weight_t>::type EdgeWeightMap;
typedef graph_traits<DirectedGraph>::vertex_descriptor vertex_descriptor;
typedef graph_traits < DirectedGraph >::edge_descriptor edge_descriptor;
typedef exterior_vertex_property<DirectedGraph, float> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;
typedef pair<int, int> Edge;

vertex_descriptor addVertex(DirectedGraph & g, int n)
{
    vertex_descriptor v0 = add_vertex(g);
    g[v0].id = n;
    return v0;
}

void addEdge(int vertex1, int vertex2, float weight, DirectedGraph & g)
{
    add_edge(vertex1-1, vertex2-1, weight, g);
}

bool vertexExists(DirectedGraph & g, int n, int & t)
{
   int pos = 0;
   DirectedGraph::vertex_iterator start,end;
   for(tie(start,end) = vertices(g); start!=end; start++)
   {
       if(g[*start].id == n)
       {
           t = pos;
           return true;
       }
       pos++;
   }
   return false;
}

bool edgeExists(DirectedGraph & g, int v1, int v2, float w)
{
   DirectedGraph::edge_iterator edgeIt, edgeEnd;
   for (tie(edgeIt, edgeEnd) = edges(g); edgeIt != edgeEnd; ++edgeIt)
   {
       if(source(*edgeIt, g) == v1 && target(*edgeIt, g) == v2 && get(edge_weight, g, *edgeIt) == w)
           return true;
   }
   return false;
}

void insertarVertice(DirectedGraph & g){
  clock_t t;
  int vertice;
  cout << "Ingrese el nodo que quiera agregar: ";
  while(!(cin >> vertice)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Intente otra vez: ";
    }
  int posicion;
  if(vertexExists(g, vertice, posicion)) {
     cout << "El vértice: " << vertice << " ya existe" << endl;
  } else {
    t = clock();
    vertex_descriptor v0 = addVertex(g,vertice);
    t = clock() - t;
    double time_taken = ((double)t)/CLOCKS_PER_SEC;
    printf("Tiempo en insertar vértice: %f ms \n", time_taken*1000);
  }
  cout << "" << endl;
}

void insertarArista(DirectedGraph & g){
  clock_t t;
  bool negativeEdge = false;
  bool unDirected = false;
  float v1, v2;
  int pos1, pos2;
  cout << "Vértice 1: ";
  while(!(cin >> v1)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(!vertexExists(g, v1, pos1)) {
    cout << "El vértice con id " << v1 << " no existe, debe crearse antes" << endl;
  } else {
      cin.ignore();
      cout << "Vértice 2: ";
        while(!(cin >> v2)){
          cin.clear();
          cin.ignore(numeric_limits<streamsize>::max(), '\n');
          cout << "Entrada inválida, intenta otra vez: ";
        }
        if(!vertexExists(g, v2, pos2)) {
          cout << "El vértice con id " << v2 << " no existe, debe crearse antes " << endl;
        } else {
            float weight;
            cin.ignore();
            cout << "Peso de la arista a insertar: ";
            while(!(cin >> weight)){
              cin.clear();
              cin.ignore(numeric_limits<streamsize>::max(), '\n');
              cout << "Entrada inválida, intenta otra vez: ";
            }
      EdgeWeightProperty w = weight;
      if(edgeExists(g,pos1,pos2,weight)) {
        cout << "La arista de " << pos1 << " a " << pos2 << " con peso " << weight << " ya existe" << endl;
      } else {
        if(weight < 0) {
          negativeEdge = true;
          cout << "No se pueden agregar aristas negativas" << endl;
        }
        t = clock();
        add_edge(pos1,pos2,w,g);
        if(unDirected){
          add_edge(pos2,pos1,w,g);
        }
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en insertar arista: %f ms \n", time_taken*1000);
      }
    }
  }
  cout << "" << endl;
}

void eliminarVertice(DirectedGraph & g){
  clock_t t;
  float vertice;
  int pos;
  cout << "Ingrese el nodo que quiera borrar: ";
  while(!(cin >> vertice)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(vertexExists(g, vertice, pos)){
    graph_traits<DirectedGraph>::vertex_iterator start,end,next;
    tie(start, end) = vertices(g);
    for (next = start; start != end; start = next) {
      ++next;
      if(g[*start].id == vertice){
        t = clock();
        clear_vertex(*start, g);
        remove_vertex(*start,g);
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en borrar vértice: %f ms \n", time_taken*1000);
      }
    }
  } else {
      cout << "El vértice con id " << vertice << " no existe " << endl;
  }
  cout << "" << endl;
}

void eliminarArista(DirectedGraph & g){
  clock_t t;
  float v1,v2;
  int pos1,pos2;
  cout << "Id del vértice 1 conectado por la arista: ";
  while(!(cin >> v1)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(vertexExists(g, v1,pos1)) {
    cin.ignore();
    cout << "Id del vértice 2 conectado por la arista: ";
    while(!(cin >> v2)){
      cin.clear();
      cin.ignore(numeric_limits<streamsize>::max(), '\n');
      cout << "Entrada inválida, intenta otra vez: ";
    }
    if(vertexExists(g, v2,pos2)){
      if(edge(pos1,pos2,g).second){
        pair<DirectedGraph::edge_descriptor, bool> retrievedEdge = edge(pos1, pos2, g);
        t = clock();
        remove_edge(retrievedEdge.first,g);
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en borrar arista: %f ms \n", time_taken*1000);
      } else {
          cout << "La arista de " << v1 << " a " << v2 << " no existe" << endl;
      }
    } else {
        cout << "El vértice con id " << v2 << " no existe " << endl;
      }
  } else {
      cout << "El vértice con id " << v1 << " no existe " << endl;
  }
  cout << "" << endl;
}

void DFS(DirectedGraph & g){

  clock_t t;
  default_dfs_visitor vis;
  t = clock();
  depth_first_search(g, visitor(vis));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de DFS: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void BFS(DirectedGraph & g){

  clock_t t;
  default_bfs_visitor vis;
  t = clock();
  breadth_first_search(g, vertex(0, g), visitor(vis));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de BFS: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void Prim(DirectedGraph & g){

  clock_t t;
  vector <vertex_descriptor> p(num_vertices(g));
  t = clock();
  prim_minimum_spanning_tree(g, &p[0]);
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Prim: %f ms \n", time_taken*1000);

  cout << "" << endl;

  for (size_t i = 0; i != p.size(); ++i)
  {
      if (p[i] != i)
          cout << g[p[i]].id << " --- " << g[i].id << endl;
      else
          cout << "start -> " << g[i].id << endl;
  }

  cout << "" << endl;

}

void Kruskal(DirectedGraph & g){

  clock_t t;
  property_map<DirectedGraph, edge_weight_t>::type weight = get(edge_weight, g);
  vector <edge_descriptor> spanning_tree;
  t = clock();
  kruskal_minimum_spanning_tree(g, back_inserter(spanning_tree));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Kruskal: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void Dijkstra(DirectedGraph & g){

  clock_t t;
  vector<vertex_descriptor> p(num_vertices(g));
  vector<int> d(num_vertices(g));
  vertex_descriptor s = vertex(0, g);
  t = clock();
  dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Dijkstra: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void FloydWarshall(DirectedGraph & g){

  clock_t t;
  map<vertex_descriptor, map<vertex_descriptor, float> >matrix;
  EdgeWeightMap weight_pmap = get(edge_weight, g);
  DistanceMatrix dist(num_vertices(g));
  DistanceMatrixMap dm(dist, g);
  t = clock();
  bool noNegCycles = floyd_warshall_all_pairs_shortest_paths (g, dm, weight_map(weight_pmap));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Floyd Warshall: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

int main(int argc, const char * argv[]){

  DirectedGraph g;
  vector<vertex_descriptor> verts;

  for(int i=1; i<15; i++)
  {
      verts.push_back(addVertex(g, i));
  }

  addEdge(1,4,8,g);
  addEdge(1,3,8,g);
  addEdge(2,5,7,g);
  addEdge(3,10,4,g);
  addEdge(3,2,7,g);
  addEdge(3,5,8,g);
  addEdge(4,7,3,g);
  addEdge(4,5,1,g);
  addEdge(4,8,2,g);
  addEdge(5,6,9,g);
  addEdge(6,13,4,g);
  addEdge(7,4,6,g);
  addEdge(8,9,3,g);
  addEdge(8,7,3,g);
  addEdge(9,10,2,g);
  addEdge(9,12,4,g);
  addEdge(10,3,10,g);
  addEdge(10,6,6,g);
  addEdge(11,12,6,g);
  addEdge(12,11,8,g);
  addEdge(12,9,2,g);
  addEdge(12,14,9,g);
  addEdge(13,14,6,g);
  addEdge(14,13,2,g);

  cout << "" << endl;

  insertarVertice(g);
  insertarArista(g);
  eliminarVertice(g);
  eliminarArista(g);
  DFS(g);
  BFS(g);
  Prim(g);
  Kruskal(g);
  Dijkstra(g);
  FloydWarshall(g);

}


#include <iostream>
#include <cassert>
#include <time.h>
#include <cstdint>
#include <stdexcept>
#include <chrono>
#include <limits.h>

#include <NTL/ZZX.h>

#include "FINAL.h"

using namespace std;
using namespace NTL;

void test_params()
{
    {
        Param param(LWE);
        cout << "Ciphertext modulus of the base scheme (LWE): " << param.q_base << endl;
        cout << "Dimension of the base scheme (LWE): " << param.n << endl;
        cout << "Ciphertext modulus for bootstrapping (LWE): " << q_boot << endl;
        cout << "Polynomial modulus (LWE): " << Param::get_def_poly() << endl;
        assert(param.l_ksk == int(ceil(log(double(param.q_base))/log(double(Param::B_ksk)))));
        cout << "Decomposition length for key-switching (LWE): " << param.l_ksk << endl;
        cout << "Decomposition bases for key-switching (LWE): " << Param::B_ksk << endl;
        cout << "Dimension for bootstrapping (LWE): " << Param::N << endl;
        cout << "Decomposition bases for bootstrapping (LWE): ";
        for (const auto &v: param.B_bsk) cout << v << ' ';
        cout << endl;
        cout << "Delta (LWE): " << param.delta_base << endl;
        cout << "Half Delta (LWE): " << param.half_delta_base << endl;
    }
    {
        Param param(NTRU);
        cout << "Ciphertext modulus of the base scheme (MNTRU): " << param.q_base << endl;
        cout << "Dimension of the base scheme (NTRU): " << param.n << endl;
        cout << "Ciphertext modulus for bootstrapping (NTRU): " << q_boot << endl;
        cout << "Polynomial modulus (NTRU): " << Param::get_def_poly() << endl;
        assert(param.l_ksk == int(ceil(log(double(param.q_base))/log(double(Param::B_ksk)))));
        cout << "Decomposition length for key-switching (MNTRU): " << param.l_ksk << endl;
        cout << "Decomposition bases for key-switching (MNTRU): " << Param::B_ksk << endl;
        cout << "Dimension for bootstrapping (MNTRU): " << Param::N << endl;
        cout << "Decomposition bases for bootstrapping (MNTRU): ";
        for (const auto &v: param.B_bsk) cout << v << ' ';
        cout << endl;
        cout << "Decomposition lengths for bootstrapping (MNTRU): ";
        for (int i = 0; i < Param::B_bsk_size; i++)
        {
            assert(param.l_bsk[i] == int(ceil(log(double(q_boot))/log(double(param.B_bsk[i])))));
            cout << param.l_bsk[i] << ' ';
        }
        cout << endl;
        cout << "Decomposition lengths for bootstrapping (MNTRU): ";
        for (int i = 0; i < Param::B_bsk_size; i++)
        {
            assert(param.l_bsk[i] == int(ceil(log(double(q_boot))/log(double(param.B_bsk[i])))));
            cout << param.l_bsk[i] << ' ';
        }
        cout << endl;
        cout << "Delta (MNTRU): " << param.delta_base << endl;
        cout << "Half Delta (MNTRU): " << param.half_delta_base << endl;

        {
            assert(0L == mod_q_boot(0L));
            assert(1L == mod_q_boot(1L));
            assert(0L == mod_q_boot(q_boot));
            assert(half_q_boot == mod_q_boot(half_q_boot));
            assert(-half_q_boot == mod_q_boot(-half_q_boot));
            cout << "MODULO REDUCTION IS OK" << endl;
        }
    }

    cout << "Plaintext modulus: " << Param::t << endl;
    cout << endl;
    cout << "PARAMS ARE OK" << endl;

    {
        vector<int> res;
        decompose(res, 0, 2, 3);
        assert(res.size() == 3);
        for (auto iter=res.begin(); iter < res.end(); iter++)
            assert(0L == *iter);
    }
    {
        vector<int> res;
        decompose(res, 1, 2, 3);
        assert(res.size() == 3);
        assert(res[0] == 1);
        for (auto iter=res.begin()+1; iter < res.end(); iter++)
            assert(0L == *iter);
    }
    {
        vector<int> res;
        decompose(res, 2, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == 1 && res[2] == 0);
    }
    {
        vector<int> res;
        decompose(res, 2, 4, 3);
        assert(res.size() == 3);
        assert(res[0] == 2 && res[1] == 0 && res[2] == 0);
        decompose(res, 3, 4, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == 1 && res[2] == 0);
    }
    {
        vector<int> res;
        try
        {
            decompose(res, 14, 3, 3);
            assert(false);
        }
        catch (overflow_error)
        {
            assert(true);
        }
    }
    {
        vector<int> res;
        try
        {
            decompose(res, -14, 3, 3);
            assert(false);
        }
        catch (overflow_error)
        {
            assert(true);
        }
    }
    {
        vector<int> res;
        decompose(res, 13, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == 1 && res[1] == 1 && res[2] == 1);
        decompose(res, -13, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == -1 && res[2] == -1);
    }

    cout << "DECOMPOSITION IS OK" << endl;

}

void test_sampler()
{
    int N = Param::N;

    Param pLWE(LWE);
    Param pNTRU(NTRU);
    for (int run = 0; run < 1; run++)
    {
        //cout << "Run: " << run+1 << endl;
        {
            vector<int> vec(pNTRU.n, 0L);
            Sampler::get_ternary_vector(vec);

            assert(vec.size() == pNTRU.n);
            for (int i = 0; i < pNTRU.n; i++)
            {
                assert((vec[i]==0) || (vec[i]==-1) || (vec[i]==1) );
            }
        }

        {
            vector<int> vec(N,0L);
            Sampler::get_ternary_vector(vec);

            assert(vec.size() == N);
            for (int i = 0; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==-1) || (vec[i]==1) );
            }
        }

        {
            vector<int> vec(N,0L);
            Sampler::get_binary_vector(vec);

            assert(vec.size() == N);
            for (int i = 0; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==1) );
            }
        }

        {
            int n = pLWE.n;
            vector<vector<int>> mat(n, vector<int>(N,0L));
            Sampler::get_ternary_matrix(mat);

            assert(mat.size() == n && mat[0].size() == N);
            for (int i = 0; i < n; i++)
            {
                vector<int>& row = mat[i];
                for (int j = 0; j < N; j++)
                    assert((row[j]==0) || (row[j]==-1) || (row[j]==1) );
            }
        }

        {
            int n = pLWE.n;
            vector<int> vec(n, 0L);
            double st_dev = 4.0;
            Sampler::get_gaussian_vector(vec, st_dev);

            assert(vec.size() == n);
            for (int i = 0; i < n; i++)
            {
                assert(conv<double>(abs(vec[i])) < 6*st_dev);
            }
        }

        {
            int n = pNTRU.n;
            vector<vector<int>> mat(n, vector<int>(N,0L));
            double st_dev = 4.0;
            Sampler::get_gaussian_matrix(mat, st_dev);

            assert(mat.size() == n && mat[0].size() == N);
            for (int i = 0; i < n; i++)
            {
                vector<int>& row = mat[i];
                for (int j = 0; j < N; j++)
                    assert(conv<double>(abs(row[j])) < 6*st_dev);
            }
        }

        {
            vector<int> vec_inv(N,0L);
            vector<int> vec(N,0L);
            Sampler s(pNTRU);
            s.get_invertible_vector(vec, vec_inv, 4, 1);

            assert(vec.size() == N && vec_inv.size() == N);
            assert((vec[0]==1) || (vec[0]==-3) || (vec[0]==5) );
            for (int i = 1; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==-4) || (vec[i]==4));
            }
        }

        {
            int n = pLWE.n;
            vector<vector<int>> mat_inv(n, vector<int>(n,0L));
            vector<vector<int>> mat(n, vector<int>(n,0L));
            Sampler s(pLWE);
            s.get_invertible_matrix(mat, mat_inv, 5, 1);

            assert(mat.size() == n && mat[0].size() == n
                && mat_inv.size() == n && mat_inv[0].size() == n);
            for (int i = 0; i < n; i++)
                assert((mat[i][i]==1) || (mat[i][i]==-4) || (mat[i][i]==6) );

            for (int i = 0; i < n; i++)
                for (int j = 0; (j < n) && (j != i); j++)
                {
                    assert((mat[i][j]==0) || (mat[i][j]==-5) || (mat[i][j]==5) );
                }
        }
    }
    cout << "SAMPLER IS OK" << endl;
}

enum GateType {NAND, AND, OR, XOR, NOT};

void test_ntruhe_gate_helper(int in1, int in2, const SchemeNTRU& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = (g == NOT ? 30 : 100);
    for (int i = 0; i < N_TESTS; i++)
    {
        Ctxt_NTRU ct_res, ct1, ct2;
        s.encrypt(ct1, in1);
        s.encrypt(ct2, in2);

        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "NAND output: " << output << endl;
            assert(output == !(in1 & in2));
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "AND output: " << output << endl;
            assert(output == (in1 & in2));
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 | in2));
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "XOR output: " << output << endl;
            assert(output == (in1 ^ in2));
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct1);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

//            cout << ".... NOT output: " << output << endl;
            assert(output == (1 - in1));
        }

    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_ntru_gate(SchemeNTRU& s, GateType g)
{
    test_ntruhe_gate_helper(0, 0, s, g);
    test_ntruhe_gate_helper(0, 1, s, g);
    test_ntruhe_gate_helper(1, 0, s, g);
    test_ntruhe_gate_helper(1, 1, s, g);
}

void test_ntruhe_nand(SchemeNTRU& s)
{
    GateType g = NAND;

    test_ntru_gate(s, g);

    cout << "NAND IS OK" << endl;
}

void test_ntruhe_and(SchemeNTRU& s)
{
    GateType g = AND;
    test_ntru_gate(s, g);
    cout << "AND IS OK" << endl;
}

void test_ntruhe_or(SchemeNTRU& s)
{
    GateType g = OR;
    test_ntru_gate(s, g);
    cout << "OR IS OK" << endl;
}

void test_ntruhe_xor(SchemeNTRU& s)
{
    GateType g = XOR;
    test_ntru_gate(s, g);
    cout << "XOR IS OK" << endl;
}

void test_ntruhe_not(SchemeNTRU& s)
{
    GateType g = NOT;
    for(int i = 0; i < 5; i++){
        int bit = binary_sampler(rand_engine);
        test_ntruhe_gate_helper(bit, 0, s, g);
    }
    cout << "NOT GATE IS OK" << endl;
}

void test_ntruhe_gate_composition_helper(SchemeNTRU& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = 100;

    int in1, in2, exp_out;
    in1 = binary_sampler(rand_engine);
    exp_out = in1;

    Ctxt_NTRU ct_res, ct;
    s.encrypt(ct_res, in1);
    for (int i = 0; i < N_TESTS; i++)
    {
        in2 = binary_sampler(rand_engine);
        s.encrypt(ct, in2);
        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct_res, ct);// ct_res should encrypt NAND(exp_out, in2)
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = !(exp_out & in2); // exp_out = NAND(exp_out, in2)
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out & in2); // exp_out = AND(exp_out, in2)
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out | in2); // exp_out = OR(exp_out, in2)
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out ^ in2); // exp_out = XOR(exp_out, in2)
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct_res);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (1 - exp_out); // exp_out = NOT(exp_out)
        }

        int output = s.decrypt(ct_res);
        assert(output == exp_out);
    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_ntruhe_composition_of_gates(SchemeNTRU& s)
{
    test_ntruhe_gate_composition_helper(s, NAND);
    cout << "COMPOSING NAND IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, AND);
    cout << "COMPOSING AND IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, OR);
    cout << "COMPOSING OR IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, XOR);
    cout << "COMPOSING XOR IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, NOT);
    cout << "COMPOSING NOT GATE IS OK" << endl;
}

// ----- LWE tests

void test_lwehe_gate_helper(int in1, int in2, SchemeLWE& s, GateType g)
{
    float avg_time = 0.0;
    for (int i = 0; i < 100; i++)
    {
        Ctxt_LWE ct_res, ct1, ct2;
        s.encrypt(ct1, in1);
        s.encrypt(ct2, in2);

        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "NAND output: " << output << endl;
            assert(output == !(in1 & in2));
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "AND output: " << output << endl;
            assert(output == (in1 & in2));
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 | in2));
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 ^ in2));
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct1);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

//            cout << "NOT output: " << output << endl;
            assert(output == (1 - in1));
        }

    }
    cout << "Avg. time: " << avg_time/100.0 << endl;
}

void test_lwe_gate(SchemeLWE& s, GateType g)
{
    test_lwehe_gate_helper(0, 0, s, g);
    test_lwehe_gate_helper(0, 1, s, g);
    test_lwehe_gate_helper(1, 0, s, g);
    test_lwehe_gate_helper(1, 1, s, g);
}

void test_lwehe_nand(SchemeLWE& s)
{
    GateType g = NAND;
    test_lwe_gate(s, g);
    cout << "NAND IS OK" << endl;
}

void test_lwehe_and(SchemeLWE& s)
{
    GateType g = AND;
    test_lwe_gate(s, g);
    cout << "AND IS OK" << endl;
}

void test_lwehe_or(SchemeLWE& s)
{
    GateType g = OR;
    test_lwe_gate(s, g);
    cout << "OR IS OK" << endl;
}

void test_lwehe_xor(SchemeLWE& s)
{
    GateType g = XOR;
    test_lwe_gate(s, g);
    cout << "XOR IS OK" << endl;
}

void test_lwehe_not(SchemeLWE& s)
{
    GateType g = NOT;
    for(int i = 0; i < 4; i++){
        int bit = binary_sampler(rand_engine);
        test_lwehe_gate_helper(bit, 0, s, g);
    }

    cout << "NOT GATE IS OK" << endl;
}

void test_lwehe_gate_composition_helper(SchemeLWE& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = 110;

    int in1, in2, exp_out;
    in1 = binary_sampler(rand_engine);
    exp_out = in1;

    Ctxt_LWE ct_res, ct;
    s.encrypt(ct_res, in1);
    for (int i = 0; i < N_TESTS; i++)
    {
        in2 = binary_sampler(rand_engine);
        s.encrypt(ct, in2);
        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct_res, ct);// ct_res should encrypt NAND(exp_out, in2)
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = !(exp_out & in2); // exp_out = NAND(exp_out, in2)
            //cout << "NAND output: " << output << endl;
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out & in2); // exp_out = AND(exp_out, in2)
            //cout << "AND output: " << output << endl;
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out | in2); // exp_out = OR(exp_out, in2)
            //cout << "OR output: " << output << endl;
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out ^ in2); // exp_out = XOR(exp_out, in2)
            //cout << "XOR output: " << output << endl;
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct_res);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (1 - exp_out); // exp_out = NOT(exp_out)
        }

        int output = s.decrypt(ct_res);
        assert(output == exp_out);
    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_lwehe_composition_of_gates(SchemeLWE& s)
{
    test_lwehe_gate_composition_helper(s, NAND);
    cout << "COMPOSING NAND IS OK" << endl;

    test_lwehe_gate_composition_helper(s, AND);
    cout << "COMPOSING AND IS OK" << endl;

    test_lwehe_gate_composition_helper(s, OR);
    cout << "COMPOSING OR IS OK" << endl;

    test_lwehe_gate_composition_helper(s, XOR);
    cout << "COMPOSING XOR IS OK" << endl;

    test_lwehe_gate_composition_helper(s, NOT);
    cout << "COMPOSING NOT GATE IS OK" << endl;
}

int main()
{
    test_params();
    test_sampler();

    cout << endl;
    cout << "-------------------------" << endl;
    cout << "NTRU tests" << endl;
    SchemeNTRU s_ntru;
    test_ntruhe_nand(s_ntru);
    test_ntruhe_and(s_ntru);
    test_ntruhe_or(s_ntru);
    test_ntruhe_xor(s_ntru);
    test_ntruhe_not(s_ntru);
    test_ntruhe_composition_of_gates(s_ntru);
    cout << "NTRU tests PASSED" << endl;

    cout << endl;
    cout << "-------------------------" << endl;
    cout << "LWE tests" << endl;
    SchemeLWE s_lwe;
    test_lwehe_not(s_lwe);
    test_lwehe_nand(s_lwe);
    test_lwehe_and(s_lwe);
    test_lwehe_or(s_lwe);
    test_lwehe_xor(s_lwe);
    test_lwehe_not(s_lwe);
    test_lwehe_composition_of_gates(s_lwe);
    cout << "LWE tests PASSED" << endl;

    return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    cholesky.cpp
 * @brief   Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
 * @author  Richard Roberts
 * @author  Frank Dellaert
 * @date    Nov 5, 2010
 */

#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>

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

using namespace std;

namespace gtsam {

static const double negativePivotThreshold = -1e-1;
static const double zeroPivotThreshold = 1e-6;
static const double underconstrainedPrior = 1e-5;
static const int underconstrainedExponentDifference = 12;

/* ************************************************************************* */
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {
  // Get pivot value
  double alpha = ATA(k, k);

  // Correct negative pivots from round-off error
  if (alpha < negativePivotThreshold) {
    return -1;
  } else if (alpha < 0.0)
    alpha = 0.0;

  const double beta = sqrt(alpha);

  if (beta > zeroPivotThreshold) {
    const double betainv = 1.0 / beta;

    // Update k,k
    ATA(k, k) = beta;

    if (k < (order - 1)) {
      // Update A(k,k+1:end) <- A(k,k+1:end) / beta
      typedef Matrix::RowXpr::SegmentReturnType BlockRow;
      BlockRow V = ATA.row(k).segment(k + 1, order - (k + 1));
      V *= betainv;

      // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
      ATA.block(k + 1, k + 1, order - (k + 1), order - (k + 1)) -= V.transpose() * V;
      //      ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
      //          .rankUpdate(V.adjoint(), -1);
    }
    return 1;
  } else {
    // For zero pivots, add the underconstrained variable prior
    ATA(k, k) = underconstrainedPrior;
    for (size_t j = k + 1; j < order; ++j)
      ATA(k, j) = 0.0;
    return 0;
  }
}

/* ************************************************************************* */
pair<size_t, bool> choleskyCareful(Matrix& ATA, int order) {
  // Check that the matrix is square (we do not check for symmetry)
  assert(ATA.rows() == ATA.cols());

  // Number of rows/columns
  const size_t n = ATA.rows();

  // Negative order means factor the entire matrix
  if (order < 0)
    order = int(n);

  assert(size_t(order) <= n);

  // The index of the row after the last non-zero row of the square-root factor
  size_t maxrank = 0;
  bool success = true;

  // Factor row-by-row
  for (size_t k = 0; k < size_t(order); ++k) {
    int stepResult = choleskyStep(ATA, k, size_t(order));
    if (stepResult == 1) {
      maxrank = k + 1;
    } else if (stepResult == -1) {
      success = false;
      break;
    } /* else if(stepResult == 0) Found zero pivot */
  }

  return make_pair(maxrank, success);
}

/* ************************************************************************* */
bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {

  gttic(choleskyPartial);
  if (nFrontal == 0)
    return true;

  assert(ABC.cols() == ABC.rows());
  const Eigen::DenseIndex n = ABC.rows() - topleft;
  assert(n >= 0 && nFrontal <= size_t(n));

  // Create views on blocks
  auto A = ABC.block(topleft, topleft, nFrontal, nFrontal);
  auto B = ABC.block(topleft, topleft + nFrontal, nFrontal, n - nFrontal);
  auto C = ABC.block(topleft + nFrontal, topleft + nFrontal, n - nFrontal, n - nFrontal);

  // Compute Cholesky factorization A = R'*R, overwrites A.
  gttic(LLT);
  Eigen::LLT<Matrix, Eigen::Upper> llt(A);
  Eigen::ComputationInfo lltResult = llt.info();

  if (lltResult != Eigen::Success) {
    return false;
  }

  auto R = A.triangularView<Eigen::Upper>();
  R = llt.matrixU();
  gttoc(LLT);

  // Compute S = inv(R') * B
  gttic(compute_S);
  if (nFrontal < n)
    R.transpose().solveInPlace(B);
  gttoc(compute_S);

  // Compute L = C - S' * S
  gttic(compute_L);
  if (nFrontal < n)
    C.selfadjointView<Eigen::Upper>().rankUpdate(B.transpose(), -1.0);
  gttoc(compute_L);

  // Check last diagonal element - Eigen does not check it
  if (nFrontal >= 2) {
    int exp2, exp1;
    (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2);
    (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1);
    return (exp2 - exp1 < underconstrainedExponentDifference);
  } else if (nFrontal == 1) {
    int exp1;
    (void)frexp(R(0, 0), &exp1);
    return (exp1 > -underconstrainedExponentDifference);
  } else {
    return true;
  }

} // END choleskyPartial()
}  // namespace gtsam


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Simon Praetorius

#ifndef ITL_GCR_INCLUDE
#define ITL_GCR_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>

namespace itl
{

  template <typename Matrix, typename Vector, typename Preconditioner, typename Iteration>
  int gcr_full(const Matrix& A, Vector& x, const Vector& b, const Preconditioner& P, Iteration& iter)
  {
    using math::reciprocal;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type Size;

    if (size(b) == 0)
      throw mtl::logic_error("empty rhs vector");

    Scalar zero= math::zero(b[0]), dbl_tol= 1.e-16; // TODO: tolerance as parameter or otherwise generalized

    Size k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));

    Vector r(b - A * x);
    Vector uk(size(x), zero), ck(size(x), zero);
    mtl::matrix::multi_vector<Vector> C(Vector(resource(x), zero), kmax);
    mtl::matrix::multi_vector<Vector> U(Vector(resource(x), zero), kmax);

    Scalar res(two_norm(r));
    Scalar alpha(zero), beta(zero), gamma(zero);

    for (k= 0; k < kmax && !iter.finished(res); ++k, ++iter)
    {
      uk = solve(P, r);
      // In order to avoid breakdown, use LSQR switch
      // Cite: C. Vuik, Further experiences with GMRESR, 1993
      //  if (two_norm(uk) < dbl_tol)
      //    uk = trans(A) * r; // requires transposed multiplication

      ck = A * uk;
      for (size_t i = 0; i < k; i++)
      {
        alpha = dot(C.vector(i), ck);

        ck -= alpha * C.vector(i);
        uk -= alpha * U.vector(i);
      }

      beta = two_norm(ck);
      if (beta < dbl_tol)
        return iter.fail(2, "search direction close to 0");

      C.vector(k) = ck * reciprocal(beta);
      U.vector(k) = uk * reciprocal(beta);

      gamma = dot(C.vector(k), r);
      x += U.vector(k) * gamma;
      r -= C.vector(k) * gamma;

      res = two_norm(r);
    }

    return iter;
  }

#if 0
  template <typename Matrix, typename Vector, typename LeftPreconditioner, typename RightPreconditioner, typename Iteration>
  int gmresr_trunc(const Matrix& A, Vector& x, const Vector& b, const LeftPreconditioner& L, const RightPreconditioner& R,
                   Iteration& iter, , typename mtl::Collection<Vector>::size_type nTrunc)
  {
TODO:
    implement truncated GCR solver, instead of restarted version
Cite:
    C. Vuik, Further experiences with GMRESR, 1993
  }
#endif

  /// Generalized Conjugate Residual method with restart
  /// Cite: S.C. Eisenstat, H.C. Elman, M.H. Schultz, Variational iterative methods for non symmetric systems of linear equations, 1983
  /// Cite: C. Vuik, New insight in GMRES-like methods with variable preconditioners
  template <typename Matrix, typename Vector, typename LeftPreconditioner,
            typename RightPreconditioner, typename Iteration>
  int gcr(const Matrix& A, Vector& x, const Vector& b, LeftPreconditioner& /*L*/, RightPreconditioner& R,
          Iteration& iter, typename mtl::Collection<Vector>::size_type restart)
  {
    do
    {
      Iteration inner(iter);
      inner.set_max_iterations(std::min(int(iter.iterations()+restart), iter.max_iterations()));
      inner.suppress_resume(true);
      gcr_full(A, x, b, R, inner);
      iter.update_progress(inner);
    }
    while (!iter.finished());

    return iter;
  }
} // namespace itl;

#endif // ITL_GMR_INCLUDE


/**
 * \file hungarian.hpp
 * \brief
 *
 * \author Andrew Price
 * \date 2016-4-19
 *
 * \copyright
 *
 * Copyright (c) 2020, Andrew Price
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 * Redistribution and use in source and binary forms, with or
 * without modification, are permitted provided that the following
 * conditions are met:
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 *   copyright notice, this list of conditions and the following
 *   disclaimer in the documentation and/or other materials provided
 *   with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef HUNGARIAN_HPP
#define HUNGARIAN_HPP

#include <Eigen/Core>
#include <vector>

// Implementation of the Hungarian Algorithm derived from https://www.topcoder.com/community/data-science/data-science-tutorials/assignment-problem-and-hungarian-algorithm/

namespace mps
{

// TODO: template for multiple cost matrix types

/**
 * @brief The Hungarian class solves the optimal nxn assignment problem in O(n^3) time.
 * The results are reported as a mapping from x to y (row to column of cost matrix)
 *
 * Formally, the system is defined as follows:
 * Given two sets \f$ X, Y \in \mathbb{R}^n \f$ and a cost matrix \f$ C:X \times Y \rightarrow \mathbb{R} \f$,
 * find a bijection \f$ f:X \rightarrow Y \f$ that minimizes the cost function \f$ \sum_{x\in X}C[x,f(x)] \f$.
 */
class Hungarian
{
public:
	Hungarian(const Eigen::MatrixXd& cost);

	/**
	 * @brief getSolutionCost returns the current linear cost of the assignment
	 * @return Solution cost of the form C(x1,xToY(x1)) + C(x2,xToY(x2)) + ...
	 */
	double getSolutionCost();

	/**
	 * @brief getAssignment Returns the optimal choice of y for each x in sequence
	 * @return y-index of optimal pairing for each x
	 */
	const std::vector<int>& getAssignment() { return xy; }

protected:
	Eigen::MatrixXd C;            ///< Cost matrix (nxn)
	const int N;                  ///< Number of pairings to find
	int maxMatch;                 ///< Number of pairings locked so far
	std::vector<double> lx;
	std::vector<double> ly;
	std::vector<int> xy;          ///< Mapping from x to y index
	std::vector<int> yx;          ///< Mapping from y to x index
	std::vector<double> slack;
	std::vector<double> slackx;
	std::vector<int> prev;        ///< Alternating tree
	std::vector<bool> S;          ///< Set of matched x indices
	std::vector<bool> T;          ///< Set of matched y indices

	void augment();
	void relabel();
	void addToTree(int x, int prevx);

};

double Hungarian::getSolutionCost()
{
	double cost = 0;
	for (int x = 0; x < N; ++x)
	{
		cost += C(x, xy[x]);
	}
	return -cost;
}

Hungarian::Hungarian(const Eigen::MatrixXd& cost)
    : C(-cost), // Negate cost, as this algorithm finds the maximum assignment
      N(C.cols()),
      maxMatch(0),
      lx(N, 0),
      ly(N, 0),
      xy(N,-1),
      yx(N,-1),
      slack(N),
      slackx(N),
      prev(N),
      S(N),
      T(N)
{
	// Initial labelling
	for (int x = 0; x < N; ++x)
		for (int y = 0; y < N; ++y)
			lx[x] = std::max(lx[x], C(x,y));

	// Solve
	augment();
}

void Hungarian::relabel()
{
	double delta = std::numeric_limits<double>::infinity();

	for (int y = 0; y < N; ++y) // compute delta with slack
		if (!T[y]) { delta = std::min(delta, slack[y]); }
	assert(std::isfinite(delta));
	for (int x = 0; x < N; ++x) // update X labels
		if (S[x]) { lx[x] -= delta; }
	for (int y = 0; y < N; ++y) // update Y labels
		if (T[y]) { ly[y] += delta; }
	for (int y = 0; y < N; ++y) // update slack array
		if (!T[y]) { slack[y] -= delta; }
}

void Hungarian::addToTree(int x, int prevx)
{
	S[x] = true;
	prev[x] = prevx;
	for (int y = 0; y < N; ++y)
	{
		double newCost = lx[x] + ly[y] - C(x,y);
		if (newCost < slack[y])
		{
			slack[y] = newCost;
			slackx[y] = x;
		}
	}
}

void Hungarian::augment()
{
	if (maxMatch == N) { return; }
	int x = -1, y = -1, root = -1;
	std::vector<int> q(N);
	int wr = 0, rd = 0;

	// Initialize sets and tree
	std::fill(S.begin(), S.end(), false);
	std::fill(T.begin(), T.end(), false);
	std::fill(prev.begin(), prev.end(), -1);

	// Find root of tree
	for (x = 0; x < N; ++x)
	{
		if (-1 == xy[x])
		{
			q[wr++] = root = x;
			prev[x] = -2;
			S[x] = true;
			break;
		}
	}

	// Initialize slack array
	for (y = 0; y < N; ++y)
	{
		slack[y] = lx[root] + ly[y] - C(root,y);
		slackx[y] = root;
	}

	while (true)
	{
		while (rd < wr) // build tree with bfs
		{
			x = q[rd++];
			for (y = 0; y < N; ++y)
			{
				if (C(x,y) == lx[x] + ly[y] && !T[y])
				{
					if (-1 == yx[y]) { break; } // found an isolated Y vertex
					T[y] = true; // else y is in T
					q[wr++] = yx[y]; // add target of y to frontier of bfs
					addToTree(yx[y], x);
				}
			}
			if (y < N) { break; } // Breaking out from inmost loop
		}
		if (y < N) { break; } // Breaking out from inmost loop

		relabel();
		wr = rd = 0;

		for (y = 0; y < N; ++y)
		{
			if (!T[y] && 0 == slack[y])
			{
				if (-1 == yx[y])
				{
					x = slackx[y];
					break;
				}
				else
				{
					T[y] = true;
					if (!S[yx[y]])
					{
						q[wr++] = yx[y];
						addToTree(yx[y], slackx[y]);
					}
				}
			}
		}
		if (y < N) { break; }
	}

	if (y < N)
	{
		maxMatch++;
		for (int cx = x, cy = y, ty; cx != -2; cx = prev[cx], cy = ty)
		{
			ty = xy[cx];
			yx[cy] = cx;
			xy[cx] = cy;
		}
		augment();
	}
}

/*
void hungarian(Eigen::MatrixXd& C)
{
	assert(C.rows() == C.cols());
	const size_t N = C.cols();

	// Initialize
	hungarianInit(C);

}

void hungarianInit(Eigen::MatrixXd& C)
{
	// Subtract smallest element from each row
	for (size_t i = 0; i < N; ++i)
	{
		double rowMin = C(i,0);
		for (size_t j = 0; j < N; ++j)
			if (C(i,j) < rowMin) { rowMin = C(i,j); }
		for (size_t j = 0; j < N; ++j)
			C(i,j) -= rowMin;
	}
	// Subtract smallest element from each column
	for (size_t j = 0; j < N; ++j)
	{
		double colMin = C(0,j);
		for (size_t i = 0; i < N; ++i)
			if (C(i,j) < colMin) { colMin = C(i,j); }
		for (size_t i = 0; i < N; ++i)
			C(i,j) -= colMin;
	}
}
*/
} // namespace gtri
#endif // HUNGARIAN_HPP


#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#include <NTL/config.h>

#ifdef NTL_GMP_HACK

#include <gmp.h>
#include <NTL/mach_desc.h>

int gcd(int a, int b)
{
   int u, v, t, x;

   if (b==0)
      x = a;
   else {
      u = a;
      v = b;
      do {
         t = u % v;
         u = v;
         v = t;
      } while (v != 0);

      x = u;
   }

   return x;
}

int pow2(int a)
{
   int m;
   int k;

   m = 1;
   k = 0;

   while (m < a) {
      m = 2*m;
      k++;
   }

   if (m == a)
      return k;
   else
      return -1;
}

const char *accum[2] = { "", "yy | " };

void lip_to_gmp(int A, int B)
{
   int d, na, nb;
   int i, j, r;
   int xx, yy;
   int shamt;

   int iter, na2, nb2;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   na2 = pow2(na);
   nb2 = pow2(nb);

   printf("static\n");
   printf("void lip_to_gmp(const long *x, mp_limb_t *y, long n)\n");
   printf("{\n");
   printf("   long r, q, xx;\n");
   printf("   mp_limb_t yy;\n\n");

   if (na2 != -1) {
      printf("   r = n & %d;\n", na-1);
      printf("   q = n >> %d;\n", na2);
   }
   else {
      printf("   r = n % %d;\n", na);
      printf("   q = n / %d;\n", na);
   }

   printf("\n");

   printf("   if (q > 0) {\n");

   if (na2 != -1)
      printf("      x += (q << %d);\n", na2);
   else
      printf("      x += (q * %d);\n", na);

   if (nb2 != -1)
      printf("      y += (q << %d);\n", nb2);
   else if (pow2(nb+1) != -1)
      printf("      y += (q << %d) - q;\n", pow2(nb+1));
   else
      printf("      y += (q * %d);\n", nb);

   printf("   }\n");

   printf("\n");
   printf("   if (r > 0) {\n");
   printf("      yy = 0;\n");
   printf("\n");

   printf("      switch (r-1) {\n");

   xx = 0;
   yy = 0;

   for (i = 1; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      if (xx) {
         printf("         ");
         printf("yy = %s (((mp_limb_t)(xx)) << %d);\n", accum[yy], B-r);
         yy = 1;
      }

      printf("      ");
      printf("case %d:\n", na-1-i);

      shamt = A-B+r;

      printf("         ");

      if (shamt >= 0) {
         printf("y[%d] = ", nb-1-j);
      }
      else {
         printf("yy = ");
      }

      printf("%s", accum[yy]);

      if (shamt == 0) {
         printf("x[0];\n");
         break;
      }
      else if (shamt < 0) {
         printf("(((mp_limb_t)(x[%d])) << %d);\n", na-1-i, -shamt);
         xx = 0;
         yy = 1;
      }
      else {
         printf("((xx=x[%d]) >> %d);\n", na-1-i, shamt);
         xx = 1;
         yy = 0;
      }
   }

   printf("      }\n\n");
   printf("      n -= r;\n");
   printf("   }\n\n");
   printf("   while (n > 0) {\n");
   printf("      x -= %d;\n", na);
   printf("      n -= %d;\n", na);
   printf("      y -= %d;\n", nb);
   printf("\n");

   xx = 0;
   yy = 0;

   for (i = 0; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      if (xx) {
         printf("      ");
         printf("yy = %s (((mp_limb_t)(xx)) << %d);\n", accum[yy], B-r);
         yy = 1;
      }

      shamt = A-B+r;

      printf("      ");

      if (shamt >= 0) {
         printf("y[%d] = ", nb-1-j);
      }
      else {
         printf("yy = ");
      }

      printf("%s", accum[yy]);

      if (shamt == 0) {
         printf("x[0];\n");
         break;
      }
      else if (shamt < 0) {
         printf("(((mp_limb_t)(x[%d])) << %d);\n", na-1-i, -shamt);
         xx = 0;
         yy = 1;
      }
      else {
         printf("((xx=x[%d]) >> %d);\n", na-1-i, shamt);
         xx = 1;
         yy = 0;
      }
   }

   printf("   }\n");
   printf("}\n");
}

void gmp_to_lip(int A, int B, int alt)
{
   int d, na, nb;
   int i, j, r;
   int shamt;

   int na2, nb2;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   na2 = pow2(na);
   nb2 = pow2(nb);

   printf("static\n");
   if (!alt)
      printf("void gmp_to_lip(long *x, const mp_limb_t *y, long n)\n");
   else
      printf("void gmp_to_lip1(long *x, const mp_limb_t *y, long n)\n");
   printf("{\n");
   printf("   long r, q; unsigned long xx;\n");
   printf("   mp_limb_t yy;\n\n");

   if (na2 != -1) {
      printf("   r = n & %d;\n", na-1);
      printf("   q = n >> %d;\n", na2);
   }
   else {
      printf("   r = n % %d;\n", na);
      printf("   q = n / %d;\n", na);
   }

   printf("\n");

   printf("   if (q > 0) {\n");

   if (na2 != -1)
      printf("      x += (q << %d);\n", na2);
   else
      printf("      x += (q * %d);\n", na);

   if (nb2 != -1)
      printf("      y += (q << %d);\n", nb2);
   else if (pow2(nb+1) != -1)
      printf("      y += (q << %d) - q;\n", pow2(nb+1));
   else
      printf("      y += (q * %d);\n", nb);

   printf("   }\n");

   printf("\n");

   printf("   if (r > 0) {\n");

   if (!alt) {
      if (na - nb == 1)
         printf("      yy = y[r-1];\n");
      else if (na2 != -1)
         printf("      yy = y[((r*%d + %d) >> %d) - 1];\n", nb, na-1, na2);
      else
         printf("      yy = y[((r*%d + %d) / %d) - 1];\n", nb, na-1, na);

   }
   else
      printf("      yy = 0;\n");

   printf("\n");

   printf("      switch (r-1) {\n");

   for (i = 1; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      printf("      ");
      printf("case %d:\n", na-1-i);

      shamt = A-B+r;

      if (shamt > 0) {
         printf("         ");
         printf("xx = ((unsigned long)(yy)) << %d;\n", shamt);
         printf("         ");
         printf("x[%d] = (xx | ((unsigned long)((yy = y[%d]) >> %d))) & NTL_RADIXM;\n",
                na-1-i, nb-2-j, 2*B-r-A);
      }
      else if (shamt == 0) {
         printf("         ");
         printf("x[0] = ((unsigned long)(yy)) & NTL_RADIXM;\n");
         break;
      }
      else {
         printf("         ");
         printf("x[%d] = ((unsigned long)(yy >> %d)) & NTL_RADIXM;\n", na-1-i, -shamt);
      }
   }

   printf("      }\n\n");
   printf("      n -= r;\n");
   printf("   }\n\n");
   printf("   while (n > 0) {\n");
   printf("      x -= %d;\n", na);
   printf("      n -= %d;\n", na);
   printf("      y -= %d;\n", nb);
   printf("\n");

   printf("      yy = y[%d];\n", nb-1);

   for (i = 0; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      shamt = A-B+r;

      if (shamt > 0) {
         printf("      ");
         printf("xx = ((unsigned long)(yy)) << %d;\n", shamt);
         printf("      ");
         printf("x[%d] = (xx | ((unsigned long)((yy = y[%d]) >> %d))) & NTL_RADIXM;\n",
                na-1-i, nb-2-j, 2*B-r-A);
      }
      else if (shamt == 0) {
         printf("      ");
         printf("x[0] = ((unsigned long)(yy)) & NTL_RADIXM;\n");
         break;
      }
      else {
         printf("      ");
         printf("x[%d] = ((unsigned long)(yy >> %d)) & NTL_RADIXM;\n", na-1-i, -shamt);
      }
   }

   printf("   }\n");
   printf("}\n");

}

void rdup(int a, int b)
{
   printf("((");
   if (pow2(a) != -1) {
      printf("(x << %d)", pow2(a));
   }
   else if (pow2(a+1) != -1) {
      printf("((x << %d) - x)", pow2(a+1));
   }
   else {
      printf("(x*%d)", a);
   }

   printf(" + %d)", b-1);

   if (pow2(b) != -1) {
      printf(" >> %d)", pow2(b));
   }
   else {
      printf(" / %d)", b);
   }
}

void G_TO_L(int A, int B)
{
   int d, na, nb;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   printf("#define G_TO_L(x) ");

   rdup(na, nb);

   printf("\n");
}

void L_TO_G(int A, int B)
{
   int d, na, nb;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   printf("#define L_TO_G(x) ");

   rdup(nb, na);

   printf("\n");
}

void L_TO_G_CHECK(int BPL, int BPI)
{
   if (BPL != BPI) {
      printf("#define L_TO_G_CHECK_LEN\n");
   }

}

void Error(const char *s)
{
   fprintf(stderr, "%s\n", s);
   abort();
}

int main()
{
   mpz_t tt;
   int A, B, BPL, BPI;

   A = NTL_NBITS_MAX;

   /*
    * We compute B as the number of bits of a gmp limb.
    * We require that this quantity correspond to the number of bits
    * of a long, or possibly a "long long" that is twice as
    * wide as a long.  These restrictions may not be entirely
    * necessary, but they are satisfied on all platforms that I know of.
    */

   if (sizeof(mp_limb_t)==sizeof(long) &&
            mp_bits_per_limb == NTL_BITS_PER_LONG)

      B = NTL_BITS_PER_LONG;

   else if (sizeof(mp_limb_t) == 2*sizeof(long) &&
            mp_bits_per_limb == 2*NTL_BITS_PER_LONG)

      B = 2*NTL_BITS_PER_LONG;

   else
      Error("sorry...this is a funny gmp");

   /*
    * The following test is a bit redundant, but it doesn't hurt.
    */

   if (A >= B) Error("sorry...this is a funny gmp");

   /*
    * Next, we check if either the _mp_size field of an mpz struct
    * or the type mp_size_t is narrower than type "long".
    * This is done to enable some overflow checks.
    * For simplicity, we require that the sizeof
    * these types is that of an int or a long, and we also make
    * the somewhat DIRTY assumption that this sizeof value implies
    * a corresponding bit count.  Since this assumption is true
    * on all platforms that I know of, and since this assumption
    * only affects some overflow tests, it seems reasonable.
    */

   if (sizeof(tt->_mp_size) == sizeof(int))
      BPI = NTL_BITS_PER_INT;
   else if (sizeof(tt->_mp_size) == sizeof(long))
      BPI = NTL_BITS_PER_LONG;
   else
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) != sizeof(int) && sizeof(mp_size_t) != sizeof(long))
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) < sizeof(tt->_mp_size))
      BPI = NTL_BITS_PER_INT;

   BPL = NTL_BITS_PER_LONG;

   fprintf(stderr, "\ngmp looks OK...%d, %d, %d, %d.\n", A, B, BPI, BPL);
   fprintf(stderr, "generating file lip_gmp_aux.c.\n");

   lip_to_gmp(A, B);
   printf("\n");

   gmp_to_lip(A, B, 0);
   printf("\n");
   gmp_to_lip(A, B, 1);
   printf("\n");

   G_TO_L(A, B);
   L_TO_G(A, B);
   L_TO_G_CHECK(BPL, BPI);

   printf("#define HAVE_LIP_GMP_AUX\n");

   return 0;
}

#else

int main()
{
   fprintf(stderr, "NTL_GMP_HACK flag not set.\n");
   return 0;
}

#endif


#ifndef MOCHIMOCHI_ADAM_HPP_
#define MOCHIMOCHI_ADAM_HPP_

#include <Eigen/Dense>
#include <cassert>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"
#include "../factory/binary_oml.hpp"

class ADAM : public BinaryOML {
private :
  const std::size_t kDim;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _m;
  Eigen::VectorXd _v;

public :
  ADAM(const std::size_t dim)
    : kDim(dim),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _m(Eigen::VectorXd::Zero(kDim)),
      _v(Eigen::VectorXd::Zero(kDim)) {

    assert(dim > 0);
  }

  virtual ~ADAM() { }

private :

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

public :

  std::string name() const override {
    return std::string("ADAM");
  }

  bool update(const Eigen::VectorXd& feature, const int label) override {
    constexpr auto kAlpha = 0.001;
    constexpr auto kBeta1 = 0.9;
    constexpr auto kBeta2 = 0.999;
    constexpr auto kEpsilon = 0.00000001;
    constexpr auto kLambda = 0.99999999;

    if (suffer_loss(feature, label) <= 0.0) { return false; }

    const Eigen::VectorXd gradiant = -label * feature;
    const auto beta1_t = std::pow(kLambda, _timestep) * kBeta1;

    _timestep++;
    functions::enumerate(gradiant.data(), gradiant.data() + gradiant.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _m[index] = beta1_t * _m[index] + (1.0 - beta1_t) * value;
                         _v[index] = kBeta2 * _v[index] + (1.0 - kBeta2) * value * value;
                         const auto m_t = _m[index] / (1.0 - std::pow(kBeta1, _timestep));
                         const auto v_t = _v[index] / (1.0 - std::pow(kBeta2, _timestep));
                         _w[index] -= kAlpha * m_t / (std::sqrt(v_t) + kEpsilon);
                       });

    return true;
  }

  int predict(const Eigen::VectorXd& feature) const override {
    return calculate_margin(feature) > 0.0 ? 1 : -1;
  }

  void save(const std::string& filename) override {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) override {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> w_vector(_w.data(), _w.data() + _w.size());
    std::vector<double> m_vector(_m.data(), _m.data() + _m.size());
    std::vector<double> v_vector(_v.data(), _v.data() + _v.size());

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("m", m_vector);
    ar & boost::serialization::make_nvp("v", v_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> w_vector;
    std::vector<double> m_vector;
    std::vector<double> v_vector;

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("m", m_vector);
    ar & boost::serialization::make_nvp("v", v_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));

    _w = Eigen::Map<Eigen::VectorXd>(&w_vector[0], w_vector.size());
    _m = Eigen::Map<Eigen::VectorXd>(&m_vector[0], m_vector.size());
    _v = Eigen::Map<Eigen::VectorXd>(&v_vector[0], v_vector.size());
  }

};

#endif //MOCHIMOCHI_ADAM_HPP_