#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core> // after MKL
#include <Eigen/Dense>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>
#include <set>
#include <chrono>
#include <thread>

#include <mpi.h>

#include "tasktorrent/tasktorrent.hpp"
#include "util_shared.hpp"

// #define DISP
#define COMPUTE_Q

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;
typedef array<int, 4> int4;
typedef array<int, 5> int5;

int VERB = 0;
bool LOG = false;
int n_threads_ = 2;
int n_ = 2;
int M_ = 5;
int N_ = 4;
int p_ = 1;
int q_ = 1;
int t_ = M_/p_;

struct denseQR {

private:
    /** Usual stuff */
    const int rank;
    const int nranks;
    const int n_threads;
    Communicator* comm;
    Threadpool* tp;

    /** Task flows needed **/
    // Setup the workspace
    Taskflow<int> initialize_tf;
    Taskflow<int> ready_tf; // All helper ranks are ready

    Taskflow<int> scatter_tf; // from origin to helper ranks

    // For GEMM
    Taskflow<int3> gemm_Cij; // += A_ik * B_kj

    // For QR
    Taskflow<int3> dgeqrt_tf;  // A[k,k] = QR
    Taskflow<int4> dtsqrt_tf;
    Taskflow<int4> dlarfb_tf;
    Taskflow<int5> dssrfb_tf;
    Taskflow<int3> dttqrt_tf;
    Taskflow<int4> dttssmqr_tf;

    Taskflow<int4> gather_tf;
    Taskflow<int> computeQ_tf; // from helper ranks to origin

    /** Taskflows obtained from parent function **/
    Taskflow<int>* notify_tf;

    /** Active messages between tasks **/
    ActiveMsg<int, int, int, int, int, int, int, int, int>* am_initialize;
    ActiveMsg<int>* am_start_scatter;

    ActiveMsg<view<double>, int, int>* send_Ai;

    // From dgeqrt
    ActiveMsg<view<double>,view<double>,view<int>,int, int, int>* am_dgeqrt_2_dlarfb;
    ActiveMsg<view<double>,int, int, int>* am_dgeqrt_2_dtsqrt;
    ActiveMsg<view<double>,int, int, int>* am_dgeqrt_2_dttqrt;

    // From dtsqrt
    ActiveMsg<view<double>,int, int, int, int>* am_dtsqrt_2_dtsqrt;
    ActiveMsg<view<double>,int, int, int>* am_dtsqrt_2_dttqrt;
    ActiveMsg<view<double>,view<double>,int, int, int, view<int>,int>* am_dtsqrt_2_dssrfb;
    // From dlarfb
    ActiveMsg<view<double>,int, int, int, int, int>* am_dlarfb_2_dssrfb;
    ActiveMsg<view<double>,int, int, int, int>* am_dlarfb_2_dttssmqr;

    // From dssrfb
    ActiveMsg<view<double>,int, int, int, int, int>* am_dssrfb_2_dssrfb;
    ActiveMsg<view<double>,int, int, int, int>* am_dssrfb_2_dttssmqr;
    // From dttqrt
    ActiveMsg<view<double>,int, int, int>* am_dttqrt_2_dttqrt;
    ActiveMsg<view<double>, view<double>, int, int, view<int>, int>* am_dttqrt_2_dttssmqr;
    // From dttssmqr
    ActiveMsg<view<double>, int, int, int>* am_dttssmqr_2_dgeqrt;
    ActiveMsg<view<double>, int, int, int, int>* am_dttssmqr_2_dlarfb;
    ActiveMsg<int, int, int, int>* am_dttssmqr_2_dtsqrt;
    ActiveMsg<int, int, int, int, int>* am_dttssmqr_2_dssrfb;
    ActiveMsg<view<double>, int, int, int, int>* am_dttssmqr_2_dttssmqr;

    ActiveMsg<view<double>,int,int, int>* am_gather_0;
    ActiveMsg<view<double>,view<double>,int,int, int>* am_gather_1;
    ActiveMsg<view<double>,view<double>,int,int, int>* am_gather_2;

    /** Matrix A (input and output) **/
    map<int,MatrixXd*> A; // Original matrix A -- on output has A(:, 1: R_.at(index)) = V -- householder vectors
    map<int,MatrixXd*> Tmat; // T from the QR of A*G
    map<int,MatrixXd*> Q; // Q (full) from the QR of A*Q

    /** Workspace **/ //workspace[origin] = map<int2, MatrixXd>
    map<int,map<int, MatrixXd>> A_i; // Store the A split across the nodes

    map<int,map<int2, MatrixXd>> Mat; // Mat[ij] = sum(A[i*]* G[*j])
    map<int,map<int2, MatrixXd>> T; // T[ij] = parts of Tmat
    map<int,map<int2, MatrixXd>> V2; // V2[ij] = Householder vectors -- for parallel panel fact
    map<int,map<int2, MatrixXd>> T2; // T2[ij] -- for parallel panel fact

    map<int,int> p_; // No. of ranks in x dim
    map<int,int> q_; // No. of ranks in y dim
    map<int,int> t_; // Length of a tile = p_/2
    map<int,int> n_; // block size
    map<int,int> M_; // no. of blocks in y dim in A -- row
    map<int,int> R_; // no. of blocks in x dim in A -- col
    map<int,int> N_; // no. of blocks in x dim in output (Q) -- max_rank
    // map<int,int> notify_index_; // Which notify_tf parent task to notify about completion
    map<int,int> origin_; // Which rank to gather output on
    map<int,int> num_dep_ranks_; // No. of ranks tht the computation uses including origin

    /* Mutex for the maps */
    mutex workspace_mutex;

    int block2rank(int2 ij, int3 pqt, int origin){
        int i = ij[0];
        int j = ij[1];
        int p = pqt[0];
        int q = pqt[1];
        int t = pqt[2];
        int ii = (i/t) % p;
        int jj = j % q;
        int r = (ii + jj * p + origin) % nranks; // want the (0,0) block to be in origin rank
        assert(r <= nranks);
        return r;
    };

    void initialize(int& p, int& q, int& block_size, int& M, int& R, int& N, int& k, int& origin, int& t){

        lock_guard<mutex> lock(workspace_mutex);
        p_[k] = p;
        q_[k] = q;
        n_[k] = block_size;
        M_[k] = M;
        R_[k] = R;
        N_[k] = N;
        origin_[k] = origin;
        t_[k] = t; // Tile size

        // Initialize Mat and T as well
        for (int i=0; i < M; ++i){
            for (int j=0; j < N; ++j){
                Mat[k][{i,j}] = MatrixXd::Zero(0,0);
                if (i>=j) {
                    V2[k][{i,j}] = MatrixXd::Zero(0,0);
                    T2[k][{i,j}] = MatrixXd::Zero(0,0);
                    T[k][{i,j}] = MatrixXd::Zero(0,0);
                }
            }
        }

        // Initialize A_i
        for (int i=0; i < M; ++i){
            A_i[k][i] = MatrixXd::Zero(0,0);
        }
    }

    void initialize(int& p, int& q, int& block_size, int& M, int& R, int& N,
                    int& k, int& origin, MatrixXd* A_, MatrixXd* Tmat_, MatrixXd* Q_, int t){
        unique_lock<mutex> lock(workspace_mutex);
        A[k] = A_; // A to be performed QR on  -- only origin will have a copy of this
        Tmat[k] = Tmat_;  //-- only origin will have a copy of this
        Q[k] = Q_; // fat Q -- only origin will have a copy of this

        p_[k] = p;
        q_[k] = q;
        n_[k] = block_size;
        M_[k] = M;
        R_[k] = R;
        N_[k] = N;
        origin_[k] = origin;
        t_[k] = t;

        // Initialize Mat and T as well
        for (int i=0; i < M; ++i){
            for (int j=0; j < N; ++j){
                Mat[k][{i,j}] = MatrixXd::Zero(0,0);
                if (i>=j) {
                    V2[k][{i,j}] = MatrixXd::Zero(0,0);
                    T2[k][{i,j}] = MatrixXd::Zero(0,0);
                    T[k][{i,j}] = MatrixXd::Zero(0,0);
                }
            }
        }

        // Initialize A_i
        for (int i=0; i < M; ++i){
            A_i[k][i] = MatrixXd::Zero(0,0);
        }

        set<int> ranks_for_index;

        for (int i = 0; i < M; ++i){
            for (int j=0; j < N; ++j){
                int r = block2rank({i,j},{p,q,t},origin);
                ranks_for_index.insert(r);
            }
        }
        num_dep_ranks_[k] = ranks_for_index.size();

        lock.unlock();

        // Fullfill one dependency on ready_tf
        ready_tf.fulfill_promise(k);
    }

public:

    /* Constructor */
    denseQR(Communicator* comm_, Threadpool* tp_, Taskflow<int>* notify_tf_): rank(comm_->comm_rank()),
    nranks(comm_->comm_size()), n_threads(tp_->size()), comm(comm_), tp(tp_), notify_tf(notify_tf_), initialize_tf(tp_),
    ready_tf(tp_), scatter_tf(tp_), gemm_Cij(tp_), dgeqrt_tf(tp_), dtsqrt_tf(tp_), dlarfb_tf(tp_),
    dssrfb_tf(tp_), dttqrt_tf(tp_), dttssmqr_tf(tp_), gather_tf(tp_), computeQ_tf(tp_) {

        am_initialize = comm->make_active_msg ([&] (int& p, int& q, int& n, int& M, int& R,
                                                    int& N, int& index, int& origin, int& t) {
            // on the receiving node
            initialize(p, q, n, M, R, N, index, origin, t);

            // Fulfill promise on ready_tf
            ready_tf.fulfill_promise(index);
        });

        // will only happen at origin for index
        initialize_tf.set_mapping([&] (int index){ // task_index
            return (index % n_threads);
        })
        .set_indegree([](int){
            return 1;
        })
        .set_task([&] (int index) {
            // origin already initialized
            int M = M_.at(index);
            int R = R_.at(index);
            int N = N_.at(index);
            int n = n_.at(index);
            int p = p_.at(index);
            int q = q_.at(index);
            int t = t_.at(index);
            int origin = origin_.at(index);

            assert(rank == origin);

            set<int> ranks_for_index;

            for (int i = 0; i < M; ++i){ // We are sending an entire set of block rows i
                for (int j=0; j < N; ++j){
                    int r = block2rank({i,j},{p,q,t},origin);
                    ranks_for_index.insert(r);
                }
            }

            for (auto r: ranks_for_index){
                if (r != origin){
                    am_initialize->send(r, p, q, n, M, R, N, index, origin, t);
                }
            }
        })
        .set_name([](int index) {
            return "initialize_" + to_string(index);
        })
        .set_priority([&](int) {
            return 6;
        });

        // Message from helper ranks saying they are ready to begin computation
        am_start_scatter = comm->make_active_msg([&] (int& index) {
            // Will be received in the origin
            ready_tf.fulfill_promise(index);
        });

        ready_tf.set_mapping([&] (int index){ // task_index
            return (index % n_threads);
        })
        .set_indegree([&](int index){
            int origin = origin_.at(index);
            if (rank == origin) {
                return num_dep_ranks_.at(index);
            }
            else return 1;
        })
        .set_task([&] (int index) {
            int origin = origin_.at(index);
            if (rank == origin){
                scatter_tf.fulfill_promise(index);
            }
            else{
                am_start_scatter->send(origin, index);
            }
        })
        .set_name([](int index) {
            return "ready_" + to_string(index);
        })
        .set_priority([&](int) {
            return 6;
        });

        send_Ai = comm->make_active_msg([&] (view<double>& Ai_sub_view, int& row_idx, int& index) {

            // In the receiving local rank
            int origin = origin_.at(index);
            int n = n_.at(index);
            int N = N_.at(index);
            int R = R_.at(index);
            int p = p_.at(index);
            int q = q_.at(index);
            int t = t_.at(index);

            MatrixXd Ai_sub = Map<MatrixXd>(Ai_sub_view.data(), n, R*n);
            A_i.at(index).at(row_idx) = Ai_sub;

            for (int j=0; j < N; ++j){
                if (block2rank({row_idx,j},{p,q,t},origin) == rank){
                    gemm_Cij.fulfill_promise({index, row_idx,j});
                }
            }
        });

        // From origin to helper ranks
        scatter_tf.set_mapping([&] (int index){ // task_index
            return (index % n_threads);
        })
        .set_indegree([](int){
            return 1;
        })
        .set_task([&] (int index) {
            int origin = origin_.at(index);
            assert(rank == origin);
            int M = M_.at(index);
            int R = R_.at(index);
            int N = N_.at(index);
            int n = n_.at(index);
            int p = p_.at(index);
            int q = q_.at(index);
            int t = t_.at(index);

            MatrixXd A_index = *(A.at(index)); // local copy of the matrix

            vector<set<int>> row2rank(nranks);

            for (int i = 0; i < M; ++i){ // We are sending an entire set of block rows i
                for (int j=0; j < N; ++j){
                    int r = block2rank({i,j},{p,q,t},origin);
                    row2rank[r].insert(i);
                }
            }

            for (int r=0; r < nranks; ++r){
                if (r != origin){
                    for (auto row_idx: row2rank[r]){
                        MatrixXd A_sub =  A_index.block(row_idx*n, 0, n, R*n);
                        auto A_sub_view = view<double>(A_sub.data(), n*R*n);
                        send_Ai->send(r, A_sub_view, row_idx, index);
                    }
                }
                else if (r == origin) {
                    for (auto row_idx: row2rank[r]){
                        A_i.at(index)[row_idx] = A_index.block(row_idx*n, 0, n, R*n);
                    }
                }
            }

            for (int j=0; j < N; ++j){
                for (auto row_idx: row2rank[rank]){
                    if (block2rank({row_idx,j},{p,q,t},origin) == rank){
                        gemm_Cij.fulfill_promise({index, row_idx,j});
                    }
                }
            }
        })
        .set_name([](int index) {
            return "scatter_" + to_string(index);
        })
        .set_priority([&](int) {
            return 6;
        });

        // GEMM
        gemm_Cij.set_task([&](int3 oij){
            int index = oij[0];
            int i = oij[1];
            int j = oij[2];

            int n = n_.at(index);
            int N = N_.at(index);
            int R = R_.at(index);
            Mat.at(index)[{i,j}] = A_i.at(index)[i].middleCols(j*n, n);
            if (i>=j) {T.at(index)[{i,j}] = MatrixXd::Zero(n,n);}

            // std::default_random_engine default_gen = default_random_engine(2020+20*j); // only based on j
            // auto gen_gaussian = [&](int i, int j){ return get_gaussian(i, j, &default_gen); };

            // // Generate a Gaussian random matrix
            // MatrixXd G = gen_gaussian(R*n, n); // local

            // cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
            //              n, n, R*n,
            //              1.0, A_i.at(index)[i].data(), n,
            //              G.data(), R*n,
            //              0.0, Mat.at(index)[{i,j}].data(), n);

        }).set_indegree([&](int3 oij) {
            return 1;
        }).set_fulfill([&](int3 oij) {
            int index = oij[0];
            int i = oij[1];
            int j = oij[2];
            int t = t_.at(index);

            if (j==0){ // First column
                if (i%t == 0){ // First row in every tile
                    // FIrst dgeqrt -- same node
                    dgeqrt_tf.fulfill_promise({index, i, j});
                }
                else {// Other tiles in first column
                    int root = (i/t)*t;
                    dtsqrt_tf.fulfill_promise({index, root, i, 0}); // j = 0
                }
            }
            else { // j >0
                if (i%t == 0){ // first row in every tile
                    dlarfb_tf.fulfill_promise({index,i, 0, j});
                }
                else {// other rows
                    int root = (i/t)*t;
                    dssrfb_tf.fulfill_promise({index, root, 0, i, j});
                }
            }
        })
        .set_mapping([&](int3 oij) {
            int index = oij[0];
            int p = p_.at(index);
            int q = q_.at(index);
            int M = M_.at(index);
            return ((index + oij[1] + oij[2] * M) % n_threads);
        }).set_name([&](int3 oij) {
            return "prod_C_" + std::to_string(oij[0]) + "_" + std::to_string(oij[1]) + "_" + std::to_string(oij[2]);
        }).set_priority([&](int3){
            return 6;
        });

        // From dgeqrt
        am_dgeqrt_2_dlarfb = comm->make_active_msg(
            [&](view<double> &V, view<double> &tau, view<int>& js, int& i, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({i,j}) = Map<MatrixXd>(V.data(), n, n);
                T.at(index).at({i,j})   = Map<MatrixXd>(tau.data(), n, n);
                for (auto& jinc: js){
                    dlarfb_tf.fulfill_promise({index,i,j,jinc});
                }
            }
        );

        am_dgeqrt_2_dtsqrt = comm->make_active_msg(
            [&](view<double> &R, int& i, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({i,j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dtsqrt_tf.fulfill_promise({index, i, i+1, j});
            }
        );

        am_dgeqrt_2_dttqrt = comm->make_active_msg(
            [&](view<double> &R, int& j, int& dttqrt_row, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dttqrt_tf.fulfill_promise({index, j, dttqrt_row});
            }
        );

        // Define taskflows
        dgeqrt_tf.set_mapping([&] (int3 oij){
                int index = oij[0];
                int i = oij[1];
                int j = oij[2];
                return ( (index + i) % n_threads);
            })
            .set_indegree([](int3){
                return 1;
            })
            .set_task([&] (int3 oij) {
                int index = oij[0];
                int i = oij[1];
                int j = oij[2];
                int n = n_.at(index);
                int info = LAPACKE_dgeqrt(LAPACK_COL_MAJOR, n, n, n,
                                          Mat.at(index).at({i,j}).data(), n,
                                          T.at(index).at({i,j}).data(), n);
                assert(info == 0);
                #ifdef DISP
                cout << "geqrt_" << to_string(oij[0]) << "_"+ to_string(oij[1]) <<  "_" + to_string(oij[2]) << endl;
                #endif
            })
            .set_fulfill([&](int3 oij) {
                // Dependencies -- dlarfb
                int index = oij[0];
                int i = oij[1];
                int j = oij[2];
                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);

                int origin = origin_.at(index);

                map<int, vector<int>> to_fulfill;

                for(int jinc = j+1; jinc<N; jinc++) { // A[i][jinc] blocks
                    int r = block2rank({i,jinc},{p,q,t}, origin);
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {jinc};
                    } else {
                        to_fulfill[r].push_back(jinc);
                    }
                }

                int tile_index = i/t;

                if (i+1<M && (i+1)/t == tile_index){ // and belongs to the same tile
                    // First dtsqrt in that tile
                    int r = block2rank({i+1,j},{p,q,t},origin);
                    if (r == rank){
                       dtsqrt_tf.fulfill_promise({index, i, i+1, j}); // root row = i
                    }
                    else {
                       auto R_ij = view<double>(Mat.at(index).at({i,j}).data(), n * n );
                       am_dgeqrt_2_dtsqrt->send(r, R_ij, i, j, index);
                    }
                }
                else if (i+1<M && i==j) {// Last row in tile
                    int last_row_in_next_tile = i+t; // i is the last row in current tile
                    if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly
                    int r = block2rank({last_row_in_next_tile,j}, {p,q,t},origin);
                    if (r == rank){
                        dttqrt_tf.fulfill_promise({index, j, i+1}); // first merge -- satisfy dep. on first row of next tile
                    }
                    else {
                        auto R_ij= view<double>(Mat.at(index).at({i,j}).data(), n * n );
                        int dttqrt_row = i+1;
                        am_dgeqrt_2_dttqrt->send(r, R_ij, j, dttqrt_row, index) ; // send A{i,j}
                    }
                }
                else if (i!=j) { // can happen when tile size = 1
                    dttqrt_tf.fulfill_promise({index, j, i});
                }

                // gather on node origin
                #ifdef COMPUTE_Q
                gather_tf.fulfill_promise({index, i, j, 1}); // 1 => send V_ij, T_ij
                #endif

                // Send data and trigger tasks -- dlarfb
                for (auto& p: to_fulfill){
                    int r = p.first; // rank
                    if (r == rank){
                        for(auto& jinc: p.second){
                            dlarfb_tf.fulfill_promise({index, i, j, jinc});
                        }
                    }
                    else {
                        auto V_ij = view<double>(Mat.at(index).at({i,j}).data(), n * n );
                        auto T_ij = view<double>(T.at(index).at({i,j}).data(), n*n);
                        auto jsv = view<int>(p.second.data(), p.second.size());
                        am_dgeqrt_2_dlarfb->send(r, V_ij, T_ij, jsv, i, j, index);
                    }
                }

            })
            .set_name([](int3 oij) {
                return "geqrt_" + to_string(oij[0]) + "_"+ to_string(oij[1]) +  "_" + to_string(oij[2]);
            })
            .set_priority([&](int3) {
                return 5;
            });

        // From dsqrt
        am_dtsqrt_2_dtsqrt = comm->make_active_msg(
            [&](view<double> &R, int& root, int& i, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({root, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dtsqrt_tf.fulfill_promise({index,root, i,j});
            }
        );

        am_dtsqrt_2_dssrfb = comm->make_active_msg(
            [&](view<double> &V_ij, view<double> &T_ij, int& root, int& j, int& i, view<int>& js,  int& index){
                int n = n_.at(index);
                Mat.at(index).at({i,j}) = Map<MatrixXd>(V_ij.data(), n, n);
                T.at(index).at({i,j}) = Map<MatrixXd>(T_ij.data(), n, n);

                for(auto& jinc: js){
                    dssrfb_tf.fulfill_promise({index,root, j, i, jinc});
                }
            }
        );

        am_dtsqrt_2_dttqrt = comm->make_active_msg(
            [&](view<double> &R, int& j, int& dttqrt_row, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dttqrt_tf.fulfill_promise({index, j, dttqrt_row});
            }
        );

        dtsqrt_tf.set_mapping([&] (int4 orij){
                int index = orij[0];
                int N = N_.at(index);
                return ((index + orij[1] + orij[2] + orij[3] * N) % n_threads);
            })
            .set_indegree([](int4 orij){
                return 2;
            })
            .set_task([&] (int4 orij) {
                int index = orij[0];
                int root = orij[1]; // root row -- which row is doing the dgeqrt
                int i = orij[2];
                int j = orij[3];
                int n = n_.at(index);
                int t = t_.at(index);

                int info = LAPACKE_dtpqrt(LAPACK_COL_MAJOR, n, n, 0, n, Mat.at(index).at({root,j}).data(), n,
                                          Mat.at(index).at({i,j}).data(), n, T.at(index).at({i,j}).data(), n);

                assert(info == 0);
                #ifdef DISP
                cout << "tsqrt_" << to_string(orij[0]) << "_" +to_string(orij[1]) <<
                         "_"  << to_string(orij[2]) << "_"  <<to_string(orij[3]) << endl;
                #endif

            })
            .set_fulfill([&](int4 orij) {
                int index = orij[0];
                int root = orij[1]; // root row -- which row is doing the dgeqrt
                int i = orij[2];
                int j = orij[3];

                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);
                int origin = origin_.at(index);

                int tile_index = i/t;
                // Dependencies
                map<int, vector<int>> to_fulfill;

                // Next dtsqrt
                // if (i+1 < M){
                if (i+1<M && (i+1)/t == tile_index){ // and belongs to the same tile
                    int r = block2rank({i+1,j},{p,q,t},origin);
                    if (r == rank){
                        dtsqrt_tf.fulfill_promise({index, root, i+1, j});
                    }
                    else {
                        auto R_rj= view<double>(Mat.at(index).at({root,j}).data(), n * n );
                        int inext = i+1;
                        am_dtsqrt_2_dtsqrt->send(r, R_rj, root, inext, j, index) ; // send A{root,j}
                    }
                }
                else if (i+1<M && root == j){ // Tile contains updated R_{jj}
                    assert((i+1)/t > tile_index); // So this is the last dtsqrt in that tile
                    int last_row_in_next_tile = i+t; // i is the last row in current tile
                    if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly
                    int r = block2rank({last_row_in_next_tile,j}, {p,q,t},origin);
                    if (r == rank){
                        dttqrt_tf.fulfill_promise({index, j, i+1}); // first merge -- satisfy dep. on first row of next tile
                    }
                    else {
                        auto R_rj= view<double>(Mat.at(index).at({root,j}).data(), n * n );
                        int dttqrt_row = i+1;
                        am_dtsqrt_2_dttqrt->send(r, R_rj, j, dttqrt_row, index) ; // send A{root,j}
                    }
                }
                else if ((i+1)/t > tile_index &&  root!=j){ // last dtsqrt in tile and root != j
                    int first_row_in_tile = (i/t)*t;
                    assert(root == first_row_in_tile);
                     // on the same rank as the one doing the last dtsqrt as it contains the updated R_{root, j};
                    dttqrt_tf.fulfill_promise({index, j, first_row_in_tile});
                }

                // gather on node origin
                #ifdef COMPUTE_Q
                gather_tf.fulfill_promise({index, i, j, 1});  // 1=> send V_ij, T_ij
                #endif
                // dssrfb
                for(int jinc = j+1; jinc<N; jinc++) {
                    int r = block2rank({i,jinc},{p,q,t},origin); // ssrfb
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {jinc};
                    } else {
                        to_fulfill[r].push_back(jinc);
                    }
                }

                // Send data and trigger tasks
                for (auto& p: to_fulfill){
                    int r = p.first; // rank
                    if (r == rank){
                        for(auto& jinc: p.second){
                            dssrfb_tf.fulfill_promise({index, root, j, i, jinc});
                        }
                    }
                    else {
                        auto V_ij = view<double>(Mat.at(index).at({i,j}).data(), n * n );
                        auto T_ij = view<double>(T.at(index).at({i,j}).data(), n*n);
                        auto jsv = view<int>(p.second.data(), p.second.size());

                        am_dtsqrt_2_dssrfb->send(r, V_ij, T_ij, root, j, i, jsv, index);
                    }
                }
            })
            .set_name([](int4 orij) {
                return "tsqrt_" + to_string(orij[0]) + "_" +to_string(orij[1]) + "_" +to_string(orij[2])+"_" +to_string(orij[3]);
            })
            .set_priority([&](int4) {
                return 4;
            });

        // From dlarfb
        am_dlarfb_2_dssrfb = comm->make_active_msg(
            [&](view<double> &R, int& i, int&k,  int& inext, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({i,j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dssrfb_tf.fulfill_promise({index, i, k, inext, j});
            }
        );

        am_dlarfb_2_dttssmqr = comm->make_active_msg(
            [&](view<double> &R, int& j, int& k, int& jinc, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, jinc}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dttssmqr_tf.fulfill_promise({index, j, k,jinc});
            }
        );

        // larfb
        dlarfb_tf.set_mapping([&] (int4 oikj){
                int index = oikj[0];
                int N = N_.at(index);
                return ((index + oikj[2] + oikj[3]*N) % n_threads);
            })
            .set_indegree([](int4 oikj){
                return 2;
            })
            .set_task([&] (int4 oikj) {
                int index = oikj[0];
                int i = oikj[1]; // who is sending - row
                int k=oikj[2]; // who is sending - col
                int j=oikj[3]; // me - col, my row == i
                int n=n_.at(index);

                int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'T', 'F', 'C', n, n, n,
                                           Mat.at(index).at({i,k}).data(), n, T.at(index).at({i,k}).data(),
                                        n, Mat.at(index).at({i,j}).data(), n);
                assert(info == 0);
                #ifdef DISP
                cout << "dlarfb_" << to_string(oikj[0]) << "_" << to_string(oikj[1]) << "_" << to_string(oikj[2])
                        << "_" << to_string(oikj[3]) << endl;
                #endif

            })
            .set_fulfill([&](int4 oikj) {
                int index = oikj[0];
                int i = oikj[1];
                int k = oikj[2];
                int j = oikj[3];

                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);
                int origin = origin_.at(index);

                int tile_index = i/t;

                if (i+1<M && (i+1)/t == tile_index){ // and next row in the same tile
                    int r = block2rank({i+1, j},{p,q,t},origin);
                    if (r == rank){
                        dssrfb_tf.fulfill_promise({index, i, k, i+1, j});
                    }
                    else {
                        auto R_ij = view<double>(Mat.at(index).at({i,j}).data(), n * n );
                        int inext = i+1;
                        am_dlarfb_2_dssrfb->send(r, R_ij, i, k, inext, j, index); //send R_ij
                    }
                }
                else if (i+1<M && i==k) { // and last row in tile
                    int last_row_in_next_tile = i+t; // i is the last row in current tile
                    if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly
                    int r = block2rank({last_row_in_next_tile, j}, {p,q,t},origin);
                    if (r == rank){
                        dttssmqr_tf.fulfill_promise({index, k, i+1, j }); // first merge -- satisfy dep. on first row of next tile
                    }
                    else {
                        auto R_kjinc= view<double>(Mat.at(index).at({k, j}).data(), n * n );
                        int dttssmqr_row = i+1;
                        am_dlarfb_2_dttssmqr->send(r, R_kjinc, k, dttssmqr_row, j, index) ; /// i== k
                    }
                }
                else if (i!=k){ // but not geqrt(j,j) -- then trigger merging
                    dttssmqr_tf.fulfill_promise({index, k, i, j});
                }
            })
            .set_name([](int4 oikj) {
                return "dlarfb_" + to_string(oikj[0]) + "_" +to_string(oikj[1]) + "_" +to_string(oikj[2])
                        + "_" +to_string(oikj[3]);
            })
            .set_priority([&](int4) {
                return 3;
            });

        // From dssrfb
        am_dssrfb_2_dssrfb = comm->make_active_msg(
            [&](view<double> &R, int& root, int&j, int& i,  int& jinc, int& index){
                int n = n_.at(index);
                Mat.at(index).at({root,jinc}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dssrfb_tf.fulfill_promise({index, root, j, i, jinc});
            }
        );

        am_dssrfb_2_dttssmqr = comm->make_active_msg(
            [&](view<double> &R, int& j, int& k, int& jinc, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, jinc}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dttssmqr_tf.fulfill_promise({index, j, k,jinc});
            }
        );

        dssrfb_tf.set_mapping([&] (int5 orjik){
                int index = orjik[0];
                int N = N_.at(index);
                return ((index + orjik[1] + orjik[3] * N + orjik[4] * N * N) % n_threads);
            })
            .set_indegree([](int5 orjik){
                return 3;
            })
            .set_task([&] (int5 orjik) {
                int index = orjik[0];
                int root = orjik[1];
                int j = orjik[2];
                int i = orjik[3];
                int jinc = orjik[4];

                int n = n_.at(index);
                int t = t_.at(index);

                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, n, n, 0, n,
                                           Mat.at(index).at({i,j}).data(), n,
                                           T.at(index).at({i,j}).data(), n,
                                           Mat.at(index).at({root,jinc}).data(), n,
                                           Mat.at(index).at({i,jinc}).data(), n);
                assert(info == 0);

                #ifdef DISP
                cout << "dssrfb_" << to_string(orjik[0]) << "_" +to_string(orjik[1]) << "_" <<
                        to_string(orjik[2]) << "_" << to_string(orjik[3]) << "_" <<
                        to_string(orjik[4]) << endl;
                #endif

            })
            .set_fulfill([&](int5 orjik) {
                int index = orjik[0];
                int root = orjik[1];
                int j = orjik[2];
                int i = orjik[3];
                int jinc = orjik[4];

                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);
                int origin = origin_.at(index);

                int tile_index = i/t;

                if (i+1<M && (i+1)/t == tile_index){
                    int r = block2rank({i+1,jinc},{p,q,t},origin);
                    if (r == rank){
                        dssrfb_tf.fulfill_promise({index, root, j, i+1, jinc});
                    }
                    else {
                        auto R_rk = view<double>(Mat.at(index).at({root,jinc}).data(), n * n );
                        int inext = i+1;
                        am_dssrfb_2_dssrfb->send(r, R_rk, root, j, inext, jinc, index);
                    }
                }
                else if (i+1<M && root == j){ // Tile contains R_{jj} and last row in tile
                    int last_row_in_next_tile = i+t; // i is the last row in current tile
                    if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly
                    int r = block2rank({last_row_in_next_tile, jinc}, {p,q,t},origin);
                    if (r == rank){
                        dttssmqr_tf.fulfill_promise({index, j, i+1, jinc }); // first merge -- satisfy dep. on first row of next tile
                    }
                    else {
                        auto R_jjinc= view<double>(Mat.at(index).at({j, jinc}).data(), n * n );
                        int dttssmqr_row = i+1;
                        am_dssrfb_2_dttssmqr->send(r, R_jjinc, j, dttssmqr_row, jinc, index) ;
                    }
                }
                else if ((i+1)/t > tile_index && !(root==j)){// last row in tile and Not the tile containing R_{jj}
                    int first_row_in_tile = (i/t)*t;
                    assert(root == first_row_in_tile);
                    // on the same rank as the one doing the last dssrfb as it contains the updated R_{root, jinc};
                    dttssmqr_tf.fulfill_promise({index, j, first_row_in_tile, jinc });
                }

                // int root1_tile = (root+1)/t;

                if (root == j){
                    if (i == root+1) {
                        if (jinc == root+1){
                            // same rank
                            dgeqrt_tf.fulfill_promise({index, i, jinc}); // i = jinc = root+1
                        }
                        else { // jinc > root+1
                            assert(jinc > root+1);
                            dlarfb_tf.fulfill_promise({index, i, i, jinc}); // i = root+1 (after dgeqrt on i+1, i+1)
                        }
                    }
                    else { // i > root+1
                        assert(i > root+1);
                        if (jinc == root+1) {
                            dtsqrt_tf.fulfill_promise({index, jinc, i, jinc});
                        }
                        else {
                            assert(jinc > root+1);
                            dssrfb_tf.fulfill_promise({index, root+1, root+1, i, jinc});
                        }
                    }
                }
            })
            .set_name([](int5 orjik) {
                return "dssrfb_" + to_string(orjik[0]) + "_" +to_string(orjik[1]) + "_" +
                        to_string(orjik[2]) + "_" + to_string(orjik[3]) + "_" +
                        to_string(orjik[4]);
            })
            .set_priority([&](int5) {
                return 2;
            });

        // From dttqrt
        am_dttqrt_2_dttqrt = comm->make_active_msg(
            [&](view<double> &R, int& j, int& k, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dttqrt_tf.fulfill_promise({index, j, k});
            }
        );

        am_dttqrt_2_dttssmqr = comm->make_active_msg(
            [&](view<double> &V2_kj, view<double> &T2_kj, int& j, int& k, view<int>& js, int& index){
                int n = n_.at(index);
                V2.at(index).at({k, j}) = Map<MatrixXd>(V2_kj.data(), n, n);
                T2.at(index).at({k, j}) = Map<MatrixXd>(T2_kj.data(), n, n);

                for(auto& jinc: js){
                    dttssmqr_tf.fulfill_promise({index, j, k, jinc});
                }
            }
        );

        dttqrt_tf.set_mapping([&] (int3 ojk){
                int index = ojk[0];
                return ((index + ojk[1]) % n_threads);
            })
            .set_indegree([](int3 ojk){
                return 2;
            })
            .set_task([&] (int3 ojk) {
                int index = ojk[0];
                int j = ojk[1]; // To identify block R_{jj}
                int k = ojk[2]; // To identify block R_{kj}

                int n = n_.at(index);
                assert(k>=j); // Otherwise no T2[{k,j}] has been assigned

                V2.at(index).at({k,j}) = Mat.at(index).at({k,j});
                T2.at(index).at({k,j}) = MatrixXd::Zero(n,n);

                int info = LAPACKE_dtpqrt(LAPACK_COL_MAJOR, n, n, n, n, Mat.at(index).at({j,j}).data(), n,
                                          V2.at(index).at({k,j}).data(), n, T2.at(index).at({k, j}).data(), n);

                assert(info == 0);
                #ifdef DISP
                cout << "ttqrt_" + to_string(ojk[0]) + "_" +to_string(ojk[1]) + "_" +to_string(ojk[2]) << endl;
                #endif
            })
            .set_fulfill([&](int3 ojk) {
                int index = ojk[0];
                int j = ojk[1]; // To identify block R_{jj}
                int k = ojk[2]; // To identify block R_{kj}

                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);
                int origin = origin_.at(index);

                assert(k%t == 0); // k is the first row in that tile

                // Dependencies
                map<int, vector<int>> to_fulfill;

                // Next dttqrt
                int last_row_in_current_tile=k+t-1;
                // Which node to send R_{jj} data to? -- block2rank({last_row_in_next_tile, j})
                int last_row_in_next_tile = k+2*t-1;
                if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly
                if (!(last_row_in_next_tile == last_row_in_current_tile)){
                    int r = block2rank({last_row_in_next_tile, j},{p,q,t},origin);
                    if (r == rank){
                        dttqrt_tf.fulfill_promise({index, j, k+t});
                    }
                    else {
                        auto R_jj= view<double>(Mat.at(index).at({j,j}).data(), n * n );
                        int knext = k+t;
                        am_dttqrt_2_dttqrt->send(r, R_jj, j, knext, index) ; // send A{j,j}
                    }
                }

                // gather on node origin
                #ifdef COMPUTE_Q
                assert(k >= j);
                gather_tf.fulfill_promise({index, k, j, 2});  // 2 => send V2, T2
                if (k/t == (M+t-1)/t-1){ // last tile
                    gather_tf.fulfill_promise({index, M-1, j, 2}); // send R:j
                }
                #endif

                // dttssmqr
                for(int jinc = j+1; jinc< N; jinc++) { // everything in the same row k
                    // Which node to send V2_{kj} and T2_{kj} data to? -- block2rank({last_row_in_current_tile, j})
                    int last_row_in_current_tile = t-1 + (k/t)*t;
                    if (last_row_in_current_tile >= M) last_row_in_current_tile = M-1;//  if tile doesn't split M evenly

                    int r = block2rank({last_row_in_current_tile,jinc},{p,q,t},origin);
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {jinc};
                    } else {
                        to_fulfill[r].push_back(jinc);
                    }
                }

                // Send data and trigger tasks
                for (auto& p: to_fulfill){
                    int r = p.first; // rank
                    if (r == rank){
                        for(auto& jinc: p.second){
                            dttssmqr_tf.fulfill_promise({index, j, k, jinc});
                        }
                    }
                    else {
                        auto V2_kj = view<double>(V2.at(index).at({k,j}).data(), n*n );
                        auto T2_kj = view<double>(T2.at(index).at({k,j}).data(), n*n);
                        auto jsv = view<int>(p.second.data(), p.second.size());
                        am_dttqrt_2_dttssmqr->send(r, V2_kj, T2_kj, j, k, jsv, index);
                    }
                }
            })
            .set_name([](int3 ojk) {
                return "ttqrt_" + to_string(ojk[0]) + "_" +to_string(ojk[1]) + "_" +to_string(ojk[2]);
            })
            .set_priority([&](int3) {
                return 3;
            });

        //From dttsmqr
        am_dttssmqr_2_dgeqrt = comm->make_active_msg(
            [&](view<double> &R, int& i, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({i, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dgeqrt_tf.fulfill_promise({index, i, j});
            }
        );

        am_dttssmqr_2_dlarfb = comm->make_active_msg(
            [&](view<double> &R, int& i, int& k, int& j, int& index){
                int n = n_.at(index);
                Mat.at(index).at({i, j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dlarfb_tf.fulfill_promise({index, i, k, j});
            }
        );

        am_dttssmqr_2_dtsqrt = comm->make_active_msg(
            [&](int& r, int& i, int& j, int& index){
                dtsqrt_tf.fulfill_promise({index, r, i, j});
            }
        );

        am_dttssmqr_2_dssrfb = comm->make_active_msg(
            [&](int& r, int& j, int& i, int& jinc, int& index){
                dssrfb_tf.fulfill_promise({index,r,j,i,jinc});
            }
        );

        am_dttssmqr_2_dttssmqr = comm->make_active_msg(
            [&](view<double> &R, int& j, int& k, int& jinc, int& index){
                int n = n_.at(index);
                Mat.at(index).at({j, jinc}) = Map<MatrixXd>(R.data(), n, n);
                dttssmqr_tf.fulfill_promise({index,j,k,jinc});
            }
        );

        dttssmqr_tf.set_mapping([&] (int4 ojki){
                int index = ojki[0];
                return ((index + ojki[3]) % n_threads);
            })
            .set_indegree([](int4 ojki){
                return 3;
            })
            .set_task([&] (int4 ojki) {
                int index = ojki[0];
                int j = ojki[1]; // because of dttqrt on block k,j
                int k = ojki[2]; // row k
                int jinc = ojki[3]; // col jinc

                int n = n_.at(index);
                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, n, n, n, n,
                                           V2.at(index).at({k,j}).data(), n,
                                           T2.at(index).at({k,j}).data(), n,
                                           Mat.at(index).at({j,jinc}).data(), n, // Receives this from dssrfb
                                           Mat.at(index).at({k,jinc}).data(), n);
                // cout << "info: " << info << endl;
                assert(info == 0);
                #ifdef DISP
                cout << "ttssmqr_" + to_string(ojki[0]) + "_" +to_string(ojki[1]) + "_" +
                        to_string(ojki[2]) + "_" +to_string(ojki[3]) << endl;
                #endif
            })
            .set_fulfill([&](int4 ojki) {
                int index = ojki[0];
                int j = ojki[1]; // because of dttqrt on block k,j
                int k = ojki[2]; // row k
                int jinc = ojki[3]; // col jinc

                int M = M_.at(index);
                int N = N_.at(index);
                int n = n_.at(index);
                int p = p_.at(index);
                int q = q_.at(index);
                int t = t_.at(index);
                int origin = origin_.at(index);

                assert(k%t == 0); // k is the first row in that tile

                // Dependencies
                map<int, vector<int>> to_fulfill;

                // same row --k
                if (jinc == j+1) { // Next column
                    int r = block2rank({k,jinc},{p,q,t}, origin);
                    if (rank == r) dgeqrt_tf.fulfill_promise({index, k, jinc}); // Next dgeqrt in that tile
                    else {
                        auto R_ki = view<double>(Mat.at(index).at({k,jinc}).data(), n * n ); // send to that row
                        am_dttssmqr_2_dgeqrt->send(r, R_ki,k, jinc, index);
                    }
                }
                else {
                    int r = block2rank({k,jinc},{p,q,t}, origin);
                    if (rank == r) dlarfb_tf.fulfill_promise({index, k, j+1, jinc}); // one dep. on the next dlarfb in that tile
                    else {
                        auto R_ki = view<double>(Mat.at(index).at({k,jinc}).data(), n * n ); // send to that row
                        int jnext = j+1;
                        am_dttssmqr_2_dlarfb->send(r, R_ki, k, jnext, jinc, index);
                    }
                }

                // other rows > k
                for (int kinc=k+1; kinc < min(k+t,M); ++kinc) { // rows below k
                    // Get ready for the next dtsqrt, dssrfb
                    int r = block2rank({kinc, jinc}, {p,q,t}, origin);
                    int jnext = j+1;
                    if (rank == r) {
                        if (jinc == j+1) dtsqrt_tf.fulfill_promise({index, k, kinc, jinc}); // after dgeqrt on (k, j+1)
                        else dssrfb_tf.fulfill_promise({index, k, jnext, kinc, jinc}); // after dgeqrt on (k, j+1)
                    }
                    else {
                        if (jinc == j+1) am_dttssmqr_2_dtsqrt->send(r, k, kinc, jinc, index); // empty messages
                        else am_dttssmqr_2_dssrfb->send(r, k, jnext, kinc, jinc, index); // empty messages
                    }
                }

                // Next tile's dttssmqr
                if (k+t < M) {
                    int last_row_in_next_tile = k+2*t-1;
                    if (last_row_in_next_tile >= M) last_row_in_next_tile = M-1;//  if tile doesn't split M evenly

                    int r = block2rank({last_row_in_next_tile, jinc}, {p,q,t}, origin);
                    if (rank == r){
                        dttssmqr_tf.fulfill_promise({index,j,k+t,jinc});
                    }
                    else {
                        /// Oooh it contains updated R_{j, jinc} data!!!!
                        auto R_jjinc = view<double>(Mat.at(index).at({j,jinc}).data(), n * n );
                        int knext = k+t;
                        am_dttssmqr_2_dttssmqr->send(r, R_jjinc, j, knext, jinc, index);
                    }
                }
            })
            .set_name([](int4 ojki) {
                return "ttssmqr_" + to_string(ojki[0]) + "_" +to_string(ojki[1]) + "_" +
                        to_string(ojki[2]) + "_" +to_string(ojki[3]);
            })
            .set_priority([&](int4) {
                return 3;
            });

        #ifdef COMPUTE_Q
        am_gather_0 = comm->make_active_msg(
        [&](view<double> &R_j,  int& i, int& j, int& index) {
            int n = n_.at(index);

            A.at(index)->block(0, j*n, (j+1)*n, n) = Map<MatrixXd>(R_j.data(), (j+1)*n, n) ;
            // V2.at(index).at({i,j}) = Map<MatrixXd>(V2_ij.data(), n, n);
            // T2.at(index).at({i,j}) = Map<MatrixXd>(T2_ij.data(), n, n);

            computeQ_tf.fulfill_promise(index);
        });

        am_gather_1 = comm->make_active_msg(
        [&](view<double> &V_ij, view<double> &T_ij,  int& i, int& j, int& index) {
            int n = n_.at(index);
            A.at(index)->block(i*n, j*n, n, n) = Map<MatrixXd>(V_ij.data(), n, n); // Last block
            Tmat.at(index)->block(i*n, j*n, n, n) = Map<MatrixXd>(T_ij.data(), n, n);
            computeQ_tf.fulfill_promise(index);
        });

        am_gather_2 = comm->make_active_msg(
        [&](view<double> &V2_ij, view<double> &T2_ij,  int& i, int& j, int& index) {
            int n = n_.at(index);
            V2.at(index).at({i,j}) = Map<MatrixXd>(V2_ij.data(), n, n);
            T2.at(index).at({i,j}) = Map<MatrixXd>(T2_ij.data(), n, n);

            computeQ_tf.fulfill_promise(index);
        });

        // gather
        gather_tf.set_mapping([&](int4 oijk) {
                int index = oijk[0];
                int N = N_.at(index);
                return ( (index + oijk[1] + oijk[2]*N) % n_threads );
            })
            .set_indegree([](int4) {
                return 1;
            })
            .set_task([&](int4 oijk) {
                int index = oijk[0];
                int i = oijk[1];
                int j = oijk[2];
                int k = oijk[3]; // is 1 or 2 -- if 1 get V, T and if 2 get V2, T2 /and R

                int n = n_.at(index);
                int M = M_.at(index);
                int origin = origin_.at(index);

                if(rank != origin) {
                    if ( (i==M-1 && j==M-1) || (i==M-1 && k==2) ){
                        cout << "entering here" << endl;
                        MatrixXd Atemp = MatrixXd::Zero( (j+1)*n, n);
                        for (int kk=0; kk< j+1; ++kk){
                            Atemp.block(kk*n, 0, n, n) = Mat.at(index).at({kk,j}); // R[:,j] of A = QR
                        }
                        auto R_j = view<double>(Atemp.data(), (j+1)*n*n);
                        am_gather_0->send(origin, R_j, i, j, index);
                    }
                    else if (k==1){ // get only V_ij T_ij -- comes from dgeqrt and dtsqrt
                        assert(i >= j);
                        auto V_ij = view<double>(Mat.at(index).at({i,j}).data(), n*n);
                        auto T_ij = view<double>(T.at(index).at({i,j}).data(), n*n);
                        am_gather_1->send(origin, V_ij, T_ij, i, j, index);
                    }
                    else if (k==2) { // k==2 -- get V2_ij, T2_ij and R_:j -- comes from dttqrt
                        auto V2_ij = view<double>(V2.at(index).at({i,j}).data(), n*n);
                        auto T2_ij = view<double>(T2.at(index).at({i,j}).data(), n*n);
                        am_gather_2->send(origin, V2_ij, T2_ij, i, j, index);
                    }
                }
                else {
                    if ( (i==M-1 && j==M-1) || (i==M-1 && k==2) ){
                        for (int kk=0; kk< j+1; ++kk){
                            A.at(index)->block(kk*n, j*n, n, n) = Mat.at(index).at({kk,j}); // R[:,j] of A = QR
                        }
                        computeQ_tf.fulfill_promise(index);
                    }
                    else if (k==1){ // get only V_ij T_ij -- comes from dgeqrt and dtsqrt
                        A.at(index)->block(i*n, j*n, n, n) = Mat.at(index).at({i,j}); // just that block; contains V
                        Tmat.at(index)->block(i*n, j*n, n, n) = T.at(index).at({i,j});
                        computeQ_tf.fulfill_promise(index);
                    }
                    else if (k==2) { // k==2 -- get V2_ij, T2_ij and R_:j -- comes from dttqrt
                        computeQ_tf.fulfill_promise(index);
                    }
                }
            })
            .set_name([](int4 oijk) {
                return "gather_"+ to_string(oijk[0]) + "_" +to_string(oijk[1]) + "_" +to_string(oijk[2]) + "_" + to_string(oijk[3]);
            });

            computeQ_tf.set_task([&] (int index){
                cout << "Computing_Q" << endl;
                assert(rank == origin_[index]);
                int n = n_.at(index); // happens only on origin
                int M = M_.at(index);
                int N = N_.at(index);
                int t = t_.at(index);

                for (int j = N-1; j > -1; --j){ // Last column operations

                    // cannot remove the for loop for k -- some error in the internal workspace of dtpmqrt
                    for (int k=0; k < M; ++k){ // Getting the M cols of matrix Q
                        int first_row_in_last_tile = ((M+t-1)/t-1)*t;
                        for (int first_row_in_tile = first_row_in_last_tile;first_row_in_tile > j; first_row_in_tile-=t){ // unmerging -- reversing dttqrt
                            MatrixXd V2_ij = V2.at(index).at({first_row_in_tile,j});
                            MatrixXd T2_ij = T2.at(index).at({first_row_in_tile,j});
                            MatrixXd Qf = Q.at(index)->block(first_row_in_tile*n, k*n, n, n);
                            MatrixXd Qj = Q.at(index)->block(j*n, k*n, n, n);
                            int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'N', n, n, n, n, n,
                                                       V2_ij.data(), n,
                                                       T2_ij.data(), n,
                                                       Qj.data(), n,
                                                       Qf.data(), n);
                            Q.at(index)->block(first_row_in_tile*n, k*n, n, n) = Qf;
                            Q.at(index)->block(j*n, k*n, n, n) = Qj;
                            assert(info==0);
                            // cout << "Q dttqrt" << endl;
                            // cout << *Q.at(index) << endl;
                        }

                        // reversing dtsqrt
                        for (int i=M-1; i > j; --i){
                            if (i%t != 0){ // Not the first row in tile
                                MatrixXd Aij = A.at(index)->block(i*n, j*n, n, n);
                                MatrixXd Tij = Tmat.at(index)->block(i*n, j*n, n, n);
                                // assert(Q->cols() == N*n);
                                MatrixXd Qi = Q.at(index)->block(i*n, k*n, n, n);
                                MatrixXd Qj = Q.at(index)->block(j*n, k*n, n, n);

                                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'N', n, n, n, 0, n,
                                                           Aij.data(), n,
                                                           Tij.data(), n,
                                                           Qj.data(), n,
                                                           Qi.data(), n);
                                Q.at(index)->block(i*n, k*n, n, n) = Qi;
                                Q.at(index)->block(j*n, k*n, n, n) = Qj;

                                assert(info ==0);
                                // cout << "Q dtsqrt" << endl;
                                // cout << *Q.at(index) << endl;
                            }
                        }
                    }

                    int first_row_in_last_tile = ((M+t-1)/t-1)*t;
                    for (int first_row_in_tile = first_row_in_last_tile; first_row_in_tile >= j; first_row_in_tile-=t){
                        MatrixXd Afj = A.at(index)->block(first_row_in_tile*n, j*n, n, n);
                        MatrixXd Tfj = Tmat.at(index)->block(first_row_in_tile*n, j*n, n, n);
                        MatrixXd Qf  = Q.at(index)->block(first_row_in_tile*n, 0, n, M*n);

                        int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'N', 'F', 'C', n, M*n, n, Afj.data(), n,
                                                  Tfj.data(), n, Qf.data(), n);

                        Q.at(index)->block(first_row_in_tile*n, 0, n, M*n) = Qf;

                        assert(info == 0);
                        // cout << "Q dgeqrt" << endl;
                        // cout << *Q.at(index) << endl;
                    }
                }
            })
            .set_indegree([&] (int index){
                int M = M_.at(index);
                int N = N_.at(index);
                int t = t_.at(index);

                int num_tiles = (M+t-1)/t;
                int num_dttqrt = num_tiles*N - t*(N/t)*(N/t+1)/2 - (N/t+1)*(N-N/t*t);
                // cout << "num_dttqrt" << num_dttqrt << endl;
                return (M* (M +1)/2 - (M-N)*(M-N+1)/2 + num_dttqrt);
            })
            .set_mapping([&] (int index){
                return (index % n_threads);
            })
            .set_name([&](int index) {
                return "computeQ_tf_" + to_string(index);
            })
            .set_fulfill([&](int index){
                notify_tf->fulfill_promise(index);
            });
            #endif
    }

    /** Function **/

    void run(MatrixXd* Y_, MatrixXd* Tmat_, MatrixXd* Q_, int block_size, int k, int max_rank, int tile_size){

        int origin = comm_rank();

        int p = pow(2,ceil(log(sqrt(nranks))/log(2)));
        int q = floor(nranks/p);
        cout << p << " " << q << endl;
        // notify_index_[origin] = k;
        int M = (Y_->rows()+block_size-1)/block_size;
        int R = (Y_->cols()+block_size-1)/block_size;

        int N = (max_rank+block_size-1)/block_size;

        // cout << M << " " << R << " " << N << endl;

        // Happens only on origin rank
        initialize(p, q, block_size, M, R, N, k, origin, Y_, Tmat_, Q_, tile_size);

        // int n = block_size;
        // scatter_tf.fulfill_promise(k);
        initialize_tf.fulfill_promise(k);
    }
};

// Every rank runs this code
int rand_range(int n_threads, int n, int M, int N, int p, int q, int t)
{
    // MPI info
    const int rank = comm_rank();
    const int n_ranks = comm_size();
    if(VERB) printf("[%d] Hello from %s\n", comm_rank(), processor_name().c_str());

    assert(p * q == n_ranks);
    assert(p >= 1);
    assert(q >= 1);

    VectorXd x ;
    VectorXd b ;
    VectorXd bref ;
    int samp=0; // Oversampling parameter

    MatrixXd A;
    MatrixXd Acopy;
    MatrixXd* Y = new MatrixXd(M*n, N*n);
    MatrixXd* Q = new MatrixXd(M*n, M*n);
    *Q = MatrixXd::Identity(M*n, M*n);
    // Q->setZero();
    // Q->block(0,0,N*n,N*n) = MatrixXd::Identity(N*n, N*n);
    MatrixXd* Tmat = new MatrixXd(M*n, N*n);
    Tmat->setZero();

    int origin = 0;
    // Factorize
    // {
        // Initialize the communicator structure
        Communicator comm(MPI_COMM_WORLD, VERB);

        // Threadpool
        Threadpool tp(n_threads, &comm, VERB, "[" + to_string(rank) + "]_");
        Taskflow<int> sparsify_tf(&tp, VERB);
        Taskflow<int> notify_tf(&tp, VERB);
        denseQR qr(&comm, &tp, &notify_tf); // Every rank does this.

        notify_tf.set_task([&] (int k){
            printf("Randomized range finder is done and gathered on rank %d\n",rank);
        })
        .set_indegree([&] (int k){
            return 1;
        })
        .set_mapping([&] (int k){
            return (k % n_threads);
        })
        .set_name([&](int k) {
            return "notify_tf_" + to_string(k);
        });

        sparsify_tf.set_mapping([&] (int k){
            return (k % n_threads);
        })
        .set_indegree([&] (int k){
            return 1;
        })
        .set_task([&] (int k){
            printf("Modelling the sparsify task; call the RRQR function\n");
            // std::default_random_engine default_gen = default_random_engine(2020+20*k);
            // auto gen_gaussian = [&](int i, int j){ return get_gaussian(i, j, &default_gen); };

            // Generate a Gaussian random matrix
            // MatrixXd G = gen_gaussian(M*n, N*n);

            // Generate a rank deficient matrix
            // MatrixXd X = gen_gaussian(M*n, M*n);
            MatrixXd X =  MatrixXd::Random(M*n, M*n);
            VectorXd d = VectorXd::Zero(M*n);
            d.head(N*n-samp) = 0.1*VectorXd::LinSpaced(N*n-samp,1, N*n-samp);

            DiagonalMatrix<double, Eigen::Dynamic> D(M*n);
            D = d.asDiagonal();
            A = X*D*X.inverse();

            x = VectorXd::Random(n * M);
            b = A*x;
            bref = b;
            Acopy = A;
            int max_rank = N*n-samp;
            qr.run(&A, Tmat, Q, n, k, max_rank, t);
        });

        timer t0 = wctime();
        if (rank == origin){
            sparsify_tf.fulfill_promise(2*rank);
            // sparsify_tf.fulfill_promise(2*rank+1);
            // A =  MatrixXd::Random(M*n, M*n);
            // x = VectorXd::Random(n * M);
            // b = A*x;
            // bref = b;
            // Acopy = A;
            // int max_rank = N*n-samp;
            // t0 = wctime();

            // qr.run(&A, Tmat, Q, n, origin, max_rank, t);

        }
        tp.join();
        timer t1 = wctime();
        MPI_Barrier(MPI_COMM_WORLD);
        if(rank == origin)
        {
            cout << "rank: " << rank << " Time : " << elapsed(t0, t1) << endl;
        }

        if(VERB && rank==origin) {
            double error = (Acopy - (Q->leftCols(N*n))*(Q->leftCols(N*n).transpose())*Acopy).norm();
            cout << "rank: " << rank << " Error solve: " << error << endl;
        }

       delete Q;
       delete Y;
       delete Tmat;

    return 0;
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    if (argc >= 2)
    {
        n_threads_ = atoi(argv[1]);
    }

    if (argc >= 3)
    {
        n_ = atoi(argv[2]);
    }

    if (argc >= 4)
    {
        M_ = atoi(argv[3]);
    }

    if (argc >= 5)
    {
        N_ = atoi(argv[4]);
    }

    if (argc >= 6)
    {
        p_ = atoi(argv[5]);
    }

    if (argc >= 7)
    {
        q_ = atoi(argv[6]);
    }

    if (argc >= 8){
        t_ = atoi(argv[7]);
    }

    if (argc >= 9)
    {
        VERB = atoi(argv[8]);
    }

    if (argc >= 10)
    {
        LOG = atoi(argv[9]);
    }

    const int return_flag = rand_range(n_threads_, n_, M_, N_, p_, q_, t_);

    // cout << "returned to main code" << endl;

    MPI_Finalize();

    return return_flag;
}


#include "TROOT.h"
#include "TStyle.h"
#include "TFile.h"
#include "TH1.h"
#include "TLegend.h"
#include "TGraphAsymmErrors.h"
#include "TCanvas.h"
#include <cmath>
#include <fstream>
#include <iostream>

#include <exception>
#include <iterator>
#include <string>
#include <vector>

#if (defined(STANDALONE) or defined(__CINT__))
#include "ClopperPearsonBinomialInterval.h"
#else
#include "PhysicsTools/RooStatsCms/interface/ClopperPearsonBinomialInterval.h"
#endif

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;
using namespace std;

// A helper function to simplify the main part.
template <class T>
ostream &operator<<(ostream &os, const vector<T> &v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

int main(int ac, char *av[]) {
  gROOT->SetBatch(kTRUE);
  gROOT->SetStyle("Plain");

  try {
    string file;
    string numDATA;
    string denDATA;
    string numMC;
    string denMC;

    string ext;
    string pname;
    unsigned int rebin = 1;
    po::options_description desc("Allowed options");
    desc.add_options()("help,h", "produce help message")("input-file,i", po::value<string>(&file), "input file")(
        "numDATA,n", po::value<string>(&numDATA), "numHistoDATA")(
        "denDATA,d", po::value<string>(&denDATA), "denHistoDATA")("numMC,m", po::value<string>(&numMC), "MCnumHisto")(
        "denMC,e", po::value<string>(&denMC), "MCdenHisto")("plotname,o", po::value<string>(&pname), "plot name")(
        "rebin,r", po::value<unsigned int>(&rebin)->default_value(1), "rebin")(
        "plot-format,p", po::value<string>(&ext)->default_value("gif"), "output plot format");

    po::positional_options_description p;
    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
    }

    TFile *root_file = new TFile(file.c_str(), "read");
    //TFile * root_file = new TFile("MuTrigger_133874_133828_768ub.root","read");
    //string d_string = *itd;
    string dirDenDATA = denDATA;
    string dirNumDATA = numDATA;
    string dirDenMC = denMC;
    string dirNumMC = numMC;

    TH1D *denhDATA = (TH1D *)root_file->Get(dirDenDATA.c_str());
    TH1D *numhDATA = (TH1D *)root_file->Get(dirNumDATA.c_str());
    denhDATA->Rebin(rebin);
    numhDATA->Rebin(rebin);
    TH1D *denhMC = (TH1D *)root_file->Get(dirDenMC.c_str());
    TH1D *numhMC = (TH1D *)root_file->Get(dirNumMC.c_str());
    denhMC->Rebin(rebin);
    numhMC->Rebin(rebin);

    const int bins = denhDATA->GetXaxis()->GetNbins();
    //  bins =  bins /  rebin;

    const double xMax = denhDATA->GetXaxis()->GetXmax();
    const double xMin = denhDATA->GetXaxis()->GetXmin();
    double *x = new double[bins];
    double *eff = new double[bins];
    double *exl = new double[bins];
    double *exh = new double[bins];
    double *eefflCP = new double[bins];
    double *eeffhCP = new double[bins];

    ClopperPearsonBinomialInterval cp;
    //  alpha = 1 - CL
    const double alpha = (1 - 0.682);
    cp.init(alpha);

    //data
    TH1D histo("histo", "Efficiency", bins, xMin, xMax);
    for (int i = 1; i <= bins; ++i) {
      int j = i - 1;
      x[j] = (double(i - 0.5)) * (xMax - xMin) / (bins) + xMin;
      int n0 = denhDATA->GetBinContent(i);
      //	  std::cout << " n0 " << n0 << endl;
      int n1 = numhDATA->GetBinContent(i);
      // std::cout << " n1 " << n1 << endl;
      if (n0 != 0) {
        eff[j] = double(n1) / double(n0);
        histo.SetBinContent(i, eff[j]);
        exl[j] = exh[j] = 0;
        cp.calculate(n1, n0);
        eefflCP[j] = eff[j] - cp.lower();
        eeffhCP[j] = cp.upper() - eff[j];
      } else {
        eff[j] = 0;
        histo.SetBinContent(i, eff[j]);
        exl[j] = exh[j] = 0;
        //cp.calculate(n1, n0);
        eefflCP[j] = 0;
        eeffhCP[j] = 0;
      }
      //histo.SetBinContent(i+1,eff[i]);
      //exl[i] = exh[i] = 0;
      //cp.calculate(n1, n0);
      //eefflCP[i] = eff[i] - cp.lower();
      //eeffhCP[i] = cp.upper() - eff[i];
    }
    TGraphAsymmErrors graphCP(bins, x, eff, exl, exh, eefflCP, eeffhCP);
    graphCP.SetTitle("trigger (HLT_Mu9 path) efficiency");
    graphCP.SetMarkerColor(kRed);
    graphCP.SetMarkerStyle(21);
    graphCP.SetLineWidth(2);
    graphCP.SetLineColor(kRed);
    string cname = pname;
    TCanvas *c = new TCanvas(cname.c_str());
    gStyle->SetOptStat(0);
    histo.SetTitle("trigger (HLT_Mu9 path) efficiency");
    //	histo.GetXaxis()->SetTitle("p_{T} (GeV/c)");
    histo.GetXaxis()->SetTitle("#eta");
    histo.GetYaxis()->SetTitle("efficiency");
    histo.Draw();
    histo.SetMinimum(0.0);
    histo.SetLineColor(kWhite);
    graphCP.Draw("P");

    //mc
    double *xMC = new double[bins];
    double *effMC = new double[bins];
    double *exlMC = new double[bins];
    double *exhMC = new double[bins];
    double *eefflCPMC = new double[bins];
    double *eeffhCPMC = new double[bins];

    TH1D histoMC("histoMC", "EfficiencyMC", bins, xMin, xMax);
    for (int i = 1; i <= bins; ++i) {
      int j = i - 1;
      xMC[j] = (double(i - 0.5)) * (xMax - xMin) / (bins) + xMin;
      int n0 = denhMC->GetBinContent(i);
      //	  std::cout << " n0 " << n0 << endl;
      int n1 = numhMC->GetBinContent(i);
      // std::cout << " n1 " << n1 << endl;
      if (n0 != 0) {
        effMC[j] = double(n1) / double(n0);
        histoMC.SetBinContent(i, effMC[j]);
        exlMC[j] = exhMC[j] = 0;
        cp.calculate(n1, n0);
        eefflCPMC[j] = effMC[j] - cp.lower();
        eeffhCPMC[j] = cp.upper() - effMC[j];
      } else {
        effMC[j] = 0;
        histoMC.SetBinContent(i, effMC[j]);
        exlMC[j] = exhMC[j] = 0;
        //cp.calculate(n1, n0);
        eefflCPMC[j] = 0;
        eeffhCPMC[j] = 0;
      }
      //histo.SetBinContent(i+1,eff[i]);
      //exl[i] = exh[i] = 0;
      //cp.calculate(n1, n0);
      //eefflCP[i] = eff[i] - cp.lower();
      //eeffhCP[i] = cp.upper() - eff[i];
    }
    TGraphAsymmErrors graphCPMC(bins, xMC, effMC, exlMC, exhMC, eefflCPMC, eeffhCPMC);
    graphCPMC.SetTitle("HLT_Mu9 efficiency (Clopper-Pearson intervals)");
    graphCPMC.SetMarkerColor(kBlue);
    graphCPMC.SetMarkerStyle(21);
    graphCPMC.SetLineWidth(2);
    graphCPMC.SetLineColor(kBlue);

    histoMC.SetTitle("HLT_Mu9 Efficiency with Clopper-Pearson intervals");
    histoMC.Draw("same");
    histoMC.SetLineColor(kBlue);
    histoMC.SetLineWidth(2);
    //graphCPMC.Draw("P");

    TLegend *leg = new TLegend(0.4, 0.40, 0.6, 0.6);
    leg->SetFillColor(kWhite);

    leg->AddEntry(&graphCP, "data", "l");
    leg->AddEntry(&graphCPMC, "MC", "l");
    leg->Draw();

    string plot = "HLTMu_9" + pname + "." + ext;
    c->SaveAs(plot.c_str());
    string outfile = "Effhisto" + pname + ".root";
    TFile *output_file = TFile::Open(outfile.c_str(), "recreate");
    string outdir = pname;
    TDirectory *dir = output_file->mkdir(outdir.c_str());
    dir->cd();
    c->Write();
    histo.Write();
    graphCP.Write();
    output_file->Close();
    delete c;
  }

  catch (std::exception &e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  } catch (...) {
    cerr << "Exception of unknown type!\n";
  }

  return 0;
}


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "allele_mapping_quality.hpp"

#include <algorithm>
#include <iterator>
#include <cmath>
#include <limits>
#include <numeric>

#include <boost/optional.hpp>
#include <boost/variant.hpp>

#include "basics/aligned_read.hpp"
#include "core/types/allele.hpp"
#include "io/variant/vcf_record.hpp"
#include "utils/maths.hpp"
#include "../facets/samples.hpp"
#include "../facets/alleles.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string AlleleMappingQuality::name_ = "AMQ";

std::unique_ptr<Measure> AlleleMappingQuality::do_clone() const
{
    return std::make_unique<AlleleMappingQuality>(*this);
}

Measure::ValueType AlleleMappingQuality::get_value_type() const
{
    return int {};
}

namespace {

int calculate_median_mapping_quality(const ReadRefSupportSet& reads)
{
    std::vector<AlignedRead::MappingQuality> mapping_qualities(reads.size());
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(mapping_qualities),
                   [](const AlignedRead& read) { return read.mapping_quality(); });
    return maths::median(mapping_qualities);
}

} // namespace

Measure::ResultType AlleleMappingQuality::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& alleles = get_value<Alleles>(facets.at("Alleles"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).alleles;
    const auto num_alleles = 1 + call.num_alt();
    Array<Array<Optional<ValueType>>> result(samples.size(), Array<Optional<ValueType>>(num_alleles));
    for (std::size_t s {0}; s < samples.size(); ++s) {
        const auto& sample_alleles = get(alleles, call, samples[s]);
        assert(sample_alleles.size() == num_alleles);
        const auto& support = assignments.at(samples[s]);
        for (std::size_t a {0}; a < num_alleles; ++a) {
            if (sample_alleles[a]) {
                const auto support_set_itr = support.find(*sample_alleles[a]);
                if (support_set_itr != std::cend(support) && !support_set_itr->second.empty()) {
                    result[s][a] = calculate_median_mapping_quality(support_set_itr->second);
                }
            }
        }
    }
    return result;
}

Measure::ResultCardinality AlleleMappingQuality::do_cardinality() const noexcept
{
    return ResultCardinality::samples_and_alleles;
}

const std::string& AlleleMappingQuality::do_name() const
{
    return name_;
}

std::string AlleleMappingQuality::do_describe() const
{
    return "Median mapping quality of reads assigned to each allele";
}

std::vector<std::string> AlleleMappingQuality::do_requirements() const
{
    return {"Samples", "Alleles", "ReadAssignments"};
}

boost::optional<Measure::Aggregator> AlleleMappingQuality::do_aggregator() const noexcept
{
    return Measure::Aggregator::min_tail;
}

} // namespace csr
} // namespace octopus


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

/*
 Copyright (C)  2017 Cord Harms

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

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

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

/*! \file localcorrsurface.hpp
    \brief Local Correlation surface derived ....
*/

#include <ql/experimental/termstructures/Helper/ParticleMethodUtils.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/experimental/templatemodels/multiasset/localcorrelationSLVmodel.hpp>
#include <ql/experimental/templatemodels/montecarlo/montecarlomodells.hpp>
#include <ql/experimental/templatemodels/montecarlo/mcpayoffT.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <math.h>
#include <boost/math/distributions.hpp>
#include <boost/shared_ptr.hpp>

namespace QuantLib {

	void ParticleMethodUtils::calibrateFX(Handle<LocalCorrSurfaceABFFX> surface, const std::string& kernelIn, unsigned int numberOfPaths, Time maxTime,
		Time deltaT, Time tMin, Real kappa, Real sigmaAVR, Real exponentN, Real gridMinQuantile,
		Real gridMaxQuantile, unsigned int ns1, unsigned int ns2) {

		boost::shared_ptr<KernelInterface> kernel;

		if (kernelIn == "QuarticKernel") {
			kernel = boost::shared_ptr<KernelInterface>(new QuarticKernel());
		}
		else {
			QL_REQUIRE(false, "Kernel not supported. Supported is: QuarticKernel");
		}

		std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>> processes = surface->getProcesses();
		boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>			    processToCal= surface->getProcessToCal();

		//build assetModel for simulation
		boost::shared_ptr<YieldTermStructure> yld(new FlatForward(processToCal->blackVolatility()->referenceDate(),0, processToCal->blackVolatility()->dayCounter()));
		Handle<YieldTermStructure> yldH = Handle<YieldTermStructure>(yld);
		std::vector<std::string> aliases(2);
		aliases[0] = "fx1";
		aliases[1] = "fx2";
		Handle<LocalCorrTermStructure> surfaceGen(surface.currentLink());
		boost::shared_ptr<LocalCorrelationSLVModel> assetModel = boost::shared_ptr<LocalCorrelationSLVModel>((new LocalCorrelationSLVModel(yldH, aliases, processes, surfaceGen)));

		std::vector<Time>& times = surface->getTimes();
		std::vector<std::vector<Real>>& strikes = surface->getStrikes();
		std::vector<std::vector<Real>>& surfaceF = surface->getSurfaceF();

		//time grid from t=0 to maxTime:
		times.resize(1);
		times[0] = 0;
		size_t i = 0;
		while (times[i] < maxTime) {
			times.push_back(times[i]+deltaT);
			i++;
		}
		surface->setInterpolationTime<Linear>();

		RealMCSimulation simulation(assetModel, times, times, numberOfPaths,1,true,true,false);
		simulation.prepareForSlicedSimulation();
		simulation.simulate(0, false);

		//create strike grid.
		//the strike grid depends on simulation results (min and max quantile)
		//the simulation itself depends on local correlation which itself is calibrated using this function.
		//Consequence: strike grid can merely be calculated iteratively during calibration

		size_t numberStrikes;
		Real strikeStep;
		surfaceF.resize(times.size());
		strikes.resize(times.size());
		std::vector<Real> state;
		std::vector<Real> assets(2);
		std::vector<Real> crossFX(numberOfPaths);

		Real vol1;
		Real vol2;
		std::vector<Real> vol3;
		std::vector<Real> eNum;
		std::vector<Real> eDen;
		std::vector<Real> eScale;
		Real a = 0;
		Real b = 0;
		Real kernelV = 0;
		Real bandwidth = 0;
		Real minPosBw;
		Real maxNegBw;
		Real bwIn;
		Real bwRatio;

		//Calculate local correlation successively over time

		for (size_t i = 1; i < surfaceF.size()-1; i++) //iteration over time, for i=0 nothing to do as correlation independent of a,b,f. In last entry, no additional simulation necessary.
		{
			numberStrikes = numberStrikeGrid(times[i],ns1,ns2);
			QL_REQUIRE(numberStrikes>1,"ns1 or ns2 has to be increased, strike grid cannot be calculated.");
			simulation.simulate(i,false);

			//Now strike grid can be calculated

			surfaceF[i].resize(numberStrikes);
			strikes[i].resize(numberStrikes);
			for (size_t k = 0; k < numberOfPaths; k++)
			{
				state = simulation.state(k, times[i]);
				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);
				crossFX[k] = getCrossFX(assets[0] , assets[1]);
			}

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

			strikes[i][0] = crossFX[(size_t)(numberOfPaths*gridMinQuantile)];
			strikes[i][strikes[i].size()-1] = crossFX[(size_t)(numberOfPaths*gridMaxQuantile)];

			strikeStep = (strikes[i][strikes[i].size() - 1] - strikes[i][0]) / (numberStrikes-1);

			bandwidth = ParticleMethodUtils::bandwidth(times[i], getCrossFX(processes[0]->s0()->value(),processes[1]->s0()->value()), kappa, sigmaAVR, tMin, numberOfPaths, exponentN);

			for (size_t j = 1; j < strikes[i].size()-1; j++)
			{
				strikes[i][j] = strikes[i][j-1] + strikeStep;
			}

			//Particle method for that time step

			eNum.resize(strikes[i].size());
			eDen.resize(strikes[i].size());
			eScale.resize(strikes[i].size());
			vol3.resize(strikes[i].size());

			for (size_t j = 0; j < strikes[i].size(); j++)
			{
				eNum[j] = 0;
				eDen[j] = 0;
				eScale[j] = 0;
				vol3[j] = processToCal->localVolatility()->localVol(times[i], strikes[i][j], true);
			}

			for (size_t k = 0; k < numberOfPaths; k++) //particle method: over MC paths
			{
				state = simulation.state(k, times[i]);

				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);

				state[2] = state[2] < 0 ? 0.0001 : state[2];//heston vol might become negative (dep. on feller constant)
				state[3] = state[3] < 0 ? 0.0001 : state[3];

				vol1 = processes[0]->leverageFct()->localVol(times[i], assets[0], true)*std::sqrt(state[2]); //*ai from Heston
				vol2 = processes[1]->leverageFct()->localVol(times[i], assets[1], true)*std::sqrt(state[3]);

				a = surface->localA(times[i], assets, true);
				b = surface->localB(times[i], assets, true);

				if (vol1 != vol1 || vol2 != vol2)
					QL_FAIL("surface of asset1 oder asset2 not well defined");

				for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
				{

					minPosBw = 10000000;
					maxNegBw = -10000000;

					bwIn = getCrossFX(assets[0], assets[1]) - strikes[i][j];
					bwRatio = bwIn / bandwidth;

					if (bwRatio > maxNegBw && bwRatio <= 0) maxNegBw = bwRatio;
					if (bwRatio < minPosBw && bwRatio >= 0) minPosBw = bwRatio;

					kernelV = assets[1] * ParticleMethodUtils::kernel(bandwidth, bwIn, kernel);

					eNum[j] += (vol1*vol1 + vol2*vol2 + 2 * a*vol1*vol2 / b)*kernelV;
					eDen[j] += vol1*vol2*kernelV / b;
					eScale[j] += kernelV;
				}
			}

			for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
			{
				QL_REQUIRE(eScale[j] != 0, std::string("ParticleMethodUtils::calibrateFX: resulting bandwidth is too small for calibration (support: < ") + std::to_string(maxNegBw)
					+ std::string(" and > ") + std::to_string(minPosBw)
					+ std::string("). Either decrease number of MC paths or increase exponentN or kappa."));
				if (eNum[j] != eNum[j] || vol3 != vol3 || eScale[j] != eScale[j] || eDen[j] != eDen[j] || eDen[j] == 0)
					QL_FAIL("surface not well defined.");
				surfaceF[i][j] = (eNum[j] - vol3[j] * vol3[j]* eScale[j]) / (2 * eDen[j]);
			}
			//set interpolation on new dimension:
			surface->setInterpolationStrike<Linear>(i);
		}
	}

	Real ParticleMethodUtils::bandwidth(Time t, Real s0, Real kappa, Real sigmaAVR, Real tMin, unsigned int numberOfPaths, Real exponentN) {
		Real mult = pow(numberOfPaths, exponentN);
		return kappa*sigmaAVR* s0 * sqrt(t>tMin ? t : tMin) * mult;
	}

	Real ParticleMethodUtils::kernel(Real bandwidth, Real x, boost::shared_ptr<KernelInterface>& kernel) {
		QL_REQUIRE(bandwidth != 0, "Error in ParticleMethodUtils: bandwidth is not allowed to be zero.");
		return kernel->value(x / bandwidth) / bandwidth;
	}
	size_t ParticleMethodUtils::numberStrikeGrid(Time t, unsigned int ns1, unsigned int ns2) {
		Real numberOfStrikes = ns1*sqrt(t);
		return (numberOfStrikes > ns2) ? ((size_t)numberOfStrikes) : (ns2);
	}

	Real ParticleMethodUtils::getCrossFX(Real asset1, Real asset2) {
		return asset1 / asset2;
	}
}


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
    @ingroup group-operator
    Function object implementing multiplies capabilities

    Perform the product of two parameters of the same type.

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    T x = multiplies(a,b);
    @endcode

    returns the product of @c a and @c b

  **/
  const boost::dispatch::functor<tag::multiplies_> multiplies = {};
} }
#endif

#include <boost/simd/function/scalar/multiplies.hpp>
#include <boost/simd/function/simd/multiplies.hpp>

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Main

#include <boost/test/unit_test.hpp>
#include <rapidcheck/boost_test.h>

#include <capnp/message.h>
#include <capnp/serialize.h>

#include "lt/slipstream/capnp/test_delta.capnp.h"

#include "lt/slipstream/capnp.h"
#include "lt/slipstream/serialize.h"
#include "lt/slipstream/testing/roundtrip.h"
#include "lt/slipstream/testing/multichannel_roundtrip.h"

namespace lt::slipstream {

class SerialInt64 : public SlipstreamCapnp<SerialInt64, capnp::SerialInt64, SerialInt64> {
   public:
    static constexpr auto encoding = "capnp/int64";

    SerialInt64(int64_t i=0) : i_(i) {}

    int64_t value() const {
        return i_;
    };

    bool operator==(const SerialInt64& other) const {
        return i_ == other.i_;
    }

    friend std::ostream& operator<<(std::ostream& os, const SerialInt64& s) {
        return os << s.i_;
    }

    static SerialInt64 decode(const reader_type& reader)
    {
        return SerialInt64(reader.getValue());
    }

    static void encode(builder_type& builder, const SerialInt64& value)
    {
        builder.setValue(value.value());
    }

   private:
    int64_t i_;
};

class BiasedInt64 {
   public:
    using header_type = SerialInt64;
    using data_type = SerialInt64;

    BiasedInt64(SerialInt64 bias) : bias_(bias)
    {
    }

    const SerialInt64& header() {
        return bias_;
    }

    SerialInt64 encode(const SerialInt64& input) {
        return {input.value() - bias_.value()};
    }

    SerialInt64 decode(const SerialInt64& input) {
        return {input.value() + bias_.value()};
    }

   private:
    SerialInt64 bias_;
};

class BiasedDeltaInt64 {
   public:
    using header_type = SerialInt64;
    using data_type = SerialInt64;
    using delta_type = SerialInt64;

    BiasedDeltaInt64(SerialInt64 bias) : bias_(bias)
    {
    }

    const SerialInt64& header() {
        return bias_;
    }

    SerialInt64 encode(const SerialInt64& input) {
        keyframe_ = input;
        return {input.value() - bias_.value()};
    }

    SerialInt64 decode(const SerialInt64& input) {
        keyframe_ = {input.value() + bias_.value()};
        return keyframe_;
    }

    SerialInt64 encode_delta(const SerialInt64& input) {
        int64_t d = input.value() - keyframe_.value();
        return {d - bias_.value()};
    }

    SerialInt64 decode_delta(const SerialInt64& input) {
        int64_t d = input.value() + keyframe_.value();
        return {d + bias_.value()};
    }

   private:
    SerialInt64 bias_;
    SerialInt64 keyframe_; //unbiased
};

using BiasedInt64Stream = HeaderStream<BiasedInt64>;
using BiasedDeltaInt64ValueStream = HeaderStream<BiasedDeltaInt64>;

using BiasedDeltaInt64Stream = HeaderDeltaStream<BiasedDeltaInt64>;

} // namespace lt::slipstream

using namespace lt::slipstream;
using namespace lt::slipstream::testing;

namespace rc {

template <>
struct Arbitrary<SerialInt64> {
    static Gen<SerialInt64> arbitrary() {
        return gen::construct<SerialInt64>(gen::arbitrary<int64_t>());
    }
};

} // namespace rc
BOOST_AUTO_TEST_CASE(biased_int64_stream_roundtrip_rw)
{
    int64_t header = 100;
    auto rt = ChannelRoundtrip<BiasedInt64Stream>(header);

    BOOST_CHECK(rt.roundtrip(SerialInt64(0)));
    BOOST_CHECK(rt.roundtrip(SerialInt64(77)));
    BOOST_CHECK(rt.roundtrip(SerialInt64(13)));
}

RC_BOOST_PROP(biased_int64_stream_roundtrip_rw_rc, (SerialInt64 header))
{
    RC_ChannelRoundtrip<BiasedInt64Stream>::run(header);
}

RC_BOOST_PROP(biased_delta_int64_value_stream_roundtrip_rw_rc, (SerialInt64 header))
{
    RC_ChannelRoundtrip<BiasedDeltaInt64ValueStream>::run(header);
}

RC_BOOST_PROP(biased_delta_int64_stream_roundtrip_rw_rc, (SerialInt64 header))
{
    RC_ChannelRoundtrip<BiasedDeltaInt64Stream>::run(header);
}

BOOST_AUTO_TEST_CASE(multichannel_biased_int64_stream_roundtrip_rw)
{
    MultiChannelWriter<BiasedInt64Stream>::header_map headers;
    headers.emplace_back("s1", SerialInt64(100));
    headers.emplace_back("s2", SerialInt64(1000));
    auto rt = MultiChannelRoundtrip<BiasedInt64Stream>(headers);

    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(0)));
    BOOST_CHECK(rt.roundtrip("s2", SerialInt64(77)));
    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(13)));
    BOOST_CHECK(rt.roundtrip("s2", SerialInt64(13)));
}

RC_BOOST_PROP(multichannel_biased_int64_stream_roundtrip_rw_rc, ())
{
    MultiChannelWriter<BiasedInt64Stream>::header_map headers;
    headers.emplace_back("s1", SerialInt64(100));
    headers.emplace_back("s2", SerialInt64(1000));
    RC_MultiChannelRoundtrip<BiasedInt64Stream>::run(headers);
}

RC_BOOST_PROP(multichannel_biased_delta_int64_value_stream_roundtrip_rw_rc, ())
{
    MultiChannelWriter<BiasedInt64Stream>::header_map headers;
    headers.emplace_back("s1", SerialInt64(100));
    headers.emplace_back("s2", SerialInt64(1000));
    RC_MultiChannelRoundtrip<BiasedDeltaInt64ValueStream>::run(headers);
}

BOOST_AUTO_TEST_CASE(multichannel_biased_delta_int64_stream_roundtrip_rw)
{
    MultiChannelWriter<BiasedDeltaInt64Stream>::header_map headers;
    headers.emplace_back("s1", SerialInt64(100));
    headers.emplace_back("s2", SerialInt64(1000));
    auto rt = MultiChannelRoundtrip<BiasedDeltaInt64Stream>(headers);

    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(0)));
    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(0)));
    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(0)));
    BOOST_CHECK(rt.roundtrip("s2", SerialInt64(77)));
    BOOST_CHECK(rt.roundtrip("s1", SerialInt64(13)));
    BOOST_CHECK(rt.roundtrip("s2", SerialInt64(13)));
}

RC_BOOST_PROP(multichannel_biased_delta_int64_stream_roundtrip_rw_rc, ())
{
    MultiChannelWriter<BiasedDeltaInt64Stream>::header_map headers;
    headers.emplace_back("s1", SerialInt64(100));
    headers.emplace_back("s2", SerialInt64(1000));
    RC_MultiChannelRoundtrip<BiasedDeltaInt64Stream>::run(headers);
}


/// @file Eigen.hpp
/// @todo convert to use template metaprgramming to define simple functions that simplify and automate the necessary conversions
#ifndef _VREP_EIGEN_HPP_
#define _VREP_EIGEN_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <array>
#include "v_repLib.h"

namespace vrepquat {
    enum vrep_quat {
       x,
       y,
       z,
       w
    };
}

template<typename InputIterator>
Eigen::Quaterniond vrepToEigenQuaternion(InputIterator vrepQuat){

  //                  0123
  //  vrep is ordered xyzw,
  // eigen is ordered wxyz

    Eigen::Quaterniond quat(vrepQuat[vrepquat::w],vrepQuat[vrepquat::x],vrepQuat[vrepquat::y],vrepQuat[vrepquat::z]);
    return quat;
}

template<typename Q>
std::array<float,4> EigenToVrepQuaternion(const Q& q){
  std::array<float,4> qa;

  //                  0123
  //  vrep is ordered xyzw,
  // eigen is ordered wxyz
  qa[vrepquat::x] = q.x();
  qa[vrepquat::y] = q.y();
  qa[vrepquat::z] = q.z();
  qa[vrepquat::w] = q.w();

  return qa;
}

template<typename P>
std::array<float,3> EigenToVrepPosition(const P& p){
   std::array<float,3> qv;
   qv[0] = p.x();
   qv[1] = p.y();
   qv[2] = p.z();

   return qv;
}

template<typename Input>
Eigen::Vector3d vrepQuatToEigenVector3dAngleAxis(Input vrepQuat){
    Eigen::AngleAxisd ax3d(vrepToEigenQuaternion(vrepQuat));
    return ax3d.angle()*ax3d.axis();
}

template<typename Input>
Eigen::Vector3d eigenRotToEigenVector3dAngleAxis(Input eigenQuat){
    Eigen::AngleAxisd ax3d(eigenQuat);
    return ax3d.angle()*ax3d.axis();
}

template<typename InputIterator>
Eigen::Vector3d vrepToEigenVector3d(InputIterator vrepVec){
    // vrep is ordered xyzw, eigen is ordered wxyz
    Eigen::Vector3d vec(vrepVec[0],vrepVec[1],vrepVec[2]);
    return vec;
}

/// @see http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetJointMatrix.htm
Eigen::Affine3d vrepToEigenTransform(const std::array<float,12>& vrepTransform)
{

  // based on comparing each of the following:
  // http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetJointMatrix.htm
  // https://en.wikipedia.org/wiki/Row-major_order
  // https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html

  // V-REP is by default Row-major order
  // Eigen is by default Column Major order
  Eigen::Map<const Eigen::Matrix<float,3,4,Eigen::RowMajor>> vmap(vrepTransform.cbegin());
  Eigen::Affine3d eigenTransform;
  eigenTransform.translation() = vmap.block<3,1>(0,3).cast<double>();
  eigenTransform.matrix().block<3,3>(0,0) = vmap.block<3,3>(0,0).cast<double>();

  return eigenTransform;
}

std::pair<Eigen::Vector3d,Eigen::Vector3d> getAxisAngleAndTranslation(int ObjectHandle, int BaseFrameObjectHandle){

	std::array<float,3> simTipPosition;
	//std::array<float,3> simTipOrientation;
    std::array<float,4> simTipQuaternion;

	int ret = simGetObjectPosition(ObjectHandle, BaseFrameObjectHandle, simTipPosition.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get position"));
    ret = simGetObjectQuaternion(ObjectHandle, BaseFrameObjectHandle, simTipQuaternion.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get quaternion"));

    auto simTipAngleAxis = vrepQuatToEigenVector3dAngleAxis(simTipQuaternion.begin());
    auto simTipVec =       vrepToEigenVector3d(simTipPosition.begin());

    return std::make_pair(simTipAngleAxis,simTipVec);
}

template<typename T>
void setObjectTransform(int objectHandle, int relativeToObjectHandle, T& transform){

   // get quaternion between end effector and
   Eigen::Quaternion<typename T::Scalar> eigenQuat(transform.rotation());
   std::array<float,4> vrepQuat = EigenToVrepQuaternion(eigenQuat);
   int ret = simSetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin());
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion"));

   std::array<float,3> vrepPos = EigenToVrepPosition(transform.translation());
   ret = simSetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin());
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion"));

}

/// @param objectHandle The V-Rep object handle for which the transform is needed
/// @param relativeToObjectHandle The frame of reference in which to get the object handle, -1 (the default) gets the absolute position aka world frame
/// @see http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetObjectPosition.htm
/// @see http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetObjectQuaternion.htm
Eigen::Affine3d getObjectTransform(int objectHandle, int relativeToObjectHandle = -1){

   std::array<float,4> vrepQuat;
   int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin());
   Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat));
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Quaternion"));

   std::array<float,3> vrepPos;
   ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin());
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Position"));
   Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos));

   Eigen::Affine3d transform;

   transform = eigenQuat;
   transform.translation() = eigenPos;

   return transform;
}

/// @param objectHandle The V-Rep object handle for which the transform is needed
/// @param relativeToObjectHandle The frame of reference in which to get the object handle, -1 (the default) gets the absolute position aka world frame
/// @see http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetObjectPosition.htm
/// @see http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetObjectQuaternion.htm
/// @todo TODO(ahundt) handle errors/return codes from simGetObjectPosition
std::pair<Eigen::Quaterniond,Eigen::Vector3d> getObjectTransformQuaternionTranslationPair(int objectHandle, int relativeToObjectHandle = -1){

   std::array<float,4> vrepQuat;
   int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin());
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Quaternion"));
   Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat));

   std::array<float,3> vrepPos;
   ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin());
   if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Position"));
   Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos));

   return std::make_pair(eigenQuat,eigenPos);
}

/// Eigen version of simGetJointMatrix
Eigen::Affine3d getJointTransform(int objectHandle)
{
  // based on comparing each of the following:
  // http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetJointMatrix.htm
  // https://en.wikipedia.org/wiki/Row-major_order
  // https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html
  std::array<float,12> vrepTransform;
  int ret = simGetJointMatrix(objectHandle,vrepTransform.begin());
  if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getJointTransform: Could not get Joint Matrix"));
  return vrepToEigenTransform(vrepTransform);
}

template<typename T>
static std::string poseString(const Eigen::Transform<T,3,Eigen::Affine>& pose, const std::string& pfx = "")
{
  std::stringstream ss;
  ss.precision(3);
  for (int y=0;y<4;y++)
  {
    ss << pfx;
    for (int x=0;x<4;x++)
    {
      ss << std::setw(8) << pose(y,x) << " ";
    }
    ss << std::endl;
  }
  return ss.str();
}

//---------
//posString
//---------
static std::string posString(const Eigen::Vector3d& pos, const std::string& pfx = "(", const std::string& sfx = ")")
{
  std::stringstream ss;
  ss.precision(3);
  ss << pfx;
  for (int x=0;x<3;x++)
  {
    if (x)
      ss << ", ";
    ss << std::setw(8) << pos(x);
  }
  ss << sfx;
  return ss.str();
}

#endif