// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.

#include "utils.h"

#pragma warning (disable : 4996) // fopen

#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <functional>
#include <limits>
#include <chrono>
#include <cstring>

#ifdef DO_EIGEN
#include <Eigen/Dense>
#include <Eigen/StdVector>
#endif

#include "light_matrix.h"

#include "defs.h"

using std::cin;
using std::cout;
using std::endl;
using std::string;
using std::vector;
using std::getline;
using std::memcpy;
using namespace std::chrono;

BASparseMat::BASparseMat() {}

BASparseMat::BASparseMat(int n_, int m_, int p_) : n(n_), m(m_), p(p_)
{
    nrows = 2 * p + p;
    ncols = BA_NCAMPARAMS * n + 3 * m + p;
    rows.reserve(nrows + 1);
    int nnonzero = (BA_NCAMPARAMS + 3 + 1) * 2 * p + p;
    cols.reserve(nnonzero);
    vals.reserve(nnonzero);
    rows.push_back(0);
}

void BASparseMat::insert_reproj_err_block(int obsIdx,
    int camIdx, int ptIdx, const double* const J)
{
    int n_new_cols = BA_NCAMPARAMS + 3 + 1;
    rows.push_back(rows.back() + n_new_cols);
    rows.push_back(rows.back() + n_new_cols);

    for (int i_row = 0; i_row < 2; i_row++)
    {
        for (int i = 0; i < BA_NCAMPARAMS; i++)
        {
            cols.push_back(BA_NCAMPARAMS * camIdx + i);
            vals.push_back(J[2 * i + i_row]);
        }
        int col_offset = BA_NCAMPARAMS * n;
        int val_offset = BA_NCAMPARAMS * 2;
        for (int i = 0; i < 3; i++)
        {
            cols.push_back(col_offset + 3 * ptIdx + i);
            vals.push_back(J[val_offset + 2 * i + i_row]);
        }
        col_offset += 3 * m;
        val_offset += 3 * 2;
        cols.push_back(col_offset + obsIdx);
        vals.push_back(J[val_offset + i_row]);
    }
}

void BASparseMat::insert_w_err_block(int wIdx, double w_d)
{
    rows.push_back(rows.back() + 1);
    cols.push_back(BA_NCAMPARAMS * n + 3 * m + wIdx);
    vals.push_back(w_d);
}

void BASparseMat::clear()
{
    rows.clear();
    cols.clear();
    vals.clear();
    rows.reserve(nrows + 1);
    int nnonzero = (BA_NCAMPARAMS + 3 + 1) * 2 * p + p;
    cols.reserve(nnonzero);
    vals.reserve(nnonzero);
    rows.push_back(0);
}

void read_gmm_instance(const string& fn,
    int* d, int* k, int* n,
    vector<double>& alphas,
    vector<double>& means,
    vector<double>& icf,
    vector<double>& x,
    Wishart& wishart,
    bool replicate_point)
{
    FILE* fid = fopen(fn.c_str(), "r");

    if (!fid) {
        std::cerr << "Cannot open " << fn << std::endl;
        throw "oiks";
    }

    fscanf(fid, "%i %i %i", d, k, n);

    int d_ = *d, k_ = *k, n_ = *n;

    int icf_sz = d_ * (d_ + 1) / 2;
    alphas.resize(k_);
    means.resize(d_ * k_);
    icf.resize(icf_sz * k_);
    x.resize(d_ * n_);

    for (int i = 0; i < k_; i++)
    {
        fscanf(fid, "%lf", &alphas[i]);
    }

    for (int i = 0; i < k_; i++)
    {
        for (int j = 0; j < d_; j++)
        {
            fscanf(fid, "%lf", &means[i * d_ + j]);
        }
    }

    for (int i = 0; i < k_; i++)
    {
        for (int j = 0; j < icf_sz; j++)
        {
            fscanf(fid, "%lf", &icf[i * icf_sz + j]);
        }
    }

    if (replicate_point)
    {
        for (int j = 0; j < d_; j++)
        {
            fscanf(fid, "%lf", &x[j]);
        }
        for (int i = 0; i < n_; i++)
        {
            memcpy(&x[i * d_], &x[0], d_ * sizeof(double));
        }
    }
    else
    {
        for (int i = 0; i < n_; i++)
        {
            for (int j = 0; j < d_; j++)
            {
                fscanf(fid, "%lf", &x[i * d_ + j]);
            }
        }
    }

    fscanf(fid, "%lf %i", &(wishart.gamma), &(wishart.m));

    fclose(fid);
}

void read_ba_instance(const string& fn,
    int& n, int& m, int& p,
    vector<double>& cams,
    vector<double>& X,
    vector<double>& w,
    vector<int>& obs,
    vector<double>& feats)
{
    FILE* fid = fopen(fn.c_str(), "r");
    if (!fid) {
        throw "oik" + fn;
    }
    std::cout << "read_ba_instance: opened " << fn << std::endl;

    fscanf(fid, "%i %i %i", &n, &m, &p);
    int nCamParams = 11;

    cams.resize(nCamParams * n);
    X.resize(3 * m);
    w.resize(p);
    obs.resize(2 * p);
    feats.resize(2 * p);

    for (int j = 0; j < nCamParams; j++)
        fscanf(fid, "%lf", &cams[j]);
    for (int i = 1; i < n; i++)
        memcpy(&cams[i * nCamParams], &cams[0], nCamParams * sizeof(double));

    for (int j = 0; j < 3; j++)
        fscanf(fid, "%lf", &X[j]);
    for (int i = 1; i < m; i++)
        memcpy(&X[i * 3], &X[0], 3 * sizeof(double));

    fscanf(fid, "%lf", &w[0]);
    for (int i = 1; i < p; i++)
        w[i] = w[0];

    int camIdx = 0;
    int ptIdx = 0;
    for (int i = 0; i < p; i++)
    {
        obs[i * 2 + 0] = (camIdx++ % n);
        obs[i * 2 + 1] = (ptIdx++ % m);
    }

    fscanf(fid, "%lf %lf", &feats[0], &feats[1]);
    for (int i = 1; i < p; i++)
    {
        feats[i * 2 + 0] = feats[0];
        feats[i * 2 + 1] = feats[1];
    }

    fclose(fid);
}

void write_J_sparse(const string& fn, const BASparseMat& J)
{
    write_J_stream<decltype(J.vals)::value_type> out(fn, J.nrows, J.ncols);

    out << J.rows.size() << endl;
    for (size_t i = 0; i < J.rows.size(); i++)
    {
        out << J.rows[i] << " ";
    }
    out << endl;
    out << J.cols.size() << endl;
    for (size_t i = 0; i < J.cols.size(); i++)
    {
        out << J.cols[i] << " ";
    }
    out << endl;
    for (size_t i = 0; i < J.vals.size(); i++)
    {
        out << J.vals[i] << " ";
    }
    out.close();
}

void write_J(const string& fn, int Jrows, int Jcols, double** J)
{
    write_J_stream<decltype(**J)> out(fn, Jrows, Jcols);

    for (int i = 0; i < Jrows; i++)
    {
        for (int j = 0; j < Jcols; j++)
        {
            out << J[i][j] << " ";
        }
        out << endl;
    }
    out.close();
}

void write_J(const string& fn, int Jrows, int Jcols, double* J)
{
    write_J_stream<decltype(*J)> out(fn, Jrows, Jcols);

    for (int i = 0; i < Jrows; i++)
    {
        for (int j = 0; j < Jcols; j++)
        {
            out << J[j * Jrows + i] << " ";
        }
        out << endl;
    }
    out.close();
}

void write_times(double tf, double tJ)
{
    cout << "tf = " << std::scientific << tf << "s" << endl;
    cout << "tJ = " << tJ << "s" << endl;
    cout << "tJ/tf = " << tJ / tf << "s" << endl;
}

void write_times(const string& fn, double tf, double tJ, double* t_sparsity)
{
    std::ofstream out(fn);
    out << std::scientific << tf << " " << tJ;
    if (t_sparsity)
        out << " " << *t_sparsity;
    out << endl;
    out << "tf tJ";
    if (t_sparsity)
        out << " t_sparsity";
    out << endl;
    out << "tf = " << std::scientific << tf << "s" << endl;
    out << "tJ = " << tJ << "s" << endl;
    out << "tJ/tf = " << tJ / tf << "s" << endl;
    out.close();
}

#ifdef DO_EIGEN
void read_hand_model(const string& path, HandModelEigen* pmodel)
{
    const char DELIMITER = ':';
    auto& model = *pmodel;
    string fn_in = path + "bones.txt";
    std::ifstream bones_in(fn_in);
    if (!bones_in.good()) {
        std::cerr << "Cannot read " << fn_in << std::endl;
        throw "zoiks";
    }

    string s;
    while (bones_in.good())
    {
        getline(bones_in, s, DELIMITER);
        if (s.empty())
            continue;
        model.bone_names.push_back(s);
        getline(bones_in, s, DELIMITER);
        model.parents.push_back(std::stoi(s));
        double tmp[16];
        for (int i = 0; i < 16; i++)
        {
            getline(bones_in, s, DELIMITER);
            tmp[i] = std::stod(s);
        }
        model.base_relatives.push_back(Eigen::Map<Eigen::Matrix4d>(tmp));
        model.base_relatives.back().transposeInPlace();
        for (int i = 0; i < 15; i++)
        {
            getline(bones_in, s, DELIMITER);
            tmp[i] = std::stod(s);
        }
        getline(bones_in, s, '\n');
        tmp[15] = std::stod(s);
        model.inverse_base_absolutes.push_back(Eigen::Map<Eigen::Matrix4d>(tmp));
        model.inverse_base_absolutes.back().transposeInPlace();
    }
    bones_in.close();
    int n_bones = (int)model.bone_names.size();

    std::ifstream vert_in(path + "vertices.txt");
    int n_vertices = 0;
    while (vert_in.good())
    {
        getline(vert_in, s);
        if (!s.empty())
            n_vertices++;
    }
    vert_in.close();

    model.base_positions.resize(3, n_vertices);
    model.weights = Eigen::ArrayXXd::Zero(n_bones, n_vertices);
    vert_in = std::ifstream(path + "vertices.txt");
    for (int i_vert = 0; i_vert < n_vertices; i_vert++)
    {
        for (int j = 0; j < 3; j++)
        {
            getline(vert_in, s, DELIMITER);
            model.base_positions(j, i_vert) = std::stod(s);
        }
        for (int j = 0; j < 3 + 2; j++)
        {
            getline(vert_in, s, DELIMITER); // skip
        }
        getline(vert_in, s, DELIMITER);
        int n = std::stoi(s);
        for (int j = 0; j < n; j++)
        {
            getline(vert_in, s, DELIMITER);
            int i_bone = std::stoi(s);
            if (j == n - 1)
                getline(vert_in, s, '\n');
            else
                getline(vert_in, s, DELIMITER);
            model.weights(i_bone, i_vert) = std::stod(s);
        }
    }
    vert_in.close();

    std::ifstream triangles_in(path + "triangles.txt");
    string ss[3];
    while (triangles_in.good())
    {
        getline(triangles_in, ss[0], DELIMITER);
        if (ss[0].empty())
            continue;

        getline(triangles_in, ss[1], DELIMITER);
        getline(triangles_in, ss[2], '\n');
        Triangle curr;
        for (int i = 0; i < 3; i++)
            curr.verts[i] = std::stoi(ss[i]);
        model.triangles.push_back(curr);
    }
    triangles_in.close();

    model.is_mirrored = false;
}

void read_hand_instance(const string& model_dir, const string& fn_in,
    vector<double>* theta, HandDataEigen* data, vector<double>* us)
{
    read_hand_model(model_dir, &data->model);
    std::ifstream in(fn_in);
    if (!in.good()) {
        std::cerr << "Cannot read " << fn_in << std::endl;
        throw "zoiks";
    }
    int n_pts, n_theta;
    in >> n_pts >> n_theta;
    std::cout << "read_hand_instance: npts = " << n_pts << ", n_theta = " << n_theta << std::endl;
    data->correspondences.resize(n_pts);
    data->points.resize(3, n_pts);
    for (int i = 0; i < n_pts; i++)
    {
        in >> data->correspondences[i];
        for (int j = 0; j < 3; j++)
        {
            in >> data->points(j, i);
        }
    }
    if (us != nullptr)
    {
        us->resize(2 * n_pts);
        for (int i = 0; i < 2 * n_pts; i++)
        {
            in >> (*us)[i];
        }
    }
    theta->resize(n_theta);
    for (int i = 0; i < n_theta; i++)
    {
        in >> (*theta)[i];
    }
    if (!in.good()) {
        std::cerr << "Cannot read " << fn_in << std::endl;
        throw "zoiks";
    }
    in.close();
}
#endif

void read_hand_model(const string& path, HandModelLightMatrix* pmodel)
{
    const char DELIMITER = ':';
    auto& model = *pmodel;
    std::ifstream bones_in(path + "bones.txt");
    string s;
    while (bones_in.good())
    {
        getline(bones_in, s, DELIMITER);
        if (s.empty())
            continue;
        model.bone_names.push_back(s);
        getline(bones_in, s, DELIMITER);
        model.parents.push_back(std::stoi(s));
        double tmp[16];
        for (int i = 0; i < 16; i++)
        {
            getline(bones_in, s, DELIMITER);
            tmp[i] = std::stod(s);
        }
        model.base_relatives.emplace_back(4, 4);
        model.base_relatives.back().set(tmp);
        model.base_relatives.back().transpose_in_place();
        for (int i = 0; i < 15; i++)
        {
            getline(bones_in, s, DELIMITER);
            tmp[i] = std::stod(s);
        }
        getline(bones_in, s, '\n');
        tmp[15] = std::stod(s);
        model.inverse_base_absolutes.emplace_back(4, 4);
        model.inverse_base_absolutes.back().set(tmp);
        model.inverse_base_absolutes.back().transpose_in_place();
    }
    bones_in.close();
    int n_bones = (int)model.bone_names.size();

    std::ifstream vert_in(path + "vertices.txt");
    int n_vertices = 0;
    while (vert_in.good())
    {
        getline(vert_in, s);
        if (!s.empty())
            n_vertices++;
    }
    vert_in.close();

    model.base_positions.resize(4, n_vertices);
    model.base_positions.set_row(3, 1.);
    model.weights.resize(n_bones, n_vertices);
    model.weights.fill(0.);
    vert_in = std::ifstream(path + "vertices.txt");
    for (int i_vert = 0; i_vert < n_vertices; i_vert++)
    {
        for (int j = 0; j < 3; j++)
        {
            getline(vert_in, s, DELIMITER);
            model.base_positions(j, i_vert) = std::stod(s);
        }
        for (int j = 0; j < 3 + 2; j++)
        {
            getline(vert_in, s, DELIMITER); // skip
        }
        getline(vert_in, s, DELIMITER);
        int n = std::stoi(s);
        for (int j = 0; j < n; j++)
        {
            getline(vert_in, s, DELIMITER);
            int i_bone = std::stoi(s);
            if (j == n - 1)
                getline(vert_in, s, '\n');
            else
                getline(vert_in, s, DELIMITER);
            model.weights(i_bone, i_vert) = std::stod(s);
        }
    }
    vert_in.close();

    std::ifstream triangles_in(path + "triangles.txt");
    string ss[3];
    while (triangles_in.good())
    {
        getline(triangles_in, ss[0], DELIMITER);
        if (ss[0].empty())
            continue;

        getline(triangles_in, ss[1], DELIMITER);
        getline(triangles_in, ss[2], '\n');
        Triangle curr;
        for (int i = 0; i < 3; i++)
            curr.verts[i] = std::stoi(ss[i]);
        model.triangles.push_back(curr);
    }
    triangles_in.close();

    model.is_mirrored = false;
}

void read_hand_instance(const string& model_dir, const string& fn_in,
    vector<double>* theta, HandDataLightMatrix* data, vector<double>* us)
{
    read_hand_model(model_dir, &data->model);
    std::ifstream in(fn_in);
    int n_pts, n_theta;
    in >> n_pts >> n_theta;
    data->correspondences.resize(n_pts);
    data->points.resize(3, n_pts);
    for (int i = 0; i < n_pts; i++)
    {
        in >> data->correspondences[i];
        for (int j = 0; j < 3; j++)
        {
            in >> data->points(j, i);
        }
    }
    if (us != nullptr)
    {
        us->resize(2 * n_pts);
        for (int i = 0; i < 2 * n_pts; i++)
        {
            in >> (*us)[i];
        }
    }
    theta->resize(n_theta);
    for (int i = 0; i < n_theta; i++)
    {
        in >> (*theta)[i];
    }
    in.close();
}

void read_lstm_instance(const string& fn,
    int* l, int* c, int* b,
    vector<double>& main_params,
    vector<double>& extra_params,
    vector<double>& state,
    vector<double>& sequence)
{
    FILE* fid = fopen(fn.c_str(), "r");

    if (!fid) {
        std::cerr << "Cannot open " << &fn << std::endl;
        throw "oiks";
    }

    fscanf(fid, "%i %i %i", l, c, b);

    int l_ = *l, c_ = *c, b_ = *b;

    int main_sz = 2 * l_ * 4 * b_;
    int extra_sz = 3 * b_;
    int state_sz = 2 * l_ * b_;
    int seq_sz = c_ * b_;

    main_params.resize(main_sz);
    extra_params.resize(extra_sz);
    state.resize(state_sz);
    sequence.resize(seq_sz);

    for (int i = 0; i < main_sz; i++) {
        fscanf(fid, "%lf", &main_params[i]);
    }

    for (int i = 0; i < extra_sz; i++) {
        fscanf(fid, "%lf", &extra_params[i]);
    }

    for (int i = 0; i < state_sz; i++) {
        fscanf(fid, "%lf", &state[i]);
    }

    for (int i = 0; i < c_ * b_; i++) {
        fscanf(fid, "%lf", &sequence[i]);
    }

    /*char ch;
    fscanf(fid, "%c", &ch);
    fscanf(fid, "%c", &ch);

    for (int i = 0; i < c_; i++) {
        unsigned char ch;
        fscanf(fid, "%c", &ch);
        int cb = ch;
        for (int j = b_ - 1; j >= 0; j--) {
            int p = pow(2, j);
            if (cb >= p) {
                sequence[(i + 1) * b_ - j - 1] = 1;
                cb -= p;
            }
            else {
                sequence[(i + 1) * b_ - j - 1] = 0;
            }
        }
    }*/

    fclose(fid);
}

// Time a function
double timer(int nruns, double limit, std::function<void()> func) {
    if (limit < 0) limit = std::numeric_limits<double>::max();

    double total = 0;
    int i = 0;

    high_resolution_clock::time_point start = high_resolution_clock::now();
    for (; i < nruns && total < limit; ++i) {
        func();
        high_resolution_clock::time_point end = high_resolution_clock::now();
        total = duration_cast<duration<double>>(end - start).count();
    }

    if (i < nruns) std::cout << "Hit time limit after " << i << " loops" << endl;

    if (i > 0)
        return total / i;
    else
        return 0;
}


//--------------------------------C-Code

#include "./include/System.h"
#include <boost/python.hpp>
#include <numpy/ndarrayobject.h>
#include <opencv2/core/core.hpp>

#include <string>
#include "lib/conversion.h"

namespace bpy = boost::python;
using namespace std;

//  need to initialize numpy
#if PY_MAJOR_VERSION >= 3
int
#else
void
#endif
init_numpy()
{
    bpy::numeric::array::set_module_and_type("numpy", "ndarray");
    import_array();
}

class SLAMClass{
    public:
        SLAMClass();

        NDArrayConverter cvt;

        void init(const std::string &path_to_vocabulary, const std::string &path_to_settings, const string camera_type, const bool flag_visualize){
            ORB_SLAM2::Config::SetParameterFile(path_to_settings);
            this->slam_obj = new ORB_SLAM2::System(path_to_vocabulary, path_to_settings, ORB_SLAM2::System::RGBD, flag_visualize);
            std::cout << path_to_vocabulary << std::endl;
        }

        PyObject* track_rgbd(PyObject* rgb_img, PyObject* d_img, const double timestamp){
            cv::Mat rgb_img_mat = cvt.toMat(rgb_img);
            cv::Mat d_img_mat = cvt.toMat(d_img);
//            d_img_mat.convertTo(d_img_mat, CV_32F);
            auto camera_matrix = this->slam_obj->TrackRGBD(rgb_img_mat, d_img_mat, timestamp);
            PyObject* ret = cvt.toNDArray(camera_matrix);
            return ret;
        }

        void prepare_dump(){
            slam_obj->PrepareDump();
        }

            // stuff for export
//    cv::Mat kf_ids_from_mps;
//    cv::Mat kf_ids;
//    cv::Mat plane_ids;
//    cv::Mat mp_3dpts;
//    cv::Mat kf_3dpts;
//    cv::Mat plane_ids_from_boundary_pts;
//    cv::Mat plane_params;
//    cv::Mat plane_boundary_pts;

        PyObject* get_kf_ids_from_mps(){
            return cvt.toNDArray(slam_obj->dump_kf_ids_from_mps());
        }
        PyObject* get_kf_ids(){
            return cvt.toNDArray(slam_obj->dump_kf_ids());
        }
        PyObject* get_mp_3dpts(){
            return cvt.toNDArray(slam_obj->dump_mp_3dpts());
        }
        PyObject* get_kf_3dpts(){
            return cvt.toNDArray(slam_obj->dump_kf_3dpts());
        }
        PyObject* get_plane_params(){
            return cvt.toNDArray(slam_obj->dump_plane_params());
        }
        PyObject* get_plane_ids(){
            return cvt.toNDArray(slam_obj->dump_plane_ids());
        }
        PyObject* get_plane_ids_from_boundary_pts(){
            return cvt.toNDArray(slam_obj->dump_plane_ids_from_boundary_pts());
        }
        PyObject* get_boundary_pts(){
            return cvt.toNDArray(slam_obj->dump_boundary_pts());
        }

        PyObject* get_boundary_update_sizes(){
            return cvt.toNDArray(slam_obj->dump_cntBoundaryUpdateSizes());
        }

        PyObject* get_grid_map(){
            return cvt.toNDArray(slam_obj->get_gridmaps());
        }

        ~SLAMClass();

    private:
        ORB_SLAM2::System *slam_obj;
};

    SLAMClass::SLAMClass() {
        // nothing
    }

    SLAMClass::~SLAMClass() {
        delete this->slam_obj; // Otherwise may end up with a SegFault!!
    }

//--------------------------------Boost-Python-Code

BOOST_PYTHON_MODULE(ORBSLAM2)
{
    using namespace boost::python;
    Py_Initialize(); // required by NumPy
    init_numpy(); // required by NumPy

    class_<SLAMClass>("SLAM")
        .def("init", &SLAMClass::init)
        .def("track_rgbd", &SLAMClass::track_rgbd)
.def("prepare_dump", &SLAMClass::prepare_dump)
.def("get_kf_ids_from_mps", &SLAMClass::get_kf_ids_from_mps)
.def("get_kf_ids", &SLAMClass::get_kf_ids)
.def("get_plane_ids", &SLAMClass::get_plane_ids)
.def("get_mp_3dpts", &SLAMClass::get_mp_3dpts)
.def("get_kf_3dpts", &SLAMClass::get_kf_3dpts)
.def("get_plane_ids_from_boundary_pts", &SLAMClass::get_plane_ids_from_boundary_pts)
.def("get_plane_params", &SLAMClass::get_plane_params)
.def("get_boundary_pts", &SLAMClass::get_boundary_pts)
.def("get_boundary_update_sizes", &SLAMClass::get_boundary_update_sizes)
.def("get_grid_map", &SLAMClass::get_grid_map);
//    cv::Mat kf_ids_from_mps;
//    cv::Mat kf_ids;
//    cv::Mat plane_ids;
//    cv::Mat mp_3dpts
//    cv::Mat kf_3dpts;
//    cv::Mat plane_ids_from_boundary_pts;
//    cv::Mat plane_params;
//    cv::Mat plane_boundary_pts;

}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_ARMADILLO_BRIDGE_HPP_
#define TURI_ARMADILLO_BRIDGE_HPP_

// Builds a thin wrapper on top of the armadillo library, providing support for

#include <armadillo>
#include <numerics/sparse_vector.hpp>
#include <numerics/row_major_matrix.hpp>
#include <logger/assertions.hpp>
#include <typeinfo>
#include <limits>
#include <sstream>

namespace turi {

using arma::exp;
using arma::approx_equal;

template <typename C, typename T>
static inline void inplace_elementwise_max(C& container, const T& v) {
  for(auto& e : container) {
    e = std::max<typename std::remove_reference<decltype(e)>::type>(e, v);
  }
}

template <typename C, typename T>
static inline void inplace_elementwise_min(C& container, const T& v) {
  for(auto& e : container) {
    e = std::min<typename std::remove_reference<decltype(e)>::type>(e, v);
  }
}

template <typename C, typename T>
static inline void inplace_elementwise_clip(C& container, const T& min, const T& max) {
  for(auto& e : container) {
    typedef typename std::remove_reference<decltype(e)>::type elem_t;
    e = std::min<elem_t>(std::max<elem_t>(e, max), min);
  }
}

template <typename Src, typename Dest, typename T>
static inline void elementwise_max_copy(Src& src, const Dest& dest, const T& v) {
  typedef typename std::remove_reference<decltype(dest(0))>::type elem_t;
  for(size_t i = 0; i < dest.size(); ++i) {
    dest(i) = std::max<elem_t>(src(i), v);
  }
}

template <typename Src, typename Dest, typename T>
static inline void elementwise_min_copy(Src& src, const Dest& dest, const T& v) {
  typedef typename std::remove_reference<decltype(dest(0))>::type elem_t;
  for(size_t i = 0; i < dest.size(); ++i) {
    dest(i) = std::min<elem_t>(src(i), v);
  }
}

////////////////////////////////////////////////////////////////////////////////
//
//  Tests.
#define ENABLE_IF_ARMA_VEC(v) \
  typename std::enable_if<std::is_convertible<decltype(arma::sum(v)), double>::value>::type* = 0

#define ENABLE_IF_ARMA_MAT(v) \
  typename std::enable_if<std::is_convertible<decltype(arma::sum(arma::sum(v))), double>::value \
      && !std::is_convertible<decltype(arma::sum(v)), double>::value>::type* = 0

///////////////////////////////////
//
template <typename C>
static inline auto _arma_tsum(C&& v, ENABLE_IF_ARMA_VEC(v))
 -> decltype(arma::sum(v)) {
  return arma::sum(v);
}

template <typename C>
static inline auto _arma_tsum(C&& v, ENABLE_IF_ARMA_MAT(v))
 -> decltype(arma::sum(arma::sum(v))) {
  return arma::sum(arma::sum(v));
}

///////////////////////////////////
// Total sum

template <typename C>
static inline auto total_sum(C&& v) -> decltype(_arma_tsum(v)) {
  return _arma_tsum(v);
}

template <typename T, typename I>
static inline typename sparse_vector<T, I>::value_type
total_sum(const sparse_vector<T, I>& container) {
  typename sparse_vector<T, I>::value_type v = 0;
  for(const auto& p : container) {
    v += p.second;
  }
  return v;
}

///////////////////////////////////
// Squared Norm

template <typename C>
static inline auto squared_norm(C&& v) -> decltype(_arma_tsum(arma::square(v))) {
  return _arma_tsum(arma::square(v));
}

template <typename T, typename I>
static inline typename sparse_vector<T, I>::value_type
squared_norm(const sparse_vector<T, I>& container) {
  typename sparse_vector<T, I>::value_type v = 0;
  for(const auto& p : container) {
    v += p.second * p.second;
  }
  return v;
}

////////////////////////////////////////////////////////////////////////////////
//
//  Additional dot implementations.

using arma::dot;

// Dot
template <typename ArmaVec, typename U, typename Index>
static inline U dot(ArmaVec&& x, const sparse_vector<U, Index>& y, ENABLE_IF_ARMA_VEC(x)) {

 U r = 0;

 for(auto p : y) {
  r += x[p.first] * p.second;
 }

 return r;
}

template <typename ArmaVec, typename U, typename Index>
static inline U dot(const sparse_vector<U, Index>& x, ArmaVec&& y, ENABLE_IF_ARMA_VEC(y)) {
  return dot(y, x);
}

////////////////////////////
// operator *
template <typename U, typename Index>
arma::vec operator*(const arma::mat& X, const sparse_vector<U, Index>& v) {

  if(v.size() != X.n_cols) {
    throw std::logic_error(
        "Number of columns does not match vector size in matrix multiply.");
  }

  arma::vec ret(X.n_rows);

  if(v.num_nonzeros() == 0) {
    ret.zeros();
    return ret;
  }

  auto it = v.begin();

  ret = X.col(it->first) * it->second;
  ++it;

  for(;it != v.end(); ++it) {
    ret += X.col(it->first) * it->second;
  }

  return ret;
}

///////////////////////////
// Interface operators between arma and our sparse vector.

template <typename T, typename U, typename Index>
arma::Col<T>& operator+=(arma::Col<T>& x, const sparse_vector<U, Index>& y) {

  for(auto p : y) {
    x(p.first) += p.second;
  }

  return x;
}

/**
 * Solve for A*x = b using ldlt solving.
 *  A must be symmetric positive definite.
 */
template <typename T>
arma::Col<T>  solve_ldlt(const arma::Mat<T>& _A, const arma::Col<T>& b) {

#ifdef ARMA_USE_LAPACK
  arma::Mat<T> A = _A;

	if(!A.is_square()) {
		throw std::invalid_argument("LDLT decomposition requires a square matrix.");
	}

  int N = A.n_rows;
  char lower = 'L';
  DASSERT_TRUE(b.n_rows == b.n_rows);

  T lquery;
  int lwork = -1, info, one = 1;
	arma::Col<int> fact;
	fact.set_size(N);
  arma::lapack::sytrf(&lower, &N, const_cast<T*>(A.memptr()), &N,
            fact.memptr(), &lquery, &lwork, &info);
  lwork = int(lquery);

  T *work = new T[lwork];
  arma::lapack::sytrf(&lower, &N, const_cast<T*>(A.memptr()), &N,
            fact.memptr(), work, &lwork, &info);
  delete [] work;

  DASSERT_FALSE(info < 0); // Lapack error
  if (info == 0) {
    arma::Col<T> x = b;
    arma::lapack::sytrs(&lower, &N, &one, const_cast<T*>(A.memptr()), &N,
                        fact.memptr(), x.memptr(), &N, &info);
    return x;
  } else {
    return arma::solve(_A, b);
  }
#else
  return arma::solve(_A, b);
#endif
}

/**
 *  Solve for A*x = b using ldlt ving
 *  A must be symmetric positive semi-definite
 */
template <typename... T, typename b_t>
auto solve_ldlt(const row_major_matrix<T...>& A, b_t&& b)
  -> decltype(solve_ldlt(A.X_raw(),b.eval())) {
  return solve_ldlt(A.X_raw(), b.eval());
}

template <typename AT, typename BT>
auto solve_ldlt(AT&& A, BT&& b) -> decltype(solve_ldlt(A.eval(), b.eval())) {
   return solve_ldlt(A.eval(), b.eval());
}

// Serialization stuff
//
namespace archive_detail {

////////////////////////////////////////////////////////////////////////////////
// Serialization routines for armadillo containers.

template <typename OutArcType, typename T>
struct serialize_impl<OutArcType, arma::Mat<T>, false> {

  static void exec(OutArcType& arc, const arma::Mat<T>& X) {
    arc << (size_t)X.n_rows << (size_t)X.n_cols;
    turi::serialize(arc, X.memptr(), X.n_rows*X.n_cols*sizeof(T));
  }
};

template <typename InArcType, typename T>
struct deserialize_impl<InArcType, arma::Mat<T>, false> {

  static void exec(InArcType& arc, arma::Mat<T>& X) {
    size_t nrows, ncols;
    arc >> nrows >> ncols;
    X.resize(nrows, ncols);
    turi::deserialize(arc, X.memptr(), X.n_rows*X.n_cols*sizeof(T));
  }
};

template <typename OutArcType, typename T>
struct serialize_impl<OutArcType, arma::Col<T>, false> {

  static void exec(OutArcType& arc, const arma::Col<T>& X) {
    arc << size_t(X.n_elem);
    arc << size_t(1);
    turi::serialize(arc, X.memptr(), X.n_elem*sizeof(T));
  }
};

template <typename InArcType, typename T>
struct deserialize_impl<InArcType, arma::Col<T>, false> {

  static void exec(InArcType& arc, arma::Col<T>& X) {
    size_t nelem1, nelem2;
    arc >> nelem1 >> nelem2;
    ASSERT_TRUE(nelem1 == 1 || nelem2 == 1);
    size_t nelem = nelem1 * nelem2;
    X.resize(nelem);
    turi::deserialize(arc, X.memptr(), X.n_elem*sizeof(T));
  }
};

template <typename OutArcType, typename T>
struct serialize_impl<OutArcType, arma::Row<T>, false> {

  static void exec(OutArcType& arc, const arma::Row<T>& X) {
    arc << size_t(1);
    arc << size_t(X.n_elem);
    turi::serialize(arc, X.memptr(), X.n_elem*sizeof(T));
  }
};

template <typename InArcType, typename T>
struct deserialize_impl<InArcType, arma::Row<T>, false> {

  static void exec(InArcType& arc, arma::Row<T>& X) {
    size_t nelem1, nelem2;
    arc >> nelem1 >> nelem2;
    ASSERT_TRUE(nelem1 == 1 || nelem2 == 1);
    size_t nelem = nelem1 * nelem2;
    X.resize(nelem);
    turi::deserialize(arc, X.memptr(), X.n_elem*sizeof(T));
  }
};

}}

#endif


/*
@copyright Louis Dionne 2015
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/hana/detail/tuple_cartesian_product.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;
using detail::tuple_cartesian_product;

static_assert(
    tuple_cartesian_product(make_tuple(1), make_tuple('a', 'b')) == make_tuple(
        make_tuple(1, 'a'),
        make_tuple(1, 'b')
    )
, "");

static_assert(
    tuple_cartesian_product(make_tuple(1, 2), make_tuple('a')) == make_tuple(
        make_tuple(1, 'a'),
        make_tuple(2, 'a')
    )
, "");

static_assert(
    tuple_cartesian_product(make_tuple(1, 2), make_tuple('a', 'b')) == make_tuple(
        make_tuple(1, 'a'),
        make_tuple(1, 'b'),
        make_tuple(2, 'a'),
        make_tuple(2, 'b')
    )
, "");

static_assert(
    tuple_cartesian_product(
        make_tuple(1),
        make_tuple('a'),
        make_tuple(1.f),
        make_tuple(1l, 2l)
    ) == make_tuple(
        make_tuple(1, 'a', 1.f, 1l),
        make_tuple(1, 'a', 1.f, 2l)
    )
, "");

static_assert(
    tuple_cartesian_product(
        make_tuple(1),
        make_tuple('a'),
        make_tuple(1.f),
        make_tuple(1l, 2l),
        make_tuple(nullptr)
    ) == make_tuple(
        make_tuple(1, 'a', 1.f, 1l, nullptr),
        make_tuple(1, 'a', 1.f, 2l, nullptr)
    )
, "");

int main() { }