#include <cstdlib>
#include <ctime>

#include <iostream>
#include <unordered_set>

#include <boost/program_options.hpp>

#include "sling.h"
#include "dfg.h"
#include "graph.h"
#include "local_push.h"
#include "simrank.h"
#include "tkde17_sim.h"
#include "cloud_walker.h"
#include "linearD.h"
#include "bprw.h"

//#include <reads/reads.h>
//#include <reads/readsd.h>
//#include <reads/readsrq.h>

using namespace std;
using namespace std::chrono;
using namespace boost::program_options;

vector<NodePair> e1 = { // the test graph
        {1, 2},
        {2, 4},
        {4, 1},
        {4, 3},
        {3, 1}
};
vector<NodePair> e2 = {
        {6, 8},
        {3, 1},
        {3, 2},
        {3, 5},
        {4, 6},
        {5, 6},
        {3, 4},
        {6, 7}
};
vector<NodePair> e3 = {
        {0, 0},
        {1, 2},
        {3, 1},
        {1, 3},
        {1, 4},
        {2, 1},
        {4, 1}
};
vector<NodePair> e4 = {
        {1, 3},
        {1, 4},
        {1, 5},
        {2, 4},
        {2, 5},
        {2, 6}
};
vector<NodePair> e6 = {
        {0, 1},
        {0, 2},
        {0, 3},
        {1, 0},
        {1, 2},
        {2, 1},
        {2, 0},
        {3, 0},
        {3, 4},
        {4, 3}
};

void build_graph(DirectedG &g, vector<NodePair> e) {
    DirectedG::vertex_descriptor i;
    for (auto &item:e) {
        add_edge(item.first, item.second, g);
    }
    // show_graph(g);
}

void compare_sim(const SimRank_matrix &s1, LocalPush &lp, int n) {
    // compare results with ground truth
    double error = 0;
    for (int i = 0; i < n; i++) {
        for (int j = i; j < n; j++) {
            cout << i << " " << j << ": " << str(format("%.4f") % s1[i][j])
                 << "    " << format("%.4f") % lp.P[NodePair(i, j)]
                 << "    error: " << str(format("%.4f") % (lp.P[NodePair(i, j)] - s1[i][j]))
                 << endl;
            error += abs(lp.P[NodePair(i, j)] - s1[i][j]);
        }
    }
    cout << "total error: " << error << endl;
}

void test_local_push_test_data(vector<NodePair> &e1, bool is_full = false) {
    DirectedG g;
    build_graph(g, e1);
    // show_graph(g);
    double C = 0.6;
    size_t n = num_vertices(g);
    SimRank_matrix s{boost::extents[n][n]};
    basic_simrank(g, C, s);
    double epsilon = 0.001;
    LocalPush *lp;
    if (is_full) {
        lp = new Full_LocalPush(g, "small_test", C, epsilon, n);
    } else {
        lp = new Reduced_LocalPush(g, "small_test", C, epsilon, n);
    }
    // Reduced_LocalPush lp(g, "small_test", C, epsilon, n);
    lp->local_push(g);
    compare_sim(s, *lp, n);
    cout << "size of R: " << lp->R.size() << endl;
    cout << "size of P: " << lp->P.size() << endl;
    delete lp;
}

void test_dynamic_local_push(vector<NodePair> &e) {
    DirectedG g;
    std::unordered_set<int> nodes;
    for (auto &edge: e) {
        nodes.insert(edge.first);
        nodes.insert(edge.second);
    }
    add_vertex(g);
    for (auto &node:nodes) {
        add_vertex(g);
    }
    int n = num_vertices(g);
    double C = 0.6;
    cout << "number of vertices of g " << num_vertices(g) << endl;
    double epsilon = 0.00001;
    Reduced_LocalPush lp(g, "small_test", C, cal_rmax(C, epsilon), n);
    lp.local_push(g);
    cout << "before showing lp" << endl;
    // lp.show();

    cout << "begin add edges" << endl;
    for (auto &edge:e) {
        // cout << "inserting " << edge.first << " "  << edge.second << endl;
        add_edge(edge.first, edge.second, g);
        lp.insert(edge.first, edge.second, g);
    }
    SimRank_matrix s{boost::extents[n][n]};
    basic_simrank(g, C, s);
    compare_sim(s, lp, n);
}

bool lp_file_exists(string data_name, double c, double epsilon, size_t n, bool is_full = false) {
    LocalPush *lp_test;
    if (is_full) {
        lp_test = new Full_LocalPush();
    } else {
        lp_test = new Reduced_LocalPush();
    }
    // Reduced_LocalPush lp_test;
    lp_test->g_name = data_name;
    lp_test->c = c;
    lp_test->epsilon = epsilon;
    string data_path = lp_test->get_file_path_base() + ".P";
    cout << "data path !!!!" << data_path << endl;
    if (file_exists(data_path)) {
        return true;
    } else {
        return false;
    }

    delete lp_test;
}

void test_localpush(string data_name, double c = 0.8, double epsilon = 0.001, bool is_full = false) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    size_t n = num_vertices(g);

    if (lp_file_exists(data_name, c, epsilon, n, is_full)) {
        cout << "already computed..." << endl;
        return;
    }

    LocalPush *lp;
    if (is_full) {
        lp = new Full_LocalPush(g, data_name, c, epsilon, n);
    } else {
        lp = new Reduced_LocalPush(g, data_name, c, epsilon, n);
        // Reduced_LocalPush lp(g, data_name, c, epsilon, n);
    }
    // compute from 0
    // Full_LocalPush lp(g, data_name, C, r_max);
    lp->local_push(g);
    // lp.save();
    cout << "size of R: " << lp->R.size() << endl;
    cout << "size of P: " << lp->P.size() << endl;
    cout << "writing to disk.." << endl;
    lp->save();
    delete lp;
}

void IO_test(string data_name) {
    DirectedG g;
    double epsilon = 0.001;
    double C = 0.6;
    double r_max = cal_rmax(C, epsilon);
    load_graph(get_edge_list_path(data_name), g);
    size_t n = num_vertices(g);
    Reduced_LocalPush lp(g, data_name, C, r_max, n);
    // Full_LocalPush lp(g, data_name, C, r_max);
    lp.local_push(g);
    lp.save();

    Reduced_LocalPush lp2(g, data_name, C, r_max, n);
    cout << "estimate of 0 0:  " << lp2.P[NodePair(0, 0)] << endl;
    // cout << "P size "<<lp2.P.size() << endl;
}

void test_hashIO() {
    // test case for serialize a set of objects
    DensePairMap<int> np, np2;
    string f_name("./test.dat");
    np.add(10);
    np[NodePair(1, 9)] = 10;
    np[NodePair(7, 8)] = 4;
    np.save(f_name);

    np2.load(f_name);
    cout << np2.size() << endl;

}

/* the experiment codes */
void exp_dynamic(string data_name, double c, double epsilon, int num_updates = 100) {
    // exp codes for dynamic graph, score for old graph must be already computed
    // load the graph
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    size_t n = num_vertices(g);
    if (!lp_file_exists(data_name, c, epsilon, n)) {
        cout << data_name << ": the lp file has not been computed!!!" << endl;
        return;
    }
    Reduced_LocalPush rlp(g, data_name, c, epsilon, n); // already load the local push data

    // generate edges
    vector<pair<unsigned int, unsigned int>> ins_edges;
    /* initialize random seed: */
    srand(time(NULL));
    while (ins_edges.size() < num_updates) {
        auto e1 = rand() % n;
        auto e2 = rand() % n;
        if (boost::edge(e1, e2, g).second == false) {
            ins_edges.push_back({e1, e2});
        }
    }

    auto start = std::chrono::high_resolution_clock::now();
    // do the test
    for (auto &edge:ins_edges) {
        // add to g
        add_edge(edge.first, edge.second, g);
        // update P and R
        rlp.insert(edge.first, edge.second, g);
    }

    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;

    cout << "data: " << data_name << endl;
    cout << "number of updates: " << num_updates << endl;
    cout << "total time: " << elapsed.count() << endl;
    cout << "avg time: " << elapsed.count() / num_updates << endl;
    cout << "mem usage: " << getValue() << endl;

    return;
}

void compute_ground_truth(string data_name, double c = 0.6, double epsilon = 0.00001) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    TruthSim ts(data_name, g, c, epsilon);
    size_t n = num_vertices(g);
    cout << "computing " << data_name << " " << c << " " << epsilon << endl;
    ts.run(g);
    ts.save();
    if (n < 100) {
        cout << "ground truth for: " << data_name << endl;
        cout << ts.sim << endl;
    }
}

void test_tkde17(string data_name, float c = 0.6, float epsilon = 0.001) {
    // run tkde 17 experiments
    cout << "computing " << data_name << " " << c << epsilon << endl;
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    LinearSystemSim lys(data_name, g, c, epsilon);
    // cout << "--------------" << endl;
    // size_t n = num_vertices(g);
    // SimRank_matrix s{boost::extents[n][n]};
    // basic_simrank(g,c, s);
    // for(size_t i = 0;i<n;i++){
    //     for(size_t j = 0; j<n;j++){
    //         cout << s[i][j] << " ";
    //     }
    //     cout << endl;
    // }
    // cout << "--------------" << endl;
    // TruthSim ts("test_g", g, c, 0.00001);
    // ts.run(g);
    // ts.save();
    // cout << ts.sim << endl;

    cout << "==============" << endl;
    return;
}

void generate_effective_error_rlp(string data_name) {
    // generate rlp effective
    string ERROR_DIR("/homes/ywangby/workspace/dynamicSim/datasets/effective_error/");
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    double c = 0.6;
    size_t n = num_vertices(g);
    double epsilon = 0.001;
    ofstream out(ERROR_DIR + data_name + string(".rlp.txt"));
    TruthSim ts(data_name, g, c, epsilon);
    cout << ts.sim.sum() << endl;
    while (epsilon < 0.011) {
        Reduced_LocalPush rlp(g, data_name, c, epsilon, n);
        // make a matrix
        MatrixXf lp_result(n, n);
        lp_result.setZero();
        for (size_t i = 0; i < n; i++) {
            for (auto &item: rlp.P.v[i]) {
                lp_result(i, item.first) = item.second; // fill up the matrix
                lp_result(item.first, i) = item.second; // fill up the matrix
            }
        }
        cout << lp_result(2, 2) << " " << ts.sim(2, 2) << endl;
        double max_error = (lp_result - ts.sim).cwiseAbs().maxCoeff();
        double mean_error = (lp_result - ts.sim).cwiseAbs().mean();
        out << max_error << " " << mean_error << endl;
        epsilon += 0.001;
    }
    out.close();
}

void generate_effective_error_flp(string data_name) {
    // generate rlp effective
    string ERROR_DIR("/homes/ywangby/workspace/dynamicSim/datasets/effective_error/");
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    double c = 0.6;
    size_t n = num_vertices(g);
    double epsilon = 0.001;
    ofstream out(ERROR_DIR + data_name + string(".flp.txt"));
    TruthSim ts(data_name, g, c, epsilon);
    cout << ts.sim.sum() << endl;
    while (epsilon < 0.011) {
        Full_LocalPush rlp(g, data_name, c, epsilon, n);
        // make a matrix
        MatrixXf lp_result(n, n);
        lp_result.setZero();
        for (size_t i = 0; i < n; i++) {
            for (auto &item: rlp.P.v[i]) {
                lp_result(i, item.first) = item.second; // fill up the matrix
                lp_result(item.first, i) = item.second; // fill up the matrix
            }
        }
        cout << lp_result(2, 2) << " " << ts.sim(2, 2) << endl;
        double max_error = (lp_result - ts.sim).cwiseAbs().maxCoeff();
        double mean_error = (lp_result - ts.sim).cwiseAbs().mean();
        out << max_error << " " << mean_error << endl;
        epsilon += 0.001;
    }
    out.close();
}

void compute_tkde17_error(string data_name) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    double c = 0.6;
    double epsilon = 0.01;
    size_t n = num_vertices(g);
    ofstream out(TKDE17_RESULT_DIR + data_name + string(".error"));
    TruthSim ts(data_name, g, c, epsilon);
    cout << ts.sim.sum() << endl;
    LinearSystemSim tkde17(data_name, g, c, epsilon);
    double max_error = (ts.sim - tkde17.sim).cwiseAbs().maxCoeff();
    double mean_error = (ts.sim - tkde17.sim).cwiseAbs().mean();
    out << max_error << " " << mean_error << endl;
    out.close();
}

void compute_cloudwalker_error(string data_name) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    cout << "load graph complete." << endl;
    double c = 0.6;
    int T = 10;
    int R = 100;
    int R_prime = 10000;
    int L = 3;
    double epsilon = 0.01;
    auto n = num_vertices(g);
    CloudWalker cw(&g, data_name, c, T, L, R, R_prime);
    cw.load();
    auto sim = cw.sim.cast<float>();
    ofstream out(CLOUD_WALKER_DIR + data_name + string(".error"));
    TruthSim ts(data_name, g, c, epsilon);
    cout << ts.sim.sum() << endl;
    double mean_error = (ts.sim - sim).cwiseAbs().mean();

    double max_error = (ts.sim - sim).cwiseAbs().maxCoeff();

    out << max_error << " " << mean_error << endl;
    out.close();
}

void compute_linearD_error(string data_name) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    cout << "load graph complete." << endl;
    double c = 0.6;
    int T = 10;
    int R = 100;
    int R_prime = 10000;
    int L = 3;
    double epsilon = 0.01;
    auto n = num_vertices(g);
    LinearD lin(&g, data_name, c, T, L, R);
    lin.load();
    auto sim = lin.sim.cast<float>();
    ofstream out(LINEAR_D_DIR + data_name + string(".error"));
    TruthSim ts(data_name, g, c, epsilon);
    cout << ts.sim.sum() << endl;
    double mean_error = (ts.sim - sim).cwiseAbs().mean();

    double max_error = (ts.sim - sim).cwiseAbs().maxCoeff();

    out << max_error << " " << mean_error << endl;
    out.close();
}

void test_cloud_walker(string data_name, double c = 0.6, int T = 10, int L = 3, int R = 100, int R_prime = 10000) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    srand(time(NULL)); // random number generator
    std::chrono::duration<double> elapsed;
    int sample_size = 1000;

    auto start = std::chrono::high_resolution_clock::now();
    CloudWalker cw(&g, data_name, c, T, L, R, R_prime);
    auto pre_time = std::chrono::high_resolution_clock::now();
    size_t n = num_vertices(g);
    ofstream out(CLOUD_WALKER_DIR + cw.g_name + string(".meta")); // the file to store the execute info
    elapsed = pre_time - start;
    out << elapsed.count() << endl; // record the pre-processing time
    out << n << endl; // number of vertices
    for (int i = 0; i < sample_size; i++) {
        if (i % 10 == 0) {
            cout << i << " th sample of " << cw.g_name << endl;
        }
        int source_node = rand() % n;
        VectorXd tmp(n);
        auto start_ss = std::chrono::high_resolution_clock::now();
        cw.mcss(source_node, tmp);
        auto end_ss = std::chrono::high_resolution_clock::now();
        elapsed = end_ss - start_ss;
        auto mem_size = cw.mem_size; // record the memeory size
        auto cpu_time = elapsed.count();
        out << cpu_time << " " << mem_size << endl;
    }
    out.close();

    // cout << "D: " << endl;
    // cout << cw.D << endl;

    // cw.mcap();
    // cout << cw.get_file_path_base() << endl;
    // cout << cw.sim << endl;
    // cw.save();
    // cout << "return ... " << endl;
    return;
}

void test_linearD(string data_name, double c, int T, int L, int R) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    srand(time(NULL)); // random number generator
    std::chrono::duration<double> elapsed;
    int sample_size = 1000;

    auto start = std::chrono::high_resolution_clock::now();
    LinearD lin(&g, data_name, c, T, L, R);
    auto pre_time = std::chrono::high_resolution_clock::now();
    size_t n = num_vertices(g);
    ofstream out(LINEAR_D_DIR + lin.g_name + string(".meta")); // the file to store the execute info
    elapsed = pre_time - start;
    out << elapsed.count() << endl; // record the pre-processing time
    out << n << endl; // number of vertices
    for (int i = 0; i < sample_size; i++) {
        if (i % 10 == 0) {
            cout << i << " th sample for" << lin.g_name << endl;
        }
        int source_node = rand() % n;
        VectorXd tmp(n);
        auto start_ss = std::chrono::high_resolution_clock::now();
        lin.single_source(source_node, tmp);
        auto end_ss = std::chrono::high_resolution_clock::now();
        elapsed = end_ss - start_ss;
        auto mem_size = lin.mem_size; // record the memeory size
        auto cpu_time = elapsed.count();
        out << cpu_time << " " << mem_size << endl;
    }
    out.close();

    // lin.mcap();
    // cout << lin.get_file_path_base() << endl;
    // cout << lin.sim << endl;
    // lin.save();
    cout << "return ... " << endl;
    return;
}

void test_lineD_all_pair(string data_name, double c, int T, int L, int R) {
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    LinearD lin(&g, data_name, c, T, L, R);
    lin.all_pair();
    cout << lin.get_file_path_base() << endl;
    lin.save();
    return;
}

//void test_readsd(string data_name, double c, double epsilon, int r, int t) {
//    string data_path = get_edge_list_path(data_name);
//    DirectedG g;
//    load_graph(data_path, g);
//    int n = num_vertices(g);
//
//    double *ansVal = new double[n];
//
//    // reads i1(argv[1], n, r, c, t);
//    // //querying one-to-all simrank value from node o
//    // i1.queryAll(0, ansVal);
//    auto start = std::chrono::high_resolution_clock::now();
//
//    readsd i2((char *) data_path.c_str(), n, r, c, t);
//    //querying one-to-all simrank value from node o
//    i2.queryAll(1, ansVal);
//
//    // readsrq i3(argv[1], n, r, rq, c, t);
//    // //querying one-to-all simrank value from node o
//    // i3.queryAll(0, ansVal);
//    auto end = std::chrono::high_resolution_clock::now();
//    std::chrono::duration<double> elapsed;
//    elapsed = end - start;
//    cout << format("total cost: %s s") % elapsed.count() << endl;
//
//
//    delete[] ansVal;
//}

//void test_readsrq(string data_name, double c, double epsilon, int r, int rq, int t) {
//    string data_path = get_edge_list_path(data_name);
//    DirectedG g;
//    load_graph(data_path, g);
//    int n = num_vertices(g);
//
//    double *ansVal = new double[n];
//
//    // reads i1(argv[1], n, r, c, t);
//    // //querying one-to-all simrank value from node o
//    // i1.queryAll(0, ansVal);
//
//    readsrq i((char *) data_path.c_str(), n, r, rq, c, t);
//    //querying one-to-all simrank value from node o
//    auto start = std::chrono::high_resolution_clock::now();
//    i.queryAll(1, ansVal);
//
//    // readsrq i3(argv[1], n, r, rq, c, t);
//    // //querying one-to-all simrank value from node o
//    // i3.queryAll(0, ansVal);
//    auto end = std::chrono::high_resolution_clock::now();
//    std::chrono::duration<double> elapsed;
//    elapsed = end - start;
//    cout << format("total cost: %s s") % elapsed.count() << endl;
//
//
//    delete[] ansVal;
//}

void test_bp(string data_name, double c, double epsilon, double delta, int x, int y) {
    // test_readsrq(data_name,c,epsilon,R_prime,R,t);
    // test the max heap functionality
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    BackPush bprw(data_name, g, c, epsilon, delta);
    size_t n = num_vertices(g);
    NodePair q{x, y};
    // for(int i = 0; i < 100;i++){
    auto start = std::chrono::high_resolution_clock::now();

    double result = bprw.query_one2one(q);

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed;
    elapsed = end - start;

    cout << q << ": " << result << endl;

    TruthSim ts(data_name, g, c, epsilon);
    cout << format("sim: %s:%s") % q % result << endl;
    cout << format("ground truth: %s") % ts.sim(q.first, q.second) << endl;
    cout << format("error: %s") % (ts.sim(q.first, q.second) - result) << endl;
    cout << format("total query cost: %s s") % elapsed.count() << endl; // record the pre-processing time

}

void test_sling(string data_name, double c, double epsilon, int x, int y) {
    cout << "testing sling" << endl;
    // load graph

    cout << format("%s %s %s") % data_name % c % epsilon << endl;
    DirectedG g;
    load_graph(get_edge_list_path(data_name), g);
    Graph sling_g(g); // the sling graph

    NodePair q{x, y};

    // print the graph
    // cout << sling_g << endl;

    Sling sling(&sling_g, c);
    // build the index
    sling.calcD(0.005);
    sling.backward(0.000725);
    // compute single-pair query
    double result;

    auto start = std::chrono::high_resolution_clock::now();
    result = sling.simrank(q.first, q.second);
    auto end = std::chrono::high_resolution_clock::now();

    std::chrono::duration<double> elapsed;
    elapsed = end - start;

    cout << result << endl;

    // ground truth
    TruthSim ts(data_name, g, c, epsilon);
    cout << format("sim: %s:%s") % q % result << endl;
    cout << format("ground truth: %s") % ts.sim(q.first, q.second) << endl;
    cout << format("error: %s") % (ts.sim(q.first, q.second) - result) << endl;
    cout << format("total query cost: %s s") % elapsed.count() << endl; // record the pre-processing time

}

int main(int args, char *argv[]) {
    try {
        options_description desc{"Options"};
        string algo_name;
        string data_name;
        double c = 0.6;
        double epsilon = 0.01;
        double delta = 0.01;
        int number_of_updates = 100; // number of edge updates for dynamic graphs
        int r = 100; // number of sample
        int rq = 10; // number of online reverse random walk
        int t = 10; // the length of random walks
        int L = 3; // the parameter for D computation
        int T = 10;
        int R = 100;
        int R_prime = 10000;
        int x, y;
        desc.add_options()
                // notice no space in the first parameter
                ("help,h", "Help Screen")
                ("DataName,d", value<string>(&data_name), "Graph Name")
                ("IterationD,l", value<int>(&L), "number of iterations to compute D")
                ("algorithm,a", value<string>(&algo_name), "Algorithm Name")
                ("decay,c", value<double>(&c), "Decay Facrot c")
                ("epsilon,e", value<double>(&epsilon), "Error bound")
                ("NumUpdates,n", value<int>(&number_of_updates), "Number of updates (used in dynamic graphs)")
                ("R,R", value<int>(&R), "number of random walk to estimates D/online random walks for READS")
                ("R_prime,r", value<int>(&R_prime), "number of random walk for MCSS/READS")
                ("T,t", value<int>(&T), "T: the number of iteration of linearization technique/length of random walks")
                ("x,x", value<int>(&x), "x: the query node")
                ("y,y", value<int>(&y), "y: the query node");

        variables_map vm;
        store(parse_command_line(args, argv, desc), vm);
        notify(vm);

        if (vm.count("help")) {
            cout << desc << endl;
        } else if (vm.count("algorithm")) {
            if (algo_name == "full") {
                test_localpush(data_name, c, epsilon, true);
            } else if (algo_name == "static") {
                test_localpush(data_name, c, epsilon);
            } else if (algo_name == "truth") {
                compute_ground_truth(data_name, c); // ignore the other parameters for ground truth
            }
            else if (algo_name == "tkde17") {
                test_tkde17(data_name, c, epsilon);
            }
            else if (algo_name == "dynamic") {
                exp_dynamic(data_name, c, epsilon, number_of_updates);
            } else if (algo_name == "cloudwalker") {
                test_cloud_walker(data_name, c, T, L, R, R_prime);
            } else if (algo_name == "lin") {
                test_linearD(data_name, c, T, L, R);
            } else if (algo_name == "linAll") { // Linearized all-pair
                test_lineD_all_pair(data_name, c, T, L, R);
            }
//            else if (algo_name == "readsd") {
//                test_readsd(data_name, c, epsilon, R_prime, t);
//            } else if (algo_name == "readsrq") {
//                test_readsrq(data_name, c, epsilon, R_prime, R, t);
//            }
            else if (algo_name == "bp") { // the backward push algorithm
                test_bp(data_name, c, epsilon, delta, x, y);
            } else if (algo_name == "sling") {
                test_sling(data_name, c, epsilon, x, y);
            }
        } else {
            // default behavior
            // vector<string> datasets{"ca-GrQc","ca-HepTh","p2p-Gnutella06","wiki-Vote"};
            // for(auto data_name:datasets){
            //     // test_lineD_all_pair(data_name, c, T, L, R);
            //     compute_linearD_error(data_name);
            // }
            //
            // test sling

        }
    }
    catch (const error &ex) {
        cerr << ex.what() << endl;
    }

    // /* old style parsing */
    // vector<NodePair> e5;
    // int n = 5;
    // for(int i = 1; i< n; i++){
    //     for(int j = 1; j< n;j++){
    //         if(i !=j ){ // no self loop
    //             e5.push_back(NodePair(i,j));
    //         }
    //     }
    // }
    // if(args >=5 ){
    //     string data_name = string(argv[2]);
    //     double c = atof(argv[3]);
    //     double epsilon = atof(argv[4]);
    //     if(string(argv[1]) == "static"){
    //         test_localpush(data_name, c, epsilon);
    //     }else if(string(argv[1]) == "dynamic"){
    //         if(args == 5){
    //             exp_dynamic(data_name, c, epsilon);
    //         }else if(args == 6){
    //             auto number_of_updates = atoi(argv[5]);
    //             exp_dynamic(data_name, c, epsilon, number_of_updates);
    //         }
    //     }else if(string(argv[1]) == "full"){
    //         test_localpush(data_name, c, epsilon, true);
    //     }else if(string(argv[1]) == "truth"){
    //         compute_ground_truth(data_name); // ignore the other parameters for ground truth
    //     }else if(string(argv[1]) == "tkde17"){
    //         test_tkde17(data_name, c, epsilon);
    //     }
    // }else if(args == 2){
    //     if(string(argv[1]) == string("test")){
    //         cout << "testing full local push " << endl;
    //         test_local_push_test_data(e1, true);
    //         test_local_push_test_data(e2, true);
    //         test_local_push_test_data(e3, true);
    //         test_local_push_test_data(e4, true);
    //         // test Dense hash map
    //         // try clique
    //         test_local_push_test_data(e5, true);

    //         cout << "testing reduced local push" << endl;
    //         test_dynamic_local_push(e1);
    //         test_dynamic_local_push(e2);
    //         test_dynamic_local_push(e3);
    //         test_dynamic_local_push(e4);
    //         // // test Dense hash map
    //         // // try clique

    //         test_dynamic_local_push(e5);
    //     }
    // }else{
    //     vector<string> datasets{"ca-GrQc","ca-HepTh","p2p-Gnutella06","wiki-Vote"};
    //     for(auto s:datasets){
    //         // compute_tkde17_error(s);
    //         generate_effective_error_rlp(s);
    //         generate_effective_error_flp(s);
    //     }
    //     // test tkde 17
    //     // vector<vector<NodePair>> egs{e1,e2,e3,e4,e5, e6};
    //     // vector<vector<NodePair>> egs{e1};
    //     // for(auto& e: egs){
    //     //     DirectedG g;
    //     //     build_graph(g,e);
    //     //     test_tkde17(g);
    //     // }
    //     return 0;
    // }
    // //     }else if(string(argv[1]) == string("dynamic")){
    // //         test_dynamic_local_push(e1);
    // //         test_dynamic_local_push(e2);
    // //         test_dynamic_local_push(e3);
    // //         test_dynamic_local_push(e4);
    // //         // // test Dense hash map
    // //         // // try clique

    // //         test_dynamic_local_push(e5);
    // //     }else if(string(argv[1]) == string("io")){
    // //         IO_test("ca-GrQc");
    // //     }else{
    // //         test_localpush(string(argv[1]));
    // //     }
    // // }else if(args == 3 ){
    // //     string data_name = string(argv[1]);
    // //     double c = atof(argv[2]);
    // //     test_localpush(data_name, c);
    // // }else if(args == 4){
    // //     string data_name = string(argv[1]);
    // //     double c = atof(argv[2]);
    // //     double epsilon = atof(argv[3]);
    // //     test_localpush(data_name, c, epsilon);
    // // }else{
    // //     // IO_test("ca-GrQc");
    // //     cout << getValue() << endl;
    // // }
    // // DirectedG  g;
    // // cout << "loading graph" << endl;
    // // load_graph(get_edge_list_path("soc-LiveJournal1"), g);
    // // string fp("dfg_test.txt");
    // // cout << "saving...." << endl;
    // // dfg.save(fp);
    // // DFG_Index test_dfg;
    // // cout << "loading..." << endl;
    // // test_dfg.load(fp);
    // // cout << test_dfg.n << endl;
    // // Owg o;
    // // load_sparse_map(o, string("one-way-livej"));
    // // cout << o.size() << endl;
}


#include <deal.II/base/utilities.h>

#include "base_problem.h"
#include "linear_elasticity.h"
#include "poisson.h"
#include "stokes.h"

/**
 * \mainpage Adaptive FEM
 *
 * This is the starting code for laboratory number 10 of the course "Theory and
 * Practice of Finite Element methods".
 */
int
main(int argc, char **argv)
{
  const std::string program_name(argv[0]);
  if (program_name.find("poisson") != std::string::npos)
    return run<Poisson<2>>(argc, argv);
  if (program_name.find("linear_elasticity") != std::string::npos)
    return run<LinearElasticity<2>>(argc, argv);
  if (program_name.find("stokes") != std::string::npos)
    return run<Stokes<2>>(argc, argv);
}


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// 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)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax convert
#include <boost/test/unit_test.hpp>

#include <mitrax/convert.hpp>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

constexpr int data[2][2] = {{0, 1}, {2, 3}};

template < typename M >
constexpr bool check(M const& m){
	return
		m.cols() == 2_CS &&
		m.rows() == 2_RS &&
		m(0_c, 0_r) == 0 &&
		m(1_c, 0_r) == 1 &&
		m(0_c, 1_r) == 2 &&
		m(1_c, 1_r) == 3;
}

using int_ref = std::reference_wrapper< int >;

using cint_ref = std::reference_wrapper< int const >;

template < typename T >
auto rt_id(T&& v){
	return type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = type_id< T >();

BOOST_AUTO_TEST_SUITE(suite_convert)

BOOST_AUTO_TEST_CASE(test_convert_as_std_matrix){
	constexpr auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);
	auto m5 = m1;

	constexpr auto n1 = as_std_matrix(m1);
	auto const n2 = as_std_matrix(m2);
	auto const n3 = as_std_matrix(m3);
	auto const n4 = as_std_matrix(m4);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int, 0_C, 0_R > >));

	auto const o1 = as_std_matrix(m5);
	auto const o2 = as_std_matrix(m2);
	auto const o3 = as_std_matrix(m3);
	auto const o4 = as_std_matrix(m4);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< int, 0_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_type){
	constexpr auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);
	auto m5 = m1;

	constexpr auto n1 = convert< double >(m1);
	auto const n2 = convert< double >(m2);
	auto const n3 = convert< double >(m3);
	auto const n4 = convert< double >(m4);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< double, 0_C, 0_R > >));

	auto const o1 = convert< double >(m5);
	auto const o2 = convert< double >(m2);
	auto const o3 = convert< double >(m3);
	auto const o4 = convert< double >(m4);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< double, 0_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2){
	constexpr auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);
	auto m5 = m1;

	constexpr auto n1 = convert(m1, 2_CS, 2_RS);
	auto const n2 = convert(m2, 2_CS, 2_RS);
	auto const n3 = convert(m3, 2_CS, 2_RS);
	auto const n4 = convert(m4, 2_CS, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int, 2_C, 2_R > >));

	auto const o1 = convert< int >(m5, 2_CS, 2_RS);
	auto const o2 = convert< int >(m2, 2_CS, 2_RS);
	auto const o3 = convert< int >(m3, 2_CS, 2_RS);
	auto const o4 = convert< int >(m4, 2_CS, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< int, 2_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert(m1, 2_CD, 2_RS);
	auto const n2 = convert(m2, 2_CD, 2_RS);
	auto const n3 = convert(m3, 2_CD, 2_RS);
	auto const n4 = convert(m4, 2_CD, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int, 0_C, 2_R > >));

	auto const o1 = convert< int >(m1, 2_CD, 2_RS);
	auto const o2 = convert< int >(m2, 2_CD, 2_RS);
	auto const o3 = convert< int >(m3, 2_CD, 2_RS);
	auto const o4 = convert< int >(m4, 2_CD, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< int, 0_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2rt){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert(m1, 2_CS, 2_RD);
	auto const n2 = convert(m2, 2_CS, 2_RD);
	auto const n3 = convert(m3, 2_CS, 2_RD);
	auto const n4 = convert(m4, 2_CS, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int, 2_C, 0_R > >));

	auto const o1 = convert< int >(m1, 2_CS, 2_RD);
	auto const o2 = convert< int >(m2, 2_CS, 2_RD);
	auto const o3 = convert< int >(m3, 2_CS, 2_RD);
	auto const o4 = convert< int >(m4, 2_CS, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< int, 2_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2rt){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert(m1, 2_CD, 2_RD);
	auto const n2 = convert(m2, 2_CD, 2_RD);
	auto const n3 = convert(m3, 2_CD, 2_RD);
	auto const n4 = convert(m4, 2_CD, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int, 0_C, 0_R > >));

	auto const o1 = convert< int >(m1, 2_CD, 2_RD);
	auto const o2 = convert< int >(m2, 2_CD, 2_RD);
	auto const o3 = convert< int >(m3, 2_CD, 2_RD);
	auto const o4 = convert< int >(m4, 2_CD, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< int, 0_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2_and_type){
	constexpr auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);
	auto m5 = m1;

	constexpr auto n1 = convert< double >(m1, 2_CS, 2_RS);
	auto const n2 = convert< double >(m2, 2_CS, 2_RS);
	auto const n3 = convert< double >(m3, 2_CS, 2_RS);
	auto const n4 = convert< double >(m4, 2_CS, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< double, 2_C, 2_R > >));

	auto const o1 = convert< double >(m5, 2_CS, 2_RS);
	auto const o2 = convert< double >(m2, 2_CS, 2_RS);
	auto const o3 = convert< double >(m3, 2_CS, 2_RS);
	auto const o4 = convert< double >(m4, 2_CS, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< double, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< double, 2_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2_and_type){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< double >(m1, 2_CD, 2_RS);
	auto const n2 = convert< double >(m2, 2_CD, 2_RS);
	auto const n3 = convert< double >(m3, 2_CD, 2_RS);
	auto const n4 = convert< double >(m4, 2_CD, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< double, 0_C, 2_R > >));

	auto const o1 = convert< double >(m1, 2_CD, 2_RS);
	auto const o2 = convert< double >(m2, 2_CD, 2_RS);
	auto const o3 = convert< double >(m3, 2_CD, 2_RS);
	auto const o4 = convert< double >(m4, 2_CD, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< double, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< double, 0_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2rt_and_type){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< double >(m1, 2_CS, 2_RD);
	auto const n2 = convert< double >(m2, 2_CS, 2_RD);
	auto const n3 = convert< double >(m3, 2_CS, 2_RD);
	auto const n4 = convert< double >(m4, 2_CS, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< double, 2_C, 0_R > >));

	auto const o1 = convert< double >(m1, 2_CS, 2_RD);
	auto const o2 = convert< double >(m2, 2_CS, 2_RD);
	auto const o3 = convert< double >(m3, 2_CS, 2_RD);
	auto const o4 = convert< double >(m4, 2_CS, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< double, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< double, 2_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2rt_and_type){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< double >(m1, 2_CD, 2_RD);
	auto const n2 = convert< double >(m2, 2_CD, 2_RD);
	auto const n3 = convert< double >(m3, 2_CD, 2_RD);
	auto const n4 = convert< double >(m4, 2_CD, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< double, 0_C, 0_R > >));

	auto const o1 = convert< double >(m1, 2_CD, 2_RD);
	auto const o2 = convert< double >(m2, 2_CD, 2_RD);
	auto const o3 = convert< double >(m3, 2_CD, 2_RD);
	auto const o4 = convert< double >(m4, 2_CD, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< double, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< double, 0_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2_and_type_to_ref){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< int_ref >(m1, 2_CS, 2_RS);
	auto const n2 = convert< int_ref >(m2, 2_CS, 2_RS);
	auto const n3 = convert< int_ref >(m3, 2_CS, 2_RS);
	auto const n4 = convert< int_ref >(m4, 2_CS, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int_ref, 2_C, 2_R > >));

	auto const o1 = convert< cint_ref >(m1, 2_CS, 2_RS);
	auto const o2 = convert< cint_ref >(m2, 2_CS, 2_RS);
	auto const o3 = convert< cint_ref >(m3, 2_CS, 2_RS);
	auto const o4 = convert< cint_ref >(m4, 2_CS, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< cint_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< cint_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< cint_ref, 2_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< cint_ref, 2_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2_and_type_to_ref){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< int_ref >(m1, 2_CD, 2_RS);
	auto const n2 = convert< int_ref >(m2, 2_CD, 2_RS);
	auto const n3 = convert< int_ref >(m3, 2_CD, 2_RS);
	auto const n4 = convert< int_ref >(m4, 2_CD, 2_RS);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int_ref, 0_C, 2_R > >));

	auto const o1 = convert< cint_ref >(m1, 2_CD, 2_RS);
	auto const o2 = convert< cint_ref >(m2, 2_CD, 2_RS);
	auto const o3 = convert< cint_ref >(m3, 2_CD, 2_RS);
	auto const o4 = convert< cint_ref >(m4, 2_CD, 2_RS);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< cint_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< cint_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< cint_ref, 0_C, 2_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< cint_ref, 0_C, 2_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2x2rt_and_type_to_ref){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< int_ref >(m1, 2_CS, 2_RD);
	auto const n2 = convert< int_ref >(m2, 2_CS, 2_RD);
	auto const n3 = convert< int_ref >(m3, 2_CS, 2_RD);
	auto const n4 = convert< int_ref >(m4, 2_CS, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int_ref, 2_C, 0_R > >));

	auto const o1 = convert< cint_ref >(m1, 2_CS, 2_RD);
	auto const o2 = convert< cint_ref >(m2, 2_CS, 2_RD);
	auto const o3 = convert< cint_ref >(m3, 2_CS, 2_RD);
	auto const o4 = convert< cint_ref >(m4, 2_CS, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< cint_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< cint_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< cint_ref, 2_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< cint_ref, 2_C, 0_R > >));
}

BOOST_AUTO_TEST_CASE(test_convert_convert_to_2rtx2rt_and_type_to_ref){
	auto m1 = make_matrix(2_CS, 2_RS, data);
	auto m2 = make_matrix(2_CD, 2_RS, data);
	auto m3 = make_matrix(2_CS, 2_RD, data);
	auto m4 = make_matrix(2_CD, 2_RD, data);

	auto const n1 = convert< int_ref >(m1, 2_CD, 2_RD);
	auto const n2 = convert< int_ref >(m2, 2_CD, 2_RD);
	auto const n3 = convert< int_ref >(m3, 2_CD, 2_RD);
	auto const n4 = convert< int_ref >(m4, 2_CD, 2_RD);

	BOOST_TEST(check(n1));
	BOOST_TEST(check(n2));
	BOOST_TEST(check(n3));
	BOOST_TEST(check(n4));

	BOOST_TEST(rt_id(n1) == (id< std_matrix< int_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n2) == (id< std_matrix< int_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n3) == (id< std_matrix< int_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(n4) == (id< std_matrix< int_ref, 0_C, 0_R > >));

	auto const o1 = convert< cint_ref >(m1, 2_CD, 2_RD);
	auto const o2 = convert< cint_ref >(m2, 2_CD, 2_RD);
	auto const o3 = convert< cint_ref >(m3, 2_CD, 2_RD);
	auto const o4 = convert< cint_ref >(m4, 2_CD, 2_RD);

	BOOST_TEST(check(o1));
	BOOST_TEST(check(o2));
	BOOST_TEST(check(o3));
	BOOST_TEST(check(o4));

	BOOST_TEST(rt_id(o1) == (id< std_matrix< cint_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o2) == (id< std_matrix< cint_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o3) == (id< std_matrix< cint_ref, 0_C, 0_R > >));
	BOOST_TEST(rt_id(o4) == (id< std_matrix< cint_ref, 0_C, 0_R > >));
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE MultiArray
#include <boost/test/unit_test.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/multi_array.hpp>
#include <vexcl/reductor.hpp>
#include <vexcl/function.hpp>
#include <boost/math/constants/constants.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(create)
{
    using vex::extents;
    using vex::indices;
    using vex::range;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::multi_array<double, 2> x(queue, extents[1024][1024]);

    BOOST_CHECK(x.size<0>() == 1024);
    BOOST_CHECK(x.size<1>() == 1024);

    auto view = x(indices[5][range(0,100)]);

    BOOST_CHECK(view.size<0>() == 100);
}

BOOST_AUTO_TEST_CASE(arithmetics)
{
    using vex::extents;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::multi_array<double, 3> x(queue, extents[32][32][32]);
    vex::multi_array<double, 3> y(queue, extents[32][32][32]);

    x.vec() = boost::math::constants::two_pi<double>() * vex::element_index();
    y.vec() = pow(sin(x.vec()), 2.0) + pow(cos(x.vec()), 2.0);

    check_sample(y.vec(), [](size_t, double v) {
            BOOST_CHECK_CLOSE(v, 1, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(slicing)
{
    using vex::extents;
    using vex::indices;
    using vex::_;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::multi_array<double, 3> x(queue, extents[32][32][32]);
    vex::multi_array<double, 3> y(queue, extents[32][32][32]);

    for(size_t i = 0; i < x.size<0>(); ++i)
        x(indices[i][_][_]).vec() = i;

    for(size_t i = 0; i < x.size<0>(); ++i)
        y(indices[_][_][i]).vec() = x(indices[i][_][_]).vec();

    check_sample(y.vec(), [](size_t idx, double v) {
            BOOST_CHECK_EQUAL(v, idx % 32);
            });
}

BOOST_AUTO_TEST_CASE(reducing)
{
    using vex::extents;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::multi_array<int, 3> x(queue, extents[32][32][32]);
    vex::vector<int> y(queue, 32 * 32);

    x.vec() = 1;

    for(int i = 0; i < 3; ++i) {
        y = vex::reduce<vex::SUM>(x, 0);
        check_sample(y, [](size_t, int v) { BOOST_CHECK_EQUAL(v, 32); });
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef _CELERITE2_REVERSE_HPP_DEFINED_
#define _CELERITE2_REVERSE_HPP_DEFINED_

#include <Eigen/Core>
#include "internal.hpp"

namespace celerite2 {
namespace core {

template <typename Input, typename Coeffs, typename Diag, typename LowRank, typename Work, typename InputOut, typename CoeffsOut, typename DiagOut,
          typename LowRankOut>
void factor_rev(const Eigen::MatrixBase<Input> &t,           // (N,)
                const Eigen::MatrixBase<Coeffs> &c,          // (J,)
                const Eigen::MatrixBase<Diag> &a,            // (N,)
                const Eigen::MatrixBase<LowRank> &U,         // (N, J)
                const Eigen::MatrixBase<LowRank> &V,         // (N, J)
                const Eigen::MatrixBase<Diag> &d,            // (N,)
                const Eigen::MatrixBase<LowRank> &W,         // (N, J)
                const Eigen::MatrixBase<Work> &S,            // (N, J*J)
                const Eigen::MatrixBase<Diag> &bd,           // (N,)
                const Eigen::MatrixBase<LowRank> &bW,        // (N, J)
                Eigen::MatrixBase<InputOut> const &bt_out,   // (N,)
                Eigen::MatrixBase<CoeffsOut> const &bc_out,  // (J,)
                Eigen::MatrixBase<DiagOut> const &ba_out,    // (N,)
                Eigen::MatrixBase<LowRankOut> const &bU_out, // (N, J)
                Eigen::MatrixBase<LowRankOut> const &bV_out  // (N, J)

) {
  UNUSED(a);
  UNUSED(V);

  ASSERT_ROW_MAJOR(Work);

  typedef typename Diag::Scalar Scalar;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, LowRank::ColsAtCompileTime> Inner;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;

  Eigen::Index N = U.rows(), J = U.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_VEC(DiagOut, ba, N);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);

  // Make local copies of the gradients that we need
  Inner Sn(J, J), bS(J, J);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Sn.data(), 1, J * J);
  Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, 1> bSWT;
  CoeffVector p(J), bp(J);

  Scalar dt, factor;
  bS.setZero();
  bt.setZero();
  bc.setZero();
  ba.noalias() = bd;
  bV.noalias() = bW;
  bV.array().colwise() /= d.array();
  for (Eigen::Index n = N - 1; n > 0; --n) {
    dt = t(n - 1) - t(n);
    p  = exp(c.array() * dt);

    ptr = S.row(n);

    // Step 6
    ba(n) -= W.row(n) * bV.row(n).transpose();
    bU.row(n).noalias() = -(bV.row(n) + 2.0 * ba(n) * U.row(n)) * Sn * p.asDiagonal();
    bS.noalias() -= U.row(n).transpose() * (bV.row(n) + ba(n) * U.row(n));

    // Step 4
    bp.array() = (bS * Sn + Sn.transpose() * bS).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n) -= factor;
    bt(n - 1) += factor;

    // Step 3
    bS   = p.asDiagonal() * bS * p.asDiagonal();
    bSWT = bS * W.row(n - 1).transpose();
    ba(n - 1) += W.row(n - 1) * bSWT;
    bV.row(n - 1).noalias() += W.row(n - 1) * (bS + bS.transpose());
  }

  bU.row(0).setZero();
  ba(0) -= bV.row(0) * W.row(0).transpose();
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void solve_lower_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                     const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                     const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                     const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                     const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                     const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                     const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                     const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                     Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                     Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                     Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                     Eigen::MatrixBase<LowRankOut> const &bW_out,      // (N, J)
                     Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bW, N, J);
  CAST_BASE(RightHandSideOut, bY);

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bW.setZero();
  bY = bZ;
  internal::forward_rev<true>(t, c, U, W, Y, Z, F, bY, bt, bc, bU, bW, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void solve_upper_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                     const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                     const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                     const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                     const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                     const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                     const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                     const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                     Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                     Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                     Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                     Eigen::MatrixBase<LowRankOut> const &bW_out,      // (N, J)
                     Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bW, N, J);
  CAST_BASE(RightHandSideOut, bY);

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bW.setZero();
  bY = bZ;
  internal::backward_rev<true>(t, c, U, W, Y, Z, F, bY, bt, bc, bU, bW, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void matmul_lower_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                      const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                      const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                      const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                      const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                      const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                      const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                      const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                      Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                      Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                      Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                      Eigen::MatrixBase<LowRankOut> const &bV_out,      // (N, J)
                      Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_MAT(RightHandSideOut, bY, N, Y.cols());

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bV.setZero();
  bY.setZero();
  internal::forward_rev<false>(t, c, U, V, Y, Z, F, bZ, bt, bc, bU, bV, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void matmul_upper_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                      const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                      const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                      const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                      const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                      const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                      const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                      const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                      Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                      Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                      Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                      Eigen::MatrixBase<LowRankOut> const &bV_out,      // (N, J)
                      Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_MAT(RightHandSideOut, bY, N, Y.cols());

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bV.setZero();
  bY.setZero();
  internal::backward_rev<false>(t, c, U, V, Y, Z, F, bZ, bt, bc, bU, bV, bY);
}

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_REVERSE_HPP_DEFINED_


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#include <boost/simd/function/std.hpp>
#include <boost/simd/arch/common/detail/generic/gammaln_kernel.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invpi.hpp>
#include <boost/simd/constant/logpi.hpp>
#include <boost/simd/constant/logsqrt2pi.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/zero.hpp>

#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/two.hpp>

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_dec.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/sinpi.hpp>
#include <boost/simd/function/sqr.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( gammaln_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::single_<A0>, X>
                             )
  {
    using sA0 = bd::scalar_of_t<A0>;
    A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      auto inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = bs::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = logical_or(is_equal(x, Inf<A0>()), inf_result);
      #endif
      auto ltza0 = is_ltz(a0);
      size_t nb = bs::nbtrue(ltza0);
      A0 r;
      A0 r1 =  other(q);
      if(nb > 0)
      {
        //treat negative
        r = bs::if_else(inf_result, Inf<A0>(), negative(q, r1));
        if (nb >= A0::static_size) return r;
      }
      A0 r2 = if_else(ltza0, r, r1);
      return bs::if_else(is_equal(a0, Minf<A0>()),
                          Nan<A0>(),
                          bs::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    using lA0 = bs::as_logical_t<A0>;
    static inline A0 negative(const A0& q,  const A0& w)
    {
      A0 p = bs::floor(q);
      A0 z = q - p;
      auto test2 = is_less(z, bs::Half<A0>() );
      z = bs::if_minus(test2, z, bs::One<A0>());
      z = q*bs::sinpi(z);
      z =  bs::abs(z);
      return -log(Invpi<A0>()*bs::abs(z))-w;
    }
    static inline A0 other(const A0& x)
    {
      auto xlt650 = is_less(x,A0(6.50) );
      size_t nb = nbtrue(xlt650);
      A0 r0x = x;
      A0 r0z = x;
      A0 r0s = One<A0>();
      A0 r1 = Zero<A0>();
      A0 p =  Nan<A0>();
      if (nb > 0)
      {
        auto kernelC = False<lA0>();
        A0 z = One<A0>();
        A0 tx = if_else_zero(xlt650, x);
        A0 nx = Zero<A0>();

        const A0 _075 = A0(0.75);
        const A0 _150 = A0(1.50);
        const A0 _125 = A0(1.25);
        const A0 _250 = A0(2.50);
        auto xge150 = is_greater_equal(x, _150);
        auto txgt250= is_greater(tx,_250);

        // x >= 1.5
        while (bs::any(logical_and(xge150, txgt250)))
        {
          nx = if_dec(txgt250, nx);
          tx = if_else(txgt250, x + nx, tx);
          z = if_else(txgt250, z*tx, z);
          txgt250= is_greater(tx,_250);
        }
        r0x = if_plus(xge150, x, nx - Two<A0>());
        r0z = if_else(xge150, z, r0z);
        r0s = if_else(xge150,One<A0>(), r0s);

        // x >= 1.25 && x < 1.5
        auto xge125 = is_greater_equal(x, _125);
        auto xge125t = logical_andnot(xge125, xge150);
        if (bs::any(xge125))
        {
          r0x =  if_else(xge125t, dec(x)    , r0x);
          r0z =  if_else(xge125t, z*x       , r0z);
          r0s =  if_else(xge125t, Mone<A0>(), r0s);
        }
        // x >= 0.75&& x < 1.5
        auto xge075  = is_greater_equal(x, _075);
        auto xge075t = logical_andnot(xge075, xge125);
        if (bs::any(xge075t))
        {
          kernelC =  xge075t;
          r0x =  if_else(xge075t, dec(x)    , r0x);
          r0z =  if_else(xge075t, One<A0>() , r0z);
          r0s =  if_else(xge075t, Mone<A0>(), r0s);
          p = detail::gammaln_kernel<A0>::gammalnC(r0x);
        }
        // tx < 1.5 && x < 0.75
        auto txlt150 = logical_andnot(is_less(tx,_150), xge075);
        if (bs::any(txlt150))
        {
          auto orig = txlt150;
          while( bs::any(txlt150) )
          {
            z  = if_else(txlt150, z*tx, z);
            nx = if_inc(txlt150, nx);
            tx = if_else(txlt150, x + nx, tx);
            txlt150= logical_andnot(is_less(tx,_150), xge075);
          }
          r0x =  if_plus(orig, r0x, nx - Two<A0>());
          r0z =  if_else(orig,z         , r0z);
          r0s =  if_else(orig,Mone<A0>(), r0s);
        }
        p =  if_else(kernelC, p, detail::gammaln_kernel<A0>::gammalnB(r0x));
        if (nb >= A0::static_size)
          return fma(r0x, p, r0s*bs::log(bs::abs(r0z)));
      }
      r0z = if_else(xlt650, bs::abs(r0z), x);
      A0 m = bs::log(r0z);
      r1 = fma(r0x, p, r0s*m);
      A0 r2 = fma(x-Half<A0>(),m,Logsqrt2pi<A0>()-x);
      r2 += detail::gammaln_kernel<A0>::gammaln2(rec(sqr(x)))/x;
      return if_else(xlt650, r1, r2);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( gammaln_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::double_<A0>, X >
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      auto inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = bs::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = is_equal(q, Inf<A0>());
      #endif
      auto test = is_less(a0, A0(-34.0));
      size_t nb = bs::nbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= A0::static_size)
          return bs::if_else(inf_result, Nan<A0>(), r);
      }
      A0 r1 = other(a0);
      A0 r2 = if_else(test, r, r1);
      return bs::if_else(is_equal(a0, Minf<A0>()),
                          Nan<A0>(),
                          bs::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 w = gammaln(q);
      A0 p = bs::floor(q);
      A0 z = q - p;
      auto test2 = is_less(z, bs::Half<A0>() );
      z = bs::if_minus(test2, z, bs::One<A0>());
      z = q*bs::sinpi(z);
      z =  bs::abs(z);
      return Logpi<A0>()-log(z)-w;
    }
    static inline A0 other(const A0& xx)
    {
      A0 x =  xx;
      auto test = is_less(x, A0(13.0) );
      size_t nb = nbtrue(test);
      A0 r1 = Zero<A0>();
      if (nb > 0)
      {
        A0 z = One<A0>();
        A0 p = Zero<A0>();
        A0 u = if_else_zero(test, x);
        auto test1 = is_greater_equal(u,Three<A0>());
        while(bs::any(test1))
        {
          p = if_dec(test1, p);
          u = if_else(test1, x+p, u);
          z = if_else(test1, z*u, z);
          test1 = is_greater_equal(u,Three<A0>());
        }
        //all u are less than 3
        auto test2 = is_less(u,Two<A0>());

        while(bs::any(test2))
        {
          z = if_else(test2, z/u, z);
          p = if_inc(test2, p);
          u = if_else(test2, x+p, u);
          test2 = is_less(u,Two<A0>());
        }
        z = bs::abs(z);
        x +=  p-Two<A0>();
        r1 = x * detail::gammaln_kernel<A0>::gammaln1(x)+bs::log(z);
        if (nb >= A0::static_size) return r1;
      }
      A0 r2 = fma(xx-Half<A0>(),bs::log(xx), Logsqrt2pi<A0>()-xx);
      A0 p = rec(sqr(xx));
      r2 += detail::gammaln_kernel<A0>::gammalnA(p)/xx;
      return if_else(test, r1, r2);
    }
  };

} } }

#endif


#pragma once

#include "../tags/Logic.hpp"

#include <boost/any.hpp>

extern "C" {
#include <aiger.h>
}

namespace metaSMT
{
  class Aiger
  {
    public:
      typedef unsigned result_type;

      aiger *aig;

    public:
      Aiger () : aig (aiger_init()) {}
      ~Aiger ()
      {
        aiger_reset ( aig );
      }

      result_type operator() (logic::tag::var_tag const& var, boost::any args)
      {
        return new_var();
      }

      result_type operator() (logic::tag::true_tag const& , boost::any args)
      {
        return aiger_true;
      }

      result_type operator() (logic::tag::false_tag const& , boost::any args)
      {
        return aiger_false;
      }

      result_type operator() (logic::tag::not_tag const&, result_type operand)
      {
        return aiger_not ( operand );
      }

      result_type operator() (logic::tag::and_tag const&, result_type lhs, result_type rhs )
      {
        result_type t = new_var();
        aiger_add_and ( aig, t, lhs, rhs );
        return t;
      }

      result_type operator() (logic::tag::nand_tag const&, result_type lhs, result_type rhs )
      {
        result_type t = new_var();
        aiger_add_and ( aig, t, lhs, rhs );
        return aiger_not ( t );
      }

      result_type operator() (logic::tag::equal_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_add_xnor ( aig, lhs, rhs );
      }

      result_type operator() (logic::tag::nequal_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_not ( aiger_add_xnor ( aig, lhs, rhs ) );
      }

      result_type operator() (logic::tag::distinct_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_not ( aiger_add_xnor ( aig, lhs, rhs ) );
      }

      result_type operator() (logic::tag::xnor_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_add_xnor ( aig, lhs, rhs );
      }

      result_type operator() (logic::tag::implies_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_add_or ( aig, aiger_not (lhs), rhs );
      }

      result_type operator() (logic::tag::or_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_add_or ( aig, lhs, rhs );
      }

      result_type operator() (logic::tag::nor_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_not ( aiger_add_or ( aig, lhs, rhs ) );
      }

      result_type operator() (logic::tag::xor_tag const&, result_type lhs, result_type rhs )
      {
        return aiger_add_xor ( aig, lhs, rhs );
      }

      result_type operator() (logic::tag::ite_tag const&, result_type I, result_type T, result_type E)
      {
        return aiger_add_ite ( aig, I, T, E );
      }

      template<typename T>
        result_type operator() (T const& , result_type lhs, result_type rhs )
        {
          assert ( false );
          return aiger_false;
        }

      template<typename T, typename Arg>
        result_type operator() (T const& var, Arg lhs )
        {
          assert ( false );
          return aiger_false;
        }

      template<typename T>
        result_type operator() (T const& var, result_type op1, result_type op2, result_type op3  )
        {
          assert ( false );
          return aiger_false;
        }

      unsigned new_var ()
      {
        ++aig->maxvar;
        return aiger_var2lit( aig->maxvar );
      }

      unsigned aiger_add_or ( aiger *aig, unsigned lhs, unsigned rhs )
      {
        result_type t = new_var ();
        aiger_add_and ( aig, t, aiger_not (lhs), aiger_not (rhs) );
        return aiger_not ( t );
      }

      unsigned aiger_add_xor ( aiger *aig, unsigned lhs, unsigned rhs )
      {
        return aiger_not ( aiger_add_xnor ( aig, lhs, rhs ) );
      }

      unsigned aiger_add_xnor ( aiger *aig, unsigned lhs, unsigned rhs )
      {
        result_type t1 = new_var ();
        result_type t2 = new_var ();

        aiger_add_and ( aig, t1, aiger_not ( lhs ), aiger_not ( rhs ) );
        aiger_add_and ( aig, t2, lhs, rhs );

        return aiger_add_or ( aig, t1, t2 );
      }

      unsigned aiger_add_ite ( aiger *aig, unsigned I, unsigned T, unsigned E )
      {
        result_type t = new_var ();
        aiger_add_and ( aig
            , t
            , aiger_add_or ( aig, aiger_not (I), T )
            , aiger_add_or ( aig, I, E ) );
        return t;
      }
  };

}


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#ifndef DATAGRAMGEOREFERENCERFORGUI_HPP
#define DATAGRAMGEOREFERENCERFORGUI_HPP

#include <Eigen/Dense>

#include <iostream> // Temp while using cout?
#include <iomanip> // Temp while using cout?

#include "../../../georeferencing/DatagramGeoreferencer.hpp"
#include "../../../Ping.hpp"
#include "../../../Position.hpp"
#include "../../../Attitude.hpp"
#include "../../../georeferencing/Georeferencing.hpp"
#include "../../../svp/SoundVelocityProfile.hpp"
#include "../../../svp/SoundVelocityProfileFactory.hpp"
#include "../../../datagrams/DatagramEventHandler.hpp"
#include "../../../math/Interpolation.hpp"

/*!
* \brief "Datagram Georeferencer to an ostream" class, extention of the Datagram Georeferencer class
* \author Christian Bouchard
*/
class DatagramGeoreferencerToOstream : public DatagramGeoreferencer{
public:
  /**Creates a datagram georeferencer*/
  DatagramGeoreferencerToOstream( std::ostream & out )
  : out( out )
  {
    out << std::setprecision(6);
    out << std::fixed;
  }

  /**Destroys the datagram georeferencer*/
  virtual ~DatagramGeoreferencerToOstream() {}

  /**
  * Displays a georeferenced ping's info
  *
  * @param georeferencedPing
  * @param quality the quality flag
  * @param intensity the intensity flag
  */
  virtual void processGeoreferencedPing(Eigen::Vector3d & georeferencedPing,uint32_t quality,int32_t intensity);

private:
  std::ostream & out; // ostream: can be used for a file or for std::cout

};

void DatagramGeoreferencerToOstream::processGeoreferencedPing(Eigen::Vector3d & georeferencedPing,uint32_t quality,int32_t intensity){
  out << georeferencedPing(0) << " " << georeferencedPing(1) << " " << georeferencedPing(2)
  << " " << quality  << " " << intensity << std::endl;
}

#endif


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/force-set.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "utils/macros.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE (test_ForceSet)
{
  using namespace pinocchio;

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  ForceSet F(12);
  ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
  F.block(10,2) = F2;
  BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
  BOOST_CHECK(std::isnan(F.matrix()(0,9)));

  ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
  ForceSet F4 = amb.act(F3);
  SE3::Matrix6 aXb= amb;
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));

  ForceSet bF = bmc.act(F3);
  ForceSet aF = amb.act(bF);
  ForceSet aF2 = amc.act(F3);
  BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));

  ForceSet F36 = amb.act(F3.block(3,6));
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));

  ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
                                                            1e-12));
}

BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
{
  using namespace pinocchio;

  Inertia Y = Inertia::Random();
  JointDataRX::Constraint_t S;

  ForceSet F1(1); F1.block(0,1) = Y*S;
  BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));

  ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
  Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
  BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
                            , 1e-12));
}

template<typename JointModel>
void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel,
                                   const int & nq_ref)
{
  BOOST_CHECK(jmodel.nq() == nq_ref);
}

template<typename JointModel>
void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel,
                                   const int & nq_ref)
{
  BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
}

template<typename JointModel, typename ConstraintDerived>
void test_nv_against_jmodel(const JointModelBase<JointModel> & jmodel,
                            const ConstraintBase<ConstraintDerived> & constraint)
{
  BOOST_CHECK(constraint.nv() == jmodel.nv());
}

template<typename JointModel, typename ConstraintDerived>
void test_nv_against_jmodel(const JointModelMimic<JointModel> & jmodel,
                            const ConstraintBase<ConstraintDerived> & constraint)
{
  BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
}

template<class JointModel>
struct buildModel
{
  static Model run(const JointModelBase<JointModel> & jmodel)
  {
    Model model;
    model.addJoint(0,jmodel,SE3::Identity(),"joint");

    return model;
  }
};

template<class JointModel>
struct buildModel< JointModelMimic<JointModel> >
{
  typedef JointModelMimic<JointModel> JointModel_;

  static Model run(const JointModel_ & jmodel)
  {
    Model model;
    model.addJoint(0,jmodel.jmodel(),SE3::Identity(),"joint");
    model.addJoint(0,jmodel,SE3::Identity(),"joint_mimic");

    return model;
  }
};

template<typename JointModel>
void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::Constraint_t ConstraintType;
  typedef typename traits<Joint>::JointDataDerived JointData;
  typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic> Matrix6x;

  JointData jdata = jmodel.createData();
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
  ConfigVector_t q(jmodel.nq());

  // We need to use a model here in order to call the randomConfiguration to init q.
  Model model = buildModel<JointModel>::run(jmodel.derived());

  test_jmodel_nq_against_nq_ref(jmodel.derived(),model.nq);

  q = randomConfiguration(model,
                          ConfigVector_t::Constant(model.nq,-1.),
                          ConfigVector_t::Constant(model.nq, 1.));

  // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
  jmodel.calc(jdata,q);

  ConstraintType constraint(jdata.S);

  test_nv_against_jmodel(jmodel.derived(),constraint);
  BOOST_CHECK(constraint.cols() == constraint.nv());
  BOOST_CHECK(constraint.rows() == 6);

  typedef typename JointModel::TangentVector_t TangentVector_t;
  TangentVector_t v = TangentVector_t::Random(constraint.nv());

  typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
  Motion m = constraint * v;
  Motion m_ref = Motion(constraint_mat * v);

  BOOST_CHECK(m.isApprox(m_ref));

  // Test SE3 action
  {
    SE3 M = SE3::Random();
    typename ConstraintType::DenseBase S = M.act(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = M.act(m_in);
    }

    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test SE3 action inverse
  {
    SE3 M = SE3::Random();
    typename ConstraintType::DenseBase S = M.actInv(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = M.actInv(m_in);
    }

    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test SE3 action and SE3 action inverse
  {
    const SE3 M = SE3::Random();
    const SE3 Minv = M.inverse();

    typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
    typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);

    BOOST_CHECK(S1_vice.isApprox(S2_vice));

    typename ConstraintType::DenseBase S1_versa = M.act(constraint);
    typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);

    BOOST_CHECK(S1_versa.isApprox(S2_versa));

  }

  // Test Motion action
  {
    Motion v = Motion::Random();

    typename ConstraintType::DenseBase S = v.cross(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = v.cross(m_in);
    }
    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test transpose operations
  {
    const Eigen::DenseIndex dim = 20;
    const Matrix6x Fin = Matrix6x::Random(6,dim);
    Eigen::MatrixXd Fout = constraint.transpose() * Fin;
    Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
    BOOST_CHECK(Fout.isApprox(Fout_ref));

    Force force_in(Force::Random());
    Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
    Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
    BOOST_CHECK(Stf_ref.isApprox(Stf));
  }

  // CRBA operations
  {
    const Inertia Y = Inertia::Random();
    Eigen::MatrixXd YS = Y * constraint;
    Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
    BOOST_CHECK(YS.isApprox(YS_ref));
  }

  // ABA operations
  {
    const Inertia Y = Inertia::Random();
    const Inertia::Matrix6 Y_mat = Y.matrix();
    Eigen::MatrixXd YS = Y_mat * constraint;
    Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
    BOOST_CHECK(YS.isApprox(YS_ref));
  }

}

template<typename JointModel_> struct init;

template<typename JointModel_>
struct init
{
  static JointModel_ run()
  {
    JointModel_ jmodel;
    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    JointModel jmodel((JointModelRX()));

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;

    JointModel jmodel(JointModelRX(),SE3::Random());
    jmodel.addJoint(JointModelRY(),SE3::Random());
    jmodel.addJoint(JointModelRZ(),SE3::Random());

    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;

  static JointModel run()
  {
    JointModel_ jmodel_ref = init<JointModel_>::run();

    JointModel jmodel(jmodel_ref,1.,0.);
    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

struct TestJointConstraint
{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel = init<JointModel>::run();
    jmodel.setIndexes(0,0,0);

    test_constraint_operations(jmodel);
  }

};

BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
{
  typedef JointCollectionDefault::JointModelVariant Variant;
  boost::mpl::for_each<Variant::types>(TestJointConstraint());
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Point<mml::Num> Vector;
  typedef mml::Polygon<mml::Num> Polygon;

  class Object
  {
    public:
      Object(const Shape& s)
        : id(sid++), shape(s)
      {
      }

      const unsigned id;
      const Shape shape;

    private:
      static unsigned sid;
  };

  unsigned Object::sid = 0;

  inline const Shape& shape(Object* const& o)
  {
    return o->shape;
  }

  typedef mml::Grid<Object*, mml::Num> Grid;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(grid)

BOOST_AUTO_TEST_CASE(EMPTY)
{
  Grid g(&shape, Point(0, 0), 10, 100, 100);

  std::pair<float, float> s(0.f, 0.f);
  BOOST_REQUIRE_MESSAGE(g.stats() == s, "wat");
}

BOOST_AUTO_TEST_CASE(BASIC_TEST)
{
  Grid g(&shape, Point(0, 0), 10, 100, 100);
  Polygon p(Shape::tetragon(Point(  0,  50), Point( 50, 100),
                            Point(100,  50), Point( 50,   0)));

  g.insert(new Object(p));

  for (int y = 0; y <= 1000; ++y)
    for (int x = 0; x <= 1000; ++x)
    {
      bool b1 = g.find(Point(x, y)) != 0;
      bool b2 = mml::contains(p, Point(x, y));

      BOOST_REQUIRE_MESSAGE(b1 == b2, Point(x, y) << " => "
                            << std::boolalpha << b1 << ", "
                            << std::boolalpha << b2);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "RevoluteJoint.h"
#include <Eigen/Geometry>

using namespace Eigen;

RevoluteJoint::RevoluteJoint(const std::string& name, const Isometry3d& transform_to_parent_body, const Vector3d& rotation_axis) :
    FixedAxisOneDoFJoint(name, transform_to_parent_body, spatialJointAxis(rotation_axis)), rotation_axis(rotation_axis)
{
}

RevoluteJoint::~RevoluteJoint()
{
  // empty
}

Isometry3d RevoluteJoint::jointTransform(double* const q) const
{
  Isometry3d ret(AngleAxisd(q[0], rotation_axis));
  ret.makeAffine();
  return ret;
}

Matrix<double, TWIST_SIZE, 1> RevoluteJoint::spatialJointAxis(const Vector3d& rotation_axis)
{
  Matrix<double, TWIST_SIZE, 1> ret;
  ret.topRows<3>() = rotation_axis;
  ret.bottomRows<3>() = Vector3d::Zero();
  return ret;
}


#include <Mahi/Util.hpp>
// #include <MeiiModel.hpp>
#include <Eqns.hpp>
#include <Eigen/Dense>

using namespace mahi::util;
using Eigen::Matrix4d;
using Eigen::Vector4d;

int main(){
    std::vector<double> qs = {0.2,-1.0,0.234,3.78,0.1,-0.16, 0.0, 0.15};

    Clock time_clock;
    std::cout << get_M(qs).inverse() << "\n\n";
    std::cout << time_clock.get_elapsed_time().as_microseconds() << "\n\n";
    time_clock.restart();
    std::cout << get_nathanisdumb(qs) << "\n\n";
    std::cout << time_clock.get_elapsed_time().as_microseconds();

    return 0;
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_PARAMETER_LINEARIZATION_PARAMETERS_HPP_
#define MIQP_COMMON_PARAMETER_LINEARIZATION_PARAMETERS_HPP_

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace miqp {
namespace common {
namespace parameter {

struct PolynomialCurvatureParameters {
  Eigen::MatrixXd POLY_KAPPA_AX_MAX;
  Eigen::MatrixXd POLY_KAPPA_AX_MIN;
};

struct PolynomialOrientationParameters {
  Eigen::MatrixXd POLY_SINT_UB;
  Eigen::MatrixXd POLY_SINT_LB;
  Eigen::MatrixXd POLY_COSS_UB;
  Eigen::MatrixXd POLY_COSS_LB;
};

typedef Eigen::MatrixXd FractionParameters;

}  // namespace parameter
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_PARAMETER_LINEARIZATION_PARAMETERS_HPP_