#include <Eigen/Core>
#include <mpi.h> // has to be before plasma
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#ifdef USE_PLASMA
#include "coreblas_.h" // plasma
#endif
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>
#include <unordered_map>

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

// #define DISP

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;
typedef array<int, 6> int6;
typedef array<int, 7> int7;

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

struct hashfunc{
    size_t operator() (const int2 &i) const{
        return hash<int>{}(i[0]) ^ hash<int>{}(i[1]);
    }
};

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

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

    int origin = 0; // Not necessary -- origin is always 0
    int nb = min(32, n); // inner blocking size

    // cout << t << endl;

    // Warmup MKL
    warmup_mkl(n_threads);

    // Mapper
    auto 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 % 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;
    };

    auto block2thread = [&](int2 ij, int3 pqt){
        int i = ij[0];
        int j = ij[1];
        int p = pqt[0];
        int q = pqt[1];
        int t = pqt[2];

        int ii = i / p;
        int jj = j / q;

        int num_blocksit = M/p;
        int r = (ii + jj * num_blocksit) % n_threads;
        return r;
    };

    VectorXd x;
    VectorXd b;
    VectorXd bref;

    // Block the matrix for every node
    vector<MatrixXd> Mat(M*N, MatrixXd::Zero(0,0));
    int tri_size = (N*(2*M-N+1))/2;
    vector<MatrixXd> TS(tri_size, MatrixXd::Zero(0,0)); // To store triangular factors obtained from TSQRT
    vector<MatrixXd> TT(tri_size, MatrixXd::Zero(0,0)); // To store triangular factors obtained from TTQRT

    #ifdef USE_PLASMA
    vector<VectorXd> tau(tri_size, VectorXd::Zero(0)); // If PLASMA is used
    vector<VectorXd> work(M*N, VectorXd::Zero(0)); // If PLASMA is used
    #endif

    MatrixXd A = MatrixXd::Zero(0,0);
    auto val = [&](int i, int j) { return 1/(double)((i-j)*(i-j)+1); };
    auto val_vec = [&](int i) {return 1/(double)(i*i+1);};

    if (TEST){
        A = MatrixXd::NullaryExpr(M*n,N*n, val);

        if(rank == 0) {
            x = VectorXd::NullaryExpr(N*n, val_vec);
            b = A*x;
            bref = b;
            if (VERB){
                cout << A << endl;
                cout << b << endl;
            }
        }
    }

    // Setup and factorize
    {
        // Setup the matrix blocks and workspace
        {
            Communicator comm(MPI_COMM_WORLD, VERB);

            // Threadpool
            Threadpool tp(n_threads, &comm, VERB, "[" + to_string(rank) + "]_");
            Taskflow<int2> setup_tf(&tp, VERB); // Set up the matrix block {i,j} and the workspace required

            setup_tf.set_mapping([&] (int2 ij){
                    return block2thread(ij,{p,q,t}); // should be the same mapping as dgeqrt and dtsqrt
                })
                .set_indegree([](int2){
                    return 1;
                })
                .set_task([&] (int2 ij) {
                    int i=ij[0];
                    int j=ij[1];
                    auto val_loc = [&](int ii, int jj) {return val(i+ii, j+jj);};
                    if (TEST) Mat.at(i+j*M) = A.block(i*n, j*n, n, n);
                    else Mat.at(i+j*M) = MatrixXd::NullaryExpr(n,n,val_loc);
                    if (i>=j) {
                        TS.at(i-(j*(j+1))/2+j*M) = MatrixXd::Zero(n,n);
                        TT.at(i-(j*(j+1))/2+j*M) = MatrixXd::Zero(0,0);

                        #ifdef USE_PLASMA
                            tau.at(i-(j*(j+1))/2+j*M) = VectorXd::Zero(n);
                        #endif
                    }
                    #ifdef USE_PLASMA
                        work.at(i+j*M) = VectorXd::Zero(n*nb);
                    #endif
                })
                .set_name([](int2 ij) {
                    return "setup_" + to_string(ij[0]) + "_" +to_string(ij[1]);
                })
                .set_priority([&](int2) {
                    return 6;
                });

            MPI_Barrier(MPI_COMM_WORLD);
            timer t0 = wctime();
            for (int j=0; j<N; ++j){
                for (int i=0; i<M; ++i){
                    if(block2rank({i,j}, {p,q,t}, origin) == rank) {
                        setup_tf.fulfill_promise({i,j});
                    }
                }
            }
            tp.join();
            timer t1 = wctime();
            MPI_Barrier(MPI_COMM_WORLD);
            if(VERB && rank == 0)
            {
                cout << "Time to setup: " << elapsed(t0, t1) << endl;
            }
        }

        // Initialize the communicator structure
        Communicator comm(MPI_COMM_WORLD, VERB);

        // Threadpool
        Threadpool tp(n_threads, &comm, VERB, "[" + to_string(rank) + "]_");
        Taskflow<int2> dgeqrt_tf(&tp, VERB);  // A[k,k] = QR
        Taskflow<int3> dtsqrt_tf(&tp, VERB);
        Taskflow<int3> dlarfb_tf(&tp, VERB);
        Taskflow<int4> dssrfb_tf(&tp, VERB);
        Taskflow<int6> dttqrt_tf(&tp, VERB);
        Taskflow<int7> dttssmqr_tf(&tp, VERB);

        // Log
        DepsLogger dlog(1000000);
        Logger log(1000000);
        if(LOG) {
            tp.set_logger(&log);
            comm.set_logger(&log);
        }

        // Active messages
        // From dgeqrt
        auto am_dgeqrt_2_dlarfb = comm.make_active_msg(
            [&](view<double> &V, view<double> &tau, view<int>& js, int& i, int& j){
                if (Mat.at(i+j*M).rows()>0){
                    Mat.at(i+j*M).triangularView<StrictlyLower>() = Map<MatrixXd>(V.data(), n, n).triangularView<StrictlyLower>();
                }
                else {
                    Mat.at(i+j*M)=Map<MatrixXd>(V.data(), n, n).triangularView<StrictlyLower>();
                }
                TS.at(i-(j*(j+1))/2+j*M) = Map<MatrixXd>(tau.data(), n, n);
                for (auto& jinc: js){
                    dlarfb_tf.fulfill_promise({i,j,jinc});
                }
            }
        );

        /* Can come from dgeqrt or dtsqrt */
        auto firstMerge = [&](int i, int j){
            int domain_index = i/t; // adjust for which column is being operated on
            int domain_start = max(j,domain_index*t);

            int it = (i-domain_start)/p; // to adjust for which column in which domain is being operated on
            int itp = (i-domain_start)%p;
            int stride = n_threads*p;

            if (it%2 == 1){ // Merge within the node -- definitely possible since there should be an "i" s.t it=0
                int iprev = i-p;
                assert(block2rank({iprev,j},{p,q,t},origin)==rank);
                dttqrt_tf.fulfill_promise({j,iprev,i,1,0,0}); // first merge
            }
            else { // Wait here -- merge within a node //if (i+p<j+stride)
                assert(it%2==0);
                dttqrt_tf.fulfill_promise({j,i,i+p,1,0,0});
            }
        };

        dgeqrt_tf.set_mapping([&] (int2 ij){
                return block2thread(ij,{p,q,t});
            })
            .set_indegree([](int2){
                return 1;
            })
            .set_task([&] (int2 ij) {
                int i = ij[0];
                int j = ij[1];

                #ifdef USE_PLASMA
                int info = CORE_dgeqrt(n, n, nb,
                                       Mat.at(i+j*M).data(), n,
                                       TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                       tau.at(i-(j*(j+1))/2+j*M).data(),
                                       work.at(i+j*M).data());
                #else
                int info = LAPACKE_dgeqrt(LAPACK_COL_MAJOR, n, n, n, // don't use nb here because of dlarfb next step
                                          Mat.at(i+j*M).data(), n,
                                          TS.at(i-(j*(j+1))/2+j*M).data(), n);
                #endif
                assert(info == 0);

                #ifdef DISP
                    cout << "geqrt_" + to_string(ij[0]) + "_" +to_string(ij[1]) << endl;
                #endif
            })
            .set_fulfill([&](int2 ij) {
                int i = ij[0];
                int j = ij[1];
                // Dependencies -- dlarfb
                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 domain_index=i/t;
                int domain_end=min(domain_index*t+t,M);

                int stride = n_threads*p;
                if (i+stride<domain_end){ // First dtsqrt will be at +stride distance
                    int r = block2rank({i+stride,j},{p,q,t},origin); // i+stride will always be in the same rank
                    assert(r == rank);
                    dtsqrt_tf.fulfill_promise({i, i+stride, j}); // root row = i
                }
                else  { // merge
                    firstMerge(i,j);
                }

                // 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({i, j, jinc});
                        }
                    }
                    else {
                        auto V_ij = view<double>(Mat.at(i+j*M).data(), n * n );
                        auto T_ij = view<double>(TS.at(i-(j*(j+1))/2+j*M).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);
                    }
                }

            })
            .set_name([](int2 ij) {
                return "geqrt_" + to_string(ij[0]) + "_" +to_string(ij[1]);
            })
            .set_priority([&](int2 ij) {
                return 5+ (N-ij[1]); // Include column index in the priority
            });

        // From dtsqrt
        auto 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){
                Mat.at(i+j*M)=Map<MatrixXd>(V_ij.data(), n, n);
                TS.at(i-(j*(j+1))/2+j*M) = Map<MatrixXd>(T_ij.data(), n, n);
                for(auto& jinc: js){
                    dssrfb_tf.fulfill_promise({root, j, i, jinc});
                }
            }
        );

        dtsqrt_tf.set_mapping([&] (int3 rij){
               return block2thread({rij[1],rij[2]},{p,q,t});
            })
            .set_indegree([](int3 rij){
                if (rij[2]==0) return 1; // Tiles in column 0
                return 2;
            })
            .set_task([&] (int3 rij) {
                int root = rij[0]; // root row -- which row is doing the dgeqrt
                int i = rij[1]; // row to do dtsqrt
                int j = rij[2]; // col to do dtsqrt
                #ifdef USE_PLASMA
                int info = CORE_dtsqrt(n, n, nb,
                                       Mat.at(root+j*M).data(), n,
                                       Mat.at(i+j*M).data(), n,
                                       TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                       tau.at(i-(j*(j+1))/2+j*M).data(),
                                       work.at(i+j*M).data());
                #else
                int info = LAPACKE_dtpqrt(LAPACK_COL_MAJOR, n, n, 0, nb,
                                         Mat.at(root+j*M).data(), n,
                                         Mat.at(i+j*M).data(), n,
                                         TS.at(i-(j*(j+1))/2+j*M).data(), n);
                assert(info == 0);
                #endif
                #ifdef DISP
                    cout << "tsqrt_" + to_string(rij[0]) + "_" +
                            to_string(rij[1]) + "_" +to_string(rij[2]) << endl;
                #endif
            })
            .set_fulfill([&](int3 rij) {
                int root = rij[0]; // root row -- which row is doing the dgeqrt
                int i = rij[1]; // row to do dtsqrt
                int j = rij[2]; // col to do dtsqrt
                // Dependencies
                map<int, vector<int>> to_fulfill;

                int domain_index=i/t;
                int domain_end=min(domain_index*t+t,M);

                int stride = n_threads*p;
                if (i+stride<domain_end){
                    int r = block2rank({i+stride,j},{p,q,t},origin);
                    assert(r == rank);
                    dtsqrt_tf.fulfill_promise({root, i+stride, j}); // Next dtsqrt
                }
                else {// last dtsqrt -- trigger merging
                    firstMerge(root,j);
                }

                // 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({root, j, i, jinc});
                        }
                    }
                    else {
                        auto V_ij = view<double>(Mat.at(i+j*M).data(), n * n );
                        auto T_ij = view<double>(TS.at(i-(j*(j+1))/2+j*M).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);
                    }
                }

            })
            .set_name([](int3 rij) {
                return "dtsqrt_" + to_string(rij[0]) + "_" +to_string(rij[1]) + "_" +to_string(rij[2]);
            })
            .set_priority([&](int3 rij) {
                return 4+(N-rij[2]);
            });

        /* Can come from dgeqrt or dtsqrt */
        auto firstMergeUpdate = [&](int i, int j, int k){
            int domain_index = i/t;
            int domain_start = max(k,domain_index*t); // adjust for which column is being operated on

            // Which dttssmqr to trigger?
            int it = (i-domain_start)/p;
            int itp = (i-domain_start)%p;
            int stride = n_threads*p;

            if (it%2 == 1){// Merge update within a node
                int r = block2rank({i-p,j}, {p,q,t},origin);
                assert(r == rank);
                dttssmqr_tf.fulfill_promise({k, i-p,i, j, 1,0,0});
            }
            else  {// wait here -- merge update within a node //if (i+p<k+stride)
                // i == k will always enter here
                dttssmqr_tf.fulfill_promise({k, i,i+p, j, 1,0,0});
            }
        };

        // larfb
        dlarfb_tf.set_mapping([&] (int3 ikj){
               return block2thread({ikj[0],ikj[2]},{p,q,t});
            })
            .set_indegree([](int3 ikj){
                if (ikj[1]==0) return 1; // Because of dgeqrt on column 0
                return 2;
            })
            .set_task([&] (int3 ikj) {
                int i=ikj[0]; // who is sending - row
                int k=ikj[1]; // who is sending - col
                int j=ikj[2]; // me - col, my row == i
                #ifdef USE_PLASMA
                int info = CORE_dormqr(PlasmaLeft,PlasmaTrans,n,n,n, nb,
                                        Mat.at(i+k*M).data(), n,
                                        TS.at(i-(k*(k+1))/2+k*M).data(), n,
                                        Mat.at(i+j*M).data(), n,
                                        work.at(i+j*M).data(), n);
                #else
                MatrixXd Vtemp = Mat.at(i+k*M); // Copying it because the diagonal entries are modified (but restored upon exit)
                // Not copying might cause race conditions with the dttqrt opn received from the root col
                int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'T', 'F', 'C', n, n, n,
                                             Vtemp.data(), n,
                                             TS.at(i-(k*(k+1))/2+k*M).data(), n,
                                             Mat.at(i+j*M).data(), n);
                assert(info == 0);
                #endif

                #ifdef DISP
                cout << "dlarfb_" + to_string(ikj[0]) + "_" +to_string(ikj[1])  + "_" +to_string(ikj[2]) << endl;
                #endif
            })
            .set_fulfill([&](int3 ikj) {
                int i=ikj[0]; // who is sending - row
                int k=ikj[1]; // who is sending - col
                int j=ikj[2]; // me - col, my row == i

                int domain_index=i/t;
                int domain_end=min(domain_index*t+t,M);

                int stride = n_threads*p;
                if (i+stride<domain_end){ // and next row in the same tile
                    int r = block2rank({i+stride, j},{p,q,t},origin);
                    assert(r == rank);
                    dssrfb_tf.fulfill_promise({i, k, i+stride, j});
                }
                else  { // trigger merging
                    firstMergeUpdate(i,j,k);
                }
            })
            .set_name([](int3 ikj) {
                return "dlarfb_" + to_string(ikj[0]) + "_" +to_string(ikj[1])  + "_" +to_string(ikj[2]);
            })
            .set_priority([&](int3 ikj) {
                return 3+(N-ikj[1]);
            });

        dssrfb_tf.set_mapping([&] (int4 rjik){
               return block2thread({rjik[2],rjik[3]},{p,q,t});
            })
            .set_indegree([](int4 rjik){
                if (rjik[1]==0) return 2; // Because of dgeqrt and dtsqrt in column 0
                return 3;
            })
            .set_task([&] (int4 rjik) {
                int root = rjik[0];
                int j = rjik[1];
                int i = rjik[2];
                int jinc = rjik[3];
                #ifdef USE_PLASMA
                int info = CORE_dtsmqr(PlasmaLeft,PlasmaTrans,n,n,n,n,n,nb,
                                        Mat.at(root+jinc*M).data(), n,
                                        Mat.at(i+jinc*M).data(), n,
                                        Mat.at(i+j*M).data(), n,
                                        TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                        work.at(i+jinc*M).data(), nb);
                #else
                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, n, n, 0, nb,
                                           Mat.at(i+j*M).data(), n,
                                           TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                           Mat.at(root+jinc*M).data(), n,
                                           Mat.at(i+jinc*M).data(), n);
                #endif
                assert(info == 0);
                #ifdef DISP
                cout << "dssrfb_" + to_string(rjik[0]) + "_" +to_string(rjik[1]) + "_"
                        + to_string(rjik[2]) + "_" +to_string(rjik[3]) << endl;
                #endif

            })
            .set_fulfill([&](int4 rjik) {
                int root = rjik[0];
                int j = rjik[1];
                int i = rjik[2];
                int jinc = rjik[3];

                int stride = n_threads*p;
                int domain_index=i/t;
                int domain_end=min(domain_index*t+t,M);

                if (i+stride<domain_end){
                    int r = block2rank({i+stride,jinc},{p,q,t},origin);
                    assert(r == rank);
                    dssrfb_tf.fulfill_promise({root, j, i+stride, jinc});
                }
                else {
                    firstMergeUpdate(root,jinc,j);
                }

                int jnext = j+1;
                int domain_index_next = i/t;
                int domain_start_next = max(jnext, domain_index*t);
                // same rank
                if (i<stride+domain_start_next){
                    if (jinc==jnext){
                        dgeqrt_tf.fulfill_promise({i, jinc});
                    }
                    else {
                        dlarfb_tf.fulfill_promise({i, jnext, jinc});
                    }
                }
                else {
                    int root_next = (i-domain_start_next)%stride+domain_start_next;
                    if (jinc==jnext){
                        dtsqrt_tf.fulfill_promise({root_next, i, jinc});
                    }
                    else {
                        dssrfb_tf.fulfill_promise({root_next, jnext, i, jinc});
                    }
                }
            })
            .set_name([](int4 rjik) {
                return "dssrfb_" + to_string(rjik[0]) + "_" +to_string(rjik[1]) + "_"
                        + to_string(rjik[2]) + "_" +to_string(rjik[3]);
            })
            .set_priority([&](int4 rjik) {
                return 2+(N-rjik[1]);
            });

        // From dttqrt
        auto am_dttqrt_2_dttqrt = comm.make_active_msg(
            [&](view<double> &R, int& j, int& k, int& m, int& l, int& lnode, int& ldomain){
                if (Mat.at(m+j*M).rows() > 0){
                    Mat.at(m+j*M).triangularView<Upper>() = Map<MatrixXd>(R.data(), n, n).triangularView<Upper>();
                }
                else {
                    Mat.at(m+j*M) = Map<MatrixXd>(R.data(), n, n).triangularView<Upper>();
                }
                // TT.at(m-(j*(j+1))/2+j*M) = MatrixXd::Zero(n,n); // Needed if TT is not resized before execution of dttqrt kernel
                dttqrt_tf.fulfill_promise({j, k, m, l, lnode,ldomain});
            }
        );

        // auto am_dttqrt_2_dttqrt_large = comm.make_large_active_msg(
        //     [&](int& j, int& k, int& m, int& l, int& lnode, int& ldomain) {
        //         dttqrt_tf.fulfill_promise({j, k, m, l, lnode,ldomain});
        //     },
        //     [&](int& j, int& k, int& m, int& l, int& lnode, int& ldomain) {
        //         if (Mat.at(m+j*M).rows() > 0){
        //         }
        //         else {
        //             Mat.at(m+j*M).resize(n,n);
        //         }
        //         return Mat.at(m+j*M).triangularView<Upper>();
        //     },
        //     [&](int& j, int& k, int& m, int& l, int& lnode, int& ldomain){
        //         return;
        //     });

        auto am_dttqrt_2_dttssmqr = comm.make_active_msg(
            [&](view<double> &Mat_mj, view<double> &TT_mj, int& j, int& k, int& m, view<int>& js, int& l,
                int& lnode, int& ldomain){
                if (Mat.at(m+j*M).rows() > 0){
                    Mat.at(m+j*M).triangularView<Upper>() = Map<MatrixXd>(Mat_mj.data(), n, n).triangularView<Upper>();
                }
                else {
                    Mat.at(m+j*M) = Map<MatrixXd>(Mat_mj.data(), n, n).triangularView<Upper>();
                }
                TT.at(m-(j*(j+1))/2+j*M) = Map<MatrixXd>(TT_mj.data(), n, n);
                for(auto& jinc: js){
                    dttssmqr_tf.fulfill_promise({j,k,m,jinc,l,lnode,ldomain});
                }
            }
        );

        // Only needed to gather back on the same node
        auto am_dttqrt = comm.make_active_msg(
            [&](view<double> &Mat_mj, view<double> &TT_mj, int& m, int& j){
                if (Mat.at(m+j*M).rows() > 0){
                    Mat.at(m+j*M).triangularView<Upper>() = Map<MatrixXd>(Mat_mj.data(), n, n).triangularView<Upper>();
                }
                else {
                    Mat.at(m+j*M) = Map<MatrixXd>(Mat_mj.data(), n, n).triangularView<Upper>();
                }
                TT.at(m-(j*(j+1))/2+j*M) = Map<MatrixXd>(TT_mj.data(), n, n);
            }
        );

        dttqrt_tf.set_mapping([&] (int6 jkmlod){
                return block2thread({jkmlod[1],jkmlod[0]},{p,q,t});
            })
            .set_indegree([&](int6 jkmlod){
                int j = jkmlod[0]; // To identify colum j
                int k = jkmlod[1]; // To identify block R_{kj}
                int m = jkmlod[2]; // To identify block R_{mj}
                int l = jkmlod[3]; // Level of merging within a node
                int lnode = jkmlod[4]; // Level of merging between nodes
                int ldomain = jkmlod[5]; // Level of merging between nodes

                int stride = n_threads*p;
                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);

                // Check with valid merge within a node or between nodes or between domains
                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;

                if (run) return 2;
                return 1;
            })
            .set_task([&] (int6 jkmlod) {
                int j = jkmlod[0]; // To identify colum j
                int k = jkmlod[1]; // To identify block R_{kj}
                int m = jkmlod[2]; // To identify block R_{mj}
                int l = jkmlod[3]; // Level of merging within a node
                int lnode = jkmlod[4]; // Level of merging between nodes
                int ldomain = jkmlod[5]; // Level of merging between domains

                assert(k>=j); // Otherwise no TT[{k,j}] has been assigned
                assert(m>=j); // Otherwise no TT[{m,j}] has been assigned
                int stride = n_threads*p;
                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);

                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;

                if (run){
                    TT.at(m-(j*(j+1))/2+j*M) = MatrixXd::Zero(n,n);
                    #ifdef USE_PLASMA
                    int info = CORE_dttqrt(n, n, nb, Mat.at(k+j*M).data(), n,
                                           Mat.at(m+j*M).data(), n,
                                           TT.at(m-(j*(j+1))/2+j*M).data(), n,
                                           tau.at(k-(j*(j+1))/2+j*M).data(), // use (k,j) space because this is on rank block2rank(k,j)
                                           work.at(k+j*M).data()); // use (k,j) because the thread doing this task is block2thread(k,j)
                    #else
                    int info = LAPACKE_dtpqrt(LAPACK_COL_MAJOR, n, n, n, nb,
                                              Mat.at(k+j*M).data(), n,
                                              Mat.at(m+j*M).data(), n,
                                              TT.at(m-(j*(j+1))/2+j*M).data(), n);
                    #endif
                    assert(info == 0);

                #ifdef DISP
                cout << "ttqrt_" +to_string(jkmlod[0]) + "_" +to_string(jkmlod[1])
                        + "_" +to_string(jkmlod[2]) + "_" +to_string(jkmlod[3]) + "_" +to_string(jkmlod[4])
                        + "_" +to_string(jkmlod[5]) << endl;
                #endif
                }
            })
            .set_fulfill([&](int6 jkmlod) {
                int j = jkmlod[0]; // To identify colum j
                int k = jkmlod[1]; // To identify block R_{kj}
                int m = jkmlod[2]; // To identify block R_{mj}
                int l = jkmlod[3]; // Level of merging within a node
                int lnode = jkmlod[4]; // Level of merging between nodes
                int ldomain = jkmlod[5]; // Level of merging between domains

                // Next dttqrt
                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);
                int domain_size=domain_end-domain_start;
                int rank_start = block2rank({domain_start,j},{p,q,t},origin);

                int domain_left_rank = domain_size/p;
                domain_left_rank = (rank-rank_start)<(domain_size%p)? domain_left_rank:domain_left_rank+1;

                double l_limit_nodes = log2(min(p,domain_size));
                double l_limit_threads = log2(min(n_threads,domain_left_rank));
                double l_limit_domains = M/t-j/t-1;
                int kt = (k-domain_start)/p;
                int ktp = (k-domain_start)%p;

                if (l < l_limit_threads){ // Merge within a node
                    if ((int)(kt/pow(2,l))%2 == 1){
                        int knext = k-pow(2,l)*p;
                        int r =  block2rank({knext,j},{p,q,t},origin);
                        assert(r==rank);
                        dttqrt_tf.fulfill_promise({j,knext,k,l+1,lnode,ldomain});
                    }
                    else {
                        // Merge with next and wait for info -- happens on the same rank
                        int mnext = k+pow(2,l)*p;
                        dttqrt_tf.fulfill_promise({j,k,mnext,l+1,lnode,ldomain});
                    }
                }
                else if (lnode < l_limit_nodes){// Merge between nodes
                    assert(k-domain_start<p);
                    // once we enter here index "l" is not needed for future runs
                    // to get uniformity between different nodes and domains set l to a high value = M
                    int lt_next = M;
                    if ((int)(ktp/pow(2,lnode))%2==1){
                        int knext = k-pow(2,lnode);
                        int r = block2rank({knext,j},{p,q,t},origin);
                        assert(r!=rank);
                        int lnext = lnode+1;
                        // int R_kj_size = n*(n+1)/2;
                        auto R_kj = view<double>(Mat.at(k+j*M).data(), n*n);

                        am_dttqrt_2_dttqrt->send(r, R_kj,j,knext,k,lt_next,lnext,ldomain);
                        // am_dttqrt_2_dttqrt_large->send_large(r, R_kj,j,knext,k,lt_next,lnext,ldomain);
                    }
                    else {
                        // Merge with next and wait for info
                        int mnext = k+pow(2,lnode);
                        dttqrt_tf.fulfill_promise({j,k,mnext,lt_next,lnode+1,ldomain});
                    }
                }
                else if (ldomain < l_limit_domains) { // Only after merging within and between nodes in a domain are done
                    // Similarly l and lnode are not needed anymore
                    int lt_next = M;
                    int ln_next = M;

                    // Flat tree to merge between domains
                    if (ldomain>0 && k==j && m%t==0){
                        // Atleast one step of merging between domains has been done
                        int mnext = (m/t)*t+t; // First tile of next domain
                        if (mnext<M){
                            dttqrt_tf.fulfill_promise({j,k,mnext,lt_next,ln_next,ldomain+1}); // on the same rank
                        }
                    }
                    else if (k==j){
                        // First domain and start the fist step of merging between domains
                        int mnext = (k/t)*t+t;
                        if (mnext<M){
                            dttqrt_tf.fulfill_promise({j,k,mnext,lt_next,ln_next,ldomain+1}); // on the same rank
                        }
                    }
                    else if (k%t==0){
                        // Not the first domain and trigger merging with the first domain
                        int kprev = j;
                        int r = block2rank({kprev,j},{p,q,t},origin);
                        if (r==rank) dttqrt_tf.fulfill_promise({j,kprev,k,lt_next,ln_next,k/t-j/t});
                        else {
                            // auto R_kj = view<double>(Mat.at(k+j*M).data(), n*n);
                            int ldomain_next=k/t-j/t;
                            // int R_kj_size = n*(n+1)/2;
                            auto R_kj = view<double>(Mat.at(k+j*M).data(), n*n);

                            am_dttqrt_2_dttqrt->send(r, R_kj,j,kprev,k,lt_next,ln_next,ldomain_next);

                            // am_dttqrt_2_dttqrt_large->send_large(r, R_kj,j,kprev,k,lt_next,ln_next,ldomain_next);
                        }
                    }
                }

                int stride=n_threads*p;
                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;

                if (run){
                    // Dependencies
                    map<int, vector<int>> to_fulfill;
                    // dttssmqr
                    for(int jinc = j+1; jinc< N; jinc++) { // everything in the same row m
                        // Which node to send Mat_{mj} and TT_{mj} data to? -- block2rank({k, jinc})
                        int r = block2rank({k,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({j, k, m, jinc, l, lnode,ldomain});
                            }
                        }
                        else {
                            auto Mat_mj = view<double>(Mat.at(m+j*M).data(), n*n );
                            auto TT_mj = view<double>(TT.at(m-(j*(j+1))/2+j*M).data(), n*n);
                            auto jsv = view<int>(p.second.data(), p.second.size());
                            am_dttqrt_2_dttssmqr->send(r, Mat_mj, TT_mj, j, k, m, jsv, l, lnode, ldomain);
                        }
                    }

                    // send a message to the rank of (m,j) so that it is easy to gather them on one node
                    int rank_mj = block2rank({m,j},{p,q,t},origin);
                    if (to_fulfill.count(rank_mj)==0){
                        auto Mat_mj = view<double>(Mat.at(m+j*M).data(), n*n );
                        auto TT_mj = view<double>(TT.at(m-(j*(j+1))/2+j*M).data(), n*n);
                        am_dttqrt->send(rank_mj, Mat_mj, TT_mj,m,j);
                    }
                }
            })
            .set_name([](int6 jkmlod) {
                return "ttqrt_" +to_string(jkmlod[0]) + "_" +to_string(jkmlod[1])
                        + "_" +to_string(jkmlod[2]) + "_" +to_string(jkmlod[3]) + "_" +to_string(jkmlod[4])
                        + "_" +to_string(jkmlod[5]);
            })
            .set_priority([&](int6 jkmlod) {
                return 3+(N-jkmlod[0]);
            });

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

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

        auto am_dttssmqr_2_dttssmqr = comm.make_active_msg(
            [&](view<double> &R, int& j, int& k, int& m, int& jinc, int& l, int& lnode, int& ldomain){
                Mat.at(m+jinc*M) = Map<MatrixXd>(R.data(), n, n);
                dttssmqr_tf.fulfill_promise({j,k,m,jinc,l, lnode, ldomain});
            }
        );

        dttssmqr_tf.set_mapping([&] (int7 jkmilod){
                return block2thread({jkmilod[1],jkmilod[3]},{p,q,t});
            })
            .set_indegree([&](int7 jkmilod){
                int j = jkmilod[0]; // because of dttqrt between blocks {k, j} and {m, j}
                int k = jkmilod[1]; // row k -- R_{k jinc}
                int m = jkmilod[2]; // row m -- R_{m jinc}
                int jinc = jkmilod[3]; // col jinc
                int l = jkmilod[4]; // level of merging
                int lnode = jkmilod[5]; // Level of merging between nodes
                int ldomain = jkmilod[6]; // Level of merging between nodes

                int stride = n_threads*p;
                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);

                // Check with valid merge within a node or between nodes or between domains
                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;
                if (run) return 3;
                return 1;
            })
            .set_task([&] (int7 jkmilod) {
                int j = jkmilod[0]; // because of dttqrt between blocks {k, j} and {m, j}
                int k = jkmilod[1]; // row k -- R_{k jinc}
                int m = jkmilod[2]; // row m -- R_{m jinc}
                int jinc = jkmilod[3]; // col jinc
                int l = jkmilod[4]; // level of merging
                int lnode = jkmilod[5]; // Level of merging between nodes
                int ldomain = jkmilod[6]; // Level of merging between domains

                int stride = n_threads*p;
                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);

                // Check with valid merge within a node or between nodes or between domains
                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;
                if (run){
                    #ifdef USE_PLASMA
                    int info = CORE_dttmqr(PlasmaLeft,PlasmaTrans,n,n,n,n,n,nb,
                                        Mat.at(k+jinc*M).data(), n,
                                        Mat.at(m+jinc*M).data(), n,
                                        Mat.at(m+j*M).data(), n,
                                        TT.at(m-(j*(j+1))/2+j*M).data(), n,
                                        work.at(k+jinc*M).data(), nb); // use (k,j) because the thread doing this task is block2thread(k,jinc)
                    #else
                    int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, n, n, n, nb,
                                           Mat.at(m+j*M).data(), n,
                                           TT.at(m-(j*(j+1))/2+j*M).data(), n,
                                           Mat.at(k+jinc*M).data(), n, // Receives this from dssrfb or dttssmqr
                                           Mat.at(m+jinc*M).data(), n);
                    #endif
                    assert(info == 0);

                #ifdef DISP
                cout << "ttssmqr_" + to_string(jkmilod[0]) + "_" +to_string(jkmilod[1]) + "_" +
                        to_string(jkmilod[2]) + "_" + to_string(jkmilod[3]) + "_" + to_string(jkmilod[4])
                        + "_" + to_string(jkmilod[5]) + "_" + to_string(jkmilod[6]) << endl;
                #endif
                }
            })
            .set_fulfill([&](int7 jkmilod) {
                int j = jkmilod[0]; // because of dttqrt between blocks {k, j} and {m, j}
                int k = jkmilod[1]; // row k -- R_{k jinc}
                int m = jkmilod[2]; // row m -- R_{m jinc}
                int jinc = jkmilod[3]; // col jinc
                int l = jkmilod[4]; // level of merging
                int lnode = jkmilod[5]; // Level of merging between nodes
                int ldomain = jkmilod[6]; // Level of merging between domains
                // Dependencies
                map<int, vector<int>> to_fulfill;

                int domain_index=k/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);
                int domain_size=domain_end-domain_start;
                int rank_start = block2rank({domain_start,jinc},{p,q,t},origin);

                int domain_left_rank = domain_size/p;
                domain_left_rank = (rank-rank_start)<(domain_size%p)? domain_left_rank:domain_left_rank+1;

                double l_limit_nodes = log2(min(p,domain_size));
                double l_limit_threads = log2(min(n_threads,domain_left_rank));
                int l_limit_domains = M/t-j/t-1;

                int kt = (k-domain_start)/p;
                int ktp = (k-domain_start)%p;

                if (l<l_limit_threads){// Merge update within a node
                    if ((int)(kt/pow(2,l))%2 == 1){
                        int knext = k-pow(2,l)*p;
                        int r =  block2rank({knext,jinc},{p,q,t},origin);
                        assert(r==rank);
                        dttssmqr_tf.fulfill_promise({j,knext,k,jinc,l+1,lnode,ldomain});
                    }
                    else {
                        // Merge with next and wait for info -- happens on the same rank
                        int mnext = k+pow(2,l)*p;
                        dttssmqr_tf.fulfill_promise({j,k,mnext,jinc,l+1,lnode,ldomain});
                    }
                }
                else if (lnode<l_limit_nodes){// Merge update between nodes
                    int lt_next = M;
                    assert(k-domain_start<p);
                    if ((int)(ktp/pow(2,lnode))%2==1){
                        int knext = k-pow(2,lnode);
                        int r = block2rank({knext,jinc},{p,q,t},origin);
                        assert(r!=rank);
                        auto R_kjinc = view<double>(Mat.at(k+jinc*M).data(), n*n);
                        int lnext = lnode+1;
                        am_dttssmqr_2_dttssmqr->send(r, R_kjinc,j,knext,k,jinc,lt_next,lnext,ldomain);
                    }
                    else{
                        // Merge with next and wait for info
                        int mnext = k+pow(2,lnode);
                        dttssmqr_tf.fulfill_promise({j,k,mnext,jinc,lt_next,lnode+1,ldomain});
                    }
                }
                else if (ldomain < l_limit_domains){
                    int lt_next = M;
                    int ln_next = M;
                    // Flat tree to merge between domains
                    if (ldomain>0 && k==j && m%t==0){
                        // Atleast one step of merging between domains has been done
                        int mnext = (m/t)*t+t; // First tile of next domain
                        if (mnext<M){
                            dttssmqr_tf.fulfill_promise({j,k,mnext,jinc,lt_next,ln_next,ldomain+1}); // keep it on the same node and wait
                        }
                    }
                    else if (k==j){
                        // First domain and start the fist step of merging between domains
                        assert(k==j);
                        int mnext = (k/t)*t+t;
                        if (mnext<M){
                            dttssmqr_tf.fulfill_promise({j,k,mnext,jinc,lt_next,ln_next,ldomain+1}); // keep it on the same node and wait
                        }
                    }
                    else if (k%t==0){
                        // Not the first domain and trigger merging with the first domain
                        int kprev = j;
                        int r = block2rank({kprev,jinc},{p,q,t},origin);
                        if (r==rank) dttssmqr_tf.fulfill_promise({j,kprev,k,jinc,lt_next,ln_next,k/t-j/t});
                        else {
                            auto R_kjinc = view<double>(Mat.at(k+jinc*M).data(), n*n);
                            int ldomain_next= k/t-j/t;
                            am_dttssmqr_2_dttssmqr->send(r, R_kjinc,j,kprev,k,jinc,lt_next,ln_next, ldomain_next);
                        }
                    }
                }

                int stride = n_threads*p;
                // Check with valid merge within a node or between nodes or between domains
                bool run = (lnode==0 && m<min(domain_start+stride,domain_end)) ||
                            (lnode>0 && m<min(domain_start+p,domain_end)) ||
                            (ldomain>0 && k==j && m%t==0) ;
                if (run){
                    // same row -- m
                    if (jinc == j+1) { // Next column
                        int r = block2rank({m,jinc},{p,q,t}, origin);
                        if (rank == r) dgeqrt_tf.fulfill_promise({m, jinc}); // Next dgeqrt in that tile
                        else {
                            auto R_mi = view<double>(Mat.at(m+jinc*M).data(), n * n ); // send to that row
                            am_dttssmqr_2_dgeqrt->send(r, R_mi, m, jinc);
                        }
                    }
                    else {
                        int r = block2rank({m,jinc},{p,q,t}, origin);
                        if (rank == r) dlarfb_tf.fulfill_promise({m, j+1, jinc}); // one dep. on the next dlarfb in that tile
                        else {
                            auto R_mi = view<double>(Mat.at(m+jinc*M).data(), n * n ); // send to that row
                            int jnext = j+1;
                            am_dttssmqr_2_dlarfb->send(r, R_mi, m, jnext, jinc);
                        }
                    }
                }
            })
            .set_name([](int7 jkmilod) {
                return "ttssmqr_" + to_string(jkmilod[0]) + "_" +to_string(jkmilod[1]) + "_" +
                        to_string(jkmilod[2]) + "_" + to_string(jkmilod[3]) + "_" + to_string(jkmilod[4])
                        + "_" + to_string(jkmilod[5]) + "_" + to_string(jkmilod[6]);

            })
            .set_priority([&](int7 jkmilod) {
                return 3+(N-jkmilod[0]);
            });

        // if(rank == 0) printf("Starting QR on a dense matrix\n");
        MPI_Barrier(MPI_COMM_WORLD);
        timer t0 = wctime();
        int stride = n_threads*p;
        for (int i=0; i<M; ++i){
            if (block2rank({i,0},{p,q,t},origin)==rank) { // same node
                if (i%t<stride){
                    // First dgeqrt
                    dgeqrt_tf.fulfill_promise({i, 0});
                }
            }
        }
        tp.join();
        timer t1 = wctime();
        MPI_Barrier(MPI_COMM_WORLD);
        if(rank == 0)
        {
            cout << "Time to factorize: " << elapsed(t0, t1) << endl;
        }

        if(LOG) {
            std::ofstream logfile;
            string filename = "hqr_"+ to_string(nranks)+".log."+to_string(rank);
            logfile.open(filename);
            logfile << log;
            logfile.close();
        }
    }

    // Gather everything on rank 0 and test for accuracy
    if (TEST)
    {
        Communicator comm(MPI_COMM_WORLD, VERB);
        Threadpool tp(n_threads, &comm, VERB);

        // 3 active messages with different data
        auto am_gather_0 = comm.make_active_msg(
        [&](view<double> &R_ij, int& i, int& j) {
            A.block(i*n,j*n,n,n) = Map<MatrixXd>(R_ij.data(), n, n);
        });

        auto am_gather_1 = comm.make_active_msg(
        [&](view<double> &Mat_ij, view<double> &TS_ij,  int& i, int& j) {
            A.block(i*n,j*n,n,n) = Map<MatrixXd>(Mat_ij.data(), n, n);
            TS.at(i-(j*(j+1))/2+j*M)= Map<MatrixXd>(TS_ij.data(), n, n);
        });

        auto am_gather_2 = comm.make_active_msg(
        [&](view<double> &Mat_ij,view<double> &TS_ij, view<double> &TT_ij,  int& i, int& j) {
            A.block(i*n,j*n,n,n) = Map<MatrixXd>(Mat_ij.data(), n, n);
            TS.at(i-(j*(j+1))/2+j*M) = Map<MatrixXd>(TS_ij.data(), n, n);
            TT.at(i-(j*(j+1))/2+j*M) = Map<MatrixXd>(TT_ij.data(), n, n);
        });

        auto am_gather_3 = comm.make_active_msg(
        [&](view<double> &Mat_j,view<double> &TS_ij, view<double> &TT_j,  int& i, int& j, int& domain_size) {
            MatrixXd Atemp = Map<MatrixXd>(Mat_j.data(), p*n, n);
            MatrixXd Ttemp = Map<MatrixXd>(TT_j.data(), p*n, n);

            if (i==j){
                 A.block(i*n,j*n,n,n) = Atemp.block(0,0,n,n);
            }
            else {
                // Only the V_ij and TS_ij from dgeqrt
                A.block(i*n,j*n,n,n).triangularView<StrictlyLower>()=Atemp.block(0,0,n,n).triangularView<StrictlyLower>();
            }
            TS.at(i-(j*(j+1))/2+j*M) = Map<MatrixXd>(TS_ij.data(), n, n);

            // DTTQRT for the next p rows in the domain
            for (int kk=1; kk< min(domain_size,p); ++kk){
                A.block((i+kk)*n, j*n, n, n).triangularView<Upper>() = Atemp.block(kk*n,0,n,n).triangularView<Upper>(); // Contains all the updates from dttqrt
                TT.at(i+kk-(j*(j+1))/2+j*M) = Ttemp.block(kk*n,0,n,n);
            }
        });

        auto am_gather_4 = comm.make_active_msg(
        [&](view<double> &Mat_ij, view<double> &TS_ij,  int& i, int& j) {
            A.block(i*n,j*n,n,n).triangularView<StrictlyLower>() = Map<MatrixXd>(Mat_ij.data(), n, n).triangularView<StrictlyLower>();
            TS.at(i-(j*(j+1))/2+j*M)= Map<MatrixXd>(TS_ij.data(), n, n);
        });

        auto am_gather_5 = comm.make_active_msg(
        [&](view<double> &Mat_j, view<double> &TT_j,  int& i, int& j) {
           int num_domains = (M+t-1)/t-j/t-1;

            MatrixXd Atemp = Map<MatrixXd>(Mat_j.data(), num_domains*n, n);
            MatrixXd Ttemp = Map<MatrixXd>(TT_j.data(), num_domains*n, n);

            // DTTQRT for merging between domains
            for (int kk=0; kk< num_domains; ++kk){
                int next_tile = ((j+t)/t)*t + kk*t;
                A.block(next_tile*n, j*n, n, n).triangularView<Upper>() = Atemp.block(kk*n,0,n,n).triangularView<Upper>(); // Contains all the updates from dttqrt
                TT.at(next_tile-(j*(j+1))/2+j*M) = Ttemp.block(kk*n,0,n,n);
            }
        });

        int stride = n_threads*p;

        for (int i=0;i<M;++i){
            for (int j=0;j<N;++j){
                int domain_index=i/t;
                int domain_start=max(j,domain_index*t);
                int domain_end=min(domain_index*t+t,M);
                int domain_size=domain_end-domain_start;

                if(block2rank({i,j},{p,q,t}, origin) == rank){ // Each block should send its own data otherwise there can be race condition
                    if (i<j){// Send Mat_ij
                        if (rank != 0){
                            auto V_ij = view<double>(Mat.at(i+j*M).data(), n*n);
                            am_gather_0->send(0, V_ij, i, j);
                            // cout << "i: " << i << " j: " << j << endl;
                            // cout << Mat.at(i+j*M) << endl << endl;
                        }
                        else {
                            A.block(i*n,j*n,n,n)=Mat.at(i+j*M);
                        }
                    }
                    else { // Send (TT_ij + TS_ij) or (TS_ij) and Mat_ij
                        assert(i>=j);
                        if (i<domain_start+stride && i>j){
                            if (rank!=0){
                                // Send TT_ij and TS_ij and Mat_ij
                                auto V_ij = view<double>(Mat.at(i+j*M).data(), n*n);
                                auto TS_ij = view<double>(TS.at(i-(j*(j+1))/2+j*M).data(), n*n);
                                auto TT_ij = view<double>(TT.at(i-(j*(j+1))/2+j*M).data(), n*n);
                                am_gather_2->send(0, V_ij, TS_ij, TT_ij, i, j);
                            }
                            else {
                                A.block(i*n,j*n,n,n)=Mat.at(i+j*M);
                            }
                        }
                        else {
                            assert(i>=domain_start+stride || i==j);
                            if (rank!=0){
                                // Send TS_ij and Mat_ij
                                auto V_ij = view<double>(Mat.at(i+j*M).data(), n*n);
                                auto T_ij = view<double>(TS.at(i-(j*(j+1))/2+j*M).data(), n*n);
                                am_gather_1->send(0, V_ij, T_ij, i, j);
                            }
                            else {
                                A.block(i*n,j*n,n,n)=Mat.at(i+j*M);
                            }
                        }
                    }
                }
            }
        }

        tp.join();
        MPI_Barrier(MPI_COMM_WORLD);

        if(rank == 0 && TEST) {
            cout << "Entering solve..." << endl;
            // cout << A.block(0,n,M*n,n) << endl;
            // cout << Mat.at(1*M) << endl;
            // Test 1
            // Q^T b
            int stride = n_threads*p;

            for (int j=0; j< N; ++j){
                for (int i=j; i<M; ++i){
                    int domain_index=i/t;
                    int domain_start=max(j,domain_index*t);
                    if (i<domain_start+stride){
                       assert(TS.at(i-(j*(j+1))/2+j*M).rows()>0);
                       int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'T', 'F', 'C', n, 1, n,
                                                 A.block(i*n,j*n,n,n).data(), M*n,
                                                 TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                                 b.segment(i*n, n).data(), n);
                       assert(info == 0);
                    }
                    else { // i >= domain_start+stride
                        assert(TS.at(i-(j*(j+1))/2+j*M).rows()>0);
                        int root_tile = (i-domain_start)%stride+domain_start;
                        assert(root_tile<domain_start+stride);
                        assert(root_tile<M);
                        int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, 1, n, 0, nb,
                                                   A.block(i*n,j*n,n,n).data(), M*n,  // Since A.block() is use, LDA should be the rows of A
                                                   TS.at(i-(j*(j+1))/2+j*M).data(), n,
                                                   b.segment(root_tile*n, n).data(), n,
                                                   b.segment(i*n, n).data(), n);
                        assert(info ==0);
                    }
                }
                // cout << "unmerging" << endl;
                // unmerging
                int domain_index=j/t;
                int num_domains = (M+t-1)/t;
                for (int d=domain_index;d<num_domains;d+=1){ // In each remaining domain
                    int domain_start=max(j,d*t);
                    int domain_end=min(d*t+t,M);
                    int node_next=domain_start;

                    while(node_next<domain_start+p){ // Merge across threads for each node
                        int update_stride = p;
                        while (update_stride<stride){ // Merge across threads
                            for (int i=node_next;i<min(domain_end,domain_start+stride);i+=2*update_stride){
                                // unmerge
                                int merge_row = i+update_stride;

                                if (merge_row < min(domain_end,domain_start+stride)){
                                    int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, 1, n, n, nb,
                                                           A.block(merge_row*n,j*n,n,n).data(), M*n,
                                                           TT.at(merge_row-(j*(j+1))/2+j*M).data(), n,
                                                           b.segment(i*n, n).data(), n,
                                                           b.segment(merge_row*n, n).data(), n);
                                    assert(info==0);
                                }
                            }
                            update_stride += update_stride;
                        }
                        node_next += 1;
                    }

                    // Merge across nodes
                    int update_stride = 1;
                    while (update_stride<p){
                        for (int i=domain_start;i<min(domain_end,domain_start+p);i+=2*update_stride){
                            int merge_row=i+update_stride;
                            if (merge_row<min(domain_end,domain_start+p)){
                                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, 1, n, n, nb,
                                                       A.block(merge_row*n,j*n,n,n).data(), M*n,
                                                       TT.at(merge_row-(j*(j+1))/2+j*M).data(), n,
                                                       b.segment(i*n, n).data(), n,
                                                       b.segment(merge_row*n, n).data(), n);
                                assert(info==0);
                            }
                        }
                        update_stride += update_stride;
                    }
                }

                // Merge across domains -- flat tree
                int first_row_in_next_tile = ((j+t)/t)*t;
                for (int fn = first_row_in_next_tile; fn < M; fn+=t){
                    int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, 1, n, n, nb,
                                               A.block(fn*n,j*n,n,n).data(), M*n,
                                               TT.at(fn-(j*(j+1))/2+j*M).data(), n,
                                               b.segment(j*n, n).data(), n,
                                               b.segment(fn*n, n).data(), n);
                    assert(info ==0);
                }

            }
            // R^{-1}b
            auto R = A.triangularView<Upper>();
            R.solveInPlace(b);

            // cout << b << endl << endl;
            // cout << x << endl;
            double error = (b.topRows(N*n) - x).norm() / x.norm();
            cout << "Error solve: " << error << endl;
            assert(error<=1e-8);
        }

    }

    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]);
    }

    if (argc >= 11)
    {
        TEST = atoi(argv[10]);
    }
    t_ = n_threads_*p_*t_;
    if (t_>M_){
        cout << "Setting t_ to the maximum allowed value..." << endl;
        t_ = M_;
    }
    const int return_flag = denseQR(n_threads_, n_, M_, N_, p_, q_, t_);
    MPI_Finalize();
    return return_flag;
}


#ifndef CONFIG_HPP
#define CONFIG_HPP

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <unordered_set>

/** floatBO defines the floating point precision to be used in EchoBay */
typedef double floatBO;
typedef Eigen::ArrayXd ArrayBO;
typedef Eigen::ArrayXi ArrayI;
typedef Eigen::Array<int8_t, Eigen::Dynamic, 1> ArrayI8;
typedef Eigen::MatrixXd MatrixBO;
typedef Eigen::VectorXcd VComplexBO;
typedef Eigen::Triplet<floatBO> TripletBO;
typedef Eigen::SparseMatrix<floatBO, 0x1> SparseBO;

// Structures for Hash Table and WR Filling
struct pair_hash
{
    template <class T1, class T2>
    std::size_t operator() (const std::pair<T1, T2> &pair) const
    {
        return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
    }
};
typedef std::unordered_set<std::pair<int,int>, pair_hash >::iterator UOMIterator;
#endif


/*
    BSD 3-Clause License

    Copyright (c) 2018, Roboy
            All rights reserved.

    Redistribution and use in source and binary forms, with or without
            modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    * Neither the name of the copyright holder nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
            IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
            FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
            DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
            SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
            CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

    author: Simon Trendel ( simon.trendel@tum.de ), 2018
    description: least squares minimzer for pose estimation using relative sensor distances
                  based on "An Improved Method of Pose Estimation for Lighthouse Base Station Extension"
                  https://www.ncbi.nlm.nih.gov/pmc/articles/PMC5677447/
*/

#pragma once

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// std
#include <iostream>

using namespace Eigen;
using namespace std;
namespace PoseEstimatorMultiLighthouse {
// Generic functor for Eigen Levenberg-Marquardt minimizer
    template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
    struct Functor {
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Matrix<Scalar, InputsAtCompileTime, 1> InputType;
        typedef Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
        typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }

        int values() const { return m_values; }
    };

    struct PoseEstimator : Functor<double> {
        /**
         * Default amount of sensors needed for Eigen templated structure
         * @param numberOfSensors you can however choose any number of sensors here
         */
        PoseEstimator(int numberOfSensors = 4);

        /**
         * This is the function that is called in each iteration
         * @param x the pose vector (3 rotational 3 translational parameters)
         * @param fvec the error function (the difference between the sensor positions)
         * @return
         */
        int operator()(const VectorXd &x, VectorXd &fvec) const;

        VectorXd pose;
        vector<Matrix4d> lighthousePose;
        vector<double> elevations, azimuths;
        vector<Vector3d> rel_pos;
        int numberOfSensors = 4;
        vector<int> lighthouse_id;
    };
}


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
//
//This algorithm is described in "Network Flows: Theory, Algorithms, and Applications"
// by Ahuja, Magnanti, Orlin.

#ifndef BOOST_GRAPH_CYCLE_CANCELING_HPP
#define BOOST_GRAPH_CYCLE_CANCELING_HPP

#include <numeric>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/pending/relaxed_heap.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/detail/augment.hpp>
#include <boost/graph/find_flow_cost.hpp>

namespace boost {

namespace detail {

template <typename PredEdgeMap, typename Vertex>
class RecordEdgeMapAndCycleVertex
    : public bellman_visitor<edge_predecessor_recorder<PredEdgeMap, on_edge_relaxed> > {
        typedef edge_predecessor_recorder<PredEdgeMap, on_edge_relaxed> PredRec;
public:
    RecordEdgeMapAndCycleVertex(PredEdgeMap pred, Vertex & v) :
        bellman_visitor<PredRec>(PredRec(pred)), m_v(v), m_pred(pred) {}

    template <typename Graph, typename Edge>
    void edge_not_minimized(Edge e, const Graph & g) const {
        typename graph_traits<Graph>::vertices_size_type n = num_vertices(g) + 1;

        //edge e is not minimized but does not have to be on the negative weight cycle
        //to find vertex on negative wieight cycle we move n+1 times backword in the PredEdgeMap graph.
        while(n > 0) {
            e = get(m_pred, source(e, g));
            --n;
        }
        m_v = source(e, g);
    }
private:
    Vertex & m_v;
    PredEdgeMap m_pred;
};

} //detail

template <class Graph, class Pred, class Distance, class Reversed, class ResidualCapacity, class Weight>
void cycle_canceling(const Graph &g, Weight weight, Reversed rev, ResidualCapacity residual_capacity, Pred pred, Distance distance) {
    typedef filtered_graph<const Graph, is_residual_edge<ResidualCapacity> > ResGraph;
    ResGraph gres = detail::residual_graph(g, residual_capacity);

    typedef graph_traits<ResGraph> ResGTraits;
    typedef graph_traits<Graph> GTraits;
    typedef typename ResGTraits::edge_descriptor edge_descriptor;
    typedef typename ResGTraits::vertex_descriptor vertex_descriptor;

    typename GTraits::vertices_size_type N = num_vertices(g);

    BGL_FORALL_VERTICES_T(v, g, Graph) {
        put(pred, v, edge_descriptor());
        put(distance, v, 0);
    }

    vertex_descriptor cycleStart;
    while(!bellman_ford_shortest_paths(gres, N,
            weight_map(weight).
            distance_map(distance).
            visitor(detail::RecordEdgeMapAndCycleVertex<Pred, vertex_descriptor>(pred, cycleStart)))) {

        detail::augment(g, cycleStart, cycleStart, pred, residual_capacity, rev);

        BGL_FORALL_VERTICES_T(v, g, Graph) {
            put(pred, v, edge_descriptor());
            put(distance, v, 0);
        }
    }
}

//in this namespace argument dispatching takes place
namespace detail {

template <class Graph, class P, class T, class R, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance>
void cycle_canceling_dispatch2(
        const Graph &g,
        Weight weight,
        Reversed rev,
        ResidualCapacity residual_capacity,
        Pred pred,
        Distance dist,
        const bgl_named_params<P, T, R>& params) {
    cycle_canceling(g, weight, rev, residual_capacity, pred, dist);
}

//setting default distance map
template <class Graph, class P, class T, class R, class Pred, class ResidualCapacity, class Weight, class Reversed>
void cycle_canceling_dispatch2(
        Graph &g,
        Weight weight,
        Reversed rev,
        ResidualCapacity residual_capacity,
        Pred pred,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename property_traits<Weight>::value_type D;

    std::vector<D> d_map(num_vertices(g));

    cycle_canceling(g, weight, rev, residual_capacity, pred,
                    make_iterator_property_map(d_map.begin(), choose_const_pmap(get_param(params, vertex_index), g, vertex_index)));
}

template <class Graph, class P, class T, class R, class ResidualCapacity, class Weight, class Reversed, class Pred>
void cycle_canceling_dispatch1(
        Graph &g,
        Weight weight,
        Reversed rev,
        ResidualCapacity residual_capacity,
        Pred pred,
        const bgl_named_params<P, T, R>& params) {
    cycle_canceling_dispatch2(g, weight, rev,residual_capacity,  pred,
                                get_param(params, vertex_distance), params);
}

//setting default predecessors map
template <class Graph, class P, class T, class R, class ResidualCapacity, class Weight, class Reversed>
void cycle_canceling_dispatch1(
        Graph &g,
        Weight weight,
        Reversed rev,
        ResidualCapacity residual_capacity,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    std::vector<edge_descriptor> p_map(num_vertices(g));

    cycle_canceling_dispatch2(g, weight, rev, residual_capacity,
                              make_iterator_property_map(p_map.begin(), choose_const_pmap(get_param(params, vertex_index), g, vertex_index)),
                                get_param(params, vertex_distance), params);
}

}//detail

template <class Graph, class  P, class T, class R>
void cycle_canceling(Graph &g,
        const bgl_named_params<P, T, R>& params) {
    cycle_canceling_dispatch1(g,
           choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           get_param(params, vertex_predecessor),
           params);
}

template <class Graph>
void cycle_canceling(Graph &g) {
    bgl_named_params<int, buffer_param_t> params(0);
    cycle_canceling(g, params);
}

}

#endif /* BOOST_GRAPH_CYCLE_CANCELING_HPP */


#pragma once

#include <Eigen/Core>

#include <interval/interval.hpp>

/// NOTE: Naming Convention
/// Point: Either a 2D or 3D point in space.
/// Edge: A line segment in either 2D or 3D defined by its endpoints.
/// Triangle: A triangle in 3D

namespace ipc::rigid {

bool is_point_along_edge(
    const VectorMax3I& p, const VectorMax3I& e0, const VectorMax3I& e1);

bool are_edges_intersecting(
    const Vector3I& ea0,
    const Vector3I& ea1,
    const Vector3I& eb0,
    const Vector3I& eb1);

bool is_point_inside_triangle(
    const Vector3I& p,
    const Vector3I& t0,
    const Vector3I& t1,
    const Vector3I& t2);

} // namespace ipc::rigid


#include <fstream>
#include <iostream>
#include <stack>
#include <chrono>
#include <boost/exception/all.hpp>

#include "common.h"

#include "Array.h"

namespace matCUDA
{
	// identity matrix generation
	template <typename TElement>
	Array<TElement> eye( index_t N )
	{
		Array<TElement> result( N, N );

		//// CPU version
		//for (int i = 0; i < result.GetDescriptor().GetDim(0); i++) {
		//	for (int j = 0; j < result.GetDescriptor().GetDim(1); j++)
		//		result(i,j) = ( i == j );
		//}

		// GPU version
		cuda_eye<TElement>( result.data(), N );

		return result;
	}

	template Array<int> eye( index_t N );
	template Array<float> eye( index_t N );
	template Array<double> eye( index_t N );
	template Array<ComplexFloat> eye( index_t N );
	template Array<ComplexDouble> eye( index_t N );

	// dpss generation following Gruenbacher and Hummels, 1994
	template <typename TElement>
	Array<TElement> dpss( index_t N, double NW, index_t degree )
	{
		cusolverStatus_t stat = CUSOLVER_STATUS_NOT_INITIALIZED;
		cusolverOperations<TElement> op;
		Array<TElement> eigenvector( N );

		try
		{
			stat = op.dpss( &eigenvector, N, NW, degree );
		}
		catch(std::exception &e)
		{
			std::cerr << boost::diagnostic_information(e);
		}

		return eigenvector;
	}

	template Array<float> dpss( index_t N, double NW, index_t degree );
	template Array<double> dpss( index_t N, double NW, index_t degree );

	//Array<ComplexFloat> fft( Array<float> *in )
	//{
	//	cufftResult_t stat = CUFFT_NOT_IMPLEMENTED;
	//	cufftOperations<ComplexFloat> op;
	//	Array<ComplexFloat> result( floor(in->GetDescriptor().GetDim(0)/2)+1, in->GetDescriptor().GetDim(1) );
	//
	//	try
	//	{
	//		stat = op.fft_stream( in, &result );
	//	}
	//	catch(std::exception &e)
	//	{
	//		std::cerr << boost::diagnostic_information(e);
	//	}
	//
	//	return result;
	//}
	//
	//Array<ComplexDouble> fft( Array<double> *in )
	//{
	//	cufftResult_t stat = CUFFT_NOT_IMPLEMENTED;
	//	cufftOperations<ComplexDouble> op;
	//	Array<ComplexDouble> result( in->GetDescriptor().GetDim(0)/2+1, in->GetDescriptor().GetDim(1) );
	//
	//	try
	//	{
	//		stat = op.fft_stream( in, &result );
	//	}
	//	catch(std::exception &e)
	//	{
	//		std::cerr << boost::diagnostic_information(e);
	//	}
	//
	//	return result;
	//}

	template<typename TElement>
	Array<std::complex<TElement>> fft( Array<TElement> *in )
	{
		cufftResult_t stat = CUFFT_NOT_IMPLEMENTED;
		cufftOperations<std::complex<TElement>> op;
		Array<std::complex<TElement>> result( floor(in->GetDescriptor().GetDim(0)/2)+1, in->GetDescriptor().GetDim(1) );

		try
		{
			stat = op.fft_stream( in, &result );
		}
		catch(std::exception &e)
		{
			std::cerr << boost::diagnostic_information(e);
		}

		return result;
	}

	template Array<std::complex<float>> fft( Array<float> *in );
	template Array<std::complex<double>> fft( Array<double> *in );

	// read files for unit tests
	template<> Array<ComplexFloat> read_file_vector( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			size_t dim = std::stoi( aux );
			Array<ComplexFloat> result(dim);

			double real, imag;
			for( int i = 0; i < dim; i++ ) {
				std::getline( inputFile, aux );
				real = std::stof( aux );
				std::getline( inputFile, aux );
				imag = std::stof( aux );
				result( i ) = ComplexFloat( real, imag );
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<ComplexFloat> (1);
		}
	}

	template<> Array<ComplexDouble> read_file_vector( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			size_t dim = std::stoi( aux );
			Array<ComplexDouble> result(dim);

			double real, imag;
			for( int i = 0; i < dim; i++ ) {
				std::getline( inputFile, aux );
				real = std::stod( aux );
				std::getline( inputFile, aux );
				imag = std::stod( aux );
				result( i ) = ComplexDouble( real, imag );
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<ComplexDouble> (1);
		}
	}

	template<> Array<float> read_file_vector( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			size_t dim = std::stoi( aux );
			Array<float> result(dim);

			for( int i = 0; i < dim; i++ ) {
				std::getline( inputFile, aux );
				result( i ) = std::stof( aux );
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<float> (1);
		}
	}

	template<> Array<double> read_file_vector( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			size_t dim = std::stoi( aux );
			Array<double> result(dim);

			for( int i = 0; i < dim; i++ ) {
				std::getline( inputFile, aux );
				result( i ) = std::stof( aux );
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<double> (1);
		}
	}

	template<> Array<ComplexFloat> read_file_matrix( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			index_t idxComma = aux.find_first_of( ',', 0 );
			size_t rows = std::stoi( aux.substr( 0, idxComma - 1 ) );
			size_t cols = std::stoi( aux.substr( idxComma + 1, aux.size() ) );
			Array<ComplexFloat> result(rows,cols);

			double real, imag;
			for( int i = 0; i < cols; i++ ) {
				for( int j = 0; j < rows; j++ ) {
				std::getline( inputFile, aux );
				real = std::stof( aux );
				std::getline( inputFile, aux );
				imag = std::stof( aux );
				result( j, i ) = ComplexFloat( real, imag );
				}
			}

			inputFile.close();

			return  result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<ComplexFloat> (1,1);
		}
	}

	template<> Array<ComplexDouble> read_file_matrix( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			index_t idxComma = aux.find_first_of( ',', 0 );
			size_t rows = std::stoi( aux.substr( 0, idxComma - 1 ) );
			size_t cols = std::stoi( aux.substr( idxComma + 1, aux.size() ) );
			Array<ComplexDouble> result(rows,cols);

			double real, imag;
			for( int i = 0; i < cols; i++ ) {
				for( int j = 0; j < rows; j++ ) {
				std::getline( inputFile, aux );
				real = std::stod( aux );
				std::getline( inputFile, aux );
				imag = std::stod( aux );
				result( j, i ) = ComplexDouble( real, imag );
				}
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<ComplexDouble> (1,1);
		}
	}

	template<> Array<float> read_file_matrix( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			index_t idxComma = aux.find_first_of( ',', 0 );
			size_t rows = std::stoi( aux.substr( 0, idxComma - 1 ) );
			size_t cols = std::stoi( aux.substr( idxComma + 1, aux.size() ) );
			Array<float> result(rows,cols);

			for( int i = 0; i < cols; i++ ) {
				for( int j = 0; j < rows; j++ ) {
					std::getline( inputFile, aux );
					result( j, i ) = std::stof( aux );
				}
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<float> (1,1);
		}
	}

	template<> Array<double> read_file_matrix( std::string s )
	{
		const size_t lim = 128;
		std::string aux;
		std::ifstream inputFile;
		inputFile.open( s );
		if( inputFile.good() )
		{
			std::getline( inputFile, aux );
			index_t idxComma = aux.find_first_of( ',', 0 );
			size_t rows = std::stoi( aux.substr( 0, idxComma - 1 ) );
			size_t cols = std::stoi( aux.substr( idxComma + 1, aux.size() ) );
			Array<double> result(rows,cols);

			for( int i = 0; i < cols; i++ ) {
				for( int j = 0; j < rows; j++ ) {
					std::getline( inputFile, aux );
					result( j, i ) = std::stof( aux );
				}
			}

			inputFile.close();

			return result;
		}
		else
		{
			std::cout<< "Could not open " << s << std::endl;
			EXIT_FAILURE;

			return Array<double> (1,1);
		}
	}

	template <typename TElement>
	Array<TElement> rand(index_t u)
	{
		curandStatus_t stat = CURAND_STATUS_NOT_INITIALIZED;
		curandOperations<TElement> op;
		Array<TElement> result( u );

		try
		{
			stat = op.rand( &result );
		}
		catch(std::exception &e)
		{
			std::cerr << boost::diagnostic_information(e);
		}

		return result;
	}

	template Array<float> rand(index_t u);
	template Array<double> rand(index_t u);
	template Array<ComplexFloat> rand(index_t u);
	template Array<ComplexDouble> rand(index_t u);

	template <typename TElement>
	Array<TElement> rand(index_t u1, index_t u2)
	{
		curandStatus_t stat = CURAND_STATUS_NOT_INITIALIZED;
		curandOperations<TElement> op;
		Array<TElement> result( u1, u2 );

		try
		{
			stat = op.rand( &result );
			//stat = op.rand_zerocopy( &result );
		}
		catch(std::exception &e)
		{
			std::cerr << boost::diagnostic_information(e);
		}

		return result;
	}

	template Array<float> rand(index_t u1, index_t u2);
	template Array<double> rand(index_t u1, index_t u2);
	template Array<ComplexFloat> rand(index_t u1, index_t u2);
	template Array<ComplexDouble> rand(index_t u1, index_t u2);

	std::stack<std::chrono::system_clock::time_point> tictoc_stack;
	void tic() {
		tictoc_stack.push(std::chrono::high_resolution_clock::now());
	}
	void toc() {
		std::cout << "Time elapsed: "
				  << std::setprecision( 8 )
				  << std::scientific
				  << 1e-9*std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now() - tictoc_stack.top()).count()
				  //<< ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
				  << " seconds"
				  << std::endl;
		tictoc_stack.pop();
	}
	long double toc( long double in ) {
		long double out = 1e-9*std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now() - tictoc_stack.top()).count();
		return out;
	}
}

//template <typename TElement>
//Array<TElement> rand(index_t u, ...)
//{
//	va_list args;
//	va_start(args, u);
//
//	int count = u;
//
//	for(int i = 1; i < count; i++)
//		m_indexer->m_pos[i] = va_arg(args, index_t);
//
//	va_end(args);
//	return Array<TElement>(1);
//}
//
//template Array<int> rand(index_t u, ...);
//template Array<float> rand(index_t u, ...);
//template Array<double> rand(index_t u, ...);
//template Array<ComplexFloat> rand(index_t u, ...);
//template Array<ComplexDouble> rand(index_t u, ...);


#include <catch2/catch.hpp>
#include <mitama/mana/data/type_list.hpp>
#include <mitama/mana/algorithm/chunk.hpp>
#include <mitama/mana/core/metafunc.hpp>
#include <mitama/mana/utility/iota.hpp>
#include <type_traits>
#include <iostream>
#include <boost/type_index.hpp>
namespace mana = mitama::mana;

TEST_CASE("chunk<2>", "[algorithm][chunk]") {
    REQUIRE(std::is_same_v<
        decltype(mana::chunk<2>(mana::type_list_c<int, int, double, double>)),
        mana::type_list<mana::type_list<int, int>, mana::type_list<double, double>>>);

    REQUIRE(std::is_same_v<
        decltype(mana::chunk<2>(mana::iota<0, 10>)),
        mana::type_list<mana::value_list<0ul, 1ul>,
                        mana::value_list<2ul, 3ul>,
                        mana::value_list<4ul, 5ul>,
                        mana::value_list<6ul, 7ul>,
                        mana::value_list<8ul, 9ul>
                        >>);
}


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

//[mpfr_eg
#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

int main()
{
   using namespace boost::multiprecision;

   // Operations at variable precision and no numeric_limits support:
   mpfr_float a = 2;
   mpfr_float::default_precision(1000);
   std::cout << mpfr_float::default_precision() << std::endl;
   std::cout << sqrt(a) << std::endl; // print root-2

   // Operations at fixed precision and full numeric_limits support:
   mpfr_float_100 b = 2;
   std::cout << std::numeric_limits<mpfr_float_100>::digits << std::endl;
   // We can use any C++ std lib function:
   std::cout << log(b) << std::endl; // print log(2)
   // We can also use any function from Boost.Math:
   std::cout << boost::math::tgamma(b) << std::endl;
   // These even work when the argument is an expression template:
   std::cout << boost::math::tgamma(b * b) << std::endl;

   // Access the underlying data:
   mpfr_t r;
   mpfr_init(r);
   mpfr_set(r, b.backend().data(), GMP_RNDN);
   mpfr_clear(r);
   return 0;
}
//]


#include <tradelayer/mdex.h>
#include <tradelayer/dex.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/uint256_extensions.h>
#include <test/test_bitcoin.h>
#include <boost/test/unit_test.hpp>
#include <stdint.h>

using namespace mastercore;

BOOST_FIXTURE_TEST_SUITE(tradelayer_mdex_functions_tests, BasicTestingSetup)

void literVolume(int64_t& amount, uint32_t propertyId, const int& fblock, const int& sblock, const std::map<int, std::map<uint32_t,int64_t>>& aMap)
{
    // BOOST_TEST_MESSAGE("iter, fblock:" << fblock);
    // BOOST_TEST_MESSAGE("iter, sblock:" << sblock);
    for(const auto &m : aMap)
    {
        const int& blk = m.first;
        // BOOST_TEST_MESSAGE("iter, block (after):" << blk);
        if(blk < fblock && fblock != 0){
            continue;
        } else if(sblock < blk){
            break;
        }

        // BOOST_TEST_MESSAGE("iter, block:" << blk);

        const auto &blockMap = m.second;
        auto itt = blockMap.find(propertyId);
        // BOOST_TEST_MESSAGE("iter, propertyId:" << propertyId);
        if (itt != blockMap.end()){

            const int64_t& newAmount = itt->second;

            // overflows?
            assert(!isOverflow(amount, newAmount));
            amount += newAmount;
            // BOOST_TEST_MESSAGE("iter, amount (after):" << amount);

        }

    }

}

int64_t lgetVWap(uint32_t propertyId, int aBlock, const std::map<uint32_t,std::map<int,std::vector<std::pair<int64_t,int64_t>>>>& aMap)
{
    int64_t volume = 0;
    arith_uint256 nvwap = 0;
    const int rollback = aBlock - 12;

    // BOOST_TEST_MESSAGE("rollback:" << rollback);

    auto it = aMap.find(propertyId);
    if (it != aMap.end())
    {
        auto &vmap = it->second;
        auto itt = (rollback > 0) ? find_if(vmap.begin(), vmap.end(), [&rollback] (const std::pair<int,std::vector<std::pair<int64_t,int64_t>>>& int_arith_pair) { return (int_arith_pair.first >= rollback);}) : vmap.begin();
        if (itt != vmap.end())
        {
            for ( ; itt != vmap.end(); ++itt)
            {
                auto v = itt->second;
                for_each(v.begin(),v.end(), [&nvwap](const std::pair<int64_t,int64_t>& num){ nvwap += ConvertTo256(num.first * (num.second / COIN));});
            }
        }

    }
    // calculating the volume
    // BOOST_TEST_MESSAGE("rollback:" << rollback);
    // BOOST_TEST_MESSAGE("aBlock:" << aBlock);
    literVolume(volume, propertyId, rollback, aBlock, MapLTCVolume);

    if (volume == 0) BOOST_TEST_MESSAGE("volume here is 0");

    // BOOST_TEST_MESSAGE("nvwap:" << ConvertTo64(nvwap));
    // BOOST_TEST_MESSAGE("volume:" << volume);
    return ((volume > 0) ? (COIN *(ConvertTo64(nvwap) / volume)) : 0);

}

int64_t lincreaseLTCVolume(uint32_t propertyId, uint32_t propertyDesired, int aBlock)
{
    int64_t total = 0, propertyAmount = 0, propertyDesiredAmount = 0;
    const int rollback = aBlock - 1000;
    iterVolume(propertyAmount, propertyId, rollback, aBlock, MapLTCVolume);
    iterVolume(propertyDesiredAmount, propertyDesired, rollback, aBlock, MapLTCVolume);

    // BOOST_TEST_MESSAGE("propertyAmount:" << propertyAmount);
    // BOOST_TEST_MESSAGE("propertyDesiredAmount:" << propertyDesiredAmount);
    // BOOST_TEST_MESSAGE("aBlock:" << aBlock);
    // BOOST_TEST_MESSAGE("rollback:" << rollback);

    if (1000 * COIN <=  propertyAmount && 1000 * COIN <=  propertyDesiredAmount)
    {

        // look up the 12-block VWAP of the denominator vs. LTC
        const int64_t vwap = getVWap(propertyDesired, aBlock, tokenvwap);
        const arith_uint256 aTotal = (ConvertTo256(propertyDesiredAmount) * ConvertTo256(vwap)) / ConvertTo256(COIN);
        total = ConvertTo64(aTotal);
        // BOOST_TEST_MESSAGE("vwap:" << vwap);
        // BOOST_TEST_MESSAGE("propertyDesiredAmount:" << propertyDesiredAmount);
        // BOOST_TEST_MESSAGE("total:" << total);

        // increment cumulative LTC volume by tokens traded * the 12-block VWAP
        MapLTCVolume[aBlock][propertyDesired] += total;

    }

    return total;

}

BOOST_AUTO_TEST_CASE(getvwap_function)
{
    const uint32_t propertyId = 3;
    // actual block
    const int aBlock = 10250;

    // adding  amount *  price
    tokenvwap[propertyId][aBlock - 1].push_back(std::make_pair(2000 * COIN, 1500 * COIN));
    tokenvwap[propertyId][aBlock - 2].push_back(std::make_pair(2000 * COIN, 1600 * COIN));
    tokenvwap[propertyId][aBlock - 3].push_back(std::make_pair(2000 * COIN, 1700 * COIN));
    tokenvwap[propertyId][aBlock - 3].push_back(std::make_pair(2000 * COIN, 1800 * COIN));
    tokenvwap[propertyId][aBlock - 4].push_back(std::make_pair(2000 * COIN, 1900 * COIN));

    // adding some volume
    MapLTCVolume[aBlock - 1][propertyId] += 2000 * COIN;
    MapLTCVolume[aBlock - 2][propertyId] += 1000 * COIN;
    MapLTCVolume[aBlock - 3][propertyId] += 3000 * COIN;
    MapLTCVolume[aBlock - 4][propertyId] += 4000 * COIN;

    // checking vwap:   17000000 / 10000 = 1700
    BOOST_CHECK_EQUAL(1700 * COIN, lgetVWap(propertyId, aBlock, tokenvwap));

    // cleaning maps
    tokenvwap.clear();
    MapLTCVolume.clear();
}

BOOST_AUTO_TEST_CASE(increase_ltc_volume_function)
{
    const uint32_t propertyId = 1;
    const uint32_t propertyDesired = 2;
    // actual block
    const int aBlock = 10250;

    // adding  amount *  price
    tokenvwap[propertyId][aBlock - 10].push_back(std::make_pair(1000 * COIN, 1500 * COIN));
    tokenvwap[propertyId][aBlock - 10].push_back(std::make_pair(3000 * COIN, 1500 * COIN));

    tokenvwap[propertyDesired][aBlock - 10].push_back(std::make_pair(1000 * COIN, 1500 * COIN));
    tokenvwap[propertyDesired][aBlock - 10].push_back(std::make_pair(3000 * COIN, 1500 * COIN));

    // adding some volume
    MapLTCVolume[aBlock - 10][propertyId] = 2000 * COIN;
    MapLTCVolume[aBlock - 10][propertyDesired] = 2000 * COIN;

    // 12-vwap * amountdesired = 3000 * 2000 = 6000000
    BOOST_CHECK_EQUAL(6000000 * COIN, lincreaseLTCVolume(propertyId, propertyDesired, aBlock));

}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once
#include <Eigen/Dense>
#include <iostream>

#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>

#include "sphere.hpp"
#include "karcherMean.hpp"
#include "kmeans.hpp"
#include "clusterer.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class SphericalKMeans : public KMeans<T>
{
public:
  SphericalKMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen);
  virtual ~SphericalKMeans();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

//  virtual void updateLabels();
//  virtual void updateCenters();
//  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& deviates);
//  virtual T avgIntraClusterDeviation();

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);
  virtual uint32_t indOfClosestCluster(int32_t i);
  virtual Matrix<T,Dynamic,1> computeCenter(uint32_t k);

};

template<class T>
class SphericalKMeansKarcher : public SphericalKMeans<T>
{
public:
  SphericalKMeansKarcher(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx,
      uint32_t K, boost::mt19937* pRndGen);
  ~SphericalKMeansKarcher();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual Matrix<T,Dynamic,1> computeCenter(uint32_t k);
};

// --------------------------------- impl -------------------------------------
template<class T>
SphericalKMeans<T>::SphericalKMeans(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen)
  : KMeans<T>(spx,K, pRndGen)
{}

template<class T>
SphericalKMeans<T>::~SphericalKMeans()
{}

template<class T>
T SphericalKMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
  return acos(min(1.0,max(-1.0,(a.transpose()*b)(0)))); // angular similarity
//  return a.transpose()*b; // cosine similarity
};

template<class T>
bool SphericalKMeans<T>::closer(T a, T b)
{
  return a<b; // if dist a is greater than dist b a is closer than b (angular dist)
//  return a>b; // if dist a is greater than dist b a is closer than b (cosine dist)
};

template<class T>
Matrix<T,Dynamic,1> SphericalKMeans<T>::computeCenter(uint32_t k)
{
  this->Ns_(k) = 0.0;
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
      this->Ns_(k) ++;
    }
  return mean_k/mean_k.norm();
}

template<class T>
uint32_t SphericalKMeans<T>::indOfClosestCluster(int32_t i)
{
  // use cosine similarity because it is faster since acos is not computed
  T sim_closest = this->ps_.col(0).transpose() * this->spx_->col(i);
  uint32_t z_i = 0;
  for(uint32_t k=1; k<this->K_; ++k)
  {
    T sim_k = this->ps_.col(k).transpose()* this->spx_->col(i);
    if( sim_k > sim_closest) // because of cosine distance
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

//template<class T>
//void SphericalKMeans<T>::initialize(const Matrix<T,Dynamic,Dynamic>& x)
//{
//
//}
//
template<class T>
SphericalKMeansKarcher<T>::SphericalKMeansKarcher(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen)
  : SphericalKMeans<T>(spx,K,pRndGen)
{}

template<class T>
SphericalKMeansKarcher<T>::~SphericalKMeansKarcher()
{}

template<class T>
Matrix<T,Dynamic,1> SphericalKMeansKarcher<T>::computeCenter(uint32_t k)
{
  Matrix<T,Dynamic,Dynamic> xPs(this->spx_->rows(),this->spx_->cols());
  Matrix<T,Dynamic,1> mean_k = karcherMean<T>(this->ps_.col(k), *(this->spx_),
        xPs, this->z_, k, 100,1);
  this->Ns_(k) = 0.0;
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      this->Ns_(k) ++;
    }
  return mean_k;
}

//template<class T>
//void SphericalKMeansKarcher<T>::updateCenters()
//{
//  Matrix<T,Dynamic,Dynamic> xPs(this->spx_->rows(),this->spx_->cols());
//#pragma omp parallel for
//  for(uint32_t k=0; k<this->K_; ++k)
//  {
////    Matrix<T,Dynamic,1> w(this->N_);
////    for(uint32_t i=0; i<this->N_; ++i)
////      if(this->z_(i) == k)
////        w(i) = 1.0;
////      else
////        w(i) = 0.0;
////    this->ps_.col(k) = karcherMeanWeighted<T>(this->ps_.col(k), *(this->spx_), w, 100);
//    this->ps_.col(k) = karcherMean<T>(this->ps_.col(k), *(this->spx_),
//        xPs, this->z_, k, 100,1);
//  }
//}


#pragma once

#include <array>
#include <utility>
#include <string>
#include <stdexcept>
#include <Eigen/Dense>

#include "fusion_layer/temporal_aligner.hpp"
#include "object_model_msgs/msg/object_model.hpp"
#include "types.hpp"

class TemporalAlignerEKF : public TemporalAligner {
    bool is_initialized;

    ctra_array_t state_array;

    ctra_matrix_t P;
    ctra_matrix_t Q;

 public:
    TemporalAlignerEKF();
    TemporalAlignerEKF(const state_t& initial_state);

    /*
     * Index for ctra_state.
     *
     * In this work heading is different from yaw. Heading is about the direction the object is going (velocity vector direction),
     *   and yaw is the orientation of the object.
     */
    enum CTRAIndexes {X_IDX, Y_IDX, HEADING_IDX, VELOCITY_IDX, YAW_RATE_IDX, ACCELERATION_IDX};

    state_t align(float delta_t);
    static void align(float delta_t, state_t& state, ctra_squared_t& covariation);

    state_t get_state() const;
    void update(const state_t& measurement, const ctra_squared_t& measurement_noise_matrix, const capable_vector_t& capable);

    /*
    * From [x, y, heading, v, yaw_rate, a] to [x, y, vx, vy, ax, ay, yaw, yaw_rate]
    * yaw must be replaced in the result manually
    */
    static state_t format_to_object_model(const ctra_array_t& state, float yaw, float velocity_heading, float acceleration_heading);

    /*
    * From [x, y, vx, vy, ax, ay, yaw, yaw_rate] to [x, y, heading, v, yaw_rate, a]
    */
    static ctra_array_t format_from_object_model(const state_t& state);

    static std::pair<float, float> get_headings(const state_t& state);

 private:
    void predict(float delta_t);

    static void predict(float delta_t, ctra_array_t& state, ctra_matrix_t& covariation);

    static ctra_matrix_t calculate_process_noise(float delta_t);

    static ctra_array_t predict_state(float delta_t, const ctra_array_t& state);

    static ctra_matrix_t predict_covariation(float delta_t, const ctra_array_t& state, const ctra_matrix_t& covariation, const ctra_matrix_t& process_noise);

    static ctra_matrix_t gen_ja_matrix(float delta_t, const ctra_array_t& state);

    static void disable_not_capable_attributes(ctra_matrix_t& JH, const capable_vector_t& capable);
};