#ifndef SKYLARK_LIBSVM_IO_HPP
#define SKYLARK_LIBSVM_IO_HPP

#include <memory>

#if SKYLARK_HAVE_BOOST_FILESYSTEM
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/path.hpp"
namespace boostfs = boost::filesystem;
#endif

#include <unordered_map>
#include <boost/serialization/list.hpp>

#include "../types.hpp"
#include "../get_communicator.hpp"

namespace skylark { namespace utility { namespace io {

/**
 * Reads X and Y from a file in libsvm format.
 * X and Y are Elemental dense matrices.
 *
 * @param fname input file name.
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 * @param max_n maximum number of columns in the matrix.
 * @param blocksize blocksize for reading for distributed outputs.
 */
template<typename T, typename R>
void ReadLIBSVM(const std::string& fname,
    El::Matrix<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0, int max_n = -1,
    int blocksize=10000) {

    std::string line;
    std::string token, val, ind;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    std::ifstream in(fname);

    if ((in.rdstate() & std::ifstream::failbit) != 0)
        SKYLARK_THROW_EXCEPTION (
           base::io_exception()
               << base::error_msg(
                "Failed to open file " + fname));

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    while(!in.eof() && n != max_n) {
        getline(in, line);

        // Ignore empty lines and comment lines (begin with #)
        if(line.length() == 0 || line[0] == '#')
            break;

        n++;

        // Figure out number of targets (only first line)
        if (n == 1) {
            std::string tstr;
            std::istringstream tokenstream (line);
            tokenstream >> tstr;
            while (tstr.find(":") == std::string::npos) {
                nt++;
                if (tokenstream.eof())
                    break;
                tokenstream >> tstr;
            }
        }

        size_t delim = line.find_last_of(":");
        if(delim == std::string::npos)
            continue;

        t = delim;
        while(line[t]!=' ') {
            t--;
        }
        val = line.substr(t+1, delim - t);
        last = atoi(val.c_str());
        if (last>d)
            d = last;
    }
    if (min_d > 0)
        d = std::max(d, min_d);

    // prepare for second pass
    in.clear();
    in.seekg(0, std::ios::beg);

    if (direction == base::COLUMNS) {
        X.Resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.Resize(n, d);
        Y.Resize(n, nt);
    }

    T *Xdata = X.Buffer();
    R *Ydata = Y.Buffer();
    int ldX = X.LDim();
    int ldY = Y.LDim();

    for (t = 0; t < n; t++) {
        getline(in, line);

        // Ignore empty lines and comment lines (begin with #)
        if(line.length() == 0 || line[0] == '#')
            break;

        std::istringstream tokenstream(line);

        for(int r = 0; r < nt; r++) {
            tokenstream >> label;
            if (direction == base::COLUMNS)
                Ydata[t * ldY + r] = label;
            else
                Ydata[r * ldY + t] = label;
        }

        while (tokenstream >> token) {
            size_t delim  = token.find(':');
            ind = token.substr(0, delim);
            val = token.substr(delim+1); //.substr(delim+1);
            j = atoi(ind.c_str()) - 1;
            if (direction == base::COLUMNS)
                Xdata[t * ldX + j] = atof(val.c_str());
            else
                Xdata[j * ldX + t] = atof(val.c_str());
        }
    }
}

/**
 * Reads X and Y from a file in libsvm format.
 * X and Y are Elemental distributed matrices.
 *
 * @param fname input file name.
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param max_n stop reading after n rows. If -1 then will read all rows.
 * @param min_d minimum number of rows in the matrix.
 * @param blocksize blocksize for blocking of read.
 */
template<typename T, El::Distribution UX, El::Distribution VX,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadLIBSVM(const std::string& fname,
    El::DistMatrix<T, UX, VX>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int max_n = -1,
    int blocksize = 10000) {

    std::string line;
    std::string token, val, ind;
    R label;
    unsigned int start = 0;
    size_t delim;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    std::ifstream in(fname);

    // TODO check that X and Y have the same grid.
    boost::mpi::communicator comm = skylark::utility::get_communicator(X);
    int rank = X.Grid().Rank();

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    if (rank==0) {
        while(!in.eof() && n != max_n) {
            getline(in, line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            n++;

            // Figure out number of targets (only first line)
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    if (tokenstream.eof())
                        break;
                    tokenstream >> tstr;
                }
            }

            size_t delim = line.find_last_of(":");
            if(delim == std::string::npos)
                continue;

            t = delim;
            while(line[t]!=' ')
                t--;

            val = line.substr(t+1, delim - t);
            last = atoi(val.c_str());
            if (last>d)
                d = last;
        }
        if (min_d > 0)
            d = std::max(d, min_d);

        // prepare for second pass
        in.clear();
        in.seekg(0, std::ios::beg);
    }

    boost::mpi::broadcast(comm, n, 0);
    boost::mpi::broadcast(comm, d, 0);
    boost::mpi::broadcast(comm, nt, 0);

    int numblocks = ((int) n/ (int) blocksize); // of size blocksize
    int leftover = n % blocksize;
    int block = blocksize;

    if (direction == base::COLUMNS) {
        X.Resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.Resize(n, d);
        Y.Resize(n, nt);
    }

    El::DistMatrix<T, El::CIRC, El::CIRC> XB(X.Grid());
    El::DistMatrix<R, El::CIRC, El::CIRC> YB(Y.Grid());
    El::DistMatrix<T, UX, VX> Xv(X.Grid());
    El::DistMatrix<R, UY, VY> Yv(Y.Grid());
    for(int i=0; i<numblocks+1; i++) {
        if (i==numblocks)
            block = leftover;
        if (block==0)
            break;

        if (direction == base::COLUMNS) {
            El::Zeros(XB, d, block);
            El::Zeros(YB, nt, block);
        } else {
            El::Zeros(XB, block, d);
            El::Zeros(YB, block, nt);
        }

        if(rank==0) {
            T *Xdata = XB.Matrix().Buffer();
            R *Ydata = YB.Matrix().Buffer();
            int ldX = XB.Matrix().LDim();
            int ldY = YB.Matrix().LDim();

            t = 0;
            while(!in.eof() && t<block) {
                getline(in, line);

                // Ignore empty lines and comment lines (begin with #)
                if(line.length() == 0 || line[0] == '#')
                    break;

                std::istringstream tokenstream(line);

                for(int r = 0; r < nt; r++) {
                    tokenstream >> label;
                    if (direction == base::COLUMNS)
                        Ydata[t * ldY + r] = label;
                    else
                        Ydata[r * ldY + t] = label;
                }

                while (tokenstream >> token) {
                    size_t delim  = token.find(':');
                    ind = token.substr(0, delim);
                    val = token.substr(delim+1); //.substr(delim+1);
                    j = atoi(ind.c_str()) - 1;
                    if (direction == base::COLUMNS)
                        Xdata[t * ldX + j] = atof(val.c_str());
                    else
                        Xdata[j * ldX + t] = atof(val.c_str());
                }

                t++;
            }
        }

        // The calls below should distribute the data to all the nodes.
        if (direction == base::COLUMNS) {
            int ldX = XB.Matrix().LDim();
            El::View(Xv, X, 0, i*blocksize, d, block);
            El::View(Yv, Y, 0, i*blocksize, nt, block);
        } else {
            El::View(Xv, X, i*blocksize, 0, block, d);
            El::View(Yv, Y, i*blocksize, 0, block, nt);
        }

        Xv = XB;
        Yv = YB;
    }
}

/**
 * Reads X and Y from a file in libsvm format.
 * X is a Skylark local sparse matrix, and Y is Elemental dense matrices.
 *
 * @param fname input file name
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 * @param max_n maximum number of cols in the matrix.
 * @param blocksize blocksize for reading for distributed outputs.
 */
template<typename T, typename R>
void ReadLIBSVM(const std::string& fname,
    base::sparse_matrix_t<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0, int max_n = -1,
    int blocksize = 10000) {

    std::string line;
    std::string token;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;
    int nnz=0;
    int nz;

    std::ifstream in(fname);

    // make one pass over the data to figure out dimensions and nnz
    // will pay in terms of preallocated storage.
    // Also find number of non-zeros per column.
    std::unordered_map<int, int> colsize;

    while(!in.eof() && n != max_n) {
        getline(in, line);

        // Ignore empty lines and comment lines (begin with #)
        if(line.length() == 0 || line[0] == '#')
            break;

        n++;

        // Figure out number of targets (only first line)
        if (n == 1) {
            std::string tstr;
            std::istringstream tokenstream (line);
            tokenstream >> tstr;
            while (tstr.find(":") == std::string::npos) {
                nt++;
                if (tokenstream.eof())
                    break;
                tokenstream >> tstr;
            }
        }

        if (direction == base::COLUMNS) {
            size_t delim = line.find_last_of(":");
            if(delim == std::string::npos)
                continue;

            t = delim;
            while(line[t]!=' ')
                t--;

            std::string val = line.substr(t+1, delim - t);
            last = atoi(val.c_str());
            if (last>d)
                d = last;

            std::istringstream tokenstream (line);
            tokenstream >> label;
            while (tokenstream >> token)
                nnz++;
        } else {
            std::istringstream tokenstream (line);
            tokenstream >> label;

            while (tokenstream >> token) {
                nnz++;
                size_t delim  = token.find(':');
                int ind = atoi(token.substr(0, delim).c_str());

                colsize[ind-1]++;

                if (ind > d)
                    d = ind;
            }
        }
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    T *values = new T[nnz];
    int *rowind = new int[nnz];
    int *col_ptr = new int[direction == base::COLUMNS ? n + 1 : d + 1];

    if (direction == base::ROWS) {
        col_ptr[0] = 0;
        for(int i = 1; i <= d; i++)
            col_ptr[i] = col_ptr[i-1] + colsize[i-1];
        Y.Resize(n, nt);
    } else
        Y.Resize(nt, n);
    R *Ydata = Y.Buffer();
    int ldY = Y.LDim();

    // prepare for second pass
    in.clear();
    in.seekg(0, std::ios::beg);
    if (direction == base::COLUMNS)
        nnz = 0;

    colsize.clear();
    for (t = 0; t < n; t++) {
        getline(in, line);

        // Ignore empty lines and comment lines (begin with #)
        if(line.length() == 0 || line[0] == '#')
            break;

        std::istringstream tokenstream (line);

        for(int r = 0; r < nt; r++) {
            tokenstream >> label;
            if (direction == base::COLUMNS)
                Ydata[t * ldY + r] = label;
            else
                Ydata[r * ldY + t] = label;
        }

        if (direction == base::COLUMNS)
            col_ptr[t] = nnz;

        while (tokenstream >> token) {
            size_t delim  = token.find(':');
            std::string ind = token.substr(0, delim);
            std::string val = token.substr(delim+1); //.substr(delim+1);
            j = atoi(ind.c_str()) - 1;

            if (direction == base::COLUMNS) {
                rowind[nnz] = j;
                values[nnz] = atof(val.c_str());
                nnz++;
            } else {
                rowind[col_ptr[j] + colsize[j]] = t;
                values[col_ptr[j] + colsize[j]] = atof(val.c_str());
                colsize[j]++;
            }
        }
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    if (direction == base::COLUMNS) {
        col_ptr[n] = nnz; // last entry (total number of nnz)
        X.attach(col_ptr, rowind, values, nnz, d, n, true);
    } else
        X.attach(col_ptr, rowind, values, nnz, n, d, true);
}

/**
 * Reads X and Y from a file in libsvm format.
 * X is a sparse distributed VC/STAR matrix and Y is a dense distributed
 * VC/STAR matrix.
 *
 * IMPORTANT: output is in column-major format (the rows are features).
 *
 * @param fname input file name.
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 * @param blocksize blocksize for blocking of read.
 */
template<typename T,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadLIBSVM(const std::string& fname,
    base::sparse_vc_star_matrix_t<T>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int max_n = -1,
    int blocksize = 10000) {

    std::string line;
    std::string token, val, ind;
    T label;
    unsigned int start = 0;
    unsigned int delim, t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    std::ifstream in(fname);

    boost::mpi::communicator comm = skylark::utility::get_communicator(Y);
    int rank = comm.rank();
    int size = comm.size();

    std::vector< std::list<int> > non_local_updates_j(size);
    std::vector< std::list<int> > non_local_updates_row(size);
    std::vector< std::list<T> > non_local_updates_v(size);

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    if (rank==0) {
        while(!in.eof() && n != max_n) {
            getline(in, line);
            if(line.length()==0)
                break;
            delim = line.find_last_of(":");
            if(delim > line.length())
                continue;
            n++;

            // Figure out number of targets
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    tokenstream >> tstr;
                }
            }

            t = delim;
            while(line[t]!=' ') {
                t--;
            }
            val = line.substr(t+1, delim - t);
            last = atoi(val.c_str());
            if (last>d)
                d = last;
        }
        if (min_d > 0)
            d = std::max(d, min_d);

        // prepare for second pass
        in.clear();
        in.seekg(0, std::ios::beg);
    }

    boost::mpi::broadcast(comm, n, 0);
    boost::mpi::broadcast(comm, d, 0);
    boost::mpi::broadcast(comm, nt, 0);

    int numblocks = ((int) n/ (int) blocksize); // of size blocksize
    int leftover = n % blocksize;
    int block = blocksize;

    if (direction == base::COLUMNS) {
        X.resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.resize(n, d);
        Y.Resize(n, nt);
    }

    El::DistMatrix<T, El::CIRC, El::CIRC> YB(Y.Grid());
    El::DistMatrix<T, El::VC, El::STAR> Yv(Y.Grid());

    int row = 0;
    for(int i=0; i<numblocks+1; i++) {
        if (i==numblocks)
            block = leftover;
        if (block==0)
            break;

        if (direction == base::COLUMNS) {
            El::Zeros(YB, nt, block);
        } else {
            El::Zeros(YB, block, nt);
        }

        if(rank==0) {
            T *Ydata = YB.Matrix().Buffer();
            int ldY = YB.Matrix().LDim();

            t = 0;
            while(!in.eof() && t<block) {
                getline(in, line);
                if( line.length()==0)
                    break;

                std::istringstream tokenstream (line);
                for(int r = 0; r < nt; r++) {
                    tokenstream >> label;
                    if (direction == base::COLUMNS)
                        Ydata[t * ldY + r] = label;
                    else
                        Ydata[r * ldY + t] = label;
                }

                while (tokenstream >> token) {
                    delim  = token.find(':');
                    ind = token.substr(0, delim);
                    val = token.substr(delim+1); //.substr(delim+1);
                    j = atoi(ind.c_str()) - 1;
                    int owner = (direction == base::COLUMNS) ?
                        X.owner(j, row) : X.owner(row, j);
                    if (owner == 0) {
                        if (direction == base::COLUMNS)
                            X.queue_update(j, row, atof(val.c_str()));
                        else
                            X.queue_update(row, j, atof(val.c_str()));
                    } else {
                        non_local_updates_j[owner].push_back(j);
                        non_local_updates_row[owner].push_back(row);
                        non_local_updates_v[owner].push_back(atof(val.c_str()));
                    }
                }

                row++;
                t++;
            }

            for (int rk = 1; rk < size; rk++) {
                comm.send(rk, 0, non_local_updates_j[rk]);
                comm.send(rk, 0, non_local_updates_row[rk]);
                comm.send(rk, 0, non_local_updates_v[rk]);

                non_local_updates_j[rk].clear();
                non_local_updates_row[rk].clear();
                non_local_updates_v[rk].clear();
            }
        } else {
                comm.recv(0, 0, non_local_updates_j[rank]);
                comm.recv(0, 0, non_local_updates_row[rank]);
                comm.recv(0, 0, non_local_updates_v[rank]);

                auto it_j = non_local_updates_j[rank].begin();
                auto it_row = non_local_updates_row[rank].begin();
                auto it_v = non_local_updates_v[rank].begin();

                for(; it_j != non_local_updates_j[rank].end();) {

                    int j = *it_j, row = *it_row;
                    T val = *it_v;

                    if (direction == base::COLUMNS)
                        X.queue_update(j, row, val);
                    else
                        X.queue_update(row, j, val);

                    it_j++; it_row++; it_v++;
                }

                non_local_updates_j[rank].clear();
                non_local_updates_row[rank].clear();
                non_local_updates_v[rank].clear();
        }

        // The calls below should distribute the data to all the nodes.
        if (direction == base::COLUMNS) {
            El::View(Yv, Y, 0, i*blocksize, nt, block);
        } else {
            El::View(Yv, Y, i*blocksize, 0, block, nt);
        }

        Yv = YB;
    }

    X.finalize();
}

void ReadLIBSVM(const std::string& fname,
    boost::any X, boost::any Y,
    base::direction_t direction, int min_d = 0, int max_n = -1,
    int blocksize = 10000) {

#define SKYLARK_READLIBSVM_APPLY_DISPATCH(XT, YT)                   \
    if (X.type() == typeid(XT*) && Y.type() == typeid(YT*))  {      \
        ReadLIBSVM(fname, *boost::any_cast<XT*>(X),                 \
            *boost::any_cast<YT*>(Y), direction,                    \
            min_d, max_n, blocksize);                               \
            return;                                                 \
    }                                       \

#if !(defined SKYLARK_NO_ANY)
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::matrix_t, mdtypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::matrix_t, mftypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::matrix_t, mdtypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::matrix_t, mftypes::matrix_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::shared_matrix_t,
        mdtypes::shared_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::shared_matrix_t,
        mftypes::shared_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::shared_matrix_t,
        mdtypes::shared_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::shared_matrix_t,
        mftypes::shared_matrix_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::root_matrix_t,
        mdtypes::root_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::root_matrix_t,
        mftypes::root_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::root_matrix_t,
        mdtypes::root_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::root_matrix_t,
        mftypes::root_matrix_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
        mdtypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
        mftypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::sparse_matrix_t,
        mdtypes::matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::sparse_matrix_t,
        mftypes::matrix_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_t,
        mdtypes::dist_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_t,
        mftypes::dist_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_t,
        mdtypes::dist_matrix_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_t,
        mftypes::dist_matrix_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
        mdtypes::dist_matrix_vc_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
        mftypes::dist_matrix_vc_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
        mdtypes::dist_matrix_vc_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
        mftypes::dist_matrix_vc_star_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
        mdtypes::dist_matrix_vr_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
        mftypes::dist_matrix_vr_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
        mdtypes::dist_matrix_vr_star_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
        mftypes::dist_matrix_vr_star_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
        mdtypes::dist_matrix_star_vc_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
        mftypes::dist_matrix_star_vc_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
        mdtypes::dist_matrix_star_vc_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
        mftypes::dist_matrix_star_vc_t);

    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
        mdtypes::dist_matrix_star_vr_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
        mftypes::dist_matrix_star_vr_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
        mdtypes::dist_matrix_star_vr_t);
    SKYLARK_READLIBSVM_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
        mftypes::dist_matrix_star_vr_t);

#endif

    SKYLARK_THROW_EXCEPTION (
        base::io_exception()
          << base::error_msg(
           "This combination has not yet been implemented for ReadLIBSVM"));

#undef SKYLARK_READLIBSVM_APPLY_DISPATCH
}

/**
 * Write X and Y from a file in libsvm format.
 * X and Y are Elemental dense matrices.
 *
 * @param fname output file name.
 * @param X input X
 * @param Y output Y
 * @param direction whether the examples are in the rows or columns of X and Y
 */
template<typename T, typename R>
void WriteLIBSVM(const std::string& fname,
    El::Matrix<T>& X, El::Matrix<R>& Y,
    base::direction_t direction) {

    std::ofstream out(fname);
    El::Int n, d;

    if (direction == base::COLUMNS) {
        n = X.Width();
        d = X.Height();
    } else {
        n = X.Height();
        d = X.Width();
    }

    for(El::Int j = 0; j < n; j++) {
        if (direction == base::COLUMNS) {
            out << Y.Get(0, j) << " ";
            for(El::Int r = 0; r < d; r++) {
                T val = X.Get(r, j);
                if (val != 0.0)
                    out << (r+1) << ":" << val << " ";
            }
            out << std::endl;
        } else {
            out << Y.Get(j, 0) << " ";
            for(El::Int r = 0; r < d; r++) {
                T val = X.Get(j, r);
                if (val != 0.0)
                    out << (r+1) << ":" << val << " ";
            }
            out << std::endl;
        }
    }

    out.close();
}

/**
 * Write X and Y from a file in libsvm format.
 * X and Y are Elemental distributed matrices.
 *
 * @param fname output file name.
 * @param X input X
 * @param Y output Y
 * @param direction whether the examples are in the rows or columns of X and Y
 * @param blocksize blocksize for blocking of read.
 */
template<typename T, El::Distribution UX, El::Distribution VX,
         typename R, El::Distribution UY, El::Distribution VY>
void WriteLIBSVM(const std::string& fname,
    El::DistMatrix<T, UX, VX>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int blocksize = 10000) {

    int rank = X.Grid().Rank();

    std::ofstream out(fname);
    El::Int n, d;

    if (direction == base::COLUMNS) {
        n = X.Width();
        d = X.Height();
    } else {
        n = X.Height();
        d = X.Width();
    }

    El::Int numblocks = ((int) n/ (int) blocksize);
    El::Int leftover = n % blocksize;
    El::Int block = blocksize;

    El::DistMatrix<T, El::CIRC, El::CIRC> XB(X.Grid());
    El::DistMatrix<R, El::CIRC, El::CIRC> YB(Y.Grid());
    El::DistMatrix<T, UX, VX> Xv(X.Grid());
    El::DistMatrix<R, UY, VY> Yv(Y.Grid());
    for(El::Int i = 0; i < numblocks + 1; i++) {
        if (i == numblocks)
            block = leftover;
        if (block == 0)
            break;

        // The calls below should distribute the data to all the nodes.
        if (direction == base::COLUMNS) {
            El::View(Xv, X, 0, i*blocksize, d, block);
            El::View(Yv, Y, 0, i*blocksize, 1, block);
        } else {
            El::View(Xv, X, i*blocksize, 0, block, d);
            El::View(Yv, Y, i*blocksize, 0, block, 1);
        }

        XB = Xv;
        YB = Yv;

        if(rank==0)
            for(El::Int j = 0; j < block; j++) {
                if (direction == base::COLUMNS) {
                    out << YB.Get(0, j) << " ";
                    for(El::Int r = 0; r < d; r++) {
                        T val = XB.Get(r, j);
                        if (val != 0.0)
                            out << (r+1) << ":" << val << " ";
                    }
                    out << std::endl;
                } else {
                        out << YB.Get(j, 0) << " ";
                        for(El::Int r = 0; r < d; r++) {
                            T val = XB.Get(j, r);
                            if (val != 0.0)
                                out << (r+1) << ":" << val << " ";
                        }
                        out << std::endl;
                }
            }
    }
    out.close();
}

#if SKYLARK_HAVE_BOOST_FILESYSTEM

/**
 * Reads X and Y from a directory of files in libsvm format.
 * X and Y are Elemental dense matrices.
 *
 * @param fname input file name
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 */
template<typename T, typename R>
void ReadDirLIBSVM(const std::string& dname,
    El::Matrix<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0) {

    std::string line;
    std::string token;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;
    int nnz=0;
    int nz;

    boostfs::path full_path(boostfs::system_complete(boostfs::path(dname)));
    boostfs::directory_iterator end_iter;

    for(boostfs::directory_iterator dirit(full_path); dirit != end_iter;
        dirit++) {

        std::string fname = dirit->path().filename().string();
        if (fname == "." || fname == ".." || fname[0] == '.')
            continue;

        std::ifstream in(dirit->path().string());

        while(!in.eof()) {
            getline(in, line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            n++;

            // Figure out number of targets (only first line)
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    if (tokenstream.eof())
                        break;
                    tokenstream >> tstr;
                }
            }

            if (direction == base::COLUMNS) {
                size_t delim = line.find_last_of(":");
                if(delim == std::string::npos)
                    continue;

                t = delim;
                while(line[t]!=' ')
                    t--;
                std::string val = line.substr(t+1, delim - t);
                last = atoi(val.c_str());
                if (last>d)
                    d = last;

                std::istringstream tokenstream (line);
                tokenstream >> label;
                while (tokenstream >> token)
                    nnz++;
            } else {
                std::istringstream tokenstream (line);
                tokenstream >> label;

                while (tokenstream >> token) {
                    nnz++;
                    size_t delim  = token.find(':');
                    int ind = atoi(token.substr(0, delim).c_str());

                    if (ind > d)
                        d = ind;
                }
            }
        }

        in.close();
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    if (direction == base::ROWS) {
        X.Resize(n, d);
        Y.Resize(n, nt);
    } else {
        X.Resize(d, n);
        Y.Resize(nt, n);
    }

    T *Xdata = X.Buffer();
    El::Int ldX = X.LDim();
    R *Ydata = Y.Buffer();
    El::Int ldY = Y.LDim();

    // prepare for second pass
    t = 0;

    for(boostfs::directory_iterator dirit(full_path); dirit != end_iter;
        dirit++) {

        std::string fname = dirit->path().filename().string();
        if (fname == "." || fname == ".." || fname[0] == '.')
            continue;

        std::ifstream in(dirit->path().string());
        while(!in.eof()) {
            getline(in, line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;
            t++;

            std::istringstream tokenstream (line);

            for(int r = 0; r < nt; r++) {
                tokenstream >> label;
                if (direction == base::COLUMNS)
                    Ydata[t * ldY + r] = label;
                else
                    Ydata[r * ldY + t] = label;
            }

            while (tokenstream >> token) {
                size_t delim  = token.find(':');
                std::string ind = token.substr(0, delim);
                std::string val = token.substr(delim+1); //.substr(delim+1);
                j = atoi(ind.c_str()) - 1;

                if (direction == base::COLUMNS)
                    Xdata[t * ldX + j] = atof(val.c_str());
                else
                    Xdata[j * ldX + t] = atof(val.c_str());

            }
        }
    }
}

/**
 * Reads X and Y from a directory of files in libsvm format.
 * X is a Skylark local sparse matrix, and Y is Elemental dense matrices.
 *
 * @param fname input file name
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 */
template<typename T, typename R>
void ReadDirLIBSVM(const std::string& dname,
    base::sparse_matrix_t<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0) {

    std::string line;
    std::string token;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;
    int nnz=0;
    int nz;

    boostfs::path full_path(boostfs::system_complete(boostfs::path(dname)));
    boostfs::directory_iterator end_iter;

    // make one pass over the data to figure out dimensions and nnz
    // will pay in terms of preallocated storage.
    // Also find number of non-zeros per column.
    std::unordered_map<int, int> colsize;

    for(boostfs::directory_iterator dirit(full_path); dirit != end_iter;
        dirit++) {

        std::string fname = dirit->path().filename().string();
        if (fname == "." || fname == ".." || fname[0] == '.')
            continue;

        std::ifstream in(dirit->path().string());

        while(!in.eof()) {
            getline(in, line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            n++;

            // Figure out number of targets (only first line)
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    if (tokenstream.eof())
                        break;
                    tokenstream >> tstr;
                }
            }

            if (direction == base::COLUMNS) {
                size_t delim = line.find_last_of(":");
                if(delim == std::string::npos)
                    continue;

                t = delim;
                while(line[t]!=' ')
                    t--;
                std::string val = line.substr(t+1, delim - t);
                last = atoi(val.c_str());
                if (last>d)
                    d = last;

                std::istringstream tokenstream (line);
                tokenstream >> label;
                while (tokenstream >> token)
                    nnz++;
            } else {
                std::istringstream tokenstream (line);
                tokenstream >> label;

                while (tokenstream >> token) {
                    nnz++;
                    size_t delim  = token.find(':');
                    int ind = atoi(token.substr(0, delim).c_str());

                    colsize[ind-1]++;
                    if (ind > d)
                        d = ind;
                }
            }
        }

        in.close();
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    T *values = new T[nnz];
    int *rowind = new int[nnz];
    int *col_ptr = new int[direction == base::COLUMNS ? n + 1 : d + 1];

    if (direction == base::ROWS) {
        col_ptr[0] = 0;
        for(int i = 1; i <= d; i++)
            col_ptr[i] = col_ptr[i-1] + colsize[i-1];
        Y.Resize(n, nt);
    } else
        Y.Resize(nt, n);

    R *Ydata = Y.Buffer();
    int ldY = Y.LDim();

    // prepare for second pass
    colsize.clear();
    t = 0;

    for(boostfs::directory_iterator dirit(full_path); dirit != end_iter;
        dirit++) {

        std::string fname = dirit->path().filename().string();
        if (fname == "." || fname == ".." || fname[0] == '.')
            continue;

        std::ifstream in(dirit->path().string());
        while(!in.eof()) {
            getline(in, line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;
            t++;

            std::istringstream tokenstream (line);

            for(int r = 0; r < nt; r++) {
                tokenstream >> label;
                if (direction == base::COLUMNS)
                    Ydata[t * ldY + r] = label;
                else
                    Ydata[r * ldY + t] = label;
            }
            if (direction == base::COLUMNS)
                col_ptr[t] = nnz;

            while (tokenstream >> token) {
                size_t delim  = token.find(':');
                std::string ind = token.substr(0, delim);
                std::string val = token.substr(delim+1); //.substr(delim+1);
                j = atoi(ind.c_str()) - 1;

                if (direction == base::COLUMNS) {
                    rowind[nnz] = j;
                    values[nnz] = atof(val.c_str());
                    nnz++;
                } else {
                    rowind[col_ptr[j] + colsize[j]] = t;
                    values[col_ptr[j] + colsize[j]] = atof(val.c_str());
                    colsize[j]++;
                }
            }
        }
    }

    if (direction == base::COLUMNS) {
        col_ptr[n] = nnz; // last entry (total number of nnz)
        X.attach(col_ptr, rowind, values, nnz, d, n, true);
    } else
        X.attach(col_ptr, rowind, values, nnz, n, d, true);
}

/**
 * reads x and y from a directory of files in libsvm format.
 * x and y are elemental distributed matrices.
 *
 * @param fname input file name.
 * @param x output x
 * @param y output y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 * @param blocksize blocksize for blocking of read.
 */
template<typename T, El::Distribution UX, El::Distribution VX,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadDirLIBSVM(const std::string& dname,
    El::DistMatrix<T, UX, VX>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int blocksize = 10000) {

    std::string line;
    std::string token, val, ind;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    // TODO check that X and Y have the same grid.
    boost::mpi::communicator comm = skylark::utility::get_communicator(X);
    int rank = X.Grid().Rank();

    boostfs::path full_path(boostfs::system_complete(boostfs::path(dname)));
    boostfs::directory_iterator end_iter;

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    if (rank==0) {
        for(boostfs::directory_iterator dirit(full_path); dirit != end_iter;
            dirit++) {

            std::string fname = dirit->path().filename().string();
            if (fname == "." || fname == ".." || fname[0] == '.')
                continue;

            std::ifstream in(dirit->path().string());

            while(!in.eof()) {
                getline(in, line);

                // Ignore empty lines and comment lines (begin with #)
                if(line.length() == 0 || line[0] == '#')
                    break;

                n++;

                // Figure out number of targets (only first line)
                if (n == 1) {
                    std::string tstr;
                    std::istringstream tokenstream (line);
                    tokenstream >> tstr;
                    while (tstr.find(":") == std::string::npos) {
                        nt++;
                        if (tokenstream.eof())
                            break;
                        tokenstream >> tstr;
                    }
                }

                size_t delim = line.find_last_of(":");
                if(delim == std::string::npos)
                    continue;

                t = delim;
                while(line[t]!=' ') {
                    t--;
                }
                val = line.substr(t+1, delim - t);
                last = atoi(val.c_str());
                if (last>d)
                    d = last;
            }

            in.close();
        }

        if (min_d > 0)
            d = std::max(d, min_d);
    }

    boost::mpi::broadcast(comm, n, 0);
    boost::mpi::broadcast(comm, d, 0);
    boost::mpi::broadcast(comm, nt, 0);

    int numblocks = ((int) n/ (int) blocksize); // of size blocksize
    int leftover = n % blocksize;
    int block = blocksize;

    if (direction == base::COLUMNS) {
        X.Resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.Resize(n, d);
        Y.Resize(n, nt);
    }

    El::DistMatrix<T, El::CIRC, El::CIRC> XB(X.Grid()), YB(Y.Grid());
    El::DistMatrix<T, UX, VX> Xv(X.Grid());
    El::DistMatrix<R, UY, VY> Yv(Y.Grid());
    boostfs::directory_iterator dirit(full_path);

    std::ifstream in;
    if (rank == 0) {
        std::string fname = dirit->path().filename().string();
        while (fname == "." || fname == ".." || fname[0] == '.') {
            dirit++;
            if (dirit == end_iter)
                break;
            fname = dirit->path().filename().string();
        }

        in.open(dirit->path().string());

        dirit++;
    }

    for(int i=0; i<numblocks+1; i++) {
        if (i==numblocks)
            block = leftover;
        if (block==0)
            break;

        if (direction == base::COLUMNS) {
            El::Zeros(XB, d, block);
            El::Zeros(YB, nt, block);
        } else {
            El::Zeros(XB, block, d);
            El::Zeros(YB, block, nt);
        }

        if(rank==0) {
            T *Xdata = XB.Matrix().Buffer();
            R *Ydata = YB.Matrix().Buffer();
            int ldX = XB.Matrix().LDim();
            int ldY = YB.Matrix().LDim();

            t = 0;
            while(t<block) {
                if (in.eof()) {
                    if (dirit == end_iter)
                        break;

                    in.close();

                    std::string fname = dirit->path().filename().string();
                    while (fname == "." || fname == ".." || fname[0] == '.') {
                        dirit++;
                        if (dirit == end_iter)
                            break;

                        fname = dirit->path().filename().string();
                    }

                    if (dirit == end_iter)
                        break;

                    in.open(dirit->path().string());
                    dirit++;
                }

                getline(in, line);

                // Ignore empty lines and comment lines (begin with #)
                if(line.length() == 0 || line[0] == '#')
                    break;

                std::istringstream tokenstream (line);
                for(int r = 0; r < nt; r++) {
                    tokenstream >> label;
                    if (direction == base::COLUMNS)
                        Ydata[t * ldY + r] = label;
                    else
                        Ydata[r * ldY + t] = label;
                }

                while (tokenstream >> token) {
                    size_t delim  = token.find(':');
                    ind = token.substr(0, delim);
                    val = token.substr(delim+1); //.substr(delim+1);
                    j = atoi(ind.c_str()) - 1;
                    if (direction == base::COLUMNS)
                        Xdata[t * ldX + j] = atof(val.c_str());
                    else
                        Xdata[j * ldX + t] = atof(val.c_str());
                }

                t++;
            }
        }

        // The calls below should distribute the data to all the nodes.
        if (direction == base::COLUMNS) {
            int ldX = XB.Matrix().LDim();
            El::View(Xv, X, 0, i*blocksize, d, block);
            El::View(Yv, Y, 0, i*blocksize, nt, block);
        } else {
            El::View(Xv, X, i*blocksize, 0, block, d);
            El::View(Yv, Y, i*blocksize, 0, block, nt);
        }

        Xv = XB;
        Yv = YB;
    }

    if (rank == 0)
        in.close();
}

template<typename T,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadDirLIBSVM(const std::string& dname,
    base::sparse_vc_star_matrix_t<T>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int blocksize = 10000) {

    SKYLARK_THROW_EXCEPTION(skylark::base::io_exception() <<
        skylark::base::error_msg(
            "readdirlibsvm not implemented for sparse_vc_star_matrix_t!"));
}

#else

template<typename XType, typename YType>
void ReadDirLIBSVM(const std::string& dname, XType& X, YType& Y,
    base::direction_t direction, int min_d = 0, int blocksize = 10000) {

    SKYLARK_THROW_EXCEPTION(base::io_exception() <<
        base::error_msg("Install Boost Filesystem for ReadDir support!"));

}

#endif

#if SKYLARK_HAVE_LIBHDFS

/**
 * Reads X and Y from a file in libsvm format (from HDFS filesystem).
 * X and Y are Elemental dense matrices.
 *
 * @param fs hdfs filesystem
 * @param fname input file name.
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 */
template<typename T, typename R>
void ReadLIBSVM(const hdfsFS &fs, const std::string& fname,
    El::Matrix<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0) {

    std::string line;
    std::string token, val, ind;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    hdfs_line_streamer_iterator_t itr(fs, fname, 1000);
    auto in = itr.next();

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    while(in != nullptr) {
        while(!in->eof()) {
            in->getline(line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            n++;

            // Figure out number of targets (only first line)
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    if (tokenstream.eof())
                        break;
                    tokenstream >> tstr;
                }
            }

            size_t delim = line.find_last_of(":");
            if(delim == std::string::npos)
                continue;

            t = delim;
            while(line[t]!=' ') {
                t--;
            }
            val = line.substr(t+1, delim - t);
            last = atoi(val.c_str());
            if (last>d)
                d = last;
        }

        in = itr.next();
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    // prepare for second pass
    itr.reset();
    in = itr.next();

    if (direction == base::COLUMNS) {
        X.Resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.Resize(n, d);
        Y.Resize(n, nt);
    }

    T *Xdata = X.Buffer();
    R *Ydata = Y.Buffer();
    int ldX = X.LDim();
    int ldY = Y.LDim();

    t = 0;
    while(in != nullptr) {

        in->rewind();
        while(!in->eof()) {
            in->getline(line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            std::istringstream tokenstream (line);

            for(int r = 0; r < nt; r++) {
                tokenstream >> label;
                if (direction == base::COLUMNS)
                    Ydata[t * ldY + r] = label;
                else
                    Ydata[r * ldY + t] = label;
            }

            while (tokenstream >> token) {
                size_t delim  = token.find(':');
                ind = token.substr(0, delim);
                val = token.substr(delim+1); //.substr(delim+1);
                j = atoi(ind.c_str()) - 1;
                if (direction == base::COLUMNS)
                    Xdata[t * ldX + j] = atof(val.c_str());
                else
                    Xdata[j * ldX + t] = atof(val.c_str());
            }

            t++;
        }

        in = itr.next();
    }
}

/**
 * Reads X and Y from a file in libsvm format (from HDFS filesystem).
 * X is a Skylark local sparse matrix, and Y is Elemental dense matrices.
 *
 * @param fname input file name
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 */
template<typename T, typename R>
void ReadLIBSVM(hdfsFS &fs, const std::string& fname,
    base::sparse_matrix_t<T>& X, El::Matrix<R>& Y,
    base::direction_t direction, int min_d = 0) {

    std::string line;
    std::string token;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;
    int nnz=0;
    int nz;

    hdfs_line_streamer_iterator_t itr(fs, fname, 1000);
    auto in = itr.next();

    // make one pass over the data to figure out dimensions and nnz
    // will pay in terms of preallocated storage.
    // Also find number of non-zeros per column.
    std::unordered_map<int, int> colsize;

    while(in != nullptr) {

        while(!in->eof()) {
            in->getline(line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            n++;

            // Figure out number of targets (only first line)
            if (n == 1) {
                std::string tstr;
                std::istringstream tokenstream (line);
                tokenstream >> tstr;
                while (tstr.find(":") == std::string::npos) {
                    nt++;
                    if (tokenstream.eof())
                        break;
                    tokenstream >> tstr;
                }
            }

            if (direction == base::COLUMNS) {
                size_t delim = line.find_last_of(":");
                if(delim == line.length())
                    continue;

                t = delim;
                while(line[t]!=' ')
                    t--;
                std::string val = line.substr(t+1, delim - t);
                last = atoi(val.c_str());
                if (last>d)
                    d = last;

                std::istringstream tokenstream (line);
                tokenstream >> label;
                while (tokenstream >> token)
                    nnz++;
            } else {
                std::istringstream tokenstream (line);
                tokenstream >> label;

                while (tokenstream >> token) {
                    nnz++;
                    size_t delim  = token.find(':');
                    int ind = atoi(token.substr(0, delim).c_str());

                    colsize[ind-1]++;

                    if (ind > d)
                        d = ind;
                }
            }
        }

        in = itr.next();
    }

    if (min_d > 0)
        d = std::max(d, min_d);

    T *values = new T[nnz];
    int *rowind = new int[nnz];
    int *col_ptr = new int[direction == base::COLUMNS ? n + 1 : d + 1];

    if (direction == base::ROWS) {
        col_ptr[0] = 0;
        for(int i = 1; i <= d; i++)
            col_ptr[i] = col_ptr[i-1] + colsize[i-1];
        Y.Resize(n, nt);
    } else
        Y.Resize(1, nt);

    R *Ydata = Y.Buffer();
    int ldY = Y.LDim();

    // prepare for second pass
    itr.reset();
    in = itr.next();

    if (direction == base::COLUMNS)
        nnz = 0;

    colsize.clear();

    t = 0;
    while(in != nullptr) {
        in->rewind();

        while(!in->eof()) {
            in->getline(line);

            // Ignore empty lines and comment lines (begin with #)
            if(line.length() == 0 || line[0] == '#')
                break;

            std::istringstream tokenstream (line);

            for(int r = 0; r < nt; r++) {
                tokenstream >> label;
                if (direction == base::COLUMNS)
                    Ydata[t * ldY + r] = label;
                else
                    Ydata[r * ldY + t] = label;
            }

            if (direction == base::COLUMNS)
                col_ptr[t] = nnz;

            while (tokenstream >> token) {
                size_t delim  = token.find(':');
                std::string ind = token.substr(0, delim);
                std::string val = token.substr(delim+1); //.substr(delim+1);
                j = atoi(ind.c_str()) - 1;

                if (direction == base::COLUMNS) {
                    rowind[nnz] = j;
                    values[nnz] = atof(val.c_str());
                    nnz++;
                } else {
                    rowind[col_ptr[j] + colsize[j]] = t;
                    values[col_ptr[j] + colsize[j]] = atof(val.c_str());
                    colsize[j]++;
                }
            }

            t++;
        }

        in->close();
        in = itr.next();
    }

    if (direction == base::COLUMNS) {
        col_ptr[n] = nnz; // last entry (total number of nnz)
        X.attach(col_ptr, rowind, values, nnz, d, n, true);
    } else
        X.attach(col_ptr, rowind, values, nnz, n, d, true);
}

/**
 * Reads X and Y from a file in libsvm format (from HDFS filesystem).
 * X and Y are Elemental distributed matrices.
 * Note that all the data is read on rank 0 (WHY??).
 *
 *
 * @param fname input file name.
 * @param X output X
 * @param Y output Y
 * @param direction whether the examples are to be put in rows or columns
 * @param min_d minimum number of rows in the matrix.
 * @param blocksize blocksize for blocking of read.
 */
template<typename T, El::Distribution UX, El::Distribution VX,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadLIBSVM(hdfsFS &fs, const std::string& fname,
    El::DistMatrix<T, UX, VX>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int blocksize = 10000) {

    std::string line;
    std::string token, val, ind;
    R label;
    unsigned int start = 0;
    unsigned int t;
    int n = 0, nt = 0;
    int d = 0;
    int i, j, last;
    char c;

    // TODO check that X and Y have the same grid.
    boost::mpi::communicator comm = skylark::utility::get_communicator(X);
    int rank = X.Grid().Rank();

    //FIXME: only required on rank 0
    //detail::hdfs_line_streamer_iterator_t itr(fs, fname, 1000);
    std::unique_ptr<hdfs_line_streamer_iterator_t> itr(nullptr);

    // make one pass over the data to figure out dimensions -
    // will pay in terms of preallocated storage.
    if (rank==0) {

        itr.reset(new hdfs_line_streamer_iterator_t (fs, fname, 1000));
        auto in = itr->next();

        while(in != nullptr) {
            while(!in->eof()) {
                in->getline(line);

                // Ignore empty lines and comment lines (begin with #)
                if(line.length() == 0 || line[0] == '#')
                    break;

                n++;

                // Figure out number of targets
                if (n == 1) {
                    std::string tstr;
                    std::istringstream tokenstream (line);
                    tokenstream >> tstr;
                    while (tstr.find(":") == std::string::npos) {
                        nt++;
                        if (tokenstream.eof())
                            break;
                        tokenstream >> tstr;
                    }
                }

                size_t delim = line.find_last_of(":");
                if(delim == std::string::npos)
                    continue;

                t = delim;
                while(line[t]!=' ') {
                    t--;
                }
                val = line.substr(t+1, delim - t);
                last = atoi(val.c_str());
                if (last>d)
                    d = last;
            }

            in = itr->next();
        }

        if (min_d > 0)
            d = std::max(d, min_d);
    }

    boost::mpi::broadcast(comm, n, 0);
    boost::mpi::broadcast(comm, d, 0);
    boost::mpi::broadcast(comm, nt, 0);

    int numblocks = ((int) n/ (int) blocksize); // of size blocksize
    int leftover = n % blocksize;
    int block = blocksize;

    if (direction == base::COLUMNS) {
        X.Resize(d, n);
        Y.Resize(nt, n);
    } else {
        X.Resize(n, d);
        Y.Resize(n, nt);
    }

    El::DistMatrix<T, El::CIRC, El::CIRC> XB(X.Grid()), YB(Y.Grid());
    El::DistMatrix<T, UX, VX> Xv(X.Grid());
    El::DistMatrix<R, UY, VY> Yv(Y.Grid());
    for(int i=0; i<numblocks+1; i++) {
        if (i==numblocks)
            block = leftover;
        if (block==0)
            break;

        if (direction == base::COLUMNS) {
            El::Zeros(XB, d, block);
            El::Zeros(YB, nt, block);
        } else {
            El::Zeros(XB, block, d);
            El::Zeros(YB, block, nt);
        }

        if(rank==0) {
            T *Xdata = XB.Matrix().Buffer();
            R *Ydata = YB.Matrix().Buffer();
            int ldX = XB.Matrix().LDim();
            int ldY = YB.Matrix().LDim();

            itr->reset();
            auto in = itr->next();

            t = 0;
            while(in != nullptr) {

                in->rewind();
                while(!in->eof() && t<block) {
                    in->getline(line);

                    // Ignore empty lines and comment lines (begin with #)
                    if(line.length() == 0 || line[0] == '#')
                        break;
                    std::istringstream tokenstream (line);
                    for(int r = 0; r < nt; r++) {
                        tokenstream >> label;
                        if (direction == base::COLUMNS)
                            Ydata[t * ldY + r] = label;
                        else
                            Ydata[r * ldY + t] = label;
                    }

                    while (tokenstream >> token) {
                        size_t delim  = token.find(':');
                        ind = token.substr(0, delim);
                        val = token.substr(delim+1); //.substr(delim+1);
                        j = atoi(ind.c_str()) - 1;
                        if (direction == base::COLUMNS)
                            Xdata[t * ldX + j] = atof(val.c_str());
                        else
                            Xdata[j * ldX + t] = atof(val.c_str());
                    }

                    t++;
                }

                in = itr->next();
            }
        }

        // The calls below should distribute the data to all the nodes.
        if (direction == base::COLUMNS) {
            int ldX = XB.Matrix().LDim();
            El::View(Xv, X, 0, i*blocksize, d, block);
            El::View(Yv, Y, 0, i*blocksize, nt, block);
        } else {
            El::View(Xv, X, i*blocksize, 0, block, d);
            El::View(Yv, Y, i*blocksize, 0, block, nt);
        }

        Xv = XB;
        Yv = YB;
    }
}

template<typename T,
         typename R, El::Distribution UY, El::Distribution VY>
void ReadLIBSVM(const hdfsFS &fs, const std::string& fname,
    base::sparse_vc_star_matrix_t<T>& X, El::DistMatrix<R, UY, VY>& Y,
    base::direction_t direction, int min_d = 0, int blocksize = 10000) {

    //TODO: implement
    SKYLARK_THROW_EXCEPTION(skylark::base::io_exception() <<
        skylark::base::error_msg(
            "ReadLIBSVM from HDFS not implemented for sparse_vc_star_matrix_t!"));
}

#endif

} } } // namespace skylark::utility::io

#endif


// Copyright (c) 2017-2018, CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//

#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp"

#include "pinocchio/multibody/joint/joint-generic.hpp"

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

#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),                            \
      "check " #Va ".isApprox(" #Vb ") failed "                                \
      "[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision)                              \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),                            \
      "check " #Va ".isApprox(" #Vb ") failed "                                \
      "[\n" << (Va) << "\n!=\n" << (Vb) << "\n]")

using namespace pinocchio;

#define VERBOSE false
#define IFVERBOSE if(VERBOSE)

template <typename T>
void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
{
  typedef double Scalar;

  const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision();
  BOOST_TEST_MESSAGE ("Testing Joint over " << jmodel.shortname());
  typedef typename T::ConfigVector_t  ConfigVector_t;
  typedef typename T::TangentVector_t TangentVector_t;

  ConfigVector_t  q1(ConfigVector_t::Random (jmodel.nq()));
  TangentVector_t q1_dot(TangentVector_t::Random (jmodel.nv()));
  ConfigVector_t  q2(ConfigVector_t::Random (jmodel.nq()));

  typedef typename LieGroup<T>::type LieGroupType;
  static ConfigVector_t Ones(ConfigVector_t::Ones(jmodel.nq()));
  const Scalar u = 0.3;
  // pinocchio::Inertia::Matrix6 Ia(pinocchio::Inertia::Random().matrix());
  // bool update_I = false;

  q1 = LieGroupType().randomConfiguration(-Ones, Ones);

  typename T::JointDataDerived jdata = jmodel.createData();

  // Check integrate
  jmodel.calc(jdata, q1, q1_dot);
  SE3 M1 = jdata.M;
  Motion v1(jdata.v);

  q2 = LieGroupType().integrate(q1,q1_dot);
  jmodel.calc(jdata,q2);
  SE3 M2 = jdata.M;

  SE3 M2_exp = M1*exp6(v1);

  if(jmodel.shortname() != "JointModelSphericalZYX")
  {
    BOOST_CHECK_MESSAGE(M2.isApprox(M2_exp), std::string("Error when integrating1 " + jmodel.shortname()));
  }

  // Check the reversability of integrate
  ConfigVector_t q3 = LieGroupType().integrate(q2,-q1_dot);
  jmodel.calc(jdata,q3);
  SE3 M3 = jdata.M;

  BOOST_CHECK_MESSAGE(M3.isApprox(M1), std::string("Error when integrating back " + jmodel.shortname()));

  // Check interpolate
  ConfigVector_t q_interpolate = LieGroupType().interpolate(q1,q2,0.);
  BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q1), std::string("Error when interpolating " + jmodel.shortname()));

  q_interpolate = LieGroupType().interpolate(q1,q2,1.);
  BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q2), std::string("Error when interpolating " + jmodel.shortname()));

  if(jmodel.shortname() != "JointModelSphericalZYX")
  {
    q_interpolate = LieGroupType().interpolate(q1,q2,u);
    jmodel.calc(jdata,q_interpolate);
    SE3 M_interpolate = jdata.M;

    SE3 M_interpolate_expected = M1*exp6(u*v1);
    BOOST_CHECK_MESSAGE(M_interpolate_expected.isApprox(M_interpolate,1e2*prec), std::string("Error when interpolating " + jmodel.shortname()));
  }

  // Check that difference between two equal configuration is exactly 0
  TangentVector_t zero = LieGroupType().difference(q1,q1);
  BOOST_CHECK_MESSAGE (zero.isZero (0), std::string ("Error: difference between two equal configurations is not 0."));
  zero = LieGroupType().difference(q2,q2);
  BOOST_CHECK_MESSAGE (zero.isZero (0), std::string ("Error: difference between two equal configurations is not 0."));

  // Check difference
  TangentVector_t vdiff = LieGroupType().difference(q1,q2);
  BOOST_CHECK_MESSAGE(vdiff.isApprox(q1_dot,1e2*prec), std::string("Error when differentiating " + jmodel.shortname()));

  // Check distance
  Scalar dist = LieGroupType().distance(q1,q2);
  BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
  BOOST_CHECK_SMALL(math::fabs(dist-q1_dot.norm()), 10*prec);

  std::string error_prefix("LieGroup");
  error_prefix += " on joint " + jmodel.shortname();

  BOOST_CHECK_MESSAGE(jmodel.nq() == LieGroupType::NQ, std::string(error_prefix + " - nq "));
  BOOST_CHECK_MESSAGE(jmodel.nv() == LieGroupType::NV, std::string(error_prefix + " - nv "));

  BOOST_CHECK_MESSAGE
  (jmodel.nq() ==
   LieGroupType().randomConfiguration(-1 * Ones, Ones).size(),
   std::string(error_prefix + " - RandomConfiguration dimensions "));

  ConfigVector_t q_normalize(ConfigVector_t::Random());
  Eigen::VectorXd q_normalize_ref(q_normalize);
  if(jmodel.shortname() == "JointModelSpherical")
  {
    q_normalize_ref /= q_normalize_ref.norm();
  }
  else if(jmodel.shortname() == "JointModelFreeFlyer")
  {
    q_normalize_ref.template tail<4>() /= q_normalize_ref.template tail<4>().norm();
  }
  else if(boost::algorithm::istarts_with(jmodel.shortname(),"JointModelRUB"))
  {
    q_normalize_ref /= q_normalize_ref.norm();
  }
  else if(jmodel.shortname() == "JointModelPlanar")
  {
    q_normalize_ref.template tail<2>() /= q_normalize_ref.template tail<2>().norm();
  }
  LieGroupType().normalize(q_normalize);
  BOOST_CHECK_MESSAGE(q_normalize.isApprox(q_normalize_ref), std::string(error_prefix + " - normalize "));
}

struct TestJoint{

  template <typename T>
  void operator()(const T ) const
  {
    T jmodel;
    jmodel.setIndexes(0,0,0);
    typename T::JointDataDerived jdata = jmodel.createData();

    test_lie_group_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelRevoluteUnaligned & ) const
  {
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);
    pinocchio::JointModelRevoluteUnaligned::JointDataDerived jdata = jmodel.createData();

    test_lie_group_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelPrismaticUnaligned & ) const
  {
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);
    pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData();

    test_lie_group_methods(jmodel, jdata);
  }

};

struct LieGroup_Jdifference{
  template <typename T>
  void operator()(const T ) const
  {
    typedef typename T::ConfigVector_t ConfigVector_t;
    typedef typename T::TangentVector_t TangentVector_t;
    typedef typename T::JacobianMatrix_t JacobianMatrix_t;
    typedef typename T::Scalar Scalar;

    T lg;
    BOOST_TEST_MESSAGE (lg.name());
    ConfigVector_t q[2], q_dv[2];
    q[0] = lg.random();
    q[1] = lg.random();
    TangentVector_t va, vb, dv;
    JacobianMatrix_t J[2];
    dv.setZero();

    lg.difference (q[0], q[1], va);
    lg.template dDifference<ARG0> (q[0], q[1], J[0]);
    lg.template dDifference<ARG1> (q[0], q[1], J[1]);

    const Scalar eps = 1e-6;
    for (int k = 0; k < 2; ++k) {
      BOOST_TEST_MESSAGE ("Checking J" << k << '\n' << J[k]);
      q_dv[0] = q[0];
      q_dv[1] = q[1];
      // Check J[k]
      for (int i = 0; i < dv.size(); ++i)
      {
        dv[i] = eps;
        lg.integrate (q[k], dv, q_dv[k]);
        lg.difference (q_dv[0], q_dv[1], vb);

        // vb - va ~ J[k] * dv
        TangentVector_t J_dv = J[k].col(i);
        TangentVector_t vb_va = (vb - va) / eps;
        EIGEN_VECTOR_IS_APPROX (vb_va, J_dv, 1e-2);
        dv[i] = 0;
      }
    }

    specificTests(lg);
  }

  template <typename T>
  void specificTests(const T ) const
  {}

  template <typename Scalar, int Options>
  void specificTests(const SpecialEuclideanOperationTpl<3,Scalar,Options>) const
  {
    typedef SE3Tpl<Scalar> SE3;
    typedef SpecialEuclideanOperationTpl<3,Scalar,Options> LG_t;
    typedef typename LG_t::ConfigVector_t ConfigVector_t;
    typedef typename LG_t::JacobianMatrix_t JacobianMatrix_t;

    LG_t lg;

    ConfigVector_t q[2];
    q[0] = lg.random();
    q[1] = lg.random();
    JacobianMatrix_t J[2];

    lg.template dDifference<ARG0> (q[0], q[1], J[0]);
    lg.template dDifference<ARG1> (q[0], q[1], J[1]);

    SE3 om0 (typename SE3::Quaternion (q[0].template tail<4>()).matrix(), q[0].template head<3>()),
        om1 (typename SE3::Quaternion (q[1].template tail<4>()).matrix(), q[1].template head<3>()),
        _1m2 (om1.actInv (om0)) ;
    EIGEN_MATRIX_IS_APPROX (J[1] * _1m2.toActionMatrix(), - J[0], 1e-8);
  }

  template <typename Scalar, int Options>
    void specificTests(const CartesianProductOperation<
        VectorSpaceOperationTpl<3,Scalar,Options>,
        SpecialOrthogonalOperationTpl<3,Scalar,Options>
        >) const
  {
    typedef SE3Tpl<Scalar> SE3;
    typedef CartesianProductOperation<
      VectorSpaceOperationTpl<3,Scalar,Options>,
      SpecialOrthogonalOperationTpl<3,Scalar,Options>
        > LG_t;
    typedef typename LG_t::ConfigVector_t ConfigVector_t;
    typedef typename LG_t::JacobianMatrix_t JacobianMatrix_t;

    LG_t lg;

    ConfigVector_t q[2];
    q[0] = lg.random();
    q[1] = lg.random();
    JacobianMatrix_t J[2];

    lg.template dDifference<ARG0> (q[0], q[1], J[0]);
    lg.template dDifference<ARG1> (q[0], q[1], J[1]);

    typename SE3::Matrix3
      oR0 (typename SE3::Quaternion (q[0].template tail<4>()).matrix()),
      oR1 (typename SE3::Quaternion (q[1].template tail<4>()).matrix());
    JacobianMatrix_t X (JacobianMatrix_t::Identity());
    X.template bottomRightCorner<3,3>() = oR1.transpose() * oR0;
    EIGEN_MATRIX_IS_APPROX (J[1] * X, - J[0], 1e-8);
  }
};

template<bool around_identity>
struct LieGroup_Jintegrate{
  template <typename T>
  void operator()(const T ) const
  {
    typedef typename T::ConfigVector_t ConfigVector_t;
    typedef typename T::TangentVector_t TangentVector_t;
    typedef typename T::JacobianMatrix_t JacobianMatrix_t;
    typedef typename T::Scalar Scalar;

    T lg;
    ConfigVector_t q = lg.random();
    TangentVector_t v, dq, dv;
    if(around_identity)
      v.setZero();
    else
      v.setRandom();

    dq.setZero();
    dv.setZero();

    ConfigVector_t q_v = lg.integrate (q, v);

    JacobianMatrix_t Jq, Jv;
    lg.dIntegrate_dq (q, v, Jq);
    lg.dIntegrate_dv (q, v, Jv);

    const Scalar eps = 1e-6;
    for (int i = 0; i < v.size(); ++i)
    {
      dq[i] = dv[i] = eps;
      ConfigVector_t q_dq = lg.integrate (q, dq);

      ConfigVector_t q_dq_v = lg.integrate (q_dq, v);
      TangentVector_t Jq_dq = Jq.col(i);
      // q_dv_v - q_v ~ Jq dv
      TangentVector_t dI_dq = lg.difference (q_v, q_dq_v) / eps;
      EIGEN_VECTOR_IS_APPROX (dI_dq, Jq_dq, 1e-2);

      ConfigVector_t q_v_dv = lg.integrate (q, (v+dv).eval());
      TangentVector_t Jv_dv = Jv.col(i);
      // q_v_dv - q_v ~ Jv dv
      TangentVector_t dI_dv = lg.difference (q_v, q_v_dv) / eps;
      EIGEN_VECTOR_IS_APPROX (dI_dv, Jv_dv, 1e-2);

      dq[i] = dv[i] = 0;
    }
  }
};

struct LieGroup_JintegrateJdifference{
  template <typename T>
  void operator()(const T ) const
  {
    typedef typename T::ConfigVector_t ConfigVector_t;
    typedef typename T::TangentVector_t TangentVector_t;
    typedef typename T::JacobianMatrix_t JacobianMatrix_t;

    T lg;
    BOOST_TEST_MESSAGE (lg.name());
    ConfigVector_t qa, qb (lg.nq());
    qa = lg.random();
    TangentVector_t v (lg.nv());
    v.setRandom ();
    lg.integrate(qa, v, qb);

    JacobianMatrix_t Jd_qb, Ji_v;

    lg.template dDifference<ARG1> (qa, qb, Jd_qb);
    lg.template dIntegrate <ARG1> (qa, v , Ji_v );

    BOOST_CHECK_MESSAGE ((Jd_qb * Ji_v).isIdentity(),
        "Jd_qb\n" <<
        Jd_qb << '\n' <<
        "* Ji_v\n" <<
        Ji_v << '\n' <<
        "!= Identity\n" <<
        Jd_qb * Ji_v << '\n');
  }
};

struct LieGroup_JintegrateCoeffWise
{
  template <typename T>
  void operator()(const T ) const
  {
    typedef typename T::ConfigVector_t ConfigVector_t;
    typedef typename T::TangentVector_t TangentVector_t;
    typedef typename T::Scalar Scalar;

    T lg;
    ConfigVector_t q = lg.random();
    TangentVector_t dv(TangentVector_t::Zero(lg.nv()));

    BOOST_TEST_MESSAGE (lg.name());
    typedef Eigen::Matrix<Scalar,T::NQ,T::NV> JacobianCoeffs;
    JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv()));
    lg.integrateCoeffWiseJacobian(q,Jintegrate);
    JacobianCoeffs Jintegrate_fd(JacobianCoeffs::Zero(lg.nq(),lg.nv()));

    const Scalar eps = 1e-8;
    for (int i = 0; i < lg.nv(); ++i)
    {
      dv[i] = eps;
      ConfigVector_t q_next(ConfigVector_t::Zero(lg.nq()));
      lg.integrate(q, dv,q_next);
      Jintegrate_fd.col(i) = (q_next - q)/eps;

      dv[i] = 0;
    }

    EIGEN_MATRIX_IS_APPROX(Jintegrate, Jintegrate_fd, sqrt(eps));
  }
};

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_all )
{
  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
                          , JointModelSpherical, JointModelSphericalZYX
                          , JointModelPX, JointModelPY, JointModelPZ
                          , JointModelPrismaticUnaligned
                          , JointModelFreeFlyer
                          , JointModelPlanar
                          , JointModelTranslation
                          , JointModelRUBX, JointModelRUBY, JointModelRUBZ
                          > Variant;
  for (int i = 0; i < 20; ++i)
    boost::mpl::for_each<Variant::types>(TestJoint());

  // FIXME JointModelComposite does not work.
  // boost::mpl::for_each<JointModelVariant::types>(TestJoint());

}

BOOST_AUTO_TEST_CASE ( Jdifference )
{
  typedef double Scalar;
  enum { Options = 0 };

  typedef boost::mpl::vector<  VectorSpaceOperationTpl<1,Scalar,Options>
                             , VectorSpaceOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<3,Scalar,Options>
                             , SpecialEuclideanOperationTpl<2,Scalar,Options>
                             , SpecialEuclideanOperationTpl<3,Scalar,Options>
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<2,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<2,Scalar,Options>
                               >
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<3,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<3,Scalar,Options>
                               >
                             > Types;
  for (int i = 0; i < 20; ++i)
    boost::mpl::for_each<Types>(LieGroup_Jdifference());
}

BOOST_AUTO_TEST_CASE ( Jintegrate )
{
  typedef double Scalar;
  enum { Options = 0 };

  typedef boost::mpl::vector<  VectorSpaceOperationTpl<1,Scalar,Options>
                             , VectorSpaceOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<3,Scalar,Options>
                             , SpecialEuclideanOperationTpl<2,Scalar,Options>
                             , SpecialEuclideanOperationTpl<3,Scalar,Options>
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<2,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<2,Scalar,Options>
                               >
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<3,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<3,Scalar,Options>
                               >
                             > Types;
  for (int i = 0; i < 20; ++i)
    boost::mpl::for_each<Types>(LieGroup_Jintegrate<false>());

  // Around identity
  boost::mpl::for_each<Types>(LieGroup_Jintegrate<true>());
}

BOOST_AUTO_TEST_CASE ( Jintegrate_Jdifference )
{
  typedef double Scalar;
  enum { Options = 0 };

  typedef boost::mpl::vector<  VectorSpaceOperationTpl<1,Scalar,Options>
                             , VectorSpaceOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<2,Scalar,Options>
                             , SpecialOrthogonalOperationTpl<3,Scalar,Options>
                             , SpecialEuclideanOperationTpl<2,Scalar,Options>
                             , SpecialEuclideanOperationTpl<3,Scalar,Options>
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<2,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<2,Scalar,Options>
                               >
                             , CartesianProductOperation<
                                 VectorSpaceOperationTpl<3,Scalar,Options>,
                                 SpecialOrthogonalOperationTpl<3,Scalar,Options>
                               >
                             > Types;
  for (int i = 0; i < 20; ++i)
    boost::mpl::for_each<Types>(LieGroup_JintegrateJdifference());
}

BOOST_AUTO_TEST_CASE(JintegrateCoeffWise)
{
  typedef double Scalar;
  enum { Options = 0 };

  typedef boost::mpl::vector<  VectorSpaceOperationTpl<1,Scalar,Options>
  , VectorSpaceOperationTpl<2,Scalar,Options>
  , SpecialOrthogonalOperationTpl<2,Scalar,Options>
  , SpecialOrthogonalOperationTpl<3,Scalar,Options>
  , SpecialEuclideanOperationTpl<2,Scalar,Options>
  , SpecialEuclideanOperationTpl<3,Scalar,Options>
  , CartesianProductOperation<
  VectorSpaceOperationTpl<2,Scalar,Options>,
  SpecialOrthogonalOperationTpl<2,Scalar,Options>
  >
  , CartesianProductOperation<
  VectorSpaceOperationTpl<3,Scalar,Options>,
  SpecialOrthogonalOperationTpl<3,Scalar,Options>
  >
  > Types;
  for (int i = 0; i < 20; ++i)
    boost::mpl::for_each<Types>(LieGroup_JintegrateCoeffWise());

  {
    typedef SpecialEuclideanOperationTpl<3,Scalar,Options> LieGroup;
    typedef LieGroup::ConfigVector_t ConfigVector_t;
    LieGroup lg;

    ConfigVector_t q = lg.random();
//    TangentVector_t dv(TangentVector_t::Zero(lg.nv()));

    typedef Eigen::Matrix<Scalar,LieGroup::NQ,LieGroup::NV> JacobianCoeffs;
    JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv()));
    lg.integrateCoeffWiseJacobian(q,Jintegrate);

  }
}

BOOST_AUTO_TEST_CASE ( test_vector_space )
{
  typedef VectorSpaceOperationTpl<3,double> VSO_t;
  VSO_t::ConfigVector_t q,
    lo(VSO_t::ConfigVector_t::Constant(-std::numeric_limits<double>::infinity())),
    // lo(VSO_t::ConfigVector_t::Constant(                                       0)),
    // up(VSO_t::ConfigVector_t::Constant( std::numeric_limits<double>::infinity()));
    up(VSO_t::ConfigVector_t::Constant(                                       0));

  bool error = false;
  try {
    VSO_t ().randomConfiguration(lo, up, q);
  } catch (const std::runtime_error&) {
    error = true;
  }
  BOOST_CHECK_MESSAGE(error, "Random configuration between infinite bounds should return an error");
}

BOOST_AUTO_TEST_CASE ( test_size )
{
  // R^1: neutral = [0]
  VectorSpaceOperationTpl <1,double> vs1;
  Eigen::VectorXd neutral;
  neutral.resize (1);
  neutral.setZero ();
  BOOST_CHECK (vs1.nq () == 1);
  BOOST_CHECK (vs1.nv () == 1);
  BOOST_CHECK (vs1.name () == "R^1");
  BOOST_CHECK (vs1.neutral () == neutral);
  // R^2: neutral = [0, 0]
  VectorSpaceOperationTpl <2,double> vs2;
  neutral.resize (2);
  neutral.setZero ();
  BOOST_CHECK (vs2.nq () == 2);
  BOOST_CHECK (vs2.nv () == 2);
  BOOST_CHECK (vs2.name () == "R^2");
  BOOST_CHECK (vs2.neutral () == neutral);
  // R^3: neutral = [0, 0, 0]
  VectorSpaceOperationTpl <3,double> vs3;
  neutral.resize (3);
  neutral.setZero ();
  BOOST_CHECK (vs3.nq () == 3);
  BOOST_CHECK (vs3.nv () == 3);
  BOOST_CHECK (vs3.name () == "R^3");
  BOOST_CHECK (vs3.neutral () == neutral);
  // SO(2): neutral = [1, 0]
  SpecialOrthogonalOperationTpl<2,double> so2;
  neutral.resize (2); neutral [0] = 1; neutral [1] = 0;
  BOOST_CHECK (so2.nq () == 2);
  BOOST_CHECK (so2.nv () == 1);
  BOOST_CHECK (so2.name () == "SO(2)");
  BOOST_CHECK (so2.neutral () == neutral);
  // SO(3): neutral = [0, 0, 0, 1]
  SpecialOrthogonalOperationTpl<3,double> so3;
  neutral.resize (4); neutral.setZero ();
  neutral [3] = 1;
  BOOST_CHECK (so3.nq () == 4);
  BOOST_CHECK (so3.nv () == 3);
  BOOST_CHECK (so3.name () == "SO(3)");
  BOOST_CHECK (so3.neutral () == neutral);
  // SE(2): neutral = [0, 0, 1, 0]
  SpecialEuclideanOperationTpl <2,double> se2;
  neutral.resize (4); neutral.setZero ();
  neutral [2] = 1;
  BOOST_CHECK (se2.nq () == 4);
  BOOST_CHECK (se2.nv () == 3);
  BOOST_CHECK (se2.name () == "SE(2)");
  BOOST_CHECK (se2.neutral () == neutral);
  // SE(3): neutral = [0, 0, 0, 0, 0, 0, 1]
  SpecialEuclideanOperationTpl <3,double> se3;
  neutral.resize (7); neutral.setZero ();
  neutral [6] = 1;
  BOOST_CHECK (se3.nq () == 7);
  BOOST_CHECK (se3.nv () == 6);
  BOOST_CHECK (se3.name () == "SE(3)");
  BOOST_CHECK (se3.neutral () == neutral);
  // R^2 x SO(2): neutral = [0, 0, 1, 0]
  CartesianProductOperation <VectorSpaceOperationTpl <2,double>,
                             SpecialOrthogonalOperationTpl <2,double> > r2xso2;
  neutral.resize (4); neutral.setZero ();
  neutral [2] = 1;
  BOOST_CHECK (r2xso2.nq () == 4);
  BOOST_CHECK (r2xso2.nv () == 3);
  BOOST_CHECK (r2xso2.name () == "R^2*SO(2)");
  BOOST_CHECK (r2xso2.neutral () == neutral);
  // R^3 x SO(3): neutral = [0, 0, 0, 0, 0, 0, 1]
  CartesianProductOperation <VectorSpaceOperationTpl <3,double>,
                             SpecialOrthogonalOperationTpl <3,double> > r3xso3;
  neutral.resize (7); neutral.setZero ();
  neutral [6] = 1;
  BOOST_CHECK (r3xso3.nq () == 7);
  BOOST_CHECK (r3xso3.nv () == 6);
  BOOST_CHECK (r3xso3.name () == "R^3*SO(3)");
  BOOST_CHECK (r3xso3.neutral () == neutral);
}

BOOST_AUTO_TEST_CASE(test_dim_computation)
{
  int dim = eval_set_dim<1,1>::value ;
  BOOST_CHECK(dim == 2);
  dim = eval_set_dim<Eigen::Dynamic,1>::value;
  BOOST_CHECK(dim == Eigen::Dynamic);
  dim = eval_set_dim<1,Eigen::Dynamic>::value;
  BOOST_CHECK(dim == Eigen::Dynamic);
}

template<typename LieGroupCollection>
struct TestLieGroupVariantVisitor
{

  typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;

  template<typename Derived>
  void operator() (const LieGroupBase<Derived> & lg) const
  {
    LieGroupGenericTpl<LieGroupCollection> lg_generic(lg.derived());
    test(lg,lg_generic);
  }

  template<typename Derived>
  static void test(const LieGroupBase<Derived> & lg,
                   const LieGroupGenericTpl<LieGroupCollection> & lg_generic)
  {
    typedef typename Derived::ConfigVector_t ConfigVector_t;
    typedef typename Derived::TangentVector_t TangentVector_t;
    BOOST_CHECK(lg.nq() == nq(lg_generic));
    BOOST_CHECK(lg.nv() == nv(lg_generic));

    BOOST_CHECK(lg.name() == name(lg_generic));

    BOOST_CHECK(lg.neutral() == neutral(lg_generic));

    typedef typename LieGroupGeneric::ConfigVector_t ConfigVectorGeneric;
    typedef typename LieGroupGeneric::TangentVector_t TangentVectorGeneric;

    ConfigVector_t q0 = lg.random();
    TangentVector_t v = TangentVector_t::Random(lg.nv());
    ConfigVector_t qout_ref(lg.nq());
    lg.integrate(q0, v, qout_ref);

    ConfigVectorGeneric qout(lg.nq());
    integrate(lg_generic, ConfigVectorGeneric(q0), TangentVectorGeneric(v), qout);
    BOOST_CHECK(qout.isApprox(qout_ref));
  }
};

BOOST_AUTO_TEST_CASE(test_liegroup_variant)
{
  boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestLieGroupVariantVisitor<LieGroupCollectionDefault>());
}

BOOST_AUTO_TEST_SUITE_END ()


#ifndef MATMATALL_HPP
#define MATMATALL_HPP

#include <vector>
#include <boost/align/aligned_allocator.hpp>

template <typename T>
using aligned_allocator = boost::alignment::aligned_allocator<T, 64>;
template <typename T>
using aligned_vector = std::vector<T, aligned_allocator<T>>;

void matmatall(double alpha, double const* a, int m, int n, double const* b, int k, double beta, double * c);

#endif // MATMATALL_HPP


// On-the-Fly Algorithm for Emptyness of NBAs [Yannakakis et. al].
// Model Checking, Institute of Computer Science, University of Innsbruck.
// Written by Daniel Strigl.

//          Copyright Daniel Strigl 2008.
// 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)

#include <boost/graph/adjacency_list.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
using namespace boost;

// Typedefs and defines
typedef adjacency_list<vecS, vecS, directedS> graph_t;
typedef graph_traits<graph_t>::vertex_descriptor vertex_t;
#define _countof(array) (sizeof(array) / sizeof(array[0]))

///////////////////////////////////////////////////////////////////////////////

#if 1

// ----------------------------------------------------------------------------
//  Sample 1
//    [http://cl-informatik.uibk.ac.at/teaching/ss08/mc/exercises.pdf, Ex. 1]
// ----------------------------------------------------------------------------

// Set up the vertex names
enum { _0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, N };
char* name[] = {
    "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11" };

// Specify the edges in the graph
typedef std::pair<int, int> E;
E edge_array[] = {
    E( _0,  _1), E( _0,  _2), E( _0,  _3),
    E( _1,  _0),
    E( _2,  _3), E( _2,  _8),
    E( _3,  _4),
    E( _4,  _5), E( _4,  _6), E( _4,  _7),
    E( _5,  _7),
    E( _6,  _2), E( _6,  _7),
    E( _7, _11),
    E( _8,  _9),
    E( _9,  _3), E( _9,  _6), E( _9, _10),
    E(_10,  _6), E(_10,  _7), E(_10,  _9), E(_10, _11),
    E(_11,  _7)
};

// Specify the final (accepting) states
vertex_t F[] = { _0, _4, _5, _9, _10 };

// Specify the initial state
vertex_t S = _0;

#else /////////////////////////////////////////////////////////////////////////

// ----------------------------------------------------------------------------
//  Sample 2
//    [http://cl-informatik.uibk.ac.at/teaching/ss08/mc/ohp/1x1.pdf, Slide 23]
// ----------------------------------------------------------------------------

// Set up the vertex names
enum { _q0, _q1, _q2, _q3, N };
char* name[] = { "q0", "q1", "q2", "q3" };

// Specify the edges in the graph
typedef std::pair<int, int> E;
E edge_array[] = {
    E(_q0, _q1), E(_q0, _q2),
    E(_q1, _q2), E(_q1, _q3),
    E(_q2, _q3),
    E(_q3, _q2),
};

// Specify the final (accepting) states
vertex_t F[] = { _q1, _q2 };

// Specify the initial state
vertex_t S = _q0;

#endif

///////////////////////////////////////////////////////////////////////////////

static std::vector<vertex_t> finalStates(F, F + _countof(F));
static graph_t g(edge_array, edge_array + sizeof(edge_array) / sizeof(E), N);
static std::vector<bool> marked(num_vertices(g), false);
static std::vector<bool> flagged(num_vertices(g), false);
static std::vector<vertex_t> outer_dfs_stack;
static std::vector<vertex_t> inner_dfs_stack;
static std::basic_ostringstream<char> word;

static void plot_list(std::vector<bool>& l)
{
    std::vector<vertex_t> tmp;
    for (size_t i = 0; i < l.size(); ++i)
        if (l[i])
            tmp.push_back(i);

    if (tmp.empty())
    {
        std::cout << "$\\varnothing$";
        return;
    }

    std::cout << "\\{";

    for (size_t i = 0; i < tmp.size(); ++i)
    {
        std::cout << name[tmp[i]];
        if (i < tmp.size() - 1)
            std::cout << ", ";
    }

    std::cout << "\\}";
}

static void plot_stack(std::vector<vertex_t>& s)
{
    if (s.empty())
    {
        std::cout << "$\\varepsilon$";
        return;
    }

    for (size_t i = s.size(); i > 0; --i)
    {
        std::cout << name[s[i-1]];
        if (i > 1)
            std::cout << ", ";
    }
}

static void plot_stat()
{
    plot_stack(outer_dfs_stack);
    std::cout << " & ";
    plot_stack(inner_dfs_stack);
    std::cout << " & ";
    plot_list(marked);
    std::cout << " & ";
    plot_list(flagged);
    std::cout << " \\\\" << std::endl;
}

static bool inner_dfs(const graph_t& g, vertex_t q)
{
    inner_dfs_stack.push_back(q);

    flagged[q] = true;

    plot_stat();

    graph_traits<graph_t>::adjacency_iterator vi, vi_end;

    for (tie(vi, vi_end) = adjacent_vertices(q, g); vi != vi_end; ++vi)
    {
        std::vector<vertex_t>::const_iterator res;

        if ((res = find(outer_dfs_stack.begin(), outer_dfs_stack.end(), *vi))
            != outer_dfs_stack.end())
        {
            for (std::vector<vertex_t>::const_iterator
                itr = outer_dfs_stack.begin();
                itr != outer_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            word << "\\left(\\:";

            for (std::vector<vertex_t>::const_iterator
                itr = inner_dfs_stack.begin() + 1;
                itr != inner_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            for (std::vector<vertex_t>::const_iterator itr = res;
                itr != outer_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            word << "\\right)" << "^{\\omega}";

            return false;
        }
        else if (!flagged[*vi])
        {
            if (!inner_dfs(g, *vi))
                return false;
        }
    }

    inner_dfs_stack.pop_back();

    return true;
}

static bool outer_dfs(const graph_t& g, vertex_t q)
{
    outer_dfs_stack.push_back(q);

    marked[q] = true;

    plot_stat();

    graph_traits<graph_t>::adjacency_iterator vi, vi_end;

    for (tie(vi, vi_end) = adjacent_vertices(q, g); vi != vi_end; ++vi)
    {
        if (!marked[*vi])
        {
            if (!outer_dfs(g, *vi))
                return false;
        }
    }

    if (find(finalStates.begin(), finalStates.end(), q) != finalStates.end())
        if (!inner_dfs(g, q))
            return false;

    outer_dfs_stack.pop_back();

    return true;
}

int main(int argc, char* argv[])
{
    std::cout << "\\documentclass[landscape]{article}" << std::endl;
    std::cout << "\\usepackage{lscape}" << std::endl;
    std::cout << "\\usepackage[latin1]{inputenc}" << std::endl;
    std::cout << "\\usepackage{amssymb}" << std::endl;
    std::cout << std::endl;
    std::cout << "\\begin{document}" << std::endl;
    std::cout << std::endl;
    std::cout << "\\begin{table}[htb]" << std::endl;
    std::cout << "\\begin{center}" << std::endl;
    std::cout << "\\begin{tabular}{cccc}" << std::endl;
    std::cout << "\\textbf{outer\\_dfs-stack} & \\textbf{inner\\_dfs-stack} &"\
        " \\textbf{marked} & \\textbf{flagged} \\\\" << std::endl;
    std::cout << "\\hline" << std::endl;
    std::cout << "$\\varepsilon$ & $\\varepsilon$ & $\\varnothing$ &"\
        " $\\varnothing$ \\\\" << std::endl;

    const bool result = outer_dfs(g, S);

    std::cout << "\\end{tabular}" << std::endl;
    std::cout << "\\end{center}" << std::endl;
    std::cout << "\\end{table}" << std::endl;
    std::cout << std::endl;

    std::cout << "\\begin{center}" << std::endl;
    if (result)
        std::cout << "\\textbf{terminate}(true)" << std::endl;
    else
        std::cout << "\\textbf{terminate}(false)" << std::endl;
    std::cout << "\\end{center}" << std::endl;

    if (!result)
    {
        std::cout << std::endl;
        std::cout << "\\begin{center}" << std::endl;
        std::cout << "$word = " << word.str() << "$" << std::endl;
        std::cout << "\\end{center}" << std::endl;
    }

    std::cout << std::endl;
    std::cout << "\\end{document}" << std::endl;

    return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/thirdrooteps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check thirdrooteps behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::thirdrooteps;
  using boost::simd::Thirdrooteps;

  STF_TYPE_IS(decltype(Thirdrooteps<T>()), T);
  STF_EQUAL(Thirdrooteps<T>(), T(1));
  STF_EQUAL(thirdrooteps( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check thirdrooteps behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::thirdrooteps;
  using boost::simd::Thirdrooteps;
  using boost::simd::Eps;

  STF_TYPE_IS(decltype(Thirdrooteps<T>()), T);
  auto z1 = Thirdrooteps<T>();
  STF_ULP_EQUAL(z1*z1*z1, Eps<T>(), 0.5);
  auto z2 = thirdrooteps( as(T{}));
  STF_ULP_EQUAL(z2*z2*z2, Eps<T>(), 0.5);
}