// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* perceptron_generator.cc
   Jeremy Barnes, 15 March 2006
   Copyright (c) 2006 Jeremy Barnes  All rights reserved.

   Generator for perceptrons.
*/

#include "perceptron_generator.h"
#include "mldb/plugins/jml/jml/registry.h"
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include "mldb/plugins/jml/jml/training_index.h"
#include "mldb/utils/distribution_simd.h"
#include "mldb/arch/simd_vector.h"
#include "mldb/plugins/jml/algebra/lapack.h"
#include "mldb/plugins/jml/algebra/matrix_ops.h"
#include "mldb/plugins/jml/algebra/irls.h"
#include <iomanip>
#include "mldb/utils/environment.h"
#include "mldb/utils/profile.h"
#include "mldb/base/parallel.h"
#include "mldb/base/scope.h"
#include "mldb/plugins/jml/jml/evaluation.h"
#include <boost/scoped_ptr.hpp>
#include "mldb/utils/smart_ptr_utils.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/utils/pair_utils.h"
#include "mldb/plugins/jml/neural/dense_layer.h"
#include "discriminative_trainer.h"

using namespace std;

namespace ML {

namespace {

EnvOption<bool> profile("PROFILE_PERCEPTRON", false);

double t_train = 0.0, t_decorrelate = 0.0;
double t_cholesky = 0.0, t_qr = 0.0, t_gs = 0.0, t_mean = 0.0, t_covar = 0.0;
double t_update = 0.0, t_fprop = 0.0, t_bprop = 0.0, t_zero = 0.0;
double t_setup = 0.0;

struct Stats {
    ~Stats()
    {
        if (profile) {
            cerr << "perceptron training profile: " << endl;
            cerr << "  decorrelate:    " << t_decorrelate << "s" << endl;
            cerr << "    qr:           " << t_qr          << "s" << endl;
            cerr << "    gram schmidt: " << t_gs          << "s" << endl;
            cerr << "    mean:         " << t_mean        << "s" << endl;
            cerr << "    covar         " << t_covar       << "s" << endl;
            cerr << "    cholesky:     " << t_cholesky    << "s" << endl;
            cerr << "  train:          " << t_train       << "s" << endl;
            cerr << "    setup:        " << t_setup       << "s" << endl;
            cerr << "    update:       " << t_update      << "s" << endl;
            cerr << "    fprop:        " << t_fprop       << "s" << endl;
            cerr << "    bprop:        " << t_bprop       << "s" << endl;
            cerr << "    zero:         " << t_zero        << "s" << endl;
        }
    }
} stats;

} // file scope

/*****************************************************************************/
/* PERCEPTRON_GENERATOR                                                      */
/*****************************************************************************/

Perceptron_Generator::
Perceptron_Generator()
{
    defaults();
}

Perceptron_Generator::~Perceptron_Generator()
{
}

void
Perceptron_Generator::
configure(const Configuration & config, vector<string> & unparsedKeys)
{
    Early_Stopping_Generator::configure(config, unparsedKeys);

    config.findAndRemove(max_iter, "max_iter", unparsedKeys);
    config.findAndRemove(min_iter, "min_iter", unparsedKeys);
    config.findAndRemove(learning_rate, "learning_rate", unparsedKeys);
    config.findAndRemove(arch_str, "arch", unparsedKeys);
    config.findAndRemove(batch_size, "batch_size", unparsedKeys);
    config.findAndRemove(activation, "activation", unparsedKeys);
    config.findAndRemove(output_activation, "output_activation", unparsedKeys);
    config.findAndRemove(do_decorrelate, "decorrelate", unparsedKeys);
    config.findAndRemove(do_normalize, "normalize", unparsedKeys);
    config.findAndRemove(target_value, "target_value", unparsedKeys);
}

void
Perceptron_Generator::
defaults()
{
    Early_Stopping_Generator::defaults();
    max_iter = 100;
    min_iter = 10;
    learning_rate = 0.01;
    arch_str = "%i";
    activation = output_activation = TF_TANH;
    do_decorrelate = true;
    do_normalize = true;
    batch_size = 1024;
    target_value = 0.8;
}

Config_Options
Perceptron_Generator::
options() const
{
    Config_Options result = Early_Stopping_Generator::options();
    result
        .add("min_iter", min_iter, "1-max_iter",
             "minimum number of training iterations to run")
        .add("max_iter", max_iter, ">=min_iter",
             "maximum number of training iterations to run")
        .add("learning_rate", learning_rate, "real",
             "positive: rate of learning relative to dataset size: negative for absolute")
        .add("arch", arch_str, "(see doc)",
             "hidden unit specification; %i=in vars, %o=out vars; eg 5_10")
        .add("activation", activation,
             "activation function for neurons")
        .add("output_activation", output_activation,
             "activation function for output layer of neurons")
        .add("decorrelate", do_decorrelate,
             "decorrelate the features before training")
        .add("normalize", do_normalize,
             "normalize to zero mean and unit std before training")
        .add("batch_size", batch_size, "0.0-1.0 or 1 - nvectors",
             "number of samples in each \"mini batch\" for stochastic")
        .add("target_value", target_value, "0.0-1.0", "the output for a 1 that we ask the network to provide");

    return result;
}

void
Perceptron_Generator::
init(std::shared_ptr<const Feature_Space> fs, Feature predicted)
{
    Early_Stopping_Generator::init(fs, predicted);
    model = Perceptron(fs, predicted);
}

std::shared_ptr<Classifier_Impl>
Perceptron_Generator::
generate(Thread_Context & context,
         const Training_Data & training_set,
         const Training_Data & validation_set,
         const distribution<float> & training_ex_weights,
         const distribution<float> & validate_ex_weights,
         const std::vector<Feature> & features, int) const
{
    boost::timer::cpu_timer timer;

    Feature predicted = model.predicted();

    bool regression = feature_space->info(predicted).type() == REAL;

    Perceptron current(model);
    Perceptron best(model);

    float best_acc = 0.0;
    float best_rmse = 0.0;
    int best_iter = 0;

    bool validate_is_train = false;
    //if (validation_set.example_count() == 0
    //    || &validation_set == &training_set)
    //    validate_is_train = true;

    if (validate_ex_weights.total() == 0.0)
        throw Exception("no validate ex weights");

    std::unique_ptr<boost::progress_display> progress;

    //cerr << "training_ex_weights = " << training_ex_weights << endl;
    //cerr << "validate_ex_weights = " << validate_ex_weights << endl;

    log("perceptron_generator", 1)
        << "training " << max_iter << " iterations..." << endl;

    if (verbosity < 3)
        progress.reset(new boost::progress_display
                       (max_iter, log("perceptron_generator", 1)));

    vector<int> arch = Perceptron::parse_architecture(arch_str);

    boost::multi_array<float, 2> decorrelated
        = init(training_set, features, arch, current, context);

    size_t nx_validate
        = (validate_is_train ? training_set : validation_set)
        .example_count();

    boost::multi_array<float, 2> val_decorrelated
        (boost::extents[nx_validate][decorrelated.shape()[1]]);

    if (validate_is_train) val_decorrelated = decorrelated;
    else val_decorrelated = current.decorrelate(validation_set);

    size_t nx = decorrelated.shape()[0];
    size_t nxv = val_decorrelated.shape()[0];
    size_t nf = decorrelated.shape()[1];

    log("perceptron_generator", 1)
        << current.parameters() << " parameters, "
        << nx << " examples, " << nx * nf << " training values" << endl;

    if (min_iter > max_iter)
        throw Exception("min_iter is greater than max_iter");

    log("perceptron_generator", 3)
        << "  it   train     rmse     val     rmse    best     diff" << endl;

    float validate_acc = 0.0, validate_rmse = 0.0;
    float train_acc = 0.0, train_rmse = 0.0;

    const std::vector<Label> & labels
        = training_set.index().labels(predicted);
    const std::vector<Label> & val_labels MLDB_UNUSED
        = validation_set.index().labels(predicted);

    double last_best_acc = 0.0;
    double last_best_rmse MLDB_UNUSED = 0.0;

    int our_batch_size = batch_size;
    if (batch_size == 0.0) our_batch_size = nx;
    else if (batch_size < 1.0) our_batch_size = nx * batch_size;

    /* If we specified a negative learning rate, that means that we wanted it
       to be absolute (and so not depend upon the size of the dataset).  In
       order to get this behaviour, we need to multipy by the number of
       examples in the dataset, since it is implicitly multiplied by the
       example weight (on average 1/num examples in training set) as part of
       the training, and we want to counteract this effect).
    */
    float learning_rate = this->learning_rate;
    if (learning_rate < 0.0) {
        learning_rate
            *= -1.0 * training_ex_weights.size() / training_ex_weights.total();
    }

    // Create a layer stack without the decorrelation layer to be trained
    Layer_Stack<Layer> train_stack;
    for (unsigned i = 1;  i < current.layers.size();  ++i)
        train_stack.add(current.layers.share(i));

    Discriminative_Trainer trainer;
    trainer.layer = &train_stack;

    bool randomize = false;
    float sample_proportion = 1.0;

    vector<const float *> examples(nx);
    for (unsigned i = 0;  i < nx;  ++i)
        examples[i] = &decorrelated[i][0];

    vector<const float *> val_examples(nxv);
    for (unsigned i = 0;  i < nxv;  ++i)
        val_examples[i] = &val_decorrelated[i][0];

    Output_Encoder & output_encoder = current.output;
    output_encoder.configure(model.feature_space()->info(model.predicted()),
                             train_stack, target_value);

    for (unsigned i = 0;  i < max_iter;  ++i) {

        //cerr << "params = " << Parameters_Copy<float>(train_stack.parameters()).values << endl;

        //cerr << "mode = " << output_encoder.mode << " value_true = " << output_encoder.value_true
        //     << " value_false = " << output_encoder.value_false << " num_inputs = "
        //     << output_encoder.num_inputs << " num_outputs = " << output_encoder.num_outputs
        //     << endl;

        {
            PROFILE_FUNCTION(t_train);

            std::tie(train_acc, train_rmse)
                = trainer.train_iter(examples, labels, training_ex_weights,
                                     output_encoder,
                                     context, our_batch_size, learning_rate,
                                     verbosity, sample_proportion, randomize);
        }

        if (validate_is_train) {
            validate_acc = train_acc;
            validate_rmse = train_rmse;
        }
        else {
            std::tie(validate_acc, validate_rmse)
                = trainer.test(val_examples, val_labels, validate_ex_weights,
                               output_encoder, context, verbosity);
        }

        last_best_acc = best_acc;
        last_best_rmse = best_rmse;

        if (i == min_iter
            || (validate_acc > best_acc && !regression)
            || (validate_acc < best_acc && regression)) {
            best = current;
            best_acc = validate_acc;
            best_rmse = validate_rmse;
            best_iter = i;
        }

        log("perceptron_generator", 3)
            << format("%4d %6.2f%% %8.6f %6.2f%% %8.6f %6.2f%% %+7.3f%%",
                      i, train_acc * 100.0, train_rmse, validate_acc * 100.0,
                      validate_rmse,
                      best_acc * 100.0, (validate_acc - last_best_acc) * 100.0)
            << endl;

        if (progress) ++(*progress);

        log("perceptron_generator", 5) << current.print() << endl;
    }

    if (profile)
        log("perceptron_generator", 1)
            << "training time: " << timer.elapsed().wall << "s" << endl;

    log("perceptron_generator", 1)
        << format("best was %6.2f%% on iteration %d", best_acc * 100.0,
                  best_iter)
        << endl;

    float trn_acc, trn_rmse, val_acc, val_rmse;
    std::tie(trn_acc, trn_rmse)
        = best.accuracy(training_set);
    std::tie(val_acc, val_rmse)
        = best.accuracy(validation_set);

    log("perceptron_generator", 1)
        << "best accuracy: " << trn_acc << "/"
        << trn_rmse << " train, "
        << val_acc << "/" << val_rmse
        << " validation" << endl;

    log("perceptron_generator", 4) << best.print() << endl;

    return make_sp(best.make_copy());
}

namespace {

boost::multi_array<double, 2>
cholesky(const boost::multi_array<double, 2> & A_)
{
    PROFILE_FUNCTION(t_cholesky);

    if (A_.shape()[0] != A_.shape()[1])
        throw Exception("cholesky: matrix isn't square");

    int n = A_.shape()[0];

    boost::multi_array<double, 2> A(boost::extents[n][n]);
    std::copy(A_.begin(), A_.end(), A.begin());

    int res = LAPack::potrf('U', n, A.data(), n);

    if (res < 0)
        throw Exception(format("cholesky: potrf: argument %d was illegal", -res));
    else if (res > 0)
        throw Exception(format("cholesky: potrf: leading minor %d of %d "
                               "not positive definite", res, n));

    for (unsigned i = 0;  i < n;  ++i)
        std::fill(&A[i][0] + i + 1, &A[i][0] + n, 0.0);

    return A;

#if 0
    //cerr << "residuals = " << endl << (A * transpose(A)) - A_ << endl;

    boost::multi_array<float, 2> result(boost::extents[n][n]);
    std::copy(A.begin(), A.end(), result.begin());

    return result;
#endif
}

template<typename Float>
boost::multi_array<Float, 2>
lower_inverse(const boost::multi_array<Float, 2> & A)
{
    if (A.shape()[0] != A.shape()[1])
        throw Exception("lower_inverse: matrix isn't square");

    int n = A.shape()[0];

    boost::multi_array<Float, 2> L = A;

    for (int j = 0;  j < n;  ++j) {
        L[j][j] = 1.0 / L[j][j];

        for (int i = j + 1;  i < n;  ++i) {
            double sum = 0.0;
            for (unsigned k = j;  k < i;  ++k)
                sum -= L[i][k] * L[k][j];
            L[i][j] = sum / L[i][i];
        }
    }

    //cerr << "L * A = " << endl << L * A << endl;

    return L;
}

} // file scope

/** Decorrelates the training data, returning a dense decorrelated dataset. */
boost::multi_array<float, 2>
Perceptron_Generator::
decorrelate(const Training_Data & data,
            const std::vector<Feature> & possible_features,
            Perceptron & result) const
{
    PROFILE_FUNCTION(t_decorrelate);

    const Dataset_Index & index = data.index();

    vector<Feature> & features = result.features;
    features.clear();

    cerr << "decorrelate: " << possible_features.size() << " features at input"
         << endl;

    /* Figure out if we can keep each feature or not. */

    for (unsigned i = 0;  i < possible_features.size();  ++i) {
        const Feature & feature = possible_features[i];

        if (feature == result.predicted()) continue;  // don't use label as a feature!

        /* Find out information about the feature from the training data. */
        if (!index.dense(feature)) {
            cerr << "feature " << result.feature_space()->print(feature)
                 << " skipped due to missing values" << endl;
            continue;
        }
        else if (!index.exactly_one(feature)) {
            cerr << "feature " << i << " ("
                 << result.feature_space()->print(feature)
                 << ") skipped due to more than one value" << endl;
            continue;
        }
        else if (index.constant(feature)) {
            cerr << "feature " << i << " ("
                 << result.feature_space()->print(feature)
                 << ") skipped as it is constant over the dataset" << endl;
            continue;
        }
        else {
            float min, max;
            std::tie(min, max) = index.range(feature);
            if (abs(min) > 1e10 || abs(max) > 1e10) {
                cerr << "feature " << i << " ("
                     << result.feature_space()->print(feature)
                     << ") skipped as its range is too large: "
                     << min << " to " << max << endl;
                continue;
            }
        }
        features.push_back(feature);
    }

    cerr << "decorrelate: " << features.size()
         << " features after trivial elimination" << endl;

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

    size_t nx = data.example_count();
    size_t nf = features.size();

    /* Get a dense matrix of the features. */
    boost::multi_array<double, 2> input(boost::extents[nx][nf + 1]);

    for (unsigned x = 0;  x < nx;  ++x) {
        result.extract_features(data[x], &input[x][0]);
        input[x][nf] = 1.0;  // bias term
    }

    //cerr << "input.shape()[0] = " << input.shape()[0] << endl;
    //cerr << "input.shape()[1] = " << input.shape()[1] << endl;
    boost::multi_array<double, 2> inputt = transpose(input);

    //cerr << "inputt.shape()[0] = " << inputt.shape()[0] << endl;
    //cerr << "inputt.shape()[1] = " << inputt.shape()[1] << endl;
    vector<distribution<double> > dependent;
    vector<int> permutations(nf + 1, 0);

#if 0
    boost::multi_array<float, 2> input2 = inputt;

    /* Factorize the matrix with partial pivoting.  This allows us to find the
       largest number of linearly independent columns possible. */

    float tau[nf];
    vector<int> permutations(nf + 1, 0);
    permutations[nf] = 1;  // make bias be a leading column

    {
        PROFILE_FUNCTION(t_qr);
        int res = LAPack::geqp3(nx, nf + 1, inputt.data_begin(), inputt.shape()[1],
                                &permutations[0],
                                tau);

        if (res != 0)
            throw Exception(format("geqp3: error in parameter %d", -res));
    }

    cerr << "permutations = " << permutations << endl;
    for (unsigned i = 0;  i <= nf;  ++i)
        cerr << "r[" << i << "][" << i << "] = " << inputt[i][i] << endl;

    /* Check for linearly dependent columns. */
    inputt = input2;
#endif

    {
        PROFILE_FUNCTION(t_gs);
        permutations = remove_dependent_impl(inputt, dependent, 1e-4);
    }

    //cerr << "permutations = " << permutations << endl;

    /* Find which features are left */
    vector<Feature> new_features;
    for (unsigned i = 0;  i < features.size();  ++i) {
        if (permutations[i] != -1) {
            //cerr << "feature " << new_features.size() << " is old feature "
            //     << i << " (" << feature_space->print(features[i]) << ")"
            //     << endl;
            new_features.push_back(features[i]);
        }
    }

    for (unsigned i = 0;  i < features.size();  ++i) {
        if (permutations[i] == -1) {
            cerr << "feature "
                 << result.feature_space()->print(features[i])
                 << " removed as it can be calculated as ";
            cerr << result.feature_space()->print(features[i])
                 << " = ";
            bool first = true;
            for (unsigned j = 0;  j < dependent[i].size();  ++j) {
                double v = dependent[i][j];
                if (abs(v) < 0.0001) continue;

                if (v < 0.0) {
                    cerr << " - ";
                }
                else {
                    if (first) ;
                    else cerr << " + ";
                }

                v = abs(v);
                if (abs(v - 1.0) < 0.0001) ;
                else cerr << setprecision(4) << v << " ";

                if (j == new_features.size()) ;
                else cerr << result.feature_space()->print(new_features[j]);

                first = false;
            }
            cerr << endl;
        }
    }

    /* Calculate the covariance matrix over those that are left.  Note that
       we could decorrelate them with the orthogonalization that we did above,
       but then we wouldn't be able to save it as a covariance matrix.
    */
    features.swap(new_features);
    nf = features.size();

    distribution<double> mean(nf, 0.0);
    distribution<double> stdev(nf, 0.0);
    boost::multi_array<double, 2> covar(boost::extents[nf][nf]);

    if (do_decorrelate && !do_normalize)
        throw Exception("normalization required if decorrelation is done");

    if (do_normalize || do_decorrelate) {
        {
            PROFILE_FUNCTION(t_mean);
            for (unsigned f = 0;  f < nf;  ++f) {
                mean[f] = SIMD::vec_sum_dp(&inputt[f][0], nx) / nx;
                for (unsigned x = 0;  x < nx;  ++x)
                    inputt[f][x] -= mean[f];
            }
        }

        cerr << "mean = " << mean << endl;

        {
            PROFILE_FUNCTION(t_covar);
            for (unsigned f = 0;  f < nf;  ++f) {
                for (unsigned f2 = 0;  f2 <= f;  ++f2)
                    covar[f][f2] = covar[f2][f]
                        = SIMD::vec_dotprod_dp(&inputt[f][0], &inputt[f2][0], nx) / nx;
            }

            for (unsigned f = 0;  f < nf;  ++f)
                stdev[f] = sqrt(covar[f][f]);

            cerr << "stdev = " << stdev << endl;
        }
    }

    boost::multi_array<double, 2> transform(boost::extents[nf][nf]);

    if (do_decorrelate) {
        /* Do the cholevsky stuff */
        PROFILE_FUNCTION(t_cholesky);
        transform = transpose(lower_inverse(cholesky(covar)));
    }
    else if (do_normalize) {
        /* Use a unit diagonal of 1/stdev for the transform; no
           decorrelation */
        std::fill(transform.origin(),
                  transform.origin() + transform.num_elements(),
                  0.0f);
        for (unsigned f = 0;  f < nf;  ++f)
            transform[f][f] = 1.0 / stdev[f];
    }
    else {
        /* Use the identity function */
        std::fill(transform.origin(),
                  transform.origin() + transform.num_elements(),
                  0.0f);
        for (unsigned f = 0;  f < nf;  ++f)
            transform[f][f] = 1.0;
    }

    /* Finally, we add a layer.  This will perform both the removal of the
       mean (via the biasing) and the decorrelation (via the application
       of the matrix).
    */

    /* y = (x - mean) * A;
         = (x * A) - (mean * A);
    */

    std::shared_ptr<Dense_Layer<float> > layer
        (new Dense_Layer<float>("decorrelation", nf, nf, TF_IDENTITY,
                                MV_NONE));
    layer->weights.resize(boost::extents[transform.shape()[0]][transform.shape()[1]]);
    layer->weights = transform;
    layer->bias = distribution<float>(nf, 0.0);  // already have mean removed

    //cerr << "transform = " << transform << endl;

    boost::multi_array<float, 2> decorrelated(boost::extents[nx][nf]);
    float fv_in[nf];

    for (unsigned x = 0;  x < nx;  ++x) {
        for (unsigned f = 0;  f < nf;  ++f)
            fv_in[f] = inputt[f][x];

        //cerr << "fv_in = " << distribution<float>(fv_in, fv_in + nf) << endl;

        layer->apply(&fv_in[0], &decorrelated[x][0]);

        //cerr << "fv_out = "
        //     << distribution<float>(&decorrelated[x][0],
        //                            &decorrelated[x][0] + nf)
        //     << endl;

    }

    layer->bias = (transform * mean) * -1.0;  // now add the bias
    result.layers.clear();
    result.add_layer(layer);

    layer->validate();

    return decorrelated;
}

boost::multi_array<float, 2>
Perceptron_Generator::
init(const Training_Data & data,
     const std::vector<Feature> & possible_features,
     const std::vector<int> & architecture,
     Perceptron & result,
     Thread_Context & context) const
{
    result = model;

    /* Find out about the output that we need (in particular, how many
       values it can have). */
    boost::multi_array<float, 2> decorrelated
        = decorrelate(data, possible_features, result);

    Feature_Info pred_info = model.feature_space()->info(model.predicted());
    int nout = pred_info.value_count();
    if (nout == 0) nout = 1;  // regression problem; one output

    int nunits = result.features.size();

    cerr << "adding decorrelating input layer with " << nunits
         << " linear units" << endl;

    /* Add hidden layers with the specified sizes */
    for (unsigned i = 0;  i < architecture.size();  ++i) {
        int units = architecture[i];
        if (units == -1) units = result.features.size();

        cerr << "adding hidden layer " << i + 1 << " with "
             << units << " units and activation function "
             << activation << endl;

        std::shared_ptr<Layer>
            layer(new Dense_Layer<float>(format("hidden%d", i),
                                         nunits, units, activation,
                                         MV_NONE, context));
        result.add_layer(layer);
        nunits = units;
    }

    /* Add the output units. */
    std::shared_ptr<Layer> layer
        (new Dense_Layer<float>("output", nunits, nout, output_activation,
                                MV_NONE, context));
    result.add_layer(layer);

    cerr << "adding output layer with " << nout << " units and activation "
         << output_activation << endl;

    return decorrelated;
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Generator, Perceptron_Generator>
    PERCEPTRON_REGISTER("perceptron");

} // file scope

} // namespace ML


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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 KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_
#define KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_

#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "kindr/common/common.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {
namespace eigen_internal {

// 1) Output: AngleAxis

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(p_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(A_IB.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(R_BI.template cast<TReturn>());
}

//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,1> getRotationVectorFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
//{
//  const Eigen::Matrix<TReturn,3,3> rotationMatrix = R_BI.template cast<TReturn>();
//
//  // Bad precision!
//  return Eigen::Matrix<TReturn, 3, 1>();
//}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitX()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitZ()));
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitZ()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitX()));
}

// 2) Output: Quaternion

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return Eigen::Quaternion<TReturn>(aa_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
//  // Untested
//  // Bad precision!
//  double w;
//  double x;
//  double y;
//  double z;
//  w = sqrt(1+mat(0,0)+mat(1,1)+mat(2,2))/2;
//  if(w>0.02){
//    x = 1/4/w*(mat(2,1)-mat(1,2));
//    y = 1/4/w*(mat(0,2)-mat(2,0));
//    z = 1/4/w*(mat(1,0)-mat(0,1));
//  } else {
//    x = sqrt(1+mat(0,0)-mat(1,1)-mat(2,2))/2;
//    y = 1/4/x*(mat(0,1)+mat(1,0));
//    z = 1/4/x*(mat(0,2)+mat(2,0));
//    w = 1/4/x*(mat(2,1)-mat(1,2));
//  }
//  q.w() = w;
//  q.x() = x;
//  q.y() = y;
//  q.z() = z;
//  q.normalize();

  return Eigen::Quaternion<TReturn>(A_IB.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
//  // Untested
//  // Bad precision!
//  double w;
//  double x;
//  double y;
//  double z;
//  w = sqrt(1+mat(0,0)+mat(1,1)+mat(2,2))/2;
//  if(w>0.02){
//    x = 1/4/w*(mat(2,1)-mat(1,2));
//    y = 1/4/w*(mat(0,2)-mat(2,0));
//    z = 1/4/w*(mat(1,0)-mat(0,1));
//  } else {
//    x = sqrt(1+mat(0,0)-mat(1,1)-mat(2,2))/2;
//    y = 1/4/x*(mat(0,1)+mat(1,0));
//    z = 1/4/x*(mat(0,2)+mat(2,0));
//    w = 1/4/x*(mat(2,1)-mat(1,2));
//  }
//  q.w() = w;
//  q.x() = x;
//  q.y() = y;
//  q.z() = z;
//  q.normalize();

  return Eigen::Quaternion<TReturn>(R_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
//  // Tested and Working
//  Eigen::Quaternion<T> p_BI;
//
//  const T sr = sin(rpy_BI(0)/2);
//  const T cr = cos(rpy_BI(0)/2);
//  const T sp = sin(rpy_BI(1)/2);
//  const T cp = cos(rpy_BI(1)/2);
//  const T sy = sin(rpy_BI(2)/2);
//  const T cy = cos(rpy_BI(2)/2);
//
//  const T srsp = sr*sp;
//  const T srcp = sr*cp;
//  const T crsp = cr*sp;
//  const T crcp = cr*cp;
//
//  p_BI.w() = -srsp*sy+crcp*cy;
//  p_BI.x() = crsp*sy+srcp*cy;
//  p_BI.y() = crsp*cy-srcp*sy;
//  p_BI.z() = srsp*cy+crcp*sy;
////  p_BI.normalize();
//
//  return p_BI;

  return Eigen::Quaternion<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitX()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitZ()));
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
//  // Tested and Working
//  Eigen::Quaternion<T> p_BI;
//
//  const T sy = sin(ypr_BI(0)/2);
//  const T cy = cos(ypr_BI(0)/2);
//  const T sp = sin(ypr_BI(1)/2);
//  const T cp = cos(ypr_BI(1)/2);
//  const T sr = sin(ypr_BI(2)/2);
//  const T cr = cos(ypr_BI(2)/2);
//
//  const T sysp = sy*sp;
//  const T sycp = sy*cp;
//  const T cysp = cy*sp;
//  const T cycp = cy*cp;
//
//  p_BI.w() = sysp*sr+cycp*cr;
//  p_BI.x() = -sysp*cr+cycp*sr;
//  p_BI.y() = sycp*sr+cysp*cr;
//  p_BI.z() = sycp*cr-cysp*sr;
////  p_BI.normalize();
//
//  return p_BI;

  return Eigen::Quaternion<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitZ()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitX()));
}

// 3) Output: Transformation Matrix

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.template cast<TReturn>()).toRotationMatrix(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.template cast<TReturn>()).toRotationMatrix(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  return R_BI.template cast<TReturn>(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  Eigen::Matrix<TReturn,3,3> A_IB;

  const TReturn sr = sin(rpy_BI(0));
  const TReturn cr = cos(rpy_BI(0));
  const TReturn sp = sin(rpy_BI(1));
  const TReturn cp = cos(rpy_BI(1));
  const TReturn sy = sin(rpy_BI(2));
  const TReturn cy = cos(rpy_BI(2));

  const TReturn srsy = sr*sy;
  const TReturn srcy = sr*cy;
  const TReturn crsy = cr*sy;
  const TReturn crcy = cr*cy;

  A_IB(0,0) = cp*cy;
  A_IB(0,1) = -cp*sy;
  A_IB(0,2) = sp;
  A_IB(1,0) = crsy+srcy*sp;
  A_IB(1,1) = crcy-srsy*sp;
  A_IB(1,2) = -sr*cp;
  A_IB(2,0) = srsy-crcy*sp;
  A_IB(2,1) = srcy+crsy*sp;
  A_IB(2,2) = cr*cp;

  return A_IB;
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  Eigen::Matrix<TReturn,3,3> A_IB;

  const TReturn sy = sin(ypr_BI(0));
  const TReturn cy = cos(ypr_BI(0));
  const TReturn sp = sin(ypr_BI(1));
  const TReturn cp = cos(ypr_BI(1));
  const TReturn sr = sin(ypr_BI(2));
  const TReturn cr = cos(ypr_BI(2));

  const TReturn sysr = sy*sr;
  const TReturn sycr = sy*cr;
  const TReturn cysr = cy*sr;
  const TReturn cycr = cy*cr;

  A_IB(0,0) = cy*cp;
  A_IB(0,1) = cysr*sp-sycr;
  A_IB(0,2) = sysr+cycr*sp;
  A_IB(1,0) = cp*sy;
  A_IB(1,1) = sysr*sp+cycr;
  A_IB(1,2) = sycr*sp-cysr;
  A_IB(2,0) = -sp;
  A_IB(2,1) = cp*sr;
  A_IB(2,2) = cp*cr;

  return A_IB;
}

// 4) Output: Rotation Matrix

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.template cast<TReturn>()).toRotationMatrix();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.template cast<TReturn>()).toRotationMatrix();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return A_IB.template cast<TReturn>(); // R_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  Eigen::Matrix<TReturn,3,3> R_BI;

  const TReturn sr = sin(rpy_BI(0));
  const TReturn cr = cos(rpy_BI(0));
  const TReturn sp = sin(rpy_BI(1));
  const TReturn cp = cos(rpy_BI(1));
  const TReturn sy = sin(rpy_BI(2));
  const TReturn cy = cos(rpy_BI(2));

  const TReturn srsy = sr*sy;
  const TReturn srcy = sr*cy;
  const TReturn crsy = cr*sy;
  const TReturn crcy = cr*cy;

  R_BI(0,0) = cp*cy;
  R_BI(0,1) = -cp*sy;
  R_BI(0,2) = sp;
  R_BI(1,0) = crsy+srcy*sp;
  R_BI(1,1) = crcy-srsy*sp;
  R_BI(1,2) = -sr*cp;
  R_BI(2,0) = srsy-crcy*sp;
  R_BI(2,1) = srcy+crsy*sp;
  R_BI(2,2) = cr*cp;

  return R_BI;
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  Eigen::Matrix<TReturn,3,3> R_BI;

  const TReturn sy = sin(ypr_BI(0));
  const TReturn cy = cos(ypr_BI(0));
  const TReturn sp = sin(ypr_BI(1));
  const TReturn cp = cos(ypr_BI(1));
  const TReturn sr = sin(ypr_BI(2));
  const TReturn cr = cos(ypr_BI(2));

  const TReturn sysr = sy*sr;
  const TReturn sycr = sy*cr;
  const TReturn cysr = cy*sr;
  const TReturn cycr = cy*cr;

  R_BI(0,0) = cy*cp;
  R_BI(0,1) = cysr*sp-sycr;
  R_BI(0,2) = sysr+cycr*sp;
  R_BI(1,0) = cp*sy;
  R_BI(1,1) = sysr*sp+cycr;
  R_BI(1,2) = sycr*sp-cysr;
  R_BI(2,0) = -sp;
  R_BI(2,1) = cp*sr;
  R_BI(2,2) = cp*cr;

  return R_BI;
}

// 5) Output: Roll-Pitch-Yaw

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.toRotationMatrix().eulerAngles(0, 1, 2)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
//  Eigen::Matrix<T,3,1> rpy_BI;
//
//  const T w = p_BI.w();
//  const T x = p_BI.x();
//  const T y = p_BI.y();
//  const T z = p_BI.z();
//
////  rpy_BI(1) = -asin(2*(x*z-w*y));
////
//////  std::cout << 2*(x*z-w*y)+1 << std::endl;
////
////  if(cos(rpy_BI(1)) == 0) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
////  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(2) = 0;
////  }
////  else
////  {
////    rpy_BI(0) = atan2(2*(y*z+w*x),1-2*(x*x+y*y));
////    rpy_BI(2) = atan2(2*(x*y+w*z),1-2*(y*y+z*z));
////  }
//
//
//  // NEW
//
////  const T rpy_BI(1) = atan2(-2*(x*z-w*y),sqrt((1-2*(y*y+z*z))*(1-2*(y*y+z*z))+4*(x*y+w*z)*(x*y+w*z)));
//  const T test = 2*(x*z-w*y);
//
//  if(test > 0.999999999) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
//  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(0) = acos(1-2*(x*x+z*z));
////    rpy_BI(0) = atan2(2*(y*z-w*x),1-2*(x*x+z*z));
//    rpy_BI(0) = 2*atan2(x,w);
//    rpy_BI(1) = -M_PI/2; // if test is a slight bit larger than 1, asin(test) does not work anymore
//    rpy_BI(2) = 0;
//  }
//  else if(test < -0.999999999)
//  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(0) = acos(1-2*(x*x+z*z));
////    rpy_BI(0) = atan2(2*(y*z-w*x),1-2*(x*x+z*z));
//    rpy_BI(0) = -2*atan2(x,w);
//    rpy_BI(1) = M_PI/2;
//    rpy_BI(2) = 0;
//  }
//  else
//  {
//    rpy_BI(0) = atan2(2*(y*z+w*x),1-2*(x*x+y*y));
//    rpy_BI(1) = -asin(test);
//    rpy_BI(2) = atan2(2*(x*y+w*z),1-2*(y*y+z*z));
//  }
//
//  return rpy_BI;

  return (p_BI.toRotationMatrix().eulerAngles(0, 1, 2)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return (A_IB.eulerAngles(0, 1, 2)).template cast<TReturn>(); // rpy_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  return (R_BI.eulerAngles(0, 1, 2)).template cast<TReturn>(); // rpy_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  return getRpyFromQuaternion(getQuaternionFromYpr(ypr_BI)).template cast<TReturn>();
//  return getRpyFromQuaternion(getQuaternionFromYpr(ypr_BI.template cast<TReturn>()));
}

// 6) Output: Yaw-Pitch-Roll

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.toRotationMatrix().eulerAngles(2, 1, 0)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
//  Eigen::Matrix<T,3,1> ypr_BI;
//
//  const T w = p_BI.w();
//  const T x = p_BI.x();
//  const T y = p_BI.y();
//  const T z = p_BI.z();
//
////  ypr_BI(1) = asin(2*(x*z+w*y));
////
////  if(cos(ypr_BI(1)) == 0) // yaw and roll axis are the same -> roll angle can be set to zero, yaw does the whole rotation
////  {
////    ypr_BI(0) = asin(2*(y*z+w*x));
////    ypr_BI(2) = 0;
////  }
////  else
////  {
////    ypr_BI(0) = -atan2(2*(x*y-w*z),1-2*(y*y+z*z));
////    ypr_BI(2) = -atan2(2*(y*z-w*x),1-2*(x*x+y*y));
////  }
//
//
//  // NEW
//
////  const T rpy_BI(1) = atan2(-2*(x*z-w*y),sqrt((1-2*(y*y+z*z))*(1-2*(y*y+z*z))+4*(x*y+w*z)*(x*y+w*z)));
//  const T test = 2*(x*z+w*y);
//
//  if(test > 0.999999999) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
//  {
////    ypr_BI(0) = asin(2*(y*z+w*x));
//    ypr_BI(0) = 2*atan2(x,w);
//    ypr_BI(1) = M_PI/2; // if test is a slight bit larger than 1, asin(test) does not work anymore
//    ypr_BI(2) = 0;
//  }
//  else if(test < -0.999999999)
//  {
////    ypr_BI(0) = -asin(2*(y*z+w*x));
//    ypr_BI(0) = -2*atan2(x,w);
//    ypr_BI(1) = -M_PI/2;
//    ypr_BI(2) = 0;
//  }
//  else
//  {
//    ypr_BI(0) = -atan2(2*(x*y-w*z),1-2*(y*y+z*z));
//    ypr_BI(1) = asin(test);
//    ypr_BI(2) = -atan2(2*(y*z-w*x),1-2*(x*x+y*y));
//  }
//
//  return ypr_BI;

//  const TReturn q0 = p_BI.w();
//  const TReturn q1 = p_BI.x();
//  const TReturn q2 = p_BI.y();
//  const TReturn q3 = p_BI.z();
//  return Eigen::Matrix<TReturn,3,1>(atan2(2.0*q1*q2 + 2.0*q0*q3, q1*q1 + q0*q0 - q3*q3 - q2*q2),
//                                    -asin(2.0*q1*q3 - 2.0*q0*q2),
//                                    atan2(2.0*q2*q3 + 2.0*q0*q1, q3*q3 - q2*q2 - q1*q1 + q0*q0));

  return (p_BI.toRotationMatrix().eulerAngles(2, 1, 0)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return (A_IB.eulerAngles(2, 1, 0)).template cast<TReturn>(); // ypr_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  const TReturn r23 = R_BI(1,2);
  const TReturn r33 = R_BI(2,2);
  const TReturn r13 = R_BI(0,2);
  const TReturn r12 = R_BI(0,1);
  const TReturn r11 = R_BI(0,0);

  return Eigen::Matrix<TReturn,3,1>(atan2(r12,r11), -asin(r13), atan2(r23,r33));
//  return (R_BI.eulerAngles(2, 1, 0)).template cast<TReturn>(); // original
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  return getYprFromQuaternion(getQuaternionFromRpy(rpy_BI)).template cast<TReturn>();
//  return getYprFromQuaternion(getQuaternionFromRpy(rpy_BI.template cast<TReturn>()));
}

// TODO inverse will be deleted in future

// 7) Output: Inverses

//template<typename T, typename TReturn = T>
//static Eigen::AngleAxis<TReturn> getInverseAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
//{
//  return (aa_BI.template cast<TReturn>()).inverse();
//}
//
template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getInverseQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.conjugate()).template cast<TReturn>();
}
//
//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,3> getInverseTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
//{
//  return (A_IB.template cast<TReturn>()).transpose();
//}
//
//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,3> getInverseRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
//{
//  return (R_BI.template cast<TReturn>()).transpose();
//}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getInverseRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  return getRpyFromQuaternion(getInverseQuaternion(getQuaternionFromRpy(rpy_BI.template cast<TReturn>())));
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getInverseYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  return getYprFromQuaternion(getInverseQuaternion(getQuaternionFromYpr(ypr_BI.template cast<TReturn>())));
}

/*

Matrix3x4d get_H_bar(const Vector4d &p)
{
  Matrix3x4d H_bar = Matrix3x4d::Zero();
  const double w = p(0);
  const Vector3d e = p.tail(3);
  const Matrix3x3d eye3 = Matrix3x3d::Identity();

  H_bar.col(0) = -e;
  H_bar.block<3,3>(0,1) = -skewsymm(e)+w*eye3;

  return H_bar;
}

MatrixDyn get_F(const VectorDyn &q)
{
  const double dim_q = q.rows();
  const double dim_u = dim_q - 1;

  MatrixDyn F = MatrixDyn::Zero(dim_q,dim_u);
  const Vector4d p = q.block<4,1>(3,0);

  F.block<3,3>(0,0) = Matrix3x3d::Identity();
  F.block<4,3>(3,3) = 0.5*get_H_bar(p).transpose();
  if(dim_u > 6)
  {
    F.block(7,6,dim_u-6,dim_u-6) = MatrixDyn::Identity(dim_u-6,dim_u-6);
  }

  return F;
}

VectorDyn get_dqdt(const VectorDyn &q, const VectorDyn &u)
{
  const double dim_q = q.rows();
  const double dim_u = dim_q - 1;

  VectorDyn dqdt = VectorDyn::Zero(dim_q);
  const Vector4d p = q.block<4,1>(3,0);

  dqdt.head(3) = u.head(3);
  dqdt.block<4,1>(3,0) = 0.5*get_H_bar(p).transpose()*u.block<3,1>(3,0);
  if(dim_u > 6)
  {
    dqdt.tail(dim_u-6) = u.tail(dim_u-6);
  }

  return dqdt;
}

VectorDyn get_dqdt_2(const VectorDyn &q, const VectorDyn &u) // older version, slower
{
  return get_F(q)*u;
}

void prox1D(double &y, const double &x, const double &min, const double &max)
{
  if     (x < min) {y = min;}
  else if(x > max) {y = max;}
  else             {y = x;  }
}

void prox2D(double &y1, double &y2, const double &x1, const double &x2, const double &max)
{
  const double r = sqrt(x1*x1 + x2*x2);
  if(r > max)
  {
    y1 = max*x1/r;
    y2 = max*x2/r;
  }
  else
  {
    y1 = x1;
    y2 = x2;
  }
}

*/

/*

Vector4d multiplyQuaternion(const Vector4d &p_CB, const Vector4d &p_BA) // same in eigen
{
  // p_CA = p_CB*p_BA
  Vector4d p_CA = Vector4d::Zero();

  p_CA(0) = p_CB(0)*p_BA(0) - p_CB.tail(3).transpose()*p_BA.tail(3);
  p_CA.tail(3) = p_CB(0)*p_BA.tail(3) + p_BA(0)*p_CB.tail(3) + skewsymm(p_BA.tail(3))*p_CB.tail(3);

  p_CA.normalize();

  return p_CA;
}

double w_to_angleVel(const Vector3d &K_w_JK, const Vector3d &n)
{
//  if(n(0) == 1) // around x
//  {
//    return K_w_JK(0);
//  }
//  else
//  if(n(1) == 1) // around y
//  {
//    return K_w_JK(1);
//  }
//  else
//  if(n(2) == 1) // around z
//  {
//    return K_w_JK(2);
//  }

  const Vector3d n_norm = n.normalized();

  return n_norm.dot(K_w_JK); // attention: dot product doesn't eliminate small errors
}

Vector3d omega2kardan(const Vector3d &K_w_IK, const Vector3d &abc) // quaternion: rotation from I to K, kardan: x-y-z with alpha-beta-gamma
{
  double alpha = abc(0);
  double beta = abc(1);
  double gamma = abc(2);

  Vector3d dadbdc = Vector3d::Zero();
  Matrix3x3d H = Matrix3x3d::Zero();

  if(cos(beta) == 0)
  {
    H << std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 0, sin(gamma), cos(gamma), 0, -cos(gamma)*tan(beta), sin(gamma)*tan(beta), 1;
  }
  else
  {
    H << cos(gamma)/cos(beta), -sin(gamma)/cos(beta), 0, sin(gamma), cos(gamma), 0, -cos(gamma)*tan(beta), sin(gamma)*tan(beta), 1;
  }

  dadbdc = H*K_w_IK;

  return dadbdc;

  */

} // namespace eigen_internal
} // namespace eigen_impl
} // namespace rotations
} // namespace rm

#endif /* KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_ */


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__FIRST_ORDER_FILTER_HPP_
#define CBR_CONTROL__FIRST_ORDER_FILTER_HPP_

#include <Eigen/Core>

#include <cbr_utils/cyber_timer.hpp>

#include <cmath>
#include <memory>
#include <type_traits>
#include <utility>

namespace cbr
{

namespace fof_details
{

template<typename T>
struct zero_value
{
  static T value()
  {
    if constexpr (std::is_base_of_v<Eigen::MatrixBase<T>, T>) {
      return T::Zero();
    } else {
      return T{0};
    }
  }
};

}  // namespace fof_details

template<typename T, typename _clock_t = std::chrono::high_resolution_clock>
class FirstOrderFilter
{
public:
  using clock_t = _clock_t;
  using timer_t = CyberTimerNoAvg<std::ratio<1>, double, clock_t>;

  FirstOrderFilter() = default;
  FirstOrderFilter(const FirstOrderFilter &) = default;
  FirstOrderFilter(FirstOrderFilter &) = default;
  FirstOrderFilter(FirstOrderFilter &&) = default;
  FirstOrderFilter & operator=(const FirstOrderFilter &) = default;
  FirstOrderFilter & operator=(FirstOrderFilter &) = default;
  FirstOrderFilter & operator=(FirstOrderFilter &&) = default;
  ~FirstOrderFilter() = default;

  template<typename T1>
  explicit FirstOrderFilter(T1 && clock, const double tau = 1.)
  : timer_(std::forward<T1>(clock)),
    tau_(tau)
  {}

  explicit FirstOrderFilter(const double tau)
  : tau_(tau)
  {}

  template<typename T1>
  void set_clock(T1 && clock)
  {
    timer_.set_clock(std::forward<T1>(clock));
  }

  void set_params(const double tau)
  {
    tau_ = tau;
  }

  double get_params() const
  {
    return tau_;
  }

  const T & update(const T & val, const typename timer_t::time_point tNow)
  {
    if (tau_ <= 0.0) {
      return valNm1_ = val;
    }

    if (init_) {
      const double dt = timer_.toctic(tNow);
      const double e = std::exp(-dt / tau_);
      return valNm1_ = valNm1_ * e + (1 - e) * val;
    }

    init_ = true;
    timer_.tic(tNow);
    return valNm1_ = val;
  }

  const T & update(const T & val)
  {
    return update(val, timer_.now());
  }
  const T & operator()(const T & val)
  {
    return update(val);
  }

  const T & getValue() const
  {
    return valNm1_;
  }

  void reset()
  {
    init_ = false;
  }

  const T & reset(const T & val, const typename timer_t::time_point tNow)
  {
    timer_.tic(tNow);
    init_ = true;
    return valNm1_ = val;
  }

  const T & reset(const T & val)
  {
    return reset(val, timer_.now());
  }

protected:
  timer_t timer_ = timer_t(clock_t{});
  double tau_{1.0};
  T valNm1_{fof_details::zero_value<T>::value()};
  bool init_{false};
};

}  // namespace cbr

#endif  // CBR_CONTROL__FIRST_ORDER_FILTER_HPP_


#pragma once

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

#include <cmath>
#include <functional>
#include <map>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace math
{
///////////////////////////////////////////////////////////////
//// Typedefs for aligned STL containers using Eigen types ////
///////////////////////////////////////////////////////////////

using VectorVector2f
  = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>;
using VectorVector2d
  = std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>;
using VectorVector3f
  = std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>;
using VectorVector3d
  = std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>;
using VectorVector4f
  = std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>>;
using VectorVector4d
  = std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>;
using VectorQuaternionf
  = std::vector<Eigen::Quaternionf,
                Eigen::aligned_allocator<Eigen::Quaternionf>>;
using VectorQuaterniond
  = std::vector<Eigen::Quaterniond,
                Eigen::aligned_allocator<Eigen::Quaterniond>>;
using VectorIsometry3f
  = std::vector<Eigen::Isometry3f,
                Eigen::aligned_allocator<Eigen::Isometry3f>>;
using VectorIsometry3d
  = std::vector<Eigen::Isometry3d,
                Eigen::aligned_allocator<Eigen::Isometry3d>>;
using MapStringVector2f
  = std::map<std::string, Eigen::Vector2f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2f>>>;
using MapStringVector2d
  = std::map<std::string, Eigen::Vector2d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2d>>>;
using MapStringVector3f
  = std::map<std::string, Eigen::Vector3f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3f>>>;
using MapStringVector3d
  = std::map<std::string, Eigen::Vector3d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3d>>>;
using MapStringVector4f
  = std::map<std::string, Eigen::Vector4f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4f>>>;
using MapStringVector4d
  = std::map<std::string, Eigen::Vector4d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4d>>>;
using MapStringQuaternionf
  = std::map<std::string, Eigen::Quaternionf, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaternionf>>>;
using MapStringQuaterniond
  = std::map<std::string, Eigen::Quaterniond, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>>;
using MapStringIsometry3f
  = std::map<std::string, Eigen::Isometry3f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3f>>>;
using MapStringIsometry3d
  = std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d>>>;

bool Equal3d(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2);

bool Equal4d(const Eigen::Vector4d& v1, const Eigen::Vector4d& v2);

bool CloseEnough(const double p1, const double p2, const double threshold);

bool CloseEnough(const Eigen::Vector3d& v1,
                 const Eigen::Vector3d& v2,
                 const double threshold);

Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat,
                             const Eigen::Vector3d& vec);

Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat,
                                    const Eigen::Vector3d& vec);

double EnforceContinuousRevoluteBounds(const double value);

Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec);

double SquaredNorm(const std::vector<double>& vec);

double Norm(const std::vector<double>& vec);

std::vector<double> Abs(const std::vector<double>& vec);

std::vector<double> Multiply(const std::vector<double>& vec,
                             const double scalar);

std::vector<double> Multiply(const std::vector<double>& vec1,
                             const std::vector<double>& vec2);

std::vector<double> Divide(const std::vector<double>& vec,
                           const double scalar);

std::vector<double> Divide(const std::vector<double>& vec1,
                           const std::vector<double>& vec2);

std::vector<double> Add(const std::vector<double>& vec,
                        const double scalar);

std::vector<double> Add(const std::vector<double>& vec1,
                        const std::vector<double>& vec2);

std::vector<double> Sub(const std::vector<double>& vec,
                        const double scalar);

std::vector<double> Sub(const std::vector<double>& vec1,
                        const std::vector<double>& vec2);

double Sum(const std::vector<double>& vec);

Eigen::Matrix3d Skew(const Eigen::Vector3d& vector);

Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix);

Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist);

Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist);

Eigen::Matrix<double, 6, 6> AdjointFromTransform(
    const Eigen::Isometry3d& transform);

Eigen::Matrix<double, 6, 1> TransformTwist(
    const Eigen::Isometry3d& transform,
    const Eigen::Matrix<double, 6, 1>& initial_twist);

Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& end);

Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity,
                               const double delta_t);

Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist,
                           const double delta_t);

double Interpolate(const double p1, const double p2, const double ratio);

double InterpolateContinuousRevolute(const double p1,
                                     const double p2,
                                     const double ratio);

std::vector<double> Interpolate(const std::vector<double>& v1,
                                const std::vector<double>& v2,
                                const double ratio);

Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1,
                               const Eigen::Quaterniond& q2,
                               const double ratio);

Eigen::VectorXd InterpolateXd(const Eigen::VectorXd& v1,
                              const Eigen::VectorXd& v2,
                              const double ratio);

Eigen::Vector3d Interpolate3d(const Eigen::Vector3d& v1,
                              const Eigen::Vector3d& v2,
                              const double ratio);

Eigen::Vector4d Interpolate4d(const Eigen::Vector4d& v1,
                              const Eigen::Vector4d& v2,
                              const double ratio);

Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1,
                              const Eigen::Isometry3d& t2,
                              const double ratio);

double SquaredDistance(const Eigen::Vector2d& v1,
                       const Eigen::Vector2d& v2);

double Distance(const Eigen::Vector2d& v1,
                const Eigen::Vector2d& v2);

double SquaredDistance(const Eigen::Vector3d& v1,
                       const Eigen::Vector3d& v2);

double Distance(const Eigen::Vector3d& v1,
                const Eigen::Vector3d& v2);

double SquaredDistance(const Eigen::VectorXd& v1,
                       const Eigen::VectorXd& v2);

double Distance(const Eigen::VectorXd& v1,
                const Eigen::VectorXd& v2);

double Distance(const Eigen::Quaterniond& q1,
                const Eigen::Quaterniond& q2);

double Distance(const Eigen::Isometry3d& t1,
                const Eigen::Isometry3d& t2,
                const double alpha=0.5);

double SquaredDistance(const std::vector<double>& p1,
                       const std::vector<double>& p2);

double Distance(const std::vector<double>& p1, const std::vector<double>& p2);

double ContinuousRevoluteSignedDistance(const double p1, const double p2);

double ContinuousRevoluteDistance(const double p1, const double p2);

double AddContinuousRevoluteValues(const double start, const double change);

double GetContinuousRevoluteRange(const double start, const double end);

bool CheckInContinuousRevoluteRange(const double start,
                                    const double range,
                                    const double val);

bool CheckInContinuousRevoluteBounds(const double start,
                                     const double end,
                                     const double val);

double AverageStdVectorDouble(
    const std::vector<double>& values,
    const std::vector<double>& weights=std::vector<double>());

double ComputeStdDevStdVectorDouble(const std::vector<double>& values,
                                    const double mean);

double ComputeStdDevStdVectorDouble(const std::vector<double>& values);

double WeightedDotProduct(const Eigen::VectorXd& vec1,
                          const Eigen::VectorXd& vec2,
                          const Eigen::VectorXd& weights);

double WeightedSquaredNorm(const Eigen::VectorXd& vec,
                           const Eigen::VectorXd weights);

double WeightedNorm(const Eigen::VectorXd& vec,
                    const Eigen::VectorXd& weights);

double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                         const Eigen::VectorXd& vec2,
                                         const Eigen::VectorXd& weights);

double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                   const Eigen::VectorXd& vec2,
                                   const Eigen::VectorXd& weights);

Eigen::Vector3d AverageEigenVector3d(
    const VectorVector3d& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Vector4d AverageEigenVector4d(
    const VectorVector4d& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::VectorXd AverageEigenVectorXd(
    const std::vector<Eigen::VectorXd>& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Quaterniond AverageEigenQuaterniond(
    const VectorQuaterniond& quaternions,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Isometry3d AverageEigenIsometry3d(
    const VectorIsometry3d& transforms,
    const std::vector<double>& weights=std::vector<double>());

// This function does not actually deal with the continuous revolute
// space correctly, it just assumes a normal real Euclidean space
double AverageContinuousRevolute(
    const std::vector<double>& angles,
    const std::vector<double>& weights=std::vector<double>());

template <typename Derived>
inline Eigen::MatrixXd ClampNorm(
    const Eigen::MatrixBase<Derived>& item_to_clamp, const double max_norm)
{
  const double current_norm = item_to_clamp.norm();
  if (current_norm > max_norm)
  {
    return item_to_clamp * (max_norm / current_norm);
  }
  return item_to_clamp;
}

// This function is really only going to work well for "almost continuous"
// types, i.e. floats and doubles, due to the implementation
template<typename ScalarType, int Rows,
         typename Allocator=std::allocator<Eigen::Matrix<ScalarType, Rows, 1>>>
inline Eigen::Matrix<ScalarType, Rows, 1> AverageEigenVector(
    const std::vector<Eigen::Matrix<ScalarType, Rows, 1>, Allocator>& vectors,
    const std::vector<double>& weights = std::vector<double>())
{
  // Get the weights
  if (vectors.empty())
  {
    throw std::invalid_argument("vectors is empty");
  }
  if ((weights.size() > 0) && (vectors.size() != weights.size()))
  {
    throw std::invalid_argument("weights.size() > 0 != vectors.size()");
  }
  const bool use_weights = (weights.size() != 0);
  // Find the first element with non-zero weight
  size_t starting_idx = 0;
  while (starting_idx < weights.size() && weights[starting_idx] == 0.0)
  {
    starting_idx++;
  }
  // If all weights are zero, result is undefined
  if (starting_idx >= vectors.size())
  {
    throw std::invalid_argument("All provided weights are zero");
  }
  // Start the recursive definition with the base case
  Eigen::Matrix<ScalarType, Rows, 1> avg_vector = vectors[starting_idx];
  const double starting_weight = use_weights ? std::abs(weights[starting_idx])
                                             : 1.0;
  double weights_running_sum = starting_weight;
  // Do the weighted averaging on the rest of the vectors
  for (size_t idx = starting_idx + 1; idx < vectors.size(); ++idx)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    weights_running_sum += weight;
    const double effective_weight = weight / weights_running_sum;
    const Eigen::Matrix<ScalarType, Rows, 1> prev_avg_vector = avg_vector;
    const Eigen::Matrix<ScalarType, Rows, 1>& current = vectors[idx];
    avg_vector = prev_avg_vector
                 + (effective_weight * (current - prev_avg_vector));
  }
  return avg_vector;
}

// Projects vector_to_project onto base_vector and
// returns the portion that is parallel to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
VectorProjection(
    const Eigen::MatrixBase<DerivedB>& base_vector,
    const Eigen::MatrixBase<DerivedV>& vector_to_project)
{
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB);
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedV);
  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB, DerivedV)
  static_assert(std::is_same<typename DerivedB::Scalar,
                typename DerivedV::Scalar>::value,
                "vectors must have the same data type");
  // Perform projection
  const typename DerivedB::Scalar b_squared_norm = base_vector.squaredNorm();
  if (b_squared_norm > 0)
  {
    return (base_vector.dot(vector_to_project) / b_squared_norm) * base_vector;
  }
  else
  {
    return Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
        ::Zero(base_vector.rows());
  }
}

// Projects vector_to_project onto base_vector and
// returns the portion that is perpendicular to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
VectorRejection(
    const Eigen::MatrixBase<DerivedB>& base_vector,
    const Eigen::MatrixBase<DerivedV>& vector_to_reject)
{
  // Rejection is defined in relation to projection
  return vector_to_reject - VectorProjection(base_vector, vector_to_reject);
}

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
GetArbitraryOrthogonalVector(const Eigen::MatrixBase<DerivedV>& vector)
{
  // We're going to try arbitrary possibilities until one of them works
  const ssize_t vector_size = vector.size();
  if (vector_size > 0)
  {
    for (ssize_t idx = 0; idx < vector_size; idx++)
    {
      Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> test_vector
          = Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
          ::Zero(vector_size);
      test_vector(idx) = static_cast<typename DerivedV::Scalar>(1.0);
      const auto rejected_vector = VectorRejection(vector, test_vector);
      const typename DerivedV::Scalar rejected_vector_squared_norm
          = rejected_vector.squaredNorm();
      if (rejected_vector_squared_norm > 0)
      {
        return rejected_vector;
      }
    }
    throw std::runtime_error("Vector rejection failed to find orthogonal"
                             " vector, probably numerical error");
  }
  else
  {
    throw std::invalid_argument("Vector size is zero");
  }
}

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
GetArbitraryOrthogonalVectorToPlane(
    const Eigen::MatrixBase<DerivedV>& plane_vector1,
    const Eigen::MatrixBase<DerivedV>& plane_vector2,
    const Eigen::MatrixBase<DerivedV>& vector)
{
  const ssize_t vector_size = vector.size();
  if ((vector_size > 0)
      && (vector_size == plane_vector1.size())
      && (vector_size == plane_vector2.size()))
  {
    const Eigen::MatrixBase<DerivedV> unit_plane_vector1
        = plane_vector1 / plane_vector1.norm();
    const Eigen::MatrixBase<DerivedV> unit_plane_vector2
        = plane_vector2 / plane_vector2.norm();
    const typename DerivedV::Scalar plane_vector_dot_product_mag
        = std::abs(unit_plane_vector1.dot(unit_plane_vector2));
    if (plane_vector_dot_product_mag == 1.0)
    {
      throw std::invalid_argument("Plane vectors do not define a valid plane");
    }
    else
    {
      // Try both plane vectors
      // (by definition, one of the two MUST have an orthogonal component!)
      const auto rejected_vector1 = VectorRejection(vector, plane_vector1);
      const typename DerivedV::Scalar rejected_vector1_squared_norm
          = rejected_vector1.squaredNorm();
      if (rejected_vector1_squared_norm > 0)
      {
        return rejected_vector1;
      }
      else
      {
        const auto rejected_vector2 = VectorRejection(vector, plane_vector2);
        const typename DerivedV::Scalar rejected_vector2_squared_norm
            = rejected_vector2.squaredNorm();
        if (rejected_vector2_squared_norm > 0)
        {
          return rejected_vector2;
        }
        else
        {
          throw std::runtime_error("Vector rejection failed to find orthogonal"
                                   " vector, probably numerical error");
        }
      }
    }
  }
  else
  {
    throw std::invalid_argument("Vector size is zero");
  }
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data.size(), data.size());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < data.size(); idx++)
  {
    for (size_t jdx = idx; jdx < data.size(); jdx++)
    {
      if (idx != jdx)
      {
        const double distance = distance_fn(data[idx], data[jdx]);
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = distance;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = distance;
      }
      else
      {
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = 0.0;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = 0.0;
      }
    }
  }
  return distance_matrix;
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data.size(), data.size());
  for (size_t idx = 0; idx < data.size(); idx++)
  {
    for (size_t jdx = idx; jdx < data.size(); jdx++)
    {
      if (idx != jdx)
      {
        const double distance = distance_fn(data[idx], data[jdx]);
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = distance;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = distance;
      }
      else
      {
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = 0.0;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = 0.0;
      }
    }
  }
  return distance_matrix;
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return BuildPairwiseDistanceMatrixParallel(data, distance_fn);
  }
  else
  {
    return BuildPairwiseDistanceMatrixSerial(data, distance_fn);
  }
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data1.size(), data2.size());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < data1.size(); idx++)
  {
    for (size_t jdx = 0; jdx < data2.size(); jdx++)
    {
      const double distance = distance_fn(data1[idx], data2[jdx]);
      distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
          = distance;
    }
  }
  return distance_matrix;
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data1.size(), data2.size());
  for (size_t idx = 0; idx < data1.size(); idx++)
  {
    for (size_t jdx = 0; jdx < data2.size(); jdx++)
    {
      const double distance = distance_fn(data1[idx], data2[jdx]);
      distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
          = distance;
    }
  }
  return distance_matrix;
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return BuildPairwiseDistanceMatrixParallel(data1, data2, distance_fn);
  }
  else
  {
    return BuildPairwiseDistanceMatrixSerial(data1, data2, distance_fn);
  }
}

class Hyperplane
{
private:
  Eigen::VectorXd plane_origin_;
  Eigen::VectorXd plane_normal_;

public:

  Hyperplane(const Eigen::VectorXd& origin, const Eigen::VectorXd& normal)
      : plane_origin_(origin), plane_normal_(normal)
  {
    if (plane_origin_.size() != plane_normal_.size())
    {
      throw std::invalid_argument("origin.size() != normal.size()");
    }
  }

  Hyperplane() {}

  size_t GetDimensionality() const
  {
    return static_cast<size_t>(plane_origin_.size());
  }

  const Eigen::VectorXd& GetOrigin() const { return plane_origin_; }

  const Eigen::VectorXd& GetNormal() const { return plane_normal_; }

  double GetNormedDotProduct(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd check_vector = point - GetOrigin();
    const Eigen::VectorXd check_vector_normed = SafeNormal(check_vector);
    const double dot_product = check_vector_normed.dot(GetNormal());
    return dot_product;
  }

  double GetRawDotProduct(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd check_vector = point - GetOrigin();
    const double dot_product = check_vector.dot(GetNormal());
    return dot_product;
  }

  Eigen::VectorXd RejectVectorOntoPlane(const Eigen::VectorXd& vector) const
  {
    return VectorProjection(GetNormal(), vector);
  }

  double GetSquaredDistanceToPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    return VectorProjection(GetNormal(), origin_to_point_vector).squaredNorm();
  }

  double GetDistanceToPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    return VectorProjection(GetNormal(), origin_to_point_vector).norm();
  }

  Eigen::VectorXd ProjectVectorOntoPlane(const Eigen::VectorXd& vector) const
  {
    return VectorRejection(GetNormal(), vector);
  }

  Eigen::VectorXd ProjectPointOntoPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    const Eigen::VectorXd projected_to_point_vector =
        VectorRejection(GetNormal(), origin_to_point_vector);
    const Eigen::VectorXd projected_point =
        GetOrigin() + projected_to_point_vector;
    return projected_point;
  }
};

Hyperplane FitPlaneToPoints(const std::vector<Eigen::VectorXd>& points);
}  // namespace math
}  // namespace common_robotics_utilities


//============================================================================
// Name        : LBFGSSolver.cpp
// Author      : Yaser
// Version     :
// Copyright   :
// Description : LBFGS MPI implementation
//============================================================================

#include "LBFGSSolver.h"
#include <boost/mpi.hpp>
#include <iomanip>

namespace mpi = boost::mpi;

//virtual void computeAndTakeStep();
//virtual double ();

LBFGSSolver::LBFGSSolver(ProblemData problemData, OptimizationParameters _optParams) :
						QuasiNewtonSolver(problemData, _optParams) {
	mpi::communicator world;
	stepSize = optParams.stepsize;
	lbfgsHistorySize = optParams.lbfgs_memory;
	m = optParams.m;
	globalN = optParams.n;
	localN = optParams.n / world.size();
	int localNTmp = localN;
	// We are going to split data by columns. Local N tells how many columns
	//	are stored locally.
	if (world.rank() == world.size() - 1) { // If optParam.n is not divisible by world.size we need to have few extra points on last node
		localN = optParams.n - (localN * (world.size() - 1));
	}
	Alocal.resize(localN * m);
	xLocalEven.resize(localN);
	xLocalOdd.resize(localN);
	cLocal.resize(localN * lbfgsHistorySize*2);
	preMatrixGlobal.resize((2*lbfgsHistorySize+1) * (2*lbfgsHistorySize+1));
	gradientLocalEven.resize(localN);
	gradientLocalOdd.resize(localN);
	xGlobalEven.resize(globalN);
	xGlobalOdd.resize(globalN);
	pK.resize(globalN);
	bLocal.resize(m);
	// Send data to other nodes
	if (world.rank() == 0) { // Sends data to others
/*
		for (int r = 0; r < m; r++) {
			for (int c = 0; c < globalN; c++) {
				cout << problemData.A[r + c * m] << " ";
			}
			cout << endl;
		}
*/
		for (int i = 1; i < world.size(); i++) {
			int nnz = localNTmp;
			if (i == world.size() - 1) {
				nnz = optParams.n - (localNTmp * (world.size() - 1));
			}
			world.send(i, 0, &problemData.A[(i) * localNTmp * m], nnz * m);

		}
		// Also copy local data
		for (int i = 0; i < localN * optParams.m; i++) {
			Alocal[i] = problemData.A[i];
		}
		for (int i = 0; i < m; i++) {
			bLocal[i] = problemData.b[i];
			cout << bLocal[i] << " ";
		}
		cout << endl;
		if (iter%2==0){
			for (int i = 0; i < globalN; i++) {
				xGlobalEven[i] = problemData.x[i];
			}
		} else {
			for (int i = 0; i < globalN; i++) {
				xGlobalOdd[i] = problemData.x[i];
			}
		}

	} else {
		// Receive data from root
		world.recv(0, 0, &Alocal[0], localN * m);

	}
	if (iter%2==0){
		this->sendXFromRootToNodes(xGlobalEven);
	} else {
		this->sendXFromRootToNodes(xGlobalOdd);
	}

}

void LBFGSSolver::sendXFromRootToNodes(std::vector<double> xIn) {

	mpi::communicator world;
	if (world.rank() == 0) { // Sends data to others
		for (int i = 1; i < world.size(); i++) {
			int nnz = localN;
			if (i == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.send(i, 1, &xIn[(i) * localN], nnz);

		}
		// Also copy local data
		if (iter%2==0){

			for (int i = 0; i < localN; i++) {
				xLocalEven[i] = xIn[i];
			}
		} else {
			for (int i = 0; i < localN; i++) {
				xLocalOdd[i] = xIn[i];
			}
		}

	} else {
		// Receive data from root
		if (iter%2==0){
			world.recv(0, 1, &xLocalEven[0], localN);
		} else {
			world.recv(0, 1, &xLocalOdd[0], localN);

		}
	}

}

std::vector<double> LBFGSSolver::computeResidualForCurrentX() { // computes Ax-b
	mpi::communicator world;
	std::vector<double> g(m, 0);

	if (iter%2==0){

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < m; j++) {
				g[j] += xLocalEven[i] * Alocal[i * m + j];
			}
		}
	} else {

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < m; j++) {
				g[j] += xLocalOdd[i] * Alocal[i * m + j];
			}
		}
	}

	std::vector<double> gTmp(m, 0);
	mpi::reduce(world, &g[0], m, &gTmp[0], std::plus<double>(), 0);

	if (world.rank() == 0) {
		for (int i = 0; i < m; i++) {
			gTmp[i] -= bLocal[i];
		}
	}
	return gTmp;
}

double LBFGSSolver::computeCurrentObjectiveValue() {
	double objVal = 0;
	// objVal = 1/2 * \|Ax - b\|^2
	// first I multiply  g = ALocal*xLocal
	std::vector<double> residual = this->computeResidualForCurrentX();
	mpi::communicator world;

	if (world.rank() == 0) {

		for (unsigned int i = 0; i < residual.size(); i++) {
			objVal += residual[i] * residual[i];
		}
	}
	return objVal / 2;
}
std::vector<double> LBFGSSolver::computeGradientForCurrentX() { // computes A'*(Ax-b)

	std::vector<double> residual = this->computeResidualForCurrentX();
	mpi::communicator world;
	if (world.rank() == 0) {
		//cout<<"residual: "<< residual[0]<< "  iter: "<<iter<<" xLocalEven: "<<xLocalEven[0]<<" xLocalOdd: "<<xLocalOdd[0]<<endl;
	}

	broadcast(world, &residual[0], residual.size(), 0);  // now residuals is the same on each node.

	// gradient = A'*residual
	if(iter%2 == 0){
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalEven[col] = 0;
			}
		}

		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalEven[col] += residual[r] * Alocal[r + col * m];
			}
		}
	} else {
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalOdd[col] = 0;
			}
		}
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalOdd[col] += residual[r] * Alocal[r + col * m];
			}
		}
	}

	// gather to a root note to store full gradient
	std::vector<double> gradient;
	if (world.rank() == 0) {
		gradient.resize(globalN);
		for (int from = 1; from < world.size(); from++) {
			int nnz = localN;
			if (from == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.recv(from, 0, &gradient[localN * from], nnz);

		}
		if(iter%2 == 0){

			for (int i = 0; i < localN; i++) {
				gradient[i] = gradientLocalEven[i];
			}
		} else {
			for (int i = 0; i < localN; i++) {
				gradient[i] = gradientLocalOdd[i];
			}
		}

	} else {
		// send data to ROOT
		if(iter%2 == 0){
			world.send(0, 0, &gradientLocalEven[0], gradientLocalEven.size());
		} else {
			world.send(0, 0, &gradientLocalOdd[0], gradientLocalOdd.size());
		}
	}

	return gradient;

}

std::vector<double> LBFGSSolver::computeMatrixPre() { // calculates the b.b' (here c.c') and returns precondition matrix of alg. 3
	mpi::communicator world;
	// First time to enter this function, iter=0
	if (iter>0){
		int mod1 = (iter-1) % lbfgsHistorySize;

		int mod2 = (iter-1) % lbfgsHistorySize + lbfgsHistorySize;

		std::vector<double> preMatUpdateLocal(2*lbfgsHistorySize+1, 0);

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < 2*lbfgsHistorySize; j++) {
				preMatUpdateLocal[j] += cLocal[localN*mod1+ i] * cLocal[j * localN + i];
			}
		}

		//std::vector<double> gTmp(2*lbfgsHistorySize, 0); ////// why?
		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize,
				&preMatrixGlobal[mod1*(2*lbfgsHistorySize+1)], std::plus<double>(), 0);

		// mod1*(2*lbfgsHistorySize) is the first element in the column 3
		std::fill(preMatUpdateLocal.begin(), preMatUpdateLocal.end(), 0);     //
		if (world.rank() == 0) {    //
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[mod1 + i * (2*lbfgsHistorySize+1) ]	=
						preMatrixGlobal[mod1*(2*lbfgsHistorySize+1)+i];
			}
		}

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < 2*lbfgsHistorySize; j++) {
				preMatUpdateLocal[j] += cLocal[localN*mod2+ i] * cLocal[j * localN + i];
			}
		}

		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize, &preMatrixGlobal[mod2*(2*lbfgsHistorySize+1)], std::plus<double>(), 0);
		std::fill(preMatUpdateLocal.begin(), preMatUpdateLocal.end(), 0);     //
		// mod2(?)*(2*lbfgsHistorySize) is the first element in the column 3

		if (world.rank() == 0) {    ////////////////////////////////
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[mod2 + i * (2*lbfgsHistorySize +1)]	= 	preMatrixGlobal[mod2*(2*lbfgsHistorySize+1)+i];
			}
		}

		if (iter%2 == 0){
//			cout<<"grad @ even= [";
			for (int i = 0; i < localN; i++) {
				for (int j = 0; j < 2*lbfgsHistorySize; j++) {
					preMatUpdateLocal[j] += gradientLocalEven[i] * cLocal[j * localN + i];
				}
				preMatUpdateLocal[2*lbfgsHistorySize] += gradientLocalEven[i]*gradientLocalEven[i];
//				cout<<gradientLocalOdd[i]<<"   "<<gradientLocalEven[i];

			}
//			cout<<endl;
//			cout<<"gradNorm @ even= "<<preMatUpdateLocal[2*lbfgsHistorySize]<<endl;

		}
		else {
//			cout<<"grad @ odd= [";
			for (int i = 0; i < localN; i++) {
				for (int j = 0; j < 2*lbfgsHistorySize; j++) {
					preMatUpdateLocal[j] += gradientLocalOdd[i] * cLocal[j * localN + i];
				}
				preMatUpdateLocal[2*lbfgsHistorySize] += gradientLocalOdd[i]*gradientLocalOdd[i];

			}
		}

		//std::vector<double> gTmp(2*lbfgsHistorySize, 0);  ////// why?
		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize+1,
				&preMatrixGlobal[(2*lbfgsHistorySize+1)*(2*lbfgsHistorySize)], std::plus<double>(), 0);
		// mod1*(2*lbfgsHistorySize) is the first element in the column 3

		if (world.rank() == 0) {    ////////////////////////////////
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[2*lbfgsHistorySize + i * (2*lbfgsHistorySize+1) ]	=
						preMatrixGlobal[(2*lbfgsHistorySize+1)*(2*lbfgsHistorySize)+i];
			}
		}
	}
	return preMatrixGlobal;

}

void LBFGSSolver::findPk() { // finds p_k
	std::vector<double> preMatrix = this->computeMatrixPre();
	mpi::communicator world;

	std::vector<double> delta(2*lbfgsHistorySize+1, 0);

	if (world.rank() == 0) {

		std::vector<double> alpha(lbfgsHistorySize, 0);
		delta[2*lbfgsHistorySize] = -1;
		int last_i = max(iter - lbfgsHistorySize, 0);
		// first loop

//		cout<<"first loop: prematrix access"<<endl;
		for (int i = iter - 1; i >= last_i; i--) {
//			int j = i - (iter-lbfgsHistorySize) + 1;
			int j = i%lbfgsHistorySize;

			for (int l = 0; l<2*lbfgsHistorySize+1;l++){
				alpha[j] += preMatrix[(2*lbfgsHistorySize+1)*j+l]*delta[l];
//				cout<<"[j,l] = ["<<j<<","<<l<<"]   ";

			}
			alpha[j] = alpha[j]/preMatrix[j*(2*lbfgsHistorySize+1)+(lbfgsHistorySize+j)];
//			cout<<"\ns[j,m+j] = ["<<j<<","<<lbfgsHistorySize+j<<"]   ";
			delta[lbfgsHistorySize+j] -=  alpha[j];

//			cout<<endl<<"next mem";

		}

		double scalarMultiplier = preMatrix[((iter - 1)%lbfgsHistorySize)*(2*lbfgsHistorySize+1)+
		                                    (iter - 1)%lbfgsHistorySize+lbfgsHistorySize]/preMatrix[((iter - 1)%lbfgsHistorySize+lbfgsHistorySize)*(2*lbfgsHistorySize+1)+((iter - 1)%lbfgsHistorySize+lbfgsHistorySize)];

		for (int i = 0; i<2*lbfgsHistorySize+1; i++) {
			delta[i] *= scalarMultiplier;
		}

		// second loop
		double bbeta = 0;
//		cout<<"second loop: prematrix access"<<endl;
		int first_i = max(0, iter - lbfgsHistorySize);
		for (int i = first_i; i < iter; i++) {
			bbeta = 0;
//			int j = i - (iter-lbfgsHistorySize) + 1;
			int j = i%lbfgsHistorySize;

			for (int l = 0; l<2*lbfgsHistorySize+1;l++){
				bbeta += preMatrix[(2*lbfgsHistorySize+1)*(lbfgsHistorySize+j)+l]*delta[l];
			}
			bbeta =  bbeta /preMatrix[j*(2*lbfgsHistorySize+1)+(lbfgsHistorySize+j)];
			delta[j] +=  alpha[j]-bbeta;

		}

	}
	broadcast(world, &delta[0], delta.size(), 0);  // now delta is the same on all nodes.
	std::vector<double> pLocal (localN);
	for (int i = 0; i < localN ; i++){
		for (int j = 0; j< 2*lbfgsHistorySize ; j++){
			pLocal[i] += delta[j] * cLocal[j*localN+i];
		}
	}

	if (iter%2 == 0){
		for (int i = 0; i < localN ; i++){
			pLocal[i] += delta[2*lbfgsHistorySize] * gradientLocalEven[i];
		}
	} else {
		for (int i = 0; i < localN ; i++){
			pLocal[i] += delta[2*lbfgsHistorySize] * gradientLocalOdd[i];
		}
	}

//	std::vector<double> pK ;

	if (world.rank() == 0) {
//		pK.resize(globalN);
		for (int from = 1; from < world.size(); from++) {
			int nnz = localN;
			if (from == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.recv(from, 0, &pK[localN * from], nnz);

		}

		for (int i = 0; i < localN; i++) {
			pK[i] = pLocal[i];
		}

	} else {
		// send data to ROOT
		world.send(0, 0, &pLocal[0], localN);
		if (world.rank()==1){
//			cout<<endl<<endl<<endl<<"pLocal on node "<<world.rank()<<" is:   "<<pLocal[0]<<endl;
		}

	}

}

void LBFGSSolver::updateYk() { // finds y_k after each iteration
	mpi::communicator world;
	int modd = iter % lbfgsHistorySize;
	if (iter%2 == 0){
		for(int i =0 ; i< localN; i++){
			cLocal[(lbfgsHistorySize+modd)*localN + i] = gradientLocalOdd[i] - gradientLocalEven[i];
		}
	} else {
		for(int i =0 ; i< localN; i++){
			cLocal[(lbfgsHistorySize+modd)*localN + i] = gradientLocalEven[i] - gradientLocalOdd[i];
		}
	}
}

void LBFGSSolver::updateSk() { // finds s_k after each iteration
	mpi::communicator world;
	// First time to enter this function, iter=0
	int modd = iter % lbfgsHistorySize;
	if (iter%2 == 0){
		for(int i =0 ; i< localN; i++){
			cLocal[modd*localN + i] = xLocalOdd[i] - xLocalEven[i];	// bb local is stack of y_k, s_k and grad_k
		}
	} else {
		for(int i =0 ; i< localN; i++){
			cLocal[modd*localN + i] = xLocalEven[i] - xLocalOdd[i];
		}
	}
}

void LBFGSSolver::computeAndTakeStep() {
	mpi::communicator world;

	if(iter == 0){
		gradientEven = this->computeGradientForCurrentX();
	}
	/*        two loop recursion	 */
	if (iter>=0){
		this->findPk();
	}

	/*      Take the step and update s_k and y_k   */

	if(iter%2 == 0){

		if (world.rank() == 0) {
			for (int i = 0; i < globalN; i++) {
				if (iter==0){
					xGlobalOdd[i] = xGlobalEven[i] - stepSize * gradientEven[i];
				}else{
					xGlobalOdd[i] = xGlobalEven[i] + stepSize * pK[i];
				}
			}
			//cout<<endl<<"gradientEven: "<< gradientEven[0]<< "  iter: "<<iter<<" xGlobal even cur: "<<xGlobalEven[0]<<" xGlobal odd next: "<<xGlobalOdd[0]<<endl;
		}

		iter +=1;
		this->sendXFromRootToNodes(xGlobalOdd);

		iter -=1;
		this->updateSk();
		iter +=1;

		gradientOdd = this->computeGradientForCurrentX();

		iter -=1;
		this->updateYk();
		iter +=1;

	} else {
		if (world.rank() == 0) {

			for (int i = 0; i < globalN; i++) {
				if (iter==0){
					xGlobalEven[i] = xGlobalOdd[i]- stepSize * gradientOdd[i];
				}else{
					xGlobalEven[i] = xGlobalOdd[i]+ stepSize * pK[i];
				}
			}

		}

		iter +=1;
		this->sendXFromRootToNodes(xGlobalEven);

		iter -=1;
		this->updateSk();
		iter +=1;
		gradientEven = this->computeGradientForCurrentX();
		iter -=1;
		this->updateYk();
		iter +=1;

	}

}


// The MIT License (MIT)
//
// Copyright (c) 2015 Markus Herb
//
// 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.
#ifndef KALMAN_MATRIX_HPP_
#define KALMAN_MATRIX_HPP_

#include <cmath>

#include <Eigen/Dense>

#define KALMAN_VECTOR(NAME, T, N)                                                       \
    typedef Kalman::Vector<T, N> Base;                                                  \
    using typename Base::Scalar;                                                        \
    using Base::RowsAtCompileTime;                                                      \
    using Base::ColsAtCompileTime;                                                      \
    using Base::SizeAtCompileTime;                                                      \
                                                                                \
    NAME(void) : Kalman::Vector<T, N>() {}                                              \
                                                                                \
    template<typename OtherDerived>                                                     \
    NAME(const Eigen::MatrixBase<OtherDerived>& other) : Kalman::Vector<T, N>(other) {} \
                                                                                \
    template<typename OtherDerived>                                                     \
    NAME& operator= (const Eigen::MatrixBase <OtherDerived>& other)                     \
    {                                                                                \
        this->Base::operator=(other);                                                   \
        return *this;                                                                   \
    }

namespace Kalman {
    const int Dynamic = Eigen::Dynamic;

    /**
     * @class Kalman::Matrix
     * @brief Template type for matrices
     * @param T The numeric scalar type
     * @param rows The number of rows
     * @param cols The number of columns
     */
    template<typename T, int rows, int cols>
    using Matrix = Eigen::Matrix<T, rows, cols>;

    /**
     * @brief Template type for vectors
     * @param T The numeric scalar type
     * @param N The vector dimension
     */
    template<typename T, int N>
    class Vector : public Matrix<T, N, 1>
    {
    public:
        //! Matrix base type
        typedef Matrix<T, N, 1> Base;

        using typename Base::Scalar;
        using Base::RowsAtCompileTime;
        using Base::ColsAtCompileTime;
        using Base::SizeAtCompileTime;

        Vector(void) : Matrix<T, N, 1>() {}

        /**
         * @brief Copy constructor
         */
        template<typename OtherDerived>
        Vector(const Eigen::MatrixBase<OtherDerived>& other)
            : Matrix<T, N, 1>(other)
        { }
        /**
         * @brief Copy assignment constructor
         */
        template<typename OtherDerived>
        Vector& operator= (const Eigen::MatrixBase <OtherDerived>& other)
        {
            this->Base::operator=(other);
            return *this;
        }
    };

    /**
     * @brief Cholesky square root decomposition of a symmetric positive-definite matrix
     * @param _MatrixType The matrix type
     * @param _UpLo Square root form (Eigen::Lower or Eigen::Upper)
     */
    template<typename _MatrixType, int _UpLo = Eigen::Lower>
    class Cholesky : public Eigen::LLT< _MatrixType, _UpLo >
    {
    public:
        Cholesky() : Eigen::LLT< _MatrixType, _UpLo >() {}

        /**
         * @brief Construct cholesky square root decomposition from matrix
         * @param m The matrix to be decomposed
         */
        Cholesky(const _MatrixType& m ) : Eigen::LLT< _MatrixType, _UpLo >(m) {}

        /**
         * @brief Set decomposition to identity
         */
        Cholesky& setIdentity()
        {
            this->m_matrix.setIdentity();
            this->m_isInitialized = true;
            return *this;
        }

        /**
         * @brief Check whether the decomposed matrix is the identity matrix
         */
        bool isIdentity() const
        {
            eigen_assert(this->m_isInitialized && "LLT is not initialized.");
            return this->m_matrix.isIdentity();
        }

        /**
         * @brief Set lower triangular part of the decomposition
         * @param matrix The lower part stored in a full matrix
         */
        template<typename Derived>
        Cholesky& setL(const Eigen::MatrixBase <Derived>& matrix)
        {
            this->m_matrix = matrix.template triangularView<Eigen::Lower>();
            this->m_isInitialized = true;
            return *this;
        }

        /**
         * @brief Set upper triangular part of the decomposition
         * @param matrix The upper part stored in a full matrix
         */
        template<typename Derived>
        Cholesky& setU(const Eigen::MatrixBase <Derived>& matrix)
        {
            this->m_matrix = matrix.template triangularView<Eigen::Upper>().adjoint();
            this->m_isInitialized = true;
            return *this;
        }
    };
}

#endif