#include <gtest/gtest.h>
#include <iostream>
#include <assert.h>
#include <algorithm>
#include <random>
#include "spaND.h"
#include "mmio.hpp"
#include "cxxopts.hpp"
#include <Eigen/SparseCholesky>

using namespace std;
using namespace Eigen;
using namespace spaND;

bool VERB = false;
int  N_THREADS = 4;
int  RUN_MANY = 4;

SymmKind symm2syk(int symm) {
    switch(symm) {
        case 0: return SymmKind::SPD;
        case 1: return SymmKind::SYM;
        case 2: return SymmKind::GEN;
        default: assert(false);
    };
    return SymmKind::SPD;
}

PartKind pki2pk(int pki) {
    switch(pki) {
        case 0: return PartKind::MND;
        case 1: return PartKind::RB;
        default: assert(false);
    };
    return PartKind::MND;
}

ScalingKind ski2sk(int ski) {
    switch(ski) {
        case 0: return ScalingKind::LLT;
        case 1: return ScalingKind::EVD;
        case 2: return ScalingKind::SVD;
        case 3: return ScalingKind::PLU;
        case 4: return ScalingKind::PLUQ;
        case 5: return ScalingKind::LDLT;
        default: assert(false);
    };
    return ScalingKind::LLT;
}

bool is_valid(SymmKind syk, ScalingKind sk, bool preserve) {
    if(syk == SymmKind::SPD) {
        if (sk != ScalingKind::LLT) return false;
    }
    if(syk == SymmKind::SYM) {
        if (sk != ScalingKind::LDLT) return false;
    }
    if(syk == SymmKind::GEN) {
        if (sk != ScalingKind::PLU && sk != ScalingKind::PLUQ) return false;
    }
    if(preserve) return false;
    return true;
}

struct params {
    SymmKind syk;
    PartKind pk;
    ScalingKind sk;
    bool preserve;
};

vector<params> get_params() {
    vector<params> configs;
    for(int symm = 0; symm < 3; symm++) {
        for(int pki = 0; pki < 2; pki++) {
            for(int ski = 0; ski < 6; ski++) {
                for(int pres = 0; pres < 2; pres++) {
                    PartKind pk = pki2pk(pki);
                    ScalingKind sk = ski2sk(ski);
                    SymmKind syk = symm2syk(symm);
                    if(! is_valid(syk, sk, pres)) continue;
                    configs.push_back({syk, pk, sk, pres == 1});
                }
            }
        }
    }
    return configs;
};

SpMat neglapl(int n, int d) {
    stringstream s;
    s << "../mats/neglapl_" << d << "_" << n << ".mm";
    string file = s.str();
    SpMat A = mmio::sp_mmread<double,int>(file);
    return A;
}

SpMat neglapl_unsym(int n, int d, int seed) {
    SpMat A = neglapl(n, d);
    default_random_engine gen;
    gen.seed(seed);
    uniform_real_distribution<double> rand(-0.1, 0.1);
    for(int k = 0; k < A.outerSize(); ++k) {
        for(SpMat::InnerIterator it(A, k); it; ++it) {
            A.coeffRef(it.row(), it.col()) += rand(gen);
        }
    }
    return A;
}

SpMat make_indef(SpMat& A, int seed) {
    Eigen::SimplicialLLT<SpMat, Eigen::Lower> sllt(A);
    VectorXd random_diagonal = random(A.rows(), seed);
    for(int i = 0; i < A.rows(); i++) {
        if(random_diagonal[i] <= 0.9) {
            random_diagonal[i] = -1;
        } else {
            random_diagonal[i] = 1;
        }
    }
    SpMat L = sllt.matrixL();
    return L * (random_diagonal.asDiagonal() * L.transpose());
}

SpMat random_SpMat(int n, double p, int seed) {
    default_random_engine gen;
    gen.seed(seed);
    uniform_real_distribution<double> dist(0.0,1.0);
    vector<Triplet<double>> triplets;
    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            auto v_ij = dist(gen);
            if(v_ij < p) {
                triplets.push_back(Triplet<double>(i,j,v_ij));
            }
        }
    }
    SpMat A(n,n);
    A.setFromTriplets(triplets.begin(), triplets.end());
    return A;
}

SpMat identity_SpMat(int n) {
    vector<Triplet<double>> triplets;
    for(int i = 0; i < n; ++i) {
        triplets.push_back(Triplet<double>(i,i,1.0));
    }
    SpMat A(n,n);
    A.setFromTriplets(triplets.begin(), triplets.end());
    return A;
}

TEST(MatrixMarket, Sparse) {
    // 1
    SpMat A = mmio::sp_mmread<double,int>("../mats/test1.mm");
    SpMat Aref(2, 3);
    Aref.insert(0, 0) = 1;
    Aref.insert(0, 1) = -2e2;
    Aref.insert(1, 1) = 3e3;
    Aref.insert(1, 2) = -4.4e4;
    EXPECT_EQ(A.nonZeros(), 4);
    EXPECT_EQ((Aref - A).norm(), 0.0);
    // 2
    A = mmio::sp_mmread<double,int>("../mats/test2.mm");
    Aref = SpMat(3, 3);
    Aref.insert(0, 0) = 1.1;
    Aref.insert(1, 1) = 2e2;
    Aref.insert(2, 0) = -3.3;
    Aref.insert(0, 2) = -3.3;
    EXPECT_EQ(A.nonZeros(), 4);
    EXPECT_EQ((Aref - A).norm(), 0.0);
    // 3
    A = mmio::sp_mmread<double,int>("../mats/test3.mm");
    Aref = SpMat(4, 1);
    Aref.insert(3, 0) = -1;
    EXPECT_EQ(A.nonZeros(), 1);
    EXPECT_EQ((Aref - A).norm(), 0.0);
    // 4
    A = mmio::sp_mmread<double,int>("../mats/test4.mm");
    Aref = SpMat(2, 2);
    Aref.insert(1, 0) = -3.3;
    Aref.insert(0, 1) = -3.3;
    EXPECT_EQ(A.nonZeros(), 2);
    EXPECT_EQ((Aref - A).norm(), 0.0);
}

TEST(MatrixMarket, Array) {
    // 5
    MatrixXd A = mmio::dense_mmread<double>("../mats/test5.mm");
    EXPECT_EQ(A.rows(), 2);
    EXPECT_EQ(A.cols(), 3);
    MatrixXd Aref(2, 3);
    Aref << 1, 3, -5, 2, 4, 1e6; // row-wise filling in eigen
    EXPECT_EQ((Aref - A).norm(), 0.0);
    // 6
    A = mmio::dense_mmread<double>("../mats/test6.mm");
    Aref = MatrixXd(2, 2);
    EXPECT_EQ(A.rows(), 2);
    EXPECT_EQ(A.cols(), 2);
    Aref << 1, -2, -2, 3; // row-wise filling in eigen
    EXPECT_EQ((Aref - A).norm(), 0.0);
}

/** Util.cpp tests **/

TEST(Util, AreConnected) {
    // 3x3 laplacian
    SpMat A = mmio::sp_mmread<double,int>("../mats/neglapl_2_3.mm");
    VectorXi a(2);
    VectorXi b(3);
    a << 0, 1;
    b << 6, 7, 8;
    EXPECT_FALSE(are_connected(a, b, A));
    a = VectorXi(2);
    b = VectorXi(3);
    a << 0, 1;
    b << 2, 5, 8;
    EXPECT_TRUE(are_connected(a, b, A));
    a = VectorXi(2);
    b = VectorXi(1);
    a << 3, 4;
    b << 5;
    EXPECT_TRUE(are_connected(a, b, A));
    a = VectorXi(1);
    b = VectorXi(1);
    a << 6;
    b << 6;
    EXPECT_TRUE(are_connected(a, b, A));
}

TEST(Util, ShouldBeDisconnected) {
    EXPECT_TRUE(should_be_disconnected(0, 0, 0, 2));
    EXPECT_TRUE(should_be_disconnected(0, 0, 1, 2));
    EXPECT_TRUE(should_be_disconnected(0, 0, 4, 2));
    EXPECT_TRUE(should_be_disconnected(0, 0, 5, 2));
    EXPECT_TRUE(should_be_disconnected(0, 0, 1000, 2));

    EXPECT_TRUE(should_be_disconnected(1, 2, 2, 0));
    EXPECT_TRUE(should_be_disconnected(0, 1, 1, 1));
    EXPECT_TRUE(should_be_disconnected(0, 2, 1, 1));
    EXPECT_TRUE(should_be_disconnected(2, 0, 0, 5));
    EXPECT_TRUE(should_be_disconnected(2, 2, 0, 1));

    EXPECT_FALSE(should_be_disconnected(0, 1, 0, 0));
    EXPECT_FALSE(should_be_disconnected(0, 2, 0, 0));
    EXPECT_FALSE(should_be_disconnected(0, 10, 0, 0));

    EXPECT_FALSE(should_be_disconnected(2, 0, 1, 5));
    EXPECT_FALSE(should_be_disconnected(2, 0, 1, 6));
    EXPECT_FALSE(should_be_disconnected(2, 1, 1, 2));
    EXPECT_FALSE(should_be_disconnected(2, 1, 1, 3));
}

TEST(Util, ChooseRank) {
    VectorXd errs = VectorXd(5);
    errs << 1.0, -0.1, 0.01, -0.001, 1e-4;
    EXPECT_EQ(choose_rank(errs, 1e-1), 2);
    EXPECT_EQ(choose_rank(errs, 1e-2), 3);
    EXPECT_EQ(choose_rank(errs, 1.0), 0);
    EXPECT_EQ(choose_rank(errs, 0), 5);
    EXPECT_EQ(choose_rank(errs, 1e-16), 5);
}

TEST(Util, Block2Dense) {
    // Usual case
    {
        SpMat A(5, 5);
        A.insert(0, 0) = 1.0;
        A.insert(2, 2) = -2.0;
        A.insert(1, 3) = 3.0;
        A.makeCompressed();
        VectorXi rowval = Map<VectorXi>(A.innerIndexPtr(), A.nonZeros());
        VectorXi colptr = Map<VectorXi>(A.outerIndexPtr(), 6);
        VectorXd nnzval = Map<VectorXd>(A.valuePtr(), A.nonZeros());
        MatrixXd Ad = MatrixXd::Zero(3, 3);
        block2dense(rowval, colptr, nnzval, 1, 1, 3, 3, &Ad, false);
        MatrixXd Adref = MatrixXd::Zero(3, 3);
        Adref << 0, 0, 3, 0, -2, 0, 0, 0, 0;
        EXPECT_EQ((Adref - Ad).norm(), 0);
    }
    // Transpose
    {
        SpMat A(3, 4);
        A.insert(0, 1) = 1.0;
        A.insert(1, 0) = 2.0;
        A.insert(1, 2) = 3.0;
        A.insert(2, 1) = 4.0;
        A.insert(0, 3) = 5.0;
        A.makeCompressed();
        VectorXi rowval = Map<VectorXi>(A.innerIndexPtr(), A.nonZeros());
        VectorXi colptr = Map<VectorXi>(A.outerIndexPtr(), 5);
        VectorXd nnzval = Map<VectorXd>(A.valuePtr(), A.nonZeros());
        MatrixXd Ad = MatrixXd::Zero(3, 2);
        block2dense(rowval, colptr, nnzval, 0, 0, 2, 3, &Ad, true);
        MatrixXd Adref = MatrixXd::Zero(3, 2);
        Adref << 0, 2, 1, 0, 0, 3;
        EXPECT_EQ((Adref - Ad).norm(), 0);
    }
}

TEST(Util, LinspaceNd) {
    MatrixXd X2 = linspace_nd(3, 2);
    MatrixXd X2ref(2, 9);
    X2ref << 0, 0, 0, 1, 1, 1, 2, 2, 2, 0, 1, 2, 0, 1, 2, 0, 1, 2;
    EXPECT_EQ((X2ref - X2).norm(), 0);
    MatrixXd X3 = linspace_nd(2, 3);
    MatrixXd X3ref(3, 8);
    X3ref << 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1;
    EXPECT_EQ((X3ref - X3).norm(), 0);
}

TEST(Util, SymmPerm) {
    vector<int> dims  = {2, 2,  2,  3, 3,  3,  3 };
    vector<int> sizes = {5, 10, 20, 5, 15, 25, 30};
    for(int test = 0; test < dims.size(); test++) {
        int s = sizes[test];
        int d = dims[test];
        stringstream ss;
        ss << "../mats/neglapl_" << d << "_" << s << ".mm";
        SpMat A = mmio::sp_mmread<double,int>(ss.str());
        // Create random perm
        int N = A.rows();
        VectorXi p = VectorXi::LinSpaced(N, 0, N-1);
        random_device rd;
        mt19937 g(rd());
        shuffle(p.data(), p.data() + N, g);
        // Compare
        SpMat pAp = symm_perm(A, p);
        SpMat pApref = p.asPermutation().inverse() * A * p.asPermutation();
        EXPECT_EQ((pAp - pApref).norm(), 0.0);
    }
}

TEST(Util, isperm) {
    VectorXi perm1(10);
    VectorXi perm2(10);
    VectorXi noperm1(10);
    VectorXi noperm2(5);
    perm1 << 0, 9, 8, 1, 4, 2, 3, 7, 5, 6;
    perm2 << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
    noperm1 << 0, 9, 8, 1, 4, 2, 3, 5, 5, 6;
    noperm2 << 0, 9, 8, 1, 4;
    EXPECT_TRUE(isperm(&perm1));
    EXPECT_TRUE(isperm(&perm2));
    EXPECT_FALSE(isperm(&noperm1));
    EXPECT_FALSE(isperm(&noperm2));
}

TEST(Util, swap2perm) {
    VectorXi swap(6);
    VectorXi perm(6);
    VectorXi permRef(6);
    swap << 3, 3, 2, 5, 4, 5;
    permRef << 3, 0, 2, 5, 4, 1;
    swap2perm(&swap, &perm); // perm.asPermutation().transpose() * x <=> x[perm]
    EXPECT_EQ((perm - permRef).norm(), 0.0);
}

SpMat symmetric_graph_ref(SpMat A) {
    SpMat ATabs = A.cwiseAbs().transpose();
    return A.cwiseAbs() + ATabs + identity_SpMat(A.rows());
}

TEST(Util, symmetric_graph) {
    for(int i = 1; i < 100; i++) {
        SpMat A = random_SpMat(i, 0.2, i);
        SpMat AAT = symmetric_graph(A);
        EXPECT_LT( (AAT - symmetric_graph_ref(A)).norm(), 1e-12);
    }
}

/** Partitioning tests **/

/**
 * Check the partitioning of a square laplacian 5x5
 */
TEST(PartitionTest, Square) {
    SpMat A = mmio::sp_mmread<double,int>("../mats/neglapl_2_5.mm");
    MatrixXd X = linspace_nd(5, 2);
    Tree t(3);
    t.set_verb(VERB);
    t.set_use_geo(true);
    t.set_Xcoo(&X);
    auto part = t.partition(A);
    vector<SepID> sepidref {
        SepID(0,0), SepID(0,0), SepID(1,0), SepID(0,1), SepID(0,1),
        SepID(0,0), SepID(0,0), SepID(1,0), SepID(0,1), SepID(0,1),
        SepID(2,0), SepID(2,0), SepID(2,0), SepID(2,0), SepID(2,0),
        SepID(0,2), SepID(0,2), SepID(1,1), SepID(0,3), SepID(0,3),
        SepID(0,2), SepID(0,2), SepID(1,1), SepID(0,3), SepID(0,3),
    } ;
    vector<SepID> leftref {
        SepID(0,0), SepID(0,0), SepID(0,0), SepID(0,1), SepID(0,1),
        SepID(0,0), SepID(0,0), SepID(0,0), SepID(0,1), SepID(0,1),
        SepID(0,0), SepID(0,0), SepID(1,0), SepID(0,1), SepID(0,1),
        SepID(0,2), SepID(0,2), SepID(0,2), SepID(0,3), SepID(0,3),
        SepID(0,2), SepID(0,2), SepID(0,2), SepID(0,3), SepID(0,3),
    } ;
    vector<SepID> rightref {
        SepID(0,0), SepID(0,0), SepID(0,1), SepID(0,1), SepID(0,1),
        SepID(0,0), SepID(0,0), SepID(0,1), SepID(0,1), SepID(0,1),
        SepID(0,2), SepID(0,2), SepID(1,1), SepID(0,3), SepID(0,3),
        SepID(0,2), SepID(0,2), SepID(0,3), SepID(0,3), SepID(0,3),
        SepID(0,2), SepID(0,2), SepID(0,3), SepID(0,3), SepID(0,3),
    } ;
    for(int i = 0; i < part.size(); i++) {
        ASSERT_TRUE(part[i].self  == sepidref[i]);
        ASSERT_TRUE(part[i].l     == leftref[i]);
        ASSERT_TRUE(part[i].r     == rightref[i]);
    }
}

/**
 * Check consistency of the partitioning
 */
TEST(PartitionTest, Consistency) {
    vector<int> dims  = {2, 2,  2,   3, 3,  3 };
    vector<int> sizes = {5, 20, 100, 5, 15, 25};
    for(int test = 0; test < dims.size(); test++) {
        int s = sizes[test];
        int d = dims[test];
        stringstream ss;
        ss << "../mats/neglapl_" << d << "_" << s << ".mm";
        int n = pow(s, d);
        string file = ss.str();
        for(int nlevels = 1; nlevels < 8; nlevels++) {
            for(int geoi = 0; geoi < 2; geoi++) {
                for(int pki = 0; pki < 2; pki++) {
                    bool geo = (geoi == 0);
                    PartKind pk = pki == 0 ? PartKind::MND : PartKind::RB;
                    // Partition tree
                    MatrixXd X = linspace_nd(s, d);
                    Tree t(nlevels);
                    t.set_verb(VERB);
                    SpMat A = mmio::sp_mmread<double,int>(file);
                    t.set_use_geo(geo);
                    t.set_Xcoo(&X);
                    t.set_part_kind(pk);
                    auto part = t.partition(A);
                    // (1) Lengths
                    ASSERT_EQ(part.size(), n);
                    // (2) Check ordering integrity
                    for(int i = 0; i < n; i++) {
                        auto pi = part[i].self;
                        for (SpMat::InnerIterator it(A,i); it; ++it) {
                            int j = it.row();
                            auto pj = part[j].self;
                            ASSERT_FALSE(should_be_disconnected(pi.lvl, pj.lvl, pi.sep, pj.sep));
                        }
                    }
                    // (3) Check left/right integrity
                    for(int i = 0; i < n; i++) {
                        auto pi = part[i].self;
                        auto li = part[i].l;
                        auto ri = part[i].r;
                        if(pi.lvl == 0) {
                            ASSERT_TRUE(pi == li);
                            ASSERT_TRUE(pi == ri);
                        } else {
                            ASSERT_TRUE(pi.lvl > li.lvl);
                            ASSERT_TRUE(pi.lvl > ri.lvl);
                            while(li.lvl < pi.lvl - 1) {
                                li.lvl += 1;
                                li.sep /= 2;
                            }
                            while(ri.lvl < pi.lvl - 1) {
                                ri.lvl += 1;
                                ri.sep /= 2;
                            }
                            ASSERT_TRUE(li.lvl == pi.lvl - 1);
                            ASSERT_TRUE(ri.lvl == pi.lvl - 1);
                            ASSERT_TRUE(li.sep == 2 * pi.sep);
                            ASSERT_TRUE(ri.sep == 2 * pi.sep + 1);
                        }
                    }
                }
            }
        }
    }
}

/** Assembly tests **/

/**
 * Check assembly
 */
TEST(Assembly, Consistency) {
    vector<int> dims  = {2, 2,  2,  3, 3,  3};
    vector<int> sizes = {5, 10, 20, 5, 10, 15};
    for(int spandlorasp = 0; spandlorasp < 2; spandlorasp++) {
        for(int test = 0; test < dims.size(); test++) {
            for(int pki = 0; pki < 2; pki++) {
                PartKind pk = pki == 0 ? PartKind::MND : PartKind::RB;
                int s = sizes[test];
                int d = dims[test];
                SpMat Aref = neglapl(s, d);
                SpMat Arefunsym = neglapl_unsym(s, d, test);
                for(int nlevels = 2; nlevels < 5 ; nlevels++) {
                    /**
                     * Symmetric case
                     */
                    {
                        // Partition and assemble
                        Tree t(nlevels);
                        t.set_verb(VERB);
                        t.set_use_geo(false);
                        t.set_part_kind(pk);
                        if(spandlorasp == 0) t.partition(Aref);
                        else                 t.partition_lorasp(Aref);
                        t.assemble(Aref);
                        // Get permutation
                        VectorXi p = t.get_assembly_perm();
                        // Check it's indeed a permutation
                        ASSERT_TRUE(isperm(&p));
                        auto P = p.asPermutation();
                        // Check get_mat()
                        SpMat A2 = t.get_trailing_mat();
                        EXPECT_EQ((P.inverse() * Aref * P - A2).norm(), 0.0);
                    }
                    /**
                     * Unsymmetric case
                     */
                    {
                        // Partition and assemble
                        Tree t(nlevels);
                        t.set_verb(VERB);
                        t.set_symm_kind(SymmKind::GEN);
                        t.set_use_geo(false);
                        t.set_part_kind(pk);
                        if(spandlorasp == 0) t.partition(Arefunsym);
                        else                 t.partition_lorasp(Arefunsym);
                        t.assemble(Arefunsym);
                        // Get permutation
                        VectorXi p = t.get_assembly_perm();
                        // Check it's indeed a permutation
                        ASSERT_TRUE(isperm(&p));
                        auto P = p.asPermutation();
                        // Check get_mat()
                        SpMat A2 = t.get_trailing_mat();
                        EXPECT_EQ((P.inverse() * Arefunsym * P - A2).norm(), 0.0);
                    }
                }
            }
        }
    }
}

/** Factorization tests **/

TEST(ApproxTest, PrintConfigs) {
    vector<params> configs = get_params();
    cout << "Preserve ? PartKind ? ScalingKind ? SymmKind ?" << endl;
    for(auto c: configs) {
        cout << c.preserve << " " << part2str(c.pk) << " " << scaling2str(c.sk) << " " << symm2str(c.syk) << endl;
    }
}

/**
 * Test that with eps=0, we get exact solutions
 */
TEST(ApproxTest, Exact) {
    vector<int> dims  = {2, 2,  2,  3, 3};
    vector<int> sizes = {5, 10, 20, 5, 15};
    vector<double> tols = {1e-14, 1e-14, 0.0};
    vector<int> skips   = {0,     4,     1000};
    vector<params> configs = get_params();
    for(int test = 0; test < dims.size(); test++) {
        cout << "Test " << test << "... ";
        int count = 0;
        int s = sizes[test];
        int d = dims[test];
        int n = pow(s, d);
        int nlevelsmin = n < 1000 ? 1 : 8;
        SpMat Aref = neglapl(s, d);
        SpMat Arefunsym = neglapl_unsym(s, d, test);
        SpMat Arefsym = make_indef(Aref, 2019+test);
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin+5 ; nlevels++) {
            for(auto c: configs) {
                SpMat A = (c.syk == SymmKind::SPD ? Aref : (c.syk == SymmKind::SYM ? Arefsym : Arefunsym));
                assert(! c.preserve);
                MatrixXd phi = random(Aref.rows(), 3, test+nlevels+2019);
                for(int it = 0; it < tols.size(); it++) {
                    double tol = tols[it];
                    double skip = skips[it];
                    Tree t(nlevels);
                    t.set_verb(VERB);
                    t.set_part_kind(c.pk);
                    t.set_scaling_kind(c.sk);
                    t.set_symm_kind(c.syk);
                    t.partition(A);
                    t.assemble(A);
                    t.set_tol(tol);
                    t.set_skip(skip);
                    t.set_preserve(c.preserve);
                    if(c.preserve) t.set_phi(&phi);
                    t.factorize();
                    VectorXd b = random(n, test+nlevels+2019+1);
                    auto x = b;
                    t.solve(x);
                    double err = (A*x-b).norm() / b.norm();
                    EXPECT_LE(err, 1e-10) << err;
                    count++;
                }
            }
        }
        cout << count << " tested.\n";
    }
}

/**
 * Test SPD on A (laplacian) and SYM+LDLT on -A (- laplacian) give the same, with or without compression
 */
TEST(ApproxTest, SPD_vs_LDLT) {
    vector<int> dims  = {2,  3, 3};
    vector<int> sizes = {128, 5, 15};
    vector<double> tols = {0,   1e-4, 1e-14};
    vector<int> skips   = {100, 1,    0};
    vector<params> configs = get_params();
    for(int test = 0; test < dims.size(); test++) {
        cout << "Test " << test << "... ";
        int count = 0;
        int s = sizes[test];
        int d = dims[test];
        int n = pow(s, d);
        int nlevelsmin = n < 1000 ? 1 : 8;
        SpMat A = neglapl(s, d);
        SpMat Aneg = -A;
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin+5 ; nlevels++) {
            for(auto c: configs) {
                if(c.sk != ScalingKind::LLT && !c.preserve) continue;
                for(int it = 0; it < tols.size(); it++) {
                    double tol = tols[it];
                    double skip = skips[it];
                    VectorXd b = random(n, test+nlevels+2019+1);
                    // Use LLT on A
                    Tree t_llt(nlevels);
                    t_llt.set_verb(VERB);
                    t_llt.set_part_kind(c.pk);
                    t_llt.set_scaling_kind(ScalingKind::LLT);
                    t_llt.set_symm_kind(SymmKind::SPD);
                    t_llt.partition(A);
                    t_llt.assemble(A);
                    t_llt.set_tol(tol);
                    t_llt.set_skip(skip);
                    t_llt.set_preserve(false);
                    t_llt.factorize();
                    VectorXd x_llt = b;
                    t_llt.solve(x_llt);
                    // Use LDLT on -A
                    Tree t_ldlt(nlevels);
                    t_ldlt.set_verb(VERB);
                    t_ldlt.set_part_kind(c.pk);
                    t_ldlt.set_scaling_kind(ScalingKind::LDLT);
                    t_ldlt.set_symm_kind(SymmKind::SYM);
                    t_ldlt.partition(Aneg);
                    t_ldlt.assemble(Aneg);
                    t_ldlt.set_tol(tol);
                    t_ldlt.set_skip(skip);
                    t_ldlt.set_preserve(false);
                    t_ldlt.factorize();
                    VectorXd x_ldlt = - b;
                    t_ldlt.solve(x_ldlt);
                    // Compare
                    double err_llt = (A*x_llt-b).norm() / b.norm();
                    double err_ldlt = (A*x_ldlt-b).norm() / b.norm();
                    double diff = (x_llt - x_ldlt).norm() / x_llt.norm();
                    if (tol == 0.0) {
                        EXPECT_LE(err_llt, 1e-12);
                        EXPECT_LE(err_ldlt, 1e-12);
                        EXPECT_LE(diff, 1e-12);
                    } else {
                        EXPECT_LE(err_llt, tol * 1e2);
                        EXPECT_LE(err_ldlt, tol * 1e2);
                        EXPECT_LE(diff, tol * 1e2);
                    }
                    count++;
                }
            }
        }
        cout << count << " tested.\n";
    }
}

/**
 * Test conservation is correct
 */
TEST(ApproxTest, Preservation) {
    vector<int> dims  = {2, 2,  2,  3, 3,  3};
    vector<int> sizes = {5, 10, 20, 5, 10, 25};
    for(int test = 0; test < dims.size(); test++) {
        cout << "Test " << test;
        int s = sizes[test];
        int d = dims[test];
        stringstream ss;
        ss << "../mats/neglapl_" << d << "_" << s << ".mm";
        int n = pow(s, d);
        string file = ss.str();
        SpMat A_spd = mmio::sp_mmread<double,int>(file);
        SpMat A_sym = - A_spd;
        int nlevelsmin = n < 1000 ? 1 : 8;
        vector<double> tols = {10, 1e-2, 1e-3, 1e-4, 1e-6, 0.0};
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin + 5; nlevels++) {
            for(int it = 0; it < tols.size(); it++) {
                for(int skip = 0; skip < 3; skip++) {
                    for(int symm = 0; symm < 2; symm++) {
                        printf("."); fflush(stdout);
                        SpMat A;
                        if(symm == 0) A = A_spd;
                        else          A = A_sym;
                        // Check a 1 is preserved
                        {
                            Tree t(nlevels);
                            if(symm == 0) {
                                t.set_scaling_kind(ScalingKind::LLT);
                                t.set_symm_kind(SymmKind::SPD);
                            } else {
                                t.set_scaling_kind(ScalingKind::LDLT);
                                t.set_symm_kind(SymmKind::SYM);
                            }
                            t.set_verb(VERB);
                            t.partition(A);
                            t.assemble(A);
                            MatrixXd phi = MatrixXd::Ones(n, 1);
                            t.set_tol(tols[it]);
                            t.set_skip(skip);
                            t.set_preserve(true);
                            t.set_phi(&phi);
                            t.factorize();
                            for(int c = 0; c < phi.cols(); c++) {
                                VectorXd b = A * phi.col(c);
                                VectorXd x = b;
                                t.solve(x);
                                double err1 = (A*x-b).norm() / b.norm();
                                double err2 = (x-phi.col(c)).norm() / phi.col(c).norm();
                                EXPECT_TRUE(err1 < 1e-12) << "err1 = " << err1 << " | " << skip << " " << it << " " << nlevels << " " << test << endl;
                                EXPECT_TRUE(err2 < 1e-12) << "err2 = " << err2 << " | " << skip << " " << it << " " << nlevels << " " << test << endl;
                            }
                            VectorXd b = random(n, nlevels+it+skip+2019);
                            auto x = b;
                            t.solve(x);
                            double err = (A*x-b).norm() / b.norm();
                            if (tols[it] == 0.0) {
                                EXPECT_TRUE(err < 1e-12);
                            } else {
                                EXPECT_TRUE(err < tols[it] * 1e2);
                            }
                        }
                        // Check that a multiple random b are preserved
                        {
                            Tree t(nlevels);
                            if(symm == 0) {
                                t.set_scaling_kind(ScalingKind::LLT);
                                t.set_symm_kind(SymmKind::SPD);
                            } else {
                                t.set_scaling_kind(ScalingKind::LDLT);
                                t.set_symm_kind(SymmKind::SYM);
                            }
                            t.set_verb(VERB);
                            t.partition(A);
                            t.assemble(A);
                            MatrixXd phi = random(n, 5, nlevels+it+skip+2019);
                            t.set_tol(tols[it]);
                            t.set_skip(skip);
                            t.set_preserve(true);
                            t.set_phi(&phi);
                            t.factorize();
                            for(int c = 0; c < phi.cols(); c++) {
                                VectorXd b = A * phi.col(c);
                                VectorXd x = b;
                                t.solve(x);
                                double err1 = (A*x-b).norm() / b.norm();
                                double err2 = (x-phi.col(c)).norm() / phi.col(c).norm();
                                EXPECT_TRUE(err1 < 1e-12) << "err1 = " << err1 << " | " << skip << " " << it << " " << nlevels << " " << test << endl;
                                EXPECT_TRUE(err2 < 1e-12) << "err2 = " << err2 << " | " << skip << " " << it << " " << nlevels << " " << test << endl;
                            }
                            VectorXd b = random(n, nlevels+it+skip+2019);
                            auto x = b;
                            t.solve(x);
                            double err = (A*x-b).norm() / b.norm();
                            if (tols[it] == 0.0) {
                                EXPECT_TRUE(err < 1e-12);
                            } else {
                                EXPECT_TRUE(err < tols[it] * 1e2);
                            }
                        }
                    }
                }
            }
        }
        printf("\n");
    }
}

/**
 * Test that the approximations are reasonnable accurate
 * with and without preservation
 */
TEST(ApproxTest, Approx) {
    vector<int> dims  = {2, 2,  2,  2,   3, 3};
    vector<int> sizes = {5, 10, 20, 128, 5, 15};
    vector<double> tols = {0.0, 1e-10, 1e-6, 1e-2, 10};
    matrix_hash<VectorXd> hash;
    vector<params> configs = get_params();
    for(int test = 0; test < dims.size(); test++) {
        vector<size_t> allhashes;
        int count = 0;
        cout << "Test " << test << "... ";
        int s = sizes[test];
        int d = dims[test];
        SpMat Aref = neglapl(s, d);
        SpMat Arefunsym = neglapl_unsym(s, d, test+2019);
        SpMat Arefsym = - Aref;
        int n = pow(s, d);
        int nlevelsmin = n < 1000 ? 1 : 8;
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin + 5; nlevels++) {
            for(int it = 0; it < tols.size(); it++) {
                for(int skip = 0; skip < 3; skip++) {
                    for(auto c: configs) {
                        SpMat A = (c.syk == SymmKind::SPD ? Aref : (c.syk == SymmKind::SYM ? Arefsym : Arefunsym));
                        assert(! c.preserve);
                        Tree t(nlevels);
                        t.set_verb(VERB);
                        t.set_symm_kind(c.syk);
                        t.set_part_kind(c.pk);
                        t.set_scaling_kind(c.sk);
                        t.partition(A);
                        t.assemble(A);
                        MatrixXd phi = random(A.rows(), 2, nlevels+it+skip+2019);
                        t.set_tol(tols[it]);
                        t.set_skip(skip);
                        t.set_preserve(c.preserve);
                        if(c.preserve) t.set_phi(&phi);
                        t.factorize();
                        VectorXd b = random(n, nlevels+it+skip+2019);
                        auto x = b;
                        t.solve(x);
                        double err = (A*x-b).norm() / b.norm();
                        auto hb = hash(b);
                        auto hx = hash(x);
                        allhashes.push_back(hb);
                        allhashes.push_back(hx);
                        if (tols[it] == 0.0) {
                            EXPECT_LE(err, 5e-12);
                        } else {
                            EXPECT_LE(err, tols[it] * 2e2);
                        }
                        count++;
                    }
                }
            }
        }
        size_t h = hashv(allhashes);
        cout << count << " tested. Overall hash(x,b) = " << h << endl;
    }
}

TEST(ApproxTest, ApproxLoRaSp) {
    vector<int> dims  = {2, 2,  2,  2,   3, 3};
    vector<int> sizes = {5, 10, 20, 64,  5, 15};
    vector<double> tols = {0.0, 1e-10, 1e-6, 1e-4};
    matrix_hash<VectorXd> hash;
    vector<params> configs = get_params();
    for(int test = 0; test < dims.size(); test++) {
        vector<size_t> allhashes;
        int count = 0;
        cout << "Test " << test << "... ";
        int s = sizes[test];
        int d = dims[test];
        SpMat Aref = neglapl(s, d);
        SpMat Arefunsym = neglapl_unsym(s, d, test+2019);
        int n = pow(s, d);
        int nlevelsmin = n < 1000 ? 1 : 8;
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin + 5; nlevels++) {
            for(int it = 0; it < tols.size(); it++) {
                for(auto c: configs) {
                    SpMat A = (c.syk == SymmKind::SPD ? Aref : (c.syk == SymmKind::SYM ? (-Aref) : Arefunsym));
                    Tree t(nlevels);
                    t.set_verb(VERB);
                    t.set_symm_kind(c.syk);
                    t.set_scaling_kind(c.sk);
                    t.partition_lorasp(A);
                    t.assemble(A);
                    MatrixXd phi = random(A.rows(), 2, nlevels+it+2019);
                    t.set_tol(tols[it]);
                    try {
                        t.factorize_lorasp();
                        VectorXd b = random(n, nlevels+it+2019);
                        auto x = b;
                        t.solve(x);
                        double err = (A*x-b).norm() / b.norm();
                        auto hb = hash(b);
                        auto hx = hash(x);
                        allhashes.push_back(hb);
                        allhashes.push_back(hx);
                        if (tols[it] == 0.0) {
                            EXPECT_LE(err, 5e-12);
                        } else {
                            EXPECT_LE(err, tols[it] * 2e2);
                        }
                    } catch (exception& ex) {
                        cout << ex.what();
                        EXPECT_TRUE(false);
                    }
                    count++;
                }
            }
        }
        size_t h = hashv(allhashes);
        cout << count << " tested. Overall hash(x,b) = " << h << endl;
    }
}

/**
 * Test that the code produce reproducable results
 */
TEST(ApproxTest, Repro) {
    int    dims[3]      = {2, 2, 2};
    int    sizes[3]     = {20, 64, 16};
    double tols[4]      = {1e-5, 10, 1e-8, 0.1};
    double skips[4]     = {1, 2, 0, 1};
    int    repeat       = 10;
    vector<params> configs = get_params();
    for(int test = 0; test < 3; test++) {
        printf("Tests "); fflush(stdout);
        int count = 0;
        int s = sizes[test];
        int d = dims[test];
        int n = pow(s, d);
        SpMat Aref = neglapl(s, d);
        SpMat Arefunsym = neglapl_unsym(s, d, test);
        for(int nlevels = 5; nlevels < 7; nlevels++) {
            for(int pr = 0; pr < 6; pr++) {
                for(auto c: configs) {
                    for(int lrsp = 0; lrsp < 2; lrsp++) {
                        printf("."); fflush(stdout);
                        SpMat A = (c.syk == SymmKind::SPD ? Aref : (c.syk == SymmKind::SYM ? (-Aref) : Arefunsym));
                        MatrixXd phi = random(A.rows(), 3, test+nlevels+pr+2019);
                        Tree t(nlevels);
                        t.set_verb(VERB);
                        t.set_symm_kind(c.syk);
                        t.set_part_kind(c.pk);
                        t.set_scaling_kind(c.sk);
                        if(lrsp == 0) {
                            t.partition(A);
                        } else {
                            t.partition_lorasp(A);
                        }
                        t.assemble(A);
                        t.set_tol(tols[pr]);
                        t.set_skip(skips[pr]);
                        t.set_preserve(c.preserve);
                        if(c.preserve) t.set_phi(&phi);
                        if(lrsp == 0) {
                            t.factorize();
                        } else {
                            t.factorize_lorasp();
                        }
                        VectorXd b = random(n, nlevels+test);
                        auto xref = b;
                        t.solve(xref);
                        count++;
                        for(int i = 0; i < repeat; i++) {
                            Tree t2(nlevels);
                            t2.set_verb(VERB);
                            t2.set_symm_kind(c.syk);
                            t2.set_part_kind(c.pk);
                            t2.set_scaling_kind(c.sk);
                            if(lrsp == 0) {
                                t2.partition(A);
                            } else {
                                t2.partition_lorasp(A);
                            }
                            t2.assemble(A);
                            t2.set_tol(tols[pr]);
                            t2.set_skip(skips[pr]);
                            t2.set_preserve(c.preserve);
                            t2.set_phi(&phi);
                            if(lrsp == 0) {
                                t2.factorize();
                            } else {
                                t2.factorize_lorasp();
                            }
                            auto x = b;
                            t2.solve(x);
                            EXPECT_EQ((xref - x).norm(), 0.0);
                        }
                    }
                }
            }
        }
        printf(": %d tested.\n", count); fflush(stdout);
    }
}

TEST(Run, Many) {
    vector<int>    dims  = {2,  2,  2,   3, 3,  3 };
    vector<int>    sizes = {5,  16, 64,  5, 10, 15};
    vector<double> tols  = {0.0, 1e-2, 1.0, 10.0};
    RUN_MANY = RUN_MANY > dims.size() ? dims.size() : RUN_MANY;
    matrix_hash<VectorXd> hash;
    vector<size_t> allhashes;
    vector<params> configs = get_params();
    for(int test = 0; test < RUN_MANY; test++) {
        cout << "Run " << test << "... \n";
        int count = 0;
        int n = sizes[test];
        int d = dims[test];
        SpMat Aref = neglapl(n, d);
        SpMat Arefunsym = neglapl_unsym(n, d, test);
        int N = Aref.rows();
        int nlevelsmin = N < 1000 ? 1 : 8;
        for(int nlevels = nlevelsmin; nlevels < nlevelsmin+3; nlevels++) {
            for(double tol : tols) {
                for(int skip = 0; skip < 3; skip++) {
                    for(int geo = 0; geo < 2; geo++) {
                        for(auto c: configs) {
                            SpMat A = (c.syk == SymmKind::SPD ? Aref : (c.syk == SymmKind::SYM ? (-Aref) : Arefunsym));
                            MatrixXd phi = random(A.rows(), 3, test+nlevels+2019);
                            MatrixXd X = linspace_nd(n, d);
                            Tree t(nlevels);
                            t.set_verb(VERB);
                            t.set_symm_kind(c.syk);
                            t.set_part_kind(c.pk);
                            t.set_scaling_kind(c.sk);
                            t.set_use_geo(geo);
                            t.set_Xcoo(&X);
                            t.set_tol(tol);
                            t.set_skip(skip);
                            t.set_preserve(c.preserve);
                            if(c.preserve) t.set_phi(&phi);
                            t.partition(A);
                            t.assemble(A);
                            t.factorize();
                            VectorXd b = random(N, nlevels+test);
                            auto x = b;
                            t.solve(x);
                            double res = (A*x-b).norm() / b.norm();
                            auto h = hash(x);
                            allhashes.push_back(h);
                            printf("%6d %4d %d] %3d %3.2e %d %d %d %d %d %d %d %d | %3.2e | %lu\n",
                                     N, n, d,   nlevels,
                                                     tol, skip,
                                                              geo,
                                                                c.preserve,
                                                                   int(c.syk), int(c.pk), int(c.sk), 1, 1,
                                                                   res, h);
                            count++;
                        }
                    }
                }
            }
        }
        cout << "Ran " << count << " tests\n";
        size_t h = hashv(allhashes);
        cout << "Overall hash so far: " << h << endl;
    }
    size_t h = hashv(allhashes);
    cout << "Overall hash: " << h << endl;
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);

    cxxopts::Options options("spaND tests", "Test suite for the spaND algorithms.");
    options.add_options()
        ("help", "Print help")
        ("v,verb", "Verbose (default: false)", cxxopts::value<bool>()->default_value("false"))
        ("n_threads", "Number of threads", cxxopts::value<int>()->default_value("4"))
        ("run", "How many Run.Many to run", cxxopts::value<int>()->default_value("4"))
        ;
    auto result = options.parse(argc, argv);

    if (result.count("help"))
    {
        cout << options.help({"", "Group"}) << endl;
        exit(0);
    }

    VERB = result["verb"].as<bool>();
    N_THREADS = result["n_threads"].as<int>();
    RUN_MANY = result["run"].as<int>();
    cout << "n_threads: " << N_THREADS << endl;
    cout << "verb: " << VERB << endl;
    cout << "run: " << RUN_MANY << endl;

    return RUN_ALL_TESTS();
}


/*
 * benchmark_hogwild_regression.cpp
 * author: Abhijit Chowdhary (achowdh2@ncsu.edu)
 *
 * Benchmark HOGWILD! as applied to regression on to a simple random normal 50
 * x 50 matrix A and random normal vector b:
 *
 *  minimize ||Ax-b||_2^2
 *
 * Outputs to results.txt and stdout time taken to reach desired tolerance for
 * each core count possible in system.
 *  ---------------------------------------------------------------------------
 * This Source Code Form is subject to the terms of the Mozilla Public License
 * v. 2.0. If a copy of the MPL was not distributed with this file, You can
 * obtain one at http://mozilla.org/MPL/2.0/.
 */
#include <omp.h>
#include <stdio.h>

#include <algorithm>
#include <array>
#include <atomic>
#include <iostream>
#include <random>

#include <Eigen/Dense>

#include "readCSV.h"

#define ETA 0.001
#define NUM_EPOCHS 50
#define TRIALS_PER_CORE 5

int
main(int argc, char **argv)
{
  // Initialize random state, and parallel parameters.
  unsigned P = omp_get_max_threads();
  Eigen::initParallel();
  omp_set_dynamic(0);
  std::mt19937 gen(0);

  // Read MSD dataset into memory and format matrices.
  unsigned const num_data = 160000; unsigned const num_features = 400;
  Eigen::MatrixXd Data;
  readCSV<double>("../simplemat.csv", Data);
  Eigen::MatrixXd A = Data.topRightCorner(num_data, num_features);
  Eigen::VectorXd b = Data.col(0);

  // Construct sampling w/ replacement vector.
  std::uniform_int_distribution<std::mt19937::result_type> distN(0,num_data-1);
  //std::array<unsigned, num_data*NUM_EPOCHS> rand_selection;
  unsigned *rand_selection = new unsigned[num_data*NUM_EPOCHS];
  for (unsigned k = 0; k < num_data*NUM_EPOCHS; k++)
  {
    rand_selection[k] = distN(gen);
  }

  std::array<std::atomic<double>, num_features> x;
  for (unsigned p = 0; p < P; ++p)
  { // Begin trials for processor count p+1
    omp_set_num_threads(p+1);
    double t = 0.0;
    for (unsigned trial = 0; trial < TRIALS_PER_CORE; ++trial)
    { // Begin SGD trial
      for (unsigned k = 0; k < num_features; k++) { x[k] = 1; }

      double t_start, t_end;
      t_start = omp_get_wtime();

      #pragma omp parallel for
      for (unsigned k = 0; k < num_data*NUM_EPOCHS; k++)
      { // Begin parallel SGD iterations
        unsigned id = rand_selection[k];
        double dg = 0;
        for (unsigned i = 0; i < num_features; i++) { dg += A(id, i)*x[i].load(); }
        dg -= b(id);
        for (unsigned i = 0; i < num_features; i++)
        {
          double dgi = x[i].load() - ETA*( A(id,i)*dg );
          x[i].exchange( dgi );
        }
      } // End parallel SGD iterations

      t_end = omp_get_wtime(); t += t_end-t_start;
    } // End SGD trial
    t /= TRIALS_PER_CORE;

    Eigen::MatrixXd xx(num_features,1);
    for (int k = 0; k < num_features; k++) { xx(k) = x[k].load(); }
    double E = 0.5*(A*xx-b).squaredNorm();
    printf("T(p=%d) = %.5f, E(p=%d) = %.5f\n", p+1, t, p+1, E);
  } // End trials for processor count p+1

  delete []rand_selection;
  return 0;
}


#ifndef EZSOLVER_H_
#define EZSOLVER_H_

#include <Eigen/Dense>

class EzSolver {
public:
  EzSolver(const Eigen::Ref<const Eigen::MatrixXd> &matA,
           const Eigen::Ref<const Eigen::MatrixXd> &matB);
  Eigen::VectorXcd compute(double sigma, int nev);
  Eigen::VectorXcd compute_sym(double sigma, int nev);

private:
  int ndim_;
  Eigen::MatrixXd matA_, matB_;
};

#endif


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// OuterExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file OuterExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades outer product.

#ifndef E3GA_OUTER_PRODUCT_EXPLICIT_HPP__
#define E3GA_OUTER_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Outer.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void outer_0_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void outer_0_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_0_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_0_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void outer_1_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_1_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(1);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_1_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_2_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_2_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_3_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4> outerFunctionsContainer = {{
		{{outer_0_0<T>,outer_0_1<T>,outer_0_2<T>,outer_0_3<T>}},
		{{outer_1_0<T>,outer_1_1<T>,outer_1_2<T>,{}}},
		{{outer_2_0<T>,outer_2_1<T>,{},{}}},
		{{outer_3_0<T>,{},{},{}}}
	}};

}/// End of Namespace

#endif // E3GA_OUTER_PRODUCT_EXPLICIT_HPP__


// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>

// Version: 1.0
// Author: Craig Carignan <craigc at ssl dot umd dot edu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

// Inverse of a positive definite symmetric matrix times a vector
// based on LDL^T Decomposition
#ifndef LDL_SOLVER_EIGEN_HPP
#define LDL_SOLVER_EIGEN_HPP

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

namespace KDL
{
    /**
     * \brief Solves the system of equations Aq = v for q via LDL decomposition,
     *        where A is a square positive definite matrix
     *
     * The algorithm factor A into the product of three matrices LDL^T, where L
     * is a lower triangular matrix and D is a diagonal matrix.  This allows q
     * to be computed without explicity inverting A.  Note that the LDL decomposition
     * is a variant of the classical Cholesky Decomposition that does not require
     * the computation of square roots.
     * Input parameters:
     * @param A matrix<double>(nxn)
     * @param v vector<double> n
     * @param vtmp vector<double> n [temp variable]
     * Output parameters:
     * @param L matrix<double>(nxn)
     * @param D vector<double> n
     * @param q vector<double> n
     * @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match
     * References:
     * https://en.wikipedia.org/wiki/Cholesky_decomposition
     */
    int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q);
}
#endif


#include <random>

#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <Eigen/Dense>

int NUM_OBSTACLE;
double NEIGHBER_RANGE = 5.0;
double LAMBDA = 2.0;
double GAMMA = 0.35;
double N_PRIME = 3.0;
double N = 2.0;
double MAX_VELOCITY = 1.5;
double RELAXATION_TIME = 0.5;
double DESIRED_FORCE_FACTOR;
double SOCIAL_FORCE_FACTOR;
double HZ;
double INITIAL_KEEP_OUT_POSITION_X;
double INITIAL_KEEP_OUT_POSITION_Y;
double INITIAL_KEEP_OUT_RANGE;

std::string ROBOT_FRAME;
std::string WORLD_FRAME;
std::string OBS_FRAME;
double SIMULATION_SQUARE_LENGTH;

class SFMObstacle
{
public:
    SFMObstacle(void);

    unsigned int id;
    Eigen::Vector3d pose;
    Eigen::Vector3d velocity;
    Eigen::Vector3d current_goal;
    double preferred_speed;
    bool dodging_right;
    Eigen::Vector3d last_desired_force;
    Eigen::Vector3d last_social_force;
private:
};

SFMObstacle::SFMObstacle(void)
{
    id = 0;
    pose = Eigen::Vector3d::Zero();
    velocity = Eigen::Vector3d::Zero();
    current_goal = Eigen::Vector3d::Zero();
    preferred_speed = 1.0;
    dodging_right = true;
    last_desired_force = Eigen::Vector3d::Zero();
    last_social_force = Eigen::Vector3d::Zero();
}

std::ostream& operator<<(std::ostream& out, const SFMObstacle& sfmo)
{
    out << "id: " << sfmo.id << "\n"
        << "pose: " << sfmo.pose.transpose() << "\n"
        << "velocity: " << sfmo.velocity.transpose() << "\n"
        << "current speed: " << sfmo.velocity.norm() << "\n"
        << "current goal: " << sfmo.current_goal.transpose() << "\n"
        << "last desired force: " << sfmo.last_desired_force.transpose() << "\n"
        << "last social force: " << sfmo.last_social_force.transpose();
    return out;
}

Eigen::Vector3d get_next_goal(const SFMObstacle& agent)
{
    Eigen::Vector3d new_goal = Eigen::Vector3d::Zero();
    while(1){
        new_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
        double distance_from_agent = (new_goal - agent.pose).norm();
        if(distance_from_agent > SIMULATION_SQUARE_LENGTH * 0.5){
            return new_goal;
        }
    }
}

Eigen::Vector3d get_social_force(const SFMObstacle& agent, const std::vector<SFMObstacle>& obstacles)
{
    Eigen::Vector3d force = Eigen::Vector3d::Zero();

    for(auto obstacle : obstacles){
        if(agent.id == obstacle.id){
            continue;
        }

        Eigen::Vector3d diff_vector = obstacle.pose - agent.pose;
        double distance = diff_vector.norm();
        if (distance > NEIGHBER_RANGE){
            continue;
        }
        // e_{ij}
        Eigen::Vector3d diff_direction = diff_vector.normalized();

        double other_angle = atan2(diff_direction(1), diff_direction(0));
        double agent_angle = atan2(agent.velocity(1), agent.velocity(0));
        double angle_fov = other_angle - agent_angle;
        angle_fov = atan2(sin(angle_fov), cos(angle_fov));
        if(fabs(angle_fov) > M_PI * 5.0 / 6.0){
            continue;
        }

        Eigen::Vector3d velocity_diff = agent.velocity - obstacle.velocity;

        // D_{ij}
        Eigen::Vector3d interaction_vector = LAMBDA * velocity_diff + diff_direction;
        // t_{ij}
        Eigen::Vector3d interaction_direction = interaction_vector.normalized();

        double interaction_angle = atan2(interaction_direction(1), interaction_direction(0));
        double theta_angle = other_angle - interaction_angle;
        theta_angle = atan2(sin(theta_angle), cos(theta_angle));

        double sign_of_theta = (fabs(theta_angle) == 0.00) ? 0.0 : theta_angle / fabs(theta_angle);

        double b = GAMMA * interaction_vector.norm();

        double force_velocity_amount = -std::exp(-diff_vector.norm() / b - (N_PRIME * b * theta_angle) * (N_PRIME * b * theta_angle));
        double force_angle_amount = -sign_of_theta * std::exp(-diff_vector.norm() / b - (N * b * theta_angle) * (N * b * theta_angle));

        Eigen::Vector3d force_velocity = force_velocity_amount * interaction_direction;

        Eigen::Vector3d interaction_direction_normal;
        if(agent.dodging_right){
            interaction_direction_normal << -interaction_direction(1), interaction_direction(0), interaction_direction(2);
        }else{
            interaction_direction_normal << interaction_direction(1), -interaction_direction(0), interaction_direction(2);
        }

        Eigen::Vector3d force_angle = force_angle_amount * interaction_direction_normal;

        force += force_velocity + force_angle;
    }
    return force;
}

Eigen::Vector3d get_desired_force(const SFMObstacle& agent)
{
    Eigen::Vector3d force = Eigen::Vector3d::Zero();
    // force = (agent.current_goal - agent.pose).normalized();
    Eigen::Vector3d desired_vector = (agent.current_goal - agent.pose).normalized();
    force = (agent.preferred_speed * desired_vector - agent.velocity) / RELAXATION_TIME;
    return force;
}

void set_obs_list(const std::vector<SFMObstacle>& obstacles, std::vector<geometry_msgs::TransformStamped>& obs_list)
{
    obs_list.clear();
    for(auto& obs : obstacles){
        geometry_msgs::TransformStamped tfs;
        tfs.header.stamp = ros::Time::now();
        tfs.header.frame_id = WORLD_FRAME;
        tfs.child_frame_id = OBS_FRAME + std::to_string(obs.id);
        tfs.transform.translation.x = obs.pose(0);
        tfs.transform.translation.y = obs.pose(1);
        tfs.transform.rotation = tf::createQuaternionMsgFromYaw(atan2(obs.velocity(1), obs.velocity(0)));
        obs_list.push_back(tfs);
    }
}

void simulate_one_step(std::vector<SFMObstacle>& obstacles, double dt)
{
    if(dt < 0.0){
        return;
    }
    // std::cout << "simulate one step" << std::endl;
    for(auto& obs : obstacles){
        // std::cout << obs << std::endl;
        Eigen::Vector3d relative_goal = obs.current_goal - obs.pose;
        if(relative_goal.norm() < 0.5){
            obs.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
            obs.current_goal = get_next_goal(obs);
            obs.current_goal(2) = 0;
        }
        Eigen::Vector3d desired_force = get_desired_force(obs);
        obs.last_desired_force = desired_force;
        // std::cout << obs.last_desired_force.transpose() << std::endl;
        Eigen::Vector3d social_force = get_social_force(obs, obstacles);
        obs.last_social_force = social_force;
        // std::cout << obs.last_social_force.transpose() << std::endl;
        Eigen::Vector3d force = DESIRED_FORCE_FACTOR * desired_force + SOCIAL_FORCE_FACTOR * social_force;
        obs.velocity += obs.velocity + force * dt;
        // std::cout << obs.velocity.transpose() << std::endl;
        double speed = obs.velocity.norm();
        if(speed > obs.preferred_speed){
            obs.velocity = obs.velocity.normalized() * obs.preferred_speed;
        }
        obs.pose += obs.velocity * dt;
        // std::cout << obs.pose.transpose() << std::endl;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sfm_obstacle_simulator");
    std::cout << "=== sfm_obstacle_simulator ===" << std::endl;
    ros::NodeHandle nh;

    ros::NodeHandle local_nh("~");

    local_nh.param<std::string>("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME, {"base_link"});
    local_nh.param<std::string>("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"map"});
    local_nh.param<std::string>("/dynamic_avoidance/OBSTACLES_FRAME", OBS_FRAME, {"obs"});
    local_nh.param<double>("HZ", HZ, {20.0});
    local_nh.param<int>("NUM_OBSTACLE", NUM_OBSTACLE, {20});
    local_nh.param<double>("DESIRED_FORCE_FACTOR", DESIRED_FORCE_FACTOR, {20.0});
    local_nh.param<double>("SOCIAL_FORCE_FACTOR", SOCIAL_FORCE_FACTOR, {100.0});
    local_nh.param<double>("SIMULATION_SQUARE_LENGTH", SIMULATION_SQUARE_LENGTH, {20.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_POSITION_X", INITIAL_KEEP_OUT_POSITION_X, {-10.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_POSITION_Y", INITIAL_KEEP_OUT_POSITION_Y, {0.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_RANGE", INITIAL_KEEP_OUT_RANGE, {3.0});
    int SEED;
    local_nh.param<int>("SEED", SEED, {-1});

    std::cout << "HZ: " << HZ << std::endl;
    std::cout << "NUM_OBSTACLE: " << NUM_OBSTACLE << std::endl;
    std::cout << "DESIRED_FORCE_FACTOR: " << DESIRED_FORCE_FACTOR << std::endl;
    std::cout << "SOCIAL_FORCE_FACTOR: " << SOCIAL_FORCE_FACTOR << std::endl;
    std::cout << "SIMULATION_SQUARE_LENGTH: " << SIMULATION_SQUARE_LENGTH << std::endl;
    std::cout << "INITIAL_KEEP_OUT_POSITION_X: " << INITIAL_KEEP_OUT_POSITION_X << std::endl;
    std::cout << "INITIAL_KEEP_OUT_POSITION_Y: " << INITIAL_KEEP_OUT_POSITION_Y << std::endl;
    std::cout << "INITIAL_KEEP_OUT_RANGE: " << INITIAL_KEEP_OUT_RANGE << std::endl;
    std::cout << "SEED: " << SEED << std::endl;

    ros::Rate loop_rate(HZ);

    tf::TransformBroadcaster obs_broadcaster;

    // srand((unsigned int)time(0));// for Eigen
    // srand((unsigned int)SEED);// for Eigen
    srand(SEED < 0 ? (unsigned int)time(0) : (unsigned int)SEED);// for Eigen
    std::random_device rnd;
    std::mt19937 mt(SEED < 0 ? rnd() : SEED);
    std::uniform_real_distribution<> dist(1.0, MAX_VELOCITY);
    std::uniform_int_distribution<> dist_bool(0, 1);

    std::vector<SFMObstacle> obstacles;
    obstacles.clear();
    // initialize
    Eigen::Vector3d init_pos(INITIAL_KEEP_OUT_POSITION_X, INITIAL_KEEP_OUT_POSITION_Y, 0.0);
    for(int i=0;i<NUM_OBSTACLE;i++){
        SFMObstacle o;
        o.id = i;
        while(1){
            o.pose = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
            o.pose(2) = 0;
            if((o.pose - init_pos).norm() > INITIAL_KEEP_OUT_RANGE){
                break;
            }
        }
        o.velocity = Eigen::Vector3d::Random();
        o.velocity(2) = 0;
        o.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
        o.current_goal(2) = 0;
        o.preferred_speed = dist(mt);
        o.dodging_right = dist(mt);
        obstacles.push_back(o);
        std::cout << o << std::endl;
    }

    tf::TransformListener listener;

    while(ros::ok()){
        bool robot_added_flag = false;
        try{
            static bool first_tf_flag = true;
            static double last_tf_time = 0;
            static Eigen::Vector3d last_robot_pose = Eigen::Vector3d::Zero();
            Eigen::Vector3d robot_velocity = Eigen::Vector3d::Zero();
            Eigen::Vector3d robot_pose = Eigen::Vector3d::Zero();

            geometry_msgs::PoseStamped robot_p;
            robot_p.header.frame_id = ROBOT_FRAME;
            robot_p.header.stamp = ros::Time(0);
            robot_p.pose.orientation = tf::createQuaternionMsgFromYaw(0);
            listener.transformPose(WORLD_FRAME, robot_p, robot_p);
            robot_pose(0) = robot_p.pose.position.x;
            robot_pose(1) = robot_p.pose.position.y;
            if(!first_tf_flag){
                double dt = robot_p.header.stamp.toSec() - last_tf_time;
                if(dt > 1e-4){
                    robot_velocity = (robot_pose - last_robot_pose) / dt;
                }else{
                    robot_velocity = Eigen::Vector3d::Zero();
                }
            }else{
                first_tf_flag = false;
            }
            SFMObstacle robot_o;
            robot_o.id = obstacles.size();
            robot_o.pose = robot_pose;
            robot_o.velocity = robot_velocity;
            robot_o.current_goal = robot_o.pose + robot_o.velocity * 5.0;// 5[s] after
            obstacles.push_back(robot_o);
            // std::cout << "robot is added" << std::endl;
            // std::cout << robot_o << std::endl;
            // std::cout << robot_p.header.stamp.toSec() - last_tf_time << std::endl;
            // std::cout << last_robot_pose.transpose() << std::endl;

            robot_added_flag = true;
            last_robot_pose = robot_pose;
            last_tf_time = robot_p.header.stamp.toSec();
        }catch(tf::TransformException& ex){
            std::cout << ex.what() << std::endl;
        }

        simulate_one_step(obstacles, 1 / HZ);

        if(robot_added_flag){
            obstacles.pop_back();
        }

        std::vector<geometry_msgs::TransformStamped> obs_list;
        obs_list.clear();
        set_obs_list(obstacles, obs_list);

        std::cout << "===" << std::endl;
        for(const auto obs : obstacles){
            std::cout << obs << std::endl;
        }

        obs_broadcaster.sendTransform(obs_list);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
};


// Copyright 2014, D. E. Shaw Research.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt )

// A few (2) rounds of a PRNG has some appeal as a 'randomizing'
// alternative to the existing boost::hash_value.
//
// This is just a sketch...
//
// A fully general version would require quite a bit more template
// armor to handle all the integer-promotion corner cases and aliases
// and the fact that philox and threefry have only uint32 and uint64
// variants.
//
// How many rounds is enough?  Hash_value's main use is to index into
// hash tables, so it may not have to be a certifiably "good" random
// number generator which would require 7 rounds for philox and 13 for
// threefry.  Two rounds of philox or four rounds of threefry "looks"
// random, which may be sufficient.  That may be "random" enough to
// satisfy people who are unhappy with the complete lack of
// randomness in the current (1.55) implementation of hash_value(v).

#include <boost/random/philox.hpp>
#include <boost/cstdint.hpp>

size_t hash_value(uint32_t x){
    typedef uint32_t Uint;
    boost::array<Uint, 2> c = {x, 0};
    return size_t(boost::random::philox<2, Uint, 2>()(c)[0]);
}

size_t hash_value(uint64_t x){
    typedef uint64_t Uint;
    boost::array<Uint, 2> c = {x, 0};
    return size_t(boost::random::philox<2, Uint, 2>()(c)[0]);
}

int main(int argc, char **argv){
    for(unsigned i=0; i<10; ++i){
        size_t hv = hash_value(i);
        std::cout << "hv(" << i << ") = " << hv << std::hex << " = " << hv << std::dec << "\n";
    }
    return 0;
}


// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_
#define STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_

#include <helper_functions/float_comparisons.hpp>
#include <motion_model/motion_model_interface.hpp>
#include <motion_model/stationary_motion_model.hpp>
#include <state_estimation/noise_model/noise_interface.hpp>
#include <state_estimation/state_estimation_interface.hpp>
#include <state_estimation/visibility_control.hpp>

#include <Eigen/LU>

#include <limits>
#include <vector>

namespace autoware
{
namespace common
{
namespace state_estimation
{
///
/// @brief      A Kalman filter implementation.
///
/// @tparam     MotionModelT  Type of the motion model.
/// @tparam     NoiseModelT   Type of the noise model.
///
template<typename MotionModelT, typename NoiseModelT>
class STATE_ESTIMATION_PUBLIC KalmanFilter
  : public StateEstimationInterface<KalmanFilter<MotionModelT, NoiseModelT>>
{
  static_assert(
    std::is_base_of<common::motion_model::MotionModelInterface<MotionModelT>, MotionModelT>::value,
    "\n\nMotion model must inherit from MotionModelInterface\n\n");
  static_assert(
    std::is_base_of<NoiseInterface<NoiseModelT>, NoiseModelT>::value,
    "\n\nNoise model must inherit from NoiseInterface\n\n");
  static_assert(
    std::is_same<typename MotionModelT::State, typename NoiseModelT::State>::value,
    "\n\nMotion model and noise model must have the same underlying state\n\n");

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using State = typename MotionModelT::State;
  using StateMatrix = typename State::Matrix;
  using MotionModel = MotionModelT;
  using NoiseModel = NoiseModelT;

  ///
  /// @brief      Constructs a new instance of a Kalman filter.
  ///
  /// @param[in]  motion_model        The motion model to be used to predict the movement.
  /// @param[in]  noise_model         The noise model that models the motion noise.
  /// @param[in]  initial_state       The initial state of the filter.
  /// @param[in]  initial_covariance  The initial state covariance.
  ///
  explicit KalmanFilter(
    MotionModelT motion_model,
    NoiseModelT noise_model,
    const State & initial_state,
    const StateMatrix & initial_covariance)
  : m_motion_model{motion_model},
    m_noise_model{noise_model},
    m_state{initial_state},
    m_covariance{initial_covariance} {}

  ///
  /// @brief      Predict next state.
  ///
  /// @param[in]  dt    Time difference to the time at which prediction is needed.
  ///
  /// @return     Predicted state.
  ///
  State crtp_predict(const std::chrono::nanoseconds & dt)
  {
    m_state = m_motion_model.predict(m_state, dt);
    const auto & motion_jacobian = m_motion_model.jacobian(m_state, dt);
    m_covariance =
      motion_jacobian * m_covariance * motion_jacobian.transpose() + m_noise_model.covariance(dt);
    return m_state;
  }

  ///
  /// @brief      Correct the predicted state given a measurement
  ///
  /// @note       It is expected that a prediction step was done right before the correction.
  ///
  /// @param[in]  measurement   Current measurement.
  ///
  /// @tparam     MeasurementT  Measurement type.
  ///
  /// @return     State corrected with the measurement.
  ///
  template<typename MeasurementT>
  State crtp_correct(const MeasurementT & measurement)
  {
    const auto expected_measurement = measurement.create_new_instance_from(m_state);
    const auto innovation = wrap_all_angles(measurement.state() - expected_measurement);
    const auto mapping_matrix = measurement.mapping_matrix_from(m_state);
    const auto innovation_covariance =
      mapping_matrix * m_covariance * mapping_matrix.transpose() + measurement.covariance();
    const auto kalman_gain =
      m_covariance * mapping_matrix.transpose() * innovation_covariance.inverse();
    m_state += kalman_gain * innovation.vector();
    m_state.wrap_all_angles();
    m_covariance = (State::Matrix::Identity() - kalman_gain * mapping_matrix) * m_covariance;
    return m_state;
  }

  ///
  /// @brief      Reset the state of the filter to a given state and covariance.
  ///
  /// @param[in]  state       The new state that overwrites one stored in the filter.
  /// @param[in]  covariance  The new covariance that overwrites one stored in the filter.
  ///
  void crtp_reset(const State & state, const StateMatrix & covariance)
  {
    m_state = state;
    m_covariance = covariance;
  }

  /// @brief      Get current state.
  auto & crtp_state() {return m_state;}
  /// @brief      Get current state.
  const auto & crtp_state() const {return m_state;}

  /// @brief      Get current covariance.
  auto & crtp_covariance() {return m_covariance;}
  /// @brief      Get current covariance.
  const auto & crtp_covariance() const {return m_covariance;}

private:
  /// Motion model used to predict the state forward.
  MotionModelT m_motion_model{};
  /// Noise model of the movement.
  NoiseModelT m_noise_model{};
  /// State of the tracked object.
  State m_state{};
  /// Covariance of the state of the tracked object.
  StateMatrix m_covariance{StateMatrix::Zero()};
};

///
/// @brief      A utility function that creates a Kalman filter.
///
/// @details    Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  motion_model        A motion model.
/// @param[in]  noise_model         A noise model.
/// @param[in]  initial_state       The initial state
/// @param[in]  initial_covariance  The initial covariance
///
/// @tparam     MotionModelT        Type of the motion model.
/// @tparam     NoiseModelT         Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename MotionModelT, typename NoiseModelT>
auto make_kalman_filter(
  const MotionModelT & motion_model,
  const NoiseModelT & noise_model,
  const typename MotionModelT::State & initial_state,
  const typename MotionModelT::State::Matrix & initial_covariance)
{
  return KalmanFilter<MotionModelT, NoiseModelT>{
    motion_model, noise_model, initial_state, initial_covariance};
}

///
/// @brief      A utility function that creates a Kalman filter that is to be used for correction
///             only, i.e., this Kalman filter cannot predict the state forward in time.
///
/// @details    Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  initial_state       The initial state
/// @param[in]  initial_covariance  The initial covariance
///
/// @tparam     StateT              { description }
/// @tparam     MotionModelT  Type of the motion model.
/// @tparam     NoiseModelT   Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename StateT>
auto make_correction_only_kalman_filter(
  const StateT & initial_state,
  const typename StateT::Matrix & initial_covariance)
{
  struct DummyNoise : public NoiseInterface<DummyNoise>
  {
    using State = StateT;
    typename State::Matrix crtp_covariance(const std::chrono::nanoseconds &) const
    {
      throw std::runtime_error(
              "Trying to use a correction-only Kalman filter to predict the state.");
    }
  };

  using MotionModel = common::motion_model::StationaryMotionModel<StateT>;

  return make_kalman_filter(
    MotionModel{}, DummyNoise{}, initial_state, initial_covariance);
}

///
/// @brief      A utility function that creates a Kalman filter from a vector of variances.
///
///             Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  motion_model       A motion model.
/// @param[in]  noise_model        A noise model.
/// @param[in]  initial_state      Initial state.
/// @param[in]  initial_variances  Initial variances as a vector.
///
/// @tparam     MotionModelT       Type of the motion model.
/// @tparam     NoiseModelT        Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename MotionModelT, typename NoiseModelT>
auto make_kalman_filter(
  const MotionModelT & motion_model,
  const NoiseModelT & noise_model,
  const typename MotionModelT::State & initial_state,
  const std::vector<typename MotionModelT::State::Scalar> & initial_variances)
{
  using State = typename MotionModelT::State;
  if (initial_variances.size() != static_cast<std::size_t>(State::size())) {
    std::runtime_error(
      "Cannot create Kalman filter - dimensions mismatch. Provided " +
      std::to_string(initial_variances.size()) + " variances, but " +
      std::to_string(State::size()) + " required.");
  }
  typename State::Vector variances{State::Vector::Zero()};
  // A small enough epsilon to compare a floating point variance with zero.
  const auto epsilon = 5.0F * std::numeric_limits<common::types::float32_t>::epsilon();
  for (std::uint32_t i = 0; i < initial_variances.size(); ++i) {
    if (common::helper_functions::comparisons::abs_lte(initial_variances[i], 0.0F, epsilon)) {
      throw std::domain_error("Variances must be positive");
    }
    variances[static_cast<std::int32_t>(i)] = initial_variances[i] * initial_variances[i];
  }
  return KalmanFilter<MotionModelT, NoiseModelT>{
    motion_model, noise_model, initial_state, variances.asDiagonal()};
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_HOMOGRAPHY_HPP
#define PIC_COMPUTER_VISION_HOMOGRAPHY_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

#include "../computer_vision/nelder_mead_opt_homography.hpp"

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief estimateHomography estimates an homography matrix H between image 1 to image 2
 * @param points0 is an array of points computed from image 1.
 * @param points1 is an array of points computed from image 2.
 * @return It returns the homography matrix H.
 */
PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0,
                                   std::vector< Eigen::Vector2f > &points1)
{
    Eigen::Matrix3d  H;

    if((points0.size() != points1.size()) || (points0.size() < 4)) {
        H.setZero();
        return H;
    }

    Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);
    Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);

    Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);
    Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);

    int n = int(points0.size());
    Eigen::MatrixXd A(n * 2, 9);

    //set up the linear system
    for(int i = 0; i < n; i++) {
        //transform coordinates for increasing stability of the system
        Eigen::Vector2f p0 = points0[i];
        Eigen::Vector2f p1 = points1[i];

        p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
        p0[1] = (p0[1] - transform_0[1]) / transform_0[2];

        p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
        p1[1] = (p1[1] - transform_1[1]) / transform_1[2];

        int j = i * 2;
        A(j, 0) = 0.0;
        A(j, 1) = 0.0;
        A(j, 2) = 0.0;
        A(j, 3) = p0[0];
        A(j, 4) = p0[1];
        A(j, 5) = 1.0;
        A(j, 6) = -p1[1] * p0[0];
        A(j, 7) = -p1[1] * p0[1];
        A(j, 8) = -p1[1];

        j++;

        A(j, 0) = p0[0];
        A(j, 1) = p0[1];
        A(j, 2) = 1.0;
        A(j, 3) = 0.0;
        A(j, 4) = 0.0;
        A(j, 5) = 0.0;
        A(j, 6) = -p1[0] * p0[0];
        A(j, 7) = -p1[0] * p0[1];
        A(j, 8) = -p1[0];
    }

    //solve the linear system
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
    Eigen::MatrixXd V = svd.matrixV();

    n = int(V.cols()) - 1;

    //assign and transpose
    H(0, 0) = V(0, n);
    H(0, 1) = V(1, n);
    H(0, 2) = V(2, n);

    H(1, 0) = V(3, n);
    H(1, 1) = V(4, n);
    H(1, 2) = V(5, n);

    H(2, 0) = V(6, n);
    H(2, 1) = V(7, n);
    H(2, 2) = V(8, n);

    H = mat_1.inverse() * H * mat_0;
    return H / H(2, 2);
}

/**
 * @brief estimateHomographyRansac computes the homography such that: points1 = H * points0
 * @param points0
 * @param points1
 * @param inliers
 * @param maxIterations
 * @return
 */
PIC_INLINE Eigen::Matrix3d estimateHomographyRansac(std::vector< Eigen::Vector2f > &points0,
                                         std::vector< Eigen::Vector2f > &points1,
                                         std::vector< unsigned int > &inliers,
                                         unsigned int maxIterations = 100,
                                         double threshold = 4.0,
                                         unsigned int seed = 1)
{
    if(points0.size() < 5) {
        return estimateHomography(points0, points1);
    }

    Eigen::Matrix3d H;
    int nSubSet = 4;

    std::mt19937 m(seed);

    unsigned int n = int(points0.size());

    unsigned int *subSet = new unsigned int [nSubSet];

    inliers.clear();

    for(unsigned int i = 0; i < maxIterations; i++) {

        getRandomPermutation(m, subSet, nSubSet, n);

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(int j = 0; j < nSubSet; j++) {
            sub_points0.push_back(points0[subSet[j]]);
            sub_points1.push_back(points1[subSet[j]]);
        }

        Eigen::Matrix3d tmpH = estimateHomography(sub_points0, sub_points1);

        //is it a good one?
        std::vector< unsigned int > tmp_inliers;

        for(unsigned int j = 0; j < n; j++) {
            Eigen::Vector3d point_hom = Eigen::Vector3d(points0[j][0], points0[j][1], 1.0);
            Eigen::Vector3d pp = tmpH * point_hom;
            pp /= pp[2];

            double dx = points1[j][0] - pp[0];
            double dy = points1[j][1] - pp[1];
            double squared_diff = (dx * dx) + (dy * dy);

            if(squared_diff < threshold) {
                tmp_inliers.push_back(j);
            }
        }

        //get the inliers
        if(tmp_inliers.size() > inliers.size()) {
            H = tmpH;
            inliers.clear();
            inliers.assign(tmp_inliers.begin(), tmp_inliers.end());
        }
    }

    //improve estimate with inliers only
    if(inliers.size() > 3) {
        #ifdef PIC_DEBUG
            printf("Better estimate using inliers only.\n");
        #endif

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(unsigned int i = 0; i < inliers.size(); i++) {
            sub_points0.push_back(points0[inliers[i]]);
            sub_points1.push_back(points1[inliers[i]]);
        }

        H = estimateHomography(sub_points0, sub_points1);
    }

    return H;
}

/**
* @brief estimateHomographyRansac computes the homography such that: points1 = H * points0
* @param points0
* @param points1
* @param inliers
* @param maxIterations
* @return
*/
PIC_INLINE Eigen::Matrix3d estimateHomographyWithNonLinearRefinement(
                                         std::vector< Eigen::Vector2f > &points0,
                                         std::vector< Eigen::Vector2f > &points1,
                                         std::vector< unsigned int > &inliers,
                                         unsigned int maxIterationsRansac = 10000,
                                         double thresholdRansac = 2.5,
                                         unsigned int seedRansac = 1,
                                         unsigned int maxIterationsNonLinear = 10000,
                                         float thresholdNonLinear = 1e-5f
                                                          ) {

    Eigen::Matrix3d H = estimateHomographyRansac(points0, points1, inliers,
                                                 maxIterationsRansac, thresholdRansac,
                                                 seedRansac);

    NelderMeadOptHomography nmoh(points0, points1, inliers);
    float *H_array = getLinearArrayFromMatrix(H);
    nmoh.run(H_array, 8, thresholdNonLinear, maxIterationsNonLinear, H_array);
    H = getMatrix3dFromLinearArray(H_array);
    return H;
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_HOMOGRAPHY_HPP


/*
    utils.hpp
    Desc: Utility functions for mvee library
    @author Chris Larson, Cornell University
    @date (2017)
    @version 1.0
*/

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <string>
#include <thread>
#include <future>
#include <exception>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>

int findIdx(Eigen::VectorXd& vec, double val)
{
    for (int i=0; i<vec.rows(); i++)
    {
        if (vec(i) == val)
        {
			return i;
        }
    }
    return -1;
}

std::vector<double> toStdVec(Eigen::VectorXd& fromX, double (*f)(double))
{
	std::vector<double> toX(fromX.size(), 0.);
	for (int i=0; i<fromX.size(); i++)
	{
		toX[i] = (*f)(fromX(i));
	}
	return toX;
}

std::vector<std::vector<double>> toStdMat(Eigen::MatrixXd& fromX, double (*f)(double))
{
	std::vector<std::vector<double>>
	toX(fromX.rows(), std::vector<double>(fromX.cols(), 0.));
	for (int i=0; i<fromX.rows(); i++)
	{
		for (int j=0; j<fromX.cols(); j++)
		{
			toX[i][j] = (*f)(fromX(i, j));
		}
	}
	return toX;
}

Eigen::MatrixXd readCSV(std::string file, char delim)
{
	// Get dimensions for Eigen::MatrixXd constructor
	int rows = 0;
	int cols = 0;
	std::ifstream in;
	in.open(file);
	if (in.is_open())
	{
		std::string s;
		while(!in.eof())
		{
			getline(in, s);
			if (s != "")
			{
				rows++;
				if (!cols)
				{
					cols = count(s.begin(), s.end(), delim) + 1;
				}
			}
		}
	}
	else
	{
		throw std::runtime_error("Error: Cannot open " + file + ".");
	}
	in.close();

	// Stream file into Eigen::MatrixXd container
	std::string line;
	Eigen::MatrixXd X = Eigen::MatrixXd(rows, cols);
	int row = 0;
	int col = 0;
	in.open(file);
	while (getline(in, line))
	{
		char* ptr = (char*) line.c_str();
		int len = line.length();
		col = 0;
		char* start = ptr;
		for (int i=0; i<len; i++)
		{
			if (ptr[i] == delim)
			{
				X(row, col++) = atof(start);
				start = ptr + i + 1;
			}
		}
		X(row, col) = atof(start);
		row++;
	}
	in.close();

	return X;
}


#include <algorithm>
#include <iostream>
#include <sstream>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <array>
#include <string>

const unsigned NUMS = 256;

int main() {
    std::vector<int> lengths;

    std::string line;
    std::getline(std::cin, line);

    std::vector<std::string> words;
    boost::algorithm::split(
            words, line, boost::is_any_of(", "), boost::algorithm::token_compress_on);
    for (const auto& w : words) {
        lengths.push_back(std::stoi(&w[0]));
    }

    std::array<int, NUMS> nums;
    for (unsigned i = 0; i < NUMS; ++i) {
        nums[i] = i;
    }

    int skip = 0;
    int pos = 0;

    for (auto w : lengths) {
        int from = pos, to = (pos + w - 1) % NUMS;
        for(unsigned i = w/2; i > 0; --i) {
            std::swap(nums[from], nums[to]);
            from = ++from % NUMS;
            if (to == 0) {
                to = NUMS;
            }
            to = --to;
        }
        pos = (pos + w + skip++) % NUMS;
    }

    std::cout << "0: " << nums[0] << "\n"
    << "1: " << nums[1] << "\n"
    << "multiple: " << nums[0]*nums[1] << "\n";
}


//
//  cal_M_global_vec.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/10/18.
//
//

#ifndef cal_M_global_vec_hpp
#define cal_M_global_vec_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "cal_M.hpp"
#include "mapglobal.hpp"

using namespace Eigen;
void cal_M_global_vec(MatrixXd &Node, MatrixXd &Element, double density, MatrixXd &index_store, int Ndofn, VectorXd &M_global_vec);

#endif /* cal_M_global_vec_hpp */


#include <stan/math/prim/mat.hpp>
#include <test/unit/math/prim/mat/util.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <stdexcept>

TEST(ProbDistributionsInvWishartRng, rng) {
  using Eigen::MatrixXd;
  using stan::math::inv_wishart_rng;
  boost::random::mt19937 rng;

  MatrixXd omega(3,4);
  EXPECT_THROW(inv_wishart_rng(3.0, omega, rng), std::invalid_argument);

  MatrixXd sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0,
    2.0, 1.0, 3.0;
  EXPECT_NO_THROW(inv_wishart_rng(3.0, sigma, rng));
  EXPECT_THROW(inv_wishart_rng(2, sigma, rng), std::domain_error);
  EXPECT_THROW(inv_wishart_rng(-1, sigma, rng), std::domain_error);
}
TEST(probdistributionsInvWishartRng, symmetry) {
  using Eigen::MatrixXd;
  using stan::math::inv_wishart_rng;
  using stan::test::unit::expect_symmetric;
  using stan::test::unit::spd_rng;

  boost::random::mt19937 rng;
  for (int k = 1; k < 20; ++k)
    for (double nu = k - 0.5; nu < k + 20; ++nu)
      for (int n = 0; n < 10; ++n)
        expect_symmetric(inv_wishart_rng(nu, spd_rng(k, rng), rng));
}

TEST(ProbDistributionsInvWishart, chiSquareGoodnessFitTest) {
  using stan::math::determinant;
  using stan::math::inv_wishart_rng;
  using boost::math::digamma;
  using boost::math::chi_squared;
  using Eigen::MatrixXd;
  using std::log;

  boost::random::mt19937 rng;
  MatrixXd sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 1.0,
    0.0, 1.0, 3.0;
  int N = 10000;

  MatrixXd siginv(3,3);
  siginv = sigma.inverse();
  int count = 0;
  double avg = 0;
  double expect = sigma.rows() * log(2.0) + log(determinant(siginv))
    + digamma(5.0 / 2.0) + digamma(4.0 / 2.0) + digamma(3.0 / 2.0);

  MatrixXd a(sigma.rows(),sigma.rows());
  while (count < N) {
    a = inv_wishart_rng(5.0, sigma, rng);
    avg += log(determinant(a)) / N;
    count++;
   }
  double chi = (expect - avg) * (expect - avg) / expect;
  chi_squared mydist(1);
  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsInvWishart, SpecialRNGTest) {
  //When the scale matrix is an identity matrix and df = k + 2
  //The avg of the samples should also be an identity matrix

  using Eigen::MatrixXd;
  using stan::math::inv_wishart_rng;

  boost::random::mt19937 rng(1234U);
  int N = 1e5;
  double tol = 0.1;
  for (int k = 1; k < 5; k++) {
    MatrixXd sigma = MatrixXd::Identity(k, k);
    MatrixXd Z = MatrixXd::Zero(k, k);
    for (int i = 0; i < N; i++)
      Z += inv_wishart_rng(k + 2, sigma, rng);
    Z /= N;
    for (int j = 0; j < k; j++) {
      for (int i = 0; i < k; i++) {
        if (j == i)
          EXPECT_NEAR(Z(i, j), 1.0, tol);
        else
          EXPECT_NEAR(Z(i, j), 0.0, tol);
      }
    }
  }
}


#ifndef PYTHONIC_INCLUDE_NUMPY_FIX_HPP
#define PYTHONIC_INCLUDE_NUMPY_FIX_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/trunc.hpp>

namespace pythonic
{

  namespace numpy
  {
    namespace wrapper
    {
      template <class T>
      double fix(T const &v)
      {
        return boost::simd::trunc(v);
      }
    }
#define NUMPY_NARY_FUNC_NAME fix
#define NUMPY_NARY_FUNC_SYM wrapper::fix
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


// This is the main file that generates the animation based on the input motion plan and mesh
// The executable created is called bbw, which can be run after compiling the project.

#include <igl/boundary_conditions.h>
#include <igl/readMESH.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/bbw.h>
#include <igl/normalize_row_sums.h>
#include <igl/forward_kinematics.h>
#include <igl/directed_edge_parents.h>

#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>

#include <fk.hpp>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <iostream>

using namespace Eigen;
using namespace std;

const Eigen::RowVector3d sea_green(70./255.,252./255.,167./255.);

Eigen::MatrixXd V,W,U,C,M, CT, T_mat;
Eigen::MatrixXi T,F,BE;
Eigen::VectorXi P;

std::vector<std::vector<double>> motion;
// handles to create skeleton constraints
std::vector<int> CE_ind = {633, 39664, 32904, 28716, 5931, 3569, 3632,
                            32099, 31372, 27125, 7050, 3014, 2040};

// joint mapping to handles (used in FK file)
std::vector<std::string> joint_names = {"FL_HAA", "FL_KFE", "FL_FOOT", "FR_HAA" ,"FR_KFE", "FR_FOOT",
                                        "HL_HAA", "HL_KFE", "HL_FOOT", "HR_HAA", "HR_KFE", "HR_FOOT"};
int k;
int selected = 0;

Eigen::VectorXd q;
std::string rpath = "../data/solo12.urdf";
// change file name to create different animations
std::string motion_file = "../motions/trotting.txt";

std::vector<std::vector<double>> read_motion(std::string file_name){
    fstream newfile;
    newfile.open(file_name,ios::in); //open a file to perform read operation using file object

    std::vector<std::vector<double>> q_vec;

    if (newfile.is_open()){   //checking whether the file is open
        string tp;
        while(getline(newfile, tp)){ //read data from file object and put it into string.
            vector <double> OutputVertices;
            istringstream ss(tp);
            copy(
            istream_iterator <double> ( ss ),
            istream_iterator <double> (),
            back_inserter( OutputVertices )
            );
            q_vec.push_back(OutputVertices);
        }
        newfile.close(); //close the file object.
    };

    return q_vec;
}

bool pre_draw(igl::opengl::glfw::Viewer & viewer){

    for (unsigned i = 0; i < motion[k].size(); ++i){
        if (i < 3 || i > 6){
            q(i) = motion[k][i];
        }
        // hack because of difference in co ordinate frames
        // of mesh and urdf
        q(0) = motion[k][1]; q(1) = -motion[k][0];
    }
    std::string rpath = "../data/solo12.urdf";
    fk::ForwardKinematics fk(rpath, joint_names, V, CE_ind, BE);
    MatrixXd CT(CE_ind.size(), V.cols());
    MatrixXd T_mat(BE.rows()*(V.cols()+1),V.cols());

    fk.compute(q, CT, T_mat);
    U = M*T_mat;
    viewer.data().set_vertices(U);
    // viewer.data().set_edges(CT,BE,sea_green);
    k = (k < motion.size() - 1 ? k + 1 : 0);
}

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

    k = 0;
    motion = read_motion(motion_file);
    std::cout << "finished reading motion .." << std::endl;

    q.resize(19);
    q <<  0.2, 0.4, 0.24, 0.0, 0.0, -0.707107, 0.707107,
          0.8, 0.8, -1.6, 0.0, 1.5, -1.6,
          0.0, -0.8, 1.6, 0.0, -0.8, 1.6;

    BE.resize(12,2);
    BE << 0,1,
          1,2,
          2,3,
          0,4,
          4,5,
          5,6,
          0,7,
          7,8,
          8,9,
          0,10,
          10,11,
          11,12;

    igl::readOFF("../data/Dog_v1.off",V,F);
    U = V;
    std::cout << "finished reading ..." << std::endl;

    fk::ForwardKinematics fk(rpath, joint_names, V, CE_ind, BE);
    fk.get_C(C);

    // BBW
    VectorXi b;
    MatrixXd bc;
    igl::boundary_conditions(V,F,C,VectorXi(),BE,MatrixXi(),b,bc);
    igl::BBWData bbw_data;
    bbw_data.active_set_params.max_iter = 8;
    bbw_data.verbosity = 2;
    if(!igl::bbw(V,F,b,bc,bbw_data,W))
    {
        return EXIT_FAILURE;
    }

    igl::normalize_row_sums(W,W);
    igl::lbs_matrix(V,W,M);

    igl::opengl::glfw::Viewer viewer;
    viewer.data().set_mesh(U, F);
    // comment to create final version of the animation
    viewer.data().set_data(W.col(selected));
    viewer.data().set_edges(C,BE,sea_green);
    viewer.callback_pre_draw = &pre_draw;
    viewer.data().show_lines = false;
    viewer.data().show_overlay_depth = false;
    viewer.data().line_width = 1;
    viewer.core().animation_max_fps = 60.;
    viewer.core().is_animating = true;
    viewer.launch();

    return 0;
}