/*

lmdb output:
------------
key: oooo_pppppppp (o: object id, p: patch id)
data: floats: Channel  x  voxels  x  voxels (different from bin file)

*/

#include <patch_generator.h>
#include <highgui.h>
#include <fstream>
#include <sstream>
#include <glog/logging.h>
#include <boost/algorithm/string.hpp>

//yaw pitch roll --> rads!
void patch_generator::get_yaw_pitch_roll_from_rot_mat(const Eigen::Matrix4f &rot_mat, float &yaw,
                                                      float &pitch, float &roll){

    yaw = atan2(rot_mat(1, 0), rot_mat(0, 0));
    float a = sqrt(rot_mat(2, 1)*rot_mat(2, 1) + rot_mat(2, 2)*rot_mat(2, 2));
    pitch = atan2(-rot_mat(2,0), a);
    roll = atan2(rot_mat(2,1), rot_mat(2,2));

}

Eigen::Vector4f patch_generator::get_object_coords(int im_width, int im_height, int w, int h,
                                                   unsigned short depth, const Eigen::Matrix4f &rot_mat){

    float focal_length = getFocalLength(im_width, im_height);
    float cx = (float)im_width / 2.0f - 0.5f;
    float cy = (float)im_height / 2.0f - 0.5f;
    float z = (float)depth / 1000.0f;
    float x = ((float)w - cx)*z/focal_length;
    float y = ((float)h - cy)*z/focal_length;
    Eigen::Vector4f pos;
    pos << x, y, z, 1;

    //rotate rot_mat (obj_pose) 180 degrees around X
    //to match xtion coordinate system
    Eigen::Matrix4f corrMat;
    corrMat << 1,  0,  0, 0,
               0, -1,  0, 0,
               0,  0, -1, 0,
               0,  0,  0, 1;
    Eigen::Matrix4f xtion_rot_mat = corrMat * rot_mat;

    return xtion_rot_mat.inverse() * pos;

}

// NOT USED
void patch_generator::insert_patches_to_db(const std::vector<float> &patches,
                                           caffe::Datum &datum,
                                           int cur_obj, int &patch_id,
                                           const Eigen::Matrix4f &obj_pose,
                                           const cv::Mat &depth,
                                           const std::vector<int> patches_loc,
                                           std::ofstream &fannot)
{

    //patches needs to be in Channel - Row - Col

    const int kMaxKeyLength = 20;
    char key_cstr[kMaxKeyLength];

    float yaw, pitch, roll;
    get_yaw_pitch_roll_from_rot_mat(obj_pose, yaw, pitch, roll);

    int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 6;
    for(int i=0; i<num_patches; ++i){

        datum.clear_float_data();
        datum.clear_data();
        std::string* datum_string = datum.mutable_data();
        datum_string->resize(patch_size_in_voxels_ * patch_size_in_voxels_ * 6);
        int pos=0;
        for(int c=0; c<6; ++c){
            for(int row=0; row<patch_size_in_voxels_; ++row){
                for(int col=0; col<patch_size_in_voxels_; ++col){
                    int idx = i * patch_size_in_voxels_ * patch_size_in_voxels_ * 6 +
                              row * patch_size_in_voxels_ * 6 +
                              col * 6 +
                              c;

                    //surface normals are also quantized into 256 bins.
                    if(c<3)
                        (*datum_string)[pos++] = static_cast<unsigned char>(patches[idx] * 255.0f);
                    else
                        (*datum_string)[pos++] = static_cast<unsigned char>( (patches[idx]/2.0 + 0.5f) * 255.0f);

                }
            }
        }

        //write datum(=patch) to db
        snprintf(key_cstr, kMaxKeyLength, "%04d_%08d", cur_obj, patch_id++);
        std::string keystr(key_cstr);

        std::string value;
        datum.SerializeToString(&value);

        mdb_data.mv_size = value.size();
        mdb_data.mv_data = reinterpret_cast<void*>(&value[0]);
        mdb_key.mv_size = keystr.size();
        mdb_key.mv_data = reinterpret_cast<void*>(&keystr[0]);
        CHECK_EQ(mdb_put(mdb_txn, mdb_dbi, &mdb_key, &mdb_data, 0), MDB_SUCCESS)
            << "mdb_put failed";

        //write annotation file
        //get patch location in depth image
        int patch_center_x = patches_loc[i*2];
        int patch_center_y = patches_loc[i*2 + 1];
        unsigned short d = depth.at<unsigned short>(patch_center_y, patch_center_x);
        Eigen::Vector4f patch_coords = get_object_coords(depth.cols, depth.rows, patch_center_x, patch_center_y, d, obj_pose);
        fannot << keystr << " " <<
                  yaw    << " " <<
                  pitch  << " " <<
                  roll   << " " <<
                  patch_coords(0) << " " <<
                  patch_coords(1) << " " <<
                  patch_coords(2) << std::endl;

    }

}

// NOT USED
void patch_generator::generatePatches(){

    CHECK(input_object_folders_.size() != 0) << "No input objects specified";
    //CHECK_EQ(mkdir(output_folder_, 0744), 0) << "mkdir " << output_folder_ << "failed";

    //for bin files
    std::ofstream fout;
    //for lmdb
    caffe::Datum datum;
    datum.clear_data();

    //write info file
    std::string finfo_str;
    if(out_type_ == OUTPUT_BIN)
        finfo_str = output_folder_ + "/patch_info_bin.txt";
    else if(out_type_ == OUTPUT_LMDB)
        finfo_str = output_folder_ + "/patch_info_lmdb.txt";

    std::ofstream finfo(finfo_str.c_str());
    CHECK(finfo) << "Cannot open file " << finfo_str << " for writing.";

    //write annotation file
    std::string fannot_str;
    if(out_type_ == OUTPUT_BIN)
        fannot_str = output_folder_ + "/patch_annotation_bin.txt";
    else if(out_type_ == OUTPUT_LMDB)
        fannot_str = output_folder_ + "/patch_annotation_lmdb.txt";

    std::ofstream fannot(fannot_str.c_str());
    CHECK(fannot) << "Cannot open file " << fannot_str << " for writing.";

    //write number of classes
    fannot << input_object_folders_.size() << std::endl;

    if(out_type_ == OUTPUT_LMDB){

        LOG(INFO) << "Opening lmdb " << output_folder_;
        CHECK_EQ(mdb_env_create(&mdb_env), MDB_SUCCESS)
                << "mdb_env_create failed";
        CHECK_EQ(mdb_env_set_mapsize(mdb_env, 1099511627776), MDB_SUCCESS)   // 1TB
                << "mdb_env_set_mapsize failed";
        CHECK_EQ(mdb_env_open(mdb_env, output_folder_.c_str(), 0, 0664), MDB_SUCCESS)
                << "mdb_env_open failed";
        CHECK_EQ(mdb_txn_begin(mdb_env, NULL, 0, &mdb_txn), MDB_SUCCESS)
            << "mdb_txn_begin failed";
        CHECK_EQ(mdb_open(mdb_txn, NULL, 0, &mdb_dbi), MDB_SUCCESS)
            << "mdb_open failed. Does the lmdb already exist? ";

        datum.set_channels(6); //B,G,R,X,Y,Z
        datum.set_height(patch_size_in_voxels_);
        datum.set_width(patch_size_in_voxels_);

    }

    finfo << "Patch size in voxels: " << patch_size_in_voxels_ << std::endl;
    finfo << "Voxel size in m: " << voxel_size_in_m_ << std::endl;
    finfo << "Stride in pixels: " << stride_in_pixels_ << std::endl;

    int num_total_patches = 0;
    for(int cur_obj = 0; cur_obj < input_object_folders_.size(); ++cur_obj){

        int num_obj_patches = 0;
        //get object name from folder
        std::vector<std::string> folder_split;
        boost::split(folder_split, input_object_folders_[cur_obj], boost::is_any_of("/") );
        std::string cur_obj_str = folder_split.back();
        std::cout << "Creating patches for object: " << cur_obj_str << std::endl;

        //Initialize proper output - write config parameters
        if(out_type_ == OUTPUT_BIN){
            std::string cur_file = output_folder_ + "/" + cur_obj_str + ".bin";
            fout.open(cur_file.c_str(), std::ios::out | std::ios::binary);
            CHECK(fout) << "Output file " << (output_folder_ + "/" + cur_obj_str) << " cannot be openned.";
            fout.write((char*)&patch_size_in_voxels_, sizeof(int));
            fout.write((char*)&voxel_size_in_m_, sizeof(float));
        } else if(out_type_ == OUTPUT_LMDB) {
            datum.set_label(cur_obj);
        }

        int file_counter = 0;
        int patch_id = 0;
        while(true){

//            if(file_counter > 100)
//                break;

            //read files
            std::stringstream fname;
            fname << input_object_folders_[cur_obj] << "/rgb" << file_counter << ".png";
            cv::Mat rgb = cv::imread(fname.str());
            if(rgb.empty())
                break;
            //read depth
            fname.str("");
            fname << input_object_folders_[cur_obj] << "/depth" << file_counter << ".png";
            cv::Mat depth = cv::imread(fname.str(), CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
            CHECK(!depth.empty()) << "File " << fname.str() << " not exist, while the rgb file does.";
            //read normals
            fname.str("");
            fname << input_object_folders_[cur_obj] << "/surface_normals" << file_counter << ".bin";
            std::ifstream fnormals(fname.str().c_str(), std::ios::out | std::ios::binary);
            CHECK(fnormals) << "File " << fname.str() << " not exist, while the rgb and depth files does.";
            std::vector<float> normals(rgb.rows * rgb.cols * 3);
            fnormals.read(reinterpret_cast<char*>(&normals[0]), normals.size() * sizeof(float));
            //read pose
            fname.str("");
            fname << input_object_folders_[cur_obj] << "/pose" << file_counter << ".txt";
            std::ifstream fpose(fname.str().c_str());
            CHECK(fpose) << "File " << fname.str() << " not exist, while the rgb and depth files does.";
            Eigen::Matrix4f obj_pose;
            for(int row=0; row<4; ++row){
                for(int col=0; col<4; ++col){
                    float a;
                    fpose >> a;
                    obj_pose(row, col) = a;
                }
            }

            //create 3D texture vector Rows x Cols x 7 (B, G, R, D, (X, Y, Z)normals )
            std::vector<float> texture_3D(rgb.rows * rgb.cols * 7);
            int tex_pos = 0;
            int norm_pos = 0;
            for(int row=0; row<rgb.rows; ++row ){
                for(int col=0; col<rgb.cols; ++col){
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[0] / 255.0f;  //B
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[1] / 255.0f;  //G
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[2] / 255.0f;  //R
                    texture_3D[tex_pos++] = (float)depth.at<unsigned short>(row, col);       //D (in mm)
                    texture_3D[tex_pos++] = normals[norm_pos++];                             //X norm
                    texture_3D[tex_pos++] = normals[norm_pos++];                             //Y norm
                    texture_3D[tex_pos++] = normals[norm_pos++];                             //Z norm
                }
            }

            std::vector<float> patches;
            //patch locations - [x1, y1, x2, y2 .. ]
            std::vector<int> patches_loc;
            patch_extractor_gpu::extract_patches(texture_3D,
                                                 rgb.cols,
                                                 rgb.rows,
                                                 patch_size_in_voxels_,
                                                 voxel_size_in_m_,
                                                 stride_in_pixels_,
                                                 getFocalLength(rgb.cols, rgb.rows),
                                                 patches,
                                                 patches_loc,
                                                 generate_random_values_,
                                                 distance_threshold_);

            if(out_type_ == OUTPUT_BIN){
                fout.write((char*)&patches[0], patches.size() * sizeof(float));
            } else if(out_type_ == OUTPUT_LMDB){
                insert_patches_to_db(patches, datum, cur_obj, patch_id, obj_pose, depth, patches_loc, fannot);
                CHECK_EQ(mdb_txn_commit(mdb_txn), MDB_SUCCESS)
                    << "mdb_txn_commit failed";
                CHECK_EQ(mdb_txn_begin(mdb_env, NULL, 0, &mdb_txn), MDB_SUCCESS)
                    << "mdb_txn_begin failed";

            }

            int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 6;
            num_obj_patches += num_patches;

            file_counter++;
            std::cout << "Extracted patches from file: " << file_counter << "\r";

// visualize patches:
//
//            //int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 6;
//            int patches_pos = 0;
//            for(int i=0; i<num_patches; ++i){

//                cv::Mat patch_rgb = cv::Mat(patch_size_in_voxels_, patch_size_in_voxels_, CV_8UC3);
//                bool is_empty = true;
//                for(int row = 0; row<patch_size_in_voxels_; ++row){
//                    for(int col=0; col<patch_size_in_voxels_; ++col){

//                        patch_rgb.at<cv::Vec3b>(row,col)[0] = (uchar)(patches[patches_pos++]*255.0f);
//                        patch_rgb.at<cv::Vec3b>(row,col)[1] = (uchar)(patches[patches_pos++]*255.0f);
//                        patch_rgb.at<cv::Vec3b>(row,col)[2] = (uchar)(patches[patches_pos++]*255.0f);

//                        float x = patches[patches_pos++];
//                        float y = patches[patches_pos++];
//                        float z = patches[patches_pos++];

//                        if(x!=0 || y!=0 || z!= 0)
//                            is_empty = false;

//                    }
//                }

//                if(!is_empty){
//                    int k = -1;
//                    while(k == -1){
//                        cv::imshow("patch", patch_rgb);
//                        k = cv::waitKey(30);
//                    }
//                }
//                else{
//                    std::cout << "Got empty patch!" << std::endl;
//                }

//            }//patch iterator
// //////////////////////////////

        } // file iterator

        if(out_type_ == OUTPUT_BIN){
            fout.close();
        }

        num_total_patches += num_obj_patches;
        std::cout << std::endl;
        std::cout << "Patches for " << cur_obj_str << ": " << num_obj_patches << std::endl;
        finfo << "Patches for " << cur_obj_str << ": " << num_obj_patches << std::endl;
    } //object iterator

    if(out_type_ == OUTPUT_LMDB){
        mdb_close(mdb_env, mdb_dbi);
        mdb_env_close(mdb_env);
    }

    std::cout << "Finished! Total patches: " << num_total_patches << std::endl;
    finfo << "Total patches: " << num_total_patches << std::endl;
    finfo.close();
    fannot.close();
} //generatePatches

void patch_generator::insert_patches_to_db_rgbd(const std::vector<float> &patches,
                                           caffe::Datum &datum,
                                           int cur_obj, int &patch_id,
                                           const Eigen::Matrix4f &obj_pose,
                                           const cv::Mat &depth,
                                           const std::vector<int> patches_loc,
                                           std::ofstream &fannot)
{

    //patches needs to be in Channel - Row - Col

    const int kMaxKeyLength = 20;
    char key_cstr[kMaxKeyLength];

    float yaw, pitch, roll;
    get_yaw_pitch_roll_from_rot_mat(obj_pose, yaw, pitch, roll);

    int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 4;
    for(int i=0; i<num_patches; ++i){

//        datum.clear_float_data();
//        datum.clear_data();
//        std::string* datum_string = datum.mutable_data();
//        datum_string->resize(patch_size_in_voxels_ * patch_size_in_voxels_ * 4);
//        int pos=0;
//        for(int c=0; c<4; ++c){
//            for(int row=0; row<patch_size_in_voxels_; ++row){
//                for(int col=0; col<patch_size_in_voxels_; ++col){
//                    int idx = i * patch_size_in_voxels_ * patch_size_in_voxels_ * 4 +
//                              row * patch_size_in_voxels_ * 4 +
//                              col * 4 +
//                              c;

//                    (*datum_string)[pos++] = static_cast<unsigned char>(patches[idx] * 255.0f);

//                }
//            }
//        }

        //get mean
        std::vector<float> buf(patch_size_in_voxels_ * patch_size_in_voxels_ * 4);
        int pos=0;
        float mean_rgb = 0;
        float mean_depth = 0;
        for(int c=0; c<4; ++c){
            for(int row=0; row<patch_size_in_voxels_; ++row){
                for(int col=0; col<patch_size_in_voxels_; ++col){
                    int idx = i * patch_size_in_voxels_ * patch_size_in_voxels_ * 4 +
                              row * patch_size_in_voxels_ * 4 +
                              col * 4 +
                              c;

                    buf[pos] = patches[idx];
                    if(c < 3)
                        mean_rgb += buf[pos] / (patch_size_in_voxels_ * patch_size_in_voxels_ * 3);
                    else
                        mean_depth += buf[pos] / (patch_size_in_voxels_ * patch_size_in_voxels_);
                    pos++;

                }
            }
        }

        //get std
        float std_rgb = 0;
        float std_depth = 0;
        for(int j=0; j<buf.size(); ++j){
            if(j < patch_size_in_voxels_ * patch_size_in_voxels_ * 3)
                std_rgb += pow(buf[j] - mean_rgb, 2)/(patch_size_in_voxels_ * patch_size_in_voxels_ * 3);
             else
                std_depth += pow(buf[j] - mean_depth, 2)/(patch_size_in_voxels_ * patch_size_in_voxels_);
        }

        for(int j=0; j<buf.size(); ++j){
            //Truncate to +/-3 standard deviations and scale to -1 to 1
            if(j < patch_size_in_voxels_ * patch_size_in_voxels_ * 3){
                buf[j] -= mean_rgb;
                if(buf[j] > 3*std_rgb)
                    buf[j] = 3*std_rgb;
                if(buf[j] < -3*std_rgb)
                    buf[j] = -3*std_rgb;
                buf[j] /= 3*std_rgb;
            }
            else{
                buf[j] -= mean_depth;
                if(buf[j] > 3*std_depth)
                    buf[j] = 3*std_depth;
                if(buf[j] < -3*std_depth)
                    buf[j] = -3*std_depth;
                buf[j] /= 3*std_depth;

            }

            // Rescale from [-1,1] to [0.1,0.9]
            buf[j] = (buf[j] + 1) * 0.4f + 0.1f;
        }

        datum.clear_float_data();
        datum.clear_data();
        std::string* datum_string = datum.mutable_data();
        datum_string->resize(patch_size_in_voxels_ * patch_size_in_voxels_ * 4);
        //convert to char for caffe
        for(int j=0; j<buf.size(); ++j)
            (*datum_string)[j] = static_cast<unsigned char>(buf[j] * 255.0f);

        //write datum(=patch) to db
        snprintf(key_cstr, kMaxKeyLength, "%04d_%08d", cur_obj, patch_id++);
        std::string keystr(key_cstr);

        std::string value;
        datum.SerializeToString(&value);

        mdb_data.mv_size = value.size();
        mdb_data.mv_data = reinterpret_cast<void*>(&value[0]);
        mdb_key.mv_size = keystr.size();
        mdb_key.mv_data = reinterpret_cast<void*>(&keystr[0]);
        CHECK_EQ(mdb_put(mdb_txn, mdb_dbi, &mdb_key, &mdb_data, 0), MDB_SUCCESS)
            << "mdb_put failed";

        //write annotation file
        //get patch location in depth image
        int patch_center_x = patches_loc[i*2];
        int patch_center_y = patches_loc[i*2 + 1];
        unsigned short d = depth.at<unsigned short>(patch_center_y, patch_center_x);
        Eigen::Vector4f patch_coords = get_object_coords(depth.cols, depth.rows, patch_center_x, patch_center_y, d, obj_pose);
        fannot << keystr << " " <<
                  yaw    << " " <<
                  pitch  << " " <<
                  roll   << " " <<
                  patch_coords(0) << " " <<
                  patch_coords(1) << " " <<
                  patch_coords(2) << std::endl;

    }

}

void patch_generator::generatePatches_rgbd(){

    CHECK(input_object_folders_.size() != 0) << "No input objects specified";
    //CHECK_EQ(mkdir(output_folder_, 0744), 0) << "mkdir " << output_folder_ << "failed";

    //for bin files
    std::ofstream fout;
    //for lmdb
    caffe::Datum datum;
    datum.clear_data();

    //write info file
    std::string finfo_str;
    if(out_type_ == OUTPUT_BIN)
        finfo_str = output_folder_ + "/patch_info_bin.txt";
    else if(out_type_ == OUTPUT_LMDB)
        finfo_str = output_folder_ + "/patch_info_lmdb.txt";

    std::ofstream finfo(finfo_str.c_str());
    CHECK(finfo) << "Cannot open file " << finfo_str << " for writing.";

    //write annotation file
    std::string fannot_str;
    if(out_type_ == OUTPUT_BIN)
        fannot_str = output_folder_ + "/patch_annotation_bin.txt";
    else if(out_type_ == OUTPUT_LMDB)
        fannot_str = output_folder_ + "/patch_annotation_lmdb.txt";

    std::ofstream fannot(fannot_str.c_str());
    CHECK(fannot) << "Cannot open file " << fannot_str << " for writing.";

    //write number of classes
    fannot << input_object_folders_.size() << std::endl;

    if(out_type_ == OUTPUT_LMDB){

        LOG(INFO) << "Opening lmdb " << output_folder_;
        CHECK_EQ(mdb_env_create(&mdb_env), MDB_SUCCESS)
                << "mdb_env_create failed";
        CHECK_EQ(mdb_env_set_mapsize(mdb_env, 1099511627776), MDB_SUCCESS)   // 1TB
                << "mdb_env_set_mapsize failed";
        CHECK_EQ(mdb_env_open(mdb_env, output_folder_.c_str(), 0, 0664), MDB_SUCCESS)
                << "mdb_env_open failed";
        CHECK_EQ(mdb_txn_begin(mdb_env, NULL, 0, &mdb_txn), MDB_SUCCESS)
            << "mdb_txn_begin failed";
        CHECK_EQ(mdb_open(mdb_txn, NULL, 0, &mdb_dbi), MDB_SUCCESS)
            << "mdb_open failed. Does the lmdb already exist? ";

        datum.set_channels(4); //B,G,R,D_trunc
        datum.set_height(patch_size_in_voxels_);
        datum.set_width(patch_size_in_voxels_);

    }

    finfo << "Patch size in voxels: " << patch_size_in_voxels_ << std::endl;
    finfo << "Voxel size in m: " << voxel_size_in_m_ << std::endl;
    finfo << "Stride in pixels: " << stride_in_pixels_ << std::endl;
    finfo << "Max Depth Range: " << max_depth_range_in_m_ << std::endl;

    int num_total_patches = 0;
    for(int cur_obj = 0; cur_obj < input_object_folders_.size(); ++cur_obj){

        int num_obj_patches = 0;
        //get object name from folder
        std::vector<std::string> folder_split;
        boost::split(folder_split, input_object_folders_[cur_obj], boost::is_any_of("/") );
        std::string cur_obj_str = folder_split.back();
        std::cout << "Creating patches for object: " << cur_obj_str << std::endl;

        //Initialize proper output - write config parameters
        if(out_type_ == OUTPUT_BIN){
            std::string cur_file = output_folder_ + "/" + cur_obj_str + ".bin";
            fout.open(cur_file.c_str(), std::ios::out | std::ios::binary);
            CHECK(fout) << "Output file " << (output_folder_ + "/" + cur_obj_str) << " cannot be openned.";
            fout.write((char*)&patch_size_in_voxels_, sizeof(int));
            fout.write((char*)&voxel_size_in_m_, sizeof(float));
            fout.write((char*)&max_depth_range_in_m_, sizeof(float));
        } else if(out_type_ == OUTPUT_LMDB) {
            datum.set_label(cur_obj);
        }

        int file_counter = 0;
        int patch_id = 0;
        while(true){

//            if(file_counter > 100)
//                break;

            //read files
            std::stringstream fname;
            fname << input_object_folders_[cur_obj] << "/rgb" << file_counter << ".png";
            cv::Mat rgb = cv::imread(fname.str());
            if(rgb.empty())
                break;
            //read depth
            fname.str("");
            fname << input_object_folders_[cur_obj] << "/depth" << file_counter << ".png";
            cv::Mat depth = cv::imread(fname.str(), CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
            CHECK(!depth.empty()) << "File " << fname.str() << " not exist, while the rgb file does.";
            //read pose
            fname.str("");
            fname << input_object_folders_[cur_obj] << "/pose" << file_counter << ".txt";
            std::ifstream fpose(fname.str().c_str());
            CHECK(fpose) << "File " << fname.str() << " not exist, while the rgb and depth files does.";
            Eigen::Matrix4f obj_pose;
            for(int row=0; row<4; ++row){
                for(int col=0; col<4; ++col){
                    float a;
                    fpose >> a;
                    obj_pose(row, col) = a;
                }
            }

            //create 3D texture vector Rows x Cols x 4 (B, G, R, D )
            std::vector<float> texture_3D(rgb.rows * rgb.cols * 4);
            int tex_pos = 0;
            for(int row=0; row<rgb.rows; ++row ){
                for(int col=0; col<rgb.cols; ++col){
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[0] / 255.0f;  //B
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[1] / 255.0f;  //G
                    texture_3D[tex_pos++] = (float)rgb.at<cv::Vec3b>(row, col)[2] / 255.0f;  //R
                    texture_3D[tex_pos++] = (float)depth.at<unsigned short>(row, col);       //D (in mm)
                }
            }

            std::vector<float> patches;
            //patch locations - [x1, y1, x2, y2 .. ]
            std::vector<int> patches_loc;
            patch_extractor_gpu::extract_patches_rgbd(texture_3D,
                                                 rgb.cols,
                                                 rgb.rows,
                                                 patch_size_in_voxels_,
                                                 voxel_size_in_m_,
                                                 max_depth_range_in_m_,
                                                 stride_in_pixels_,
                                                 percent_,
                                                 getFocalLength(rgb.cols, rgb.rows),
                                                 patches,
                                                 patches_loc,
                                                 generate_random_values_,
                                                 distance_threshold_);

            if(out_type_ == OUTPUT_BIN){
                fout.write((char*)&patches[0], patches.size() * sizeof(float));
            } else if(out_type_ == OUTPUT_LMDB){
                insert_patches_to_db_rgbd(patches, datum, cur_obj, patch_id, obj_pose, depth, patches_loc, fannot);
                CHECK_EQ(mdb_txn_commit(mdb_txn), MDB_SUCCESS)
                    << "mdb_txn_commit failed";
                CHECK_EQ(mdb_txn_begin(mdb_env, NULL, 0, &mdb_txn), MDB_SUCCESS)
                    << "mdb_txn_begin failed";

            }

            int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 4;
            num_obj_patches += num_patches;

            file_counter++;
            std::cout << "Extracted patches from file: " << file_counter << "\r";

// visualize patches:
//
            //int num_patches = patches.size() / patch_size_in_voxels_ / patch_size_in_voxels_ / 4;
//            int patches_pos = 0;
//            for(int i=0; i<num_patches; ++i){

//                cv::Mat patch_rgb = cv::Mat(patch_size_in_voxels_, patch_size_in_voxels_, CV_8UC3);
//                cv::Mat patch_depth = cv::Mat(patch_size_in_voxels_, patch_size_in_voxels_, CV_32FC1);
//                for(int row = 0; row<patch_size_in_voxels_; ++row){
//                    for(int col=0; col<patch_size_in_voxels_; ++col){

//                        patch_rgb.at<cv::Vec3b>(row,col)[0] = (uchar)(patches[patches_pos++]*255.0f);
//                        patch_rgb.at<cv::Vec3b>(row,col)[1] = (uchar)(patches[patches_pos++]*255.0f);
//                        patch_rgb.at<cv::Vec3b>(row,col)[2] = (uchar)(patches[patches_pos++]*255.0f);
//                        patch_depth.at<float>(row, col) = patches[patches_pos++];

//                    }
//                }

//                cv::resize(patch_rgb, patch_rgb, cv::Size(100, 100));
//                cv::resize(patch_depth, patch_depth, cv::Size(100, 100));

//                int k = -1;
//                while(k == -1){
//                    cv::imshow("patch_rgb", patch_rgb);
//                    cv::imshow("patch_depth", patch_depth);
//                    k = cv::waitKey(30);
//                }

//            }//patch iterator
// //////////////////////////

        } // file iterator

        if(out_type_ == OUTPUT_BIN){
            fout.close();
        }

        num_total_patches += num_obj_patches;
        std::cout << std::endl;
        std::cout << "Patches for " << cur_obj_str << ": " << num_obj_patches << std::endl;
        finfo << "Patches for " << cur_obj_str << ": " << num_obj_patches << std::endl;
    } //object iterator

    if(out_type_ == OUTPUT_LMDB){
        mdb_close(mdb_env, mdb_dbi);
        mdb_env_close(mdb_env);
    }

    std::cout << "Finished! Total patches: " << num_total_patches << std::endl;
    finfo << "Total patches: " << num_total_patches << std::endl;
    finfo.close();
    fannot.close();
} //generatePatches


// Shasta.
#include "ConflictReadGraph.hpp"
using namespace shasta;

// Boost libraries.
#include <boost/container/flat_set.hpp>

// Standard library.
#include <map>
#include "fstream.hpp"

/*******************************************************************************

See this Wikipedia article for greedy coloring in general:
https://en.wikipedia.org/wiki/Greedy_coloring

This code uses the Brelaz (1979) Dsatur
algorithm described there in this section
https://en.wikipedia.org/wiki/Greedy_coloring#Adaptive

The main Wikipedia article for this algorithm is here:
https://en.wikipedia.org/wiki/DSatur

Brelaz, Daniel (April 1979),
"New methods to color the vertices of a graph",
Communications of the ACM, 22 (4): 251–256, doi:10.1145/359094.359101

Many alternative approximate coloring methods are also available, for example
https://www.gerad.ca/~alainh/RLFPaper.pdf

For a partial coloring of a graph, the Dsatur algorithm defines
the saturation degree of a vertex as the number of distinct colors
to which the vertex is adjacent (only counting, of course,
vertices that have already been colored).
With this definition, the Dsatur algorithm is simply described
as follows:

- Starting with the uncolored graph, iterate over vertices in the order
  defined below.
- At each iteration, assign to each vertex the lowest possible color.
  This is the lowest color that the vertex is not adjacent to.
- At each iteration choose the vertex with maximum saturation degree
  (maximum number of adjacent colors). In case of ties, break
  the ties by selecting the vertex with maximum degree in the uncolored
  subgraph - that is, the vertex with the greatest number of uncolored
  adjacent vertices.

The DSatur algoritm is an approximate coloring algorithm, but it
is exact for 2-colorable graphs.

This function applies the above algorithm to the specified set of vertices,
which are assumed to be a connected component of the graph
and stored in sorted order.

*******************************************************************************/

void ConflictReadGraph::colorConnectedComponent(const vector<VertexId>& component)
{
    SHASTA_ASSERT(std::is_sorted(component.begin(), component.end()));

    // A LocalVertexId is an index into the component vector.
    using Int = VertexId;

    // The number of vertices in this connected component.
    const Int n = component.size();
    SHASTA_ASSERT(n > 1);

    // Vector to contain the number of colored vertices adjacent to
    // each uncolored vertex. This is used to break ties in the Dsatur algorithm.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    vector<Int> adjacentColoredCount(n, 0);

    // The colors adjacent to each uncolored vertex. The number of colors adjacent
    // to each vertex is small, so we use boost::container::flat_set
    // instead of std::set.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    using boost::container::flat_set;
    vector< flat_set<Int> > adjacentColors(n);

    // A multimap that stores uncolored vertices (as local vertex ids)
    // keyed by saturation degree (that is, the number of colors adjacent to
    // each vertex), in decreasing order.
    // This is used to select, at each iteration, the next vertex to be colored.
    using MultiMap = std::multimap<Int, Int, std::greater<Int> >;
    MultiMap verticesBySaturationDegree;
    for(Int v=0; v<n; v++) {
        verticesBySaturationDegree.insert(make_pair(0, v));
    }

    // Make sure all vertices have no color.
    for(Int v=0; v<n; v++) {
        getVertex(component[v]).componentId = ConflictReadGraphVertex::invalid;
        getVertex(component[v]).color = ConflictReadGraphVertex::invalid;
    }

    // Main iteration loop. At each iteration we color one vertex.
    for(Int iteration=0; iteration<n; iteration++) {
        // cout << "Iteration " << iteration << " begins." << endl;

        // To choose the vertex to color at this iteration, loop over
        // all uncolored vertices with maximum saturation degree.
        SHASTA_ASSERT(not verticesBySaturationDegree.empty());
        const Int highestSaturationDegree = verticesBySaturationDegree.begin()->first;
        MultiMap::iterator begin, end;
        tie(begin, end) = verticesBySaturationDegree.equal_range(highestSaturationDegree);
        SHASTA_ASSERT(begin != end);
        Int v0 = begin->second;
        Int adjacentColoredCount0 = adjacentColoredCount[v0];
        MultiMap::iterator it0 = begin;
        for(MultiMap::iterator it=begin; it!=end; ++it) {
            const Int v1 = it->second;
            if(adjacentColoredCount[v1] > adjacentColoredCount0) {
                v0 = v1;
                adjacentColoredCount0 = adjacentColoredCount[v1];
                it0 = it;
            }
        }

        // Remove v0 from verticesBySaturationDegree.
        verticesBySaturationDegree.erase(it0);

        // This iteration is coloring LocalVertexId v0.
        // We choose the lowest color that is not present in adjacentColors[v0].
        const auto& adjacentColors0 = adjacentColors[v0];
        Int color0 = ConflictReadGraphVertex::invalid;
        for(Int color=0; ; color++) {
            if(adjacentColors0.find(color) == adjacentColors0.end()) {
                color0 = color;
                break;
            }
        }

        // Color v0 with the color chosen in this way.
        const VertexId u0 = component[v0];
        // cout << "Iteration " << iteration << " colored vertex " << v0 << " " << getOrientedReadId(u0) <<
        //     " color " << color0 << endl;
        ConflictReadGraphVertex& vertex0 = getVertex(u0);
        SHASTA_ASSERT(vertex0.color = ConflictReadGraphVertex::invalid);
        vertex0.color = uint32_t(color0);

        // Increment adjacentColoredCount for adjacent uncolored vertices.
        for(EdgeId edgeId: incidentEdges(u0)) {
            const VertexId u1 = otherVertex(edgeId, u0);
            const ConflictReadGraphVertex& vertex1 = getVertex(u1);
            if(vertex1.color == ConflictReadGraphVertex::invalid) {
                const auto it = lower_bound(component.begin(), component.end(), u1);
                SHASTA_ASSERT(*it == u1);
                const Int v1 = it - component.begin();
                adjacentColoredCount[v1]++;
            }
        }
        // cout << "***A" << endl;

        // Update adjacentColors for adjacent uncolored vertices, and
        // update verticesBySaturationDegree accordingly.
        for(EdgeId edgeId: incidentEdges(u0)) {
            const VertexId u1 = otherVertex(edgeId, u0);
            const ConflictReadGraphVertex& vertex1 = getVertex(u1);
            if(vertex1.color == ConflictReadGraphVertex::invalid) {
                const auto it = lower_bound(component.begin(), component.end(), u1);
                SHASTA_ASSERT(*it == u1);
                const Int v1 = it - component.begin();
                auto& adjacentColors1 = adjacentColors[v1];
                if(adjacentColors1.find(color0) == adjacentColors1.end()) {
                    adjacentColors1.insert(color0);

                    // The saturation degree of this vertex has increased.
                    MultiMap::iterator begin, end;
                    tie(begin, end) = verticesBySaturationDegree.equal_range(adjacentColors1.size()-1);
                    SHASTA_ASSERT(begin != end);
                    bool done = false;
                    for(MultiMap::iterator it=begin; it!=end; ++it) {
                        if(it->second == v1) {
                            verticesBySaturationDegree.erase(it);
                            verticesBySaturationDegree.insert(make_pair(adjacentColors1.size(), v1));
                            done = true;
                            break;
                        }
                    }
                    SHASTA_ASSERT(done);
                }
            }
        }
        // cout << "***B" << endl;

    }

    // Find the number of colors used.
    uint32_t maxColor = 0;
    for(Int v=0; v<n; v++) {
        maxColor = max(maxColor, getVertex(component[v]).color);
    }
    cout << "Used " << maxColor+1 << " colors." << endl;

}

void ConflictReadGraph::writeGraphviz(const string& fileName) const
{
    ofstream s(fileName);
    writeGraphviz(s);
}
void ConflictReadGraph::writeGraphviz(ostream& s) const
{
    s <<
        "graph G {\n" <<
        "node [shape=point];\n";

    // Write the vertices.
    for(VertexId v=0; v<vertices.size(); v++) {
        const OrientedReadId orientedReadId = getOrientedReadId(v);
        s << v << "[tooltip=\"" << orientedReadId << "\"];\n";
    }

    // Write the edges.
    for(EdgeId e=0; e<edges.size(); e++) {
        s << v0(e) << "--" << v1(e) << ";\n";
    }

    s << "}\n";
}


#define BOOST_TEST_MODULE "test_dynamic_vector"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../src/DynamicVector.hpp"
using VectorDd = ax::Vector<double, ax::DYNAMIC>;

#include "test_Defs.hpp"
using ax::test::seed;
using ax::test::tolerance;

#include <random>

BOOST_AUTO_TEST_CASE(VectorNd_Constructable)
{
    const VectorDd vec(10);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec[i], 0e0);

    const VectorDd vec_1(10, 1e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_1[i], 1e0);

    const VectorDd vec_cp_0(vec_1);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_cp_0[i], 1e0);

    const VectorDd vec_cp_1 = vec_1;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_cp_1[i], 1e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Add)
{
    const VectorDd vec1(10, 1e0);
    const VectorDd vec2(10, 2e0);
    const VectorDd vec3(vec1 + vec2);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 3e0);

    VectorDd vec4;
    vec4 = vec1 + vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 3e0);

    VectorDd vec5(10);
    vec5 += vec1;
    vec5 += vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 3e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Sub)
{
    const VectorDd vec1(10, 3e0);
    const VectorDd vec2(10, 2e0);
    const VectorDd vec3(vec1 - vec2);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 1e0);

    VectorDd vec4;
    vec4 = vec1 - vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 1e0);

    VectorDd vec5(vec1);
    vec5 -= vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 1e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Scalar_multiple)
{
    const VectorDd vec1(10, 1e0);
    const VectorDd vec2(2e0 * vec1);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec2[i], 2e0);

    const VectorDd vec3(vec1 * 2e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 2e0);

    VectorDd vec4;
    vec4 = vec1 * 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 2e0);

    vec4 = 2e0 * vec1;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 2e0);

    VectorDd vec5(vec1);
    vec5 *= 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 2e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Scalar_division)
{
    const VectorDd vec1(10, 2e0);
    const VectorDd vec2(vec1 / 2e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec2[i], 1e0);

    VectorDd vec3;
    vec3 = vec1 / 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 1e0);

    VectorDd vec4(vec1);
    vec4 /= 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 1e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_dot_product)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    for(auto i=0; i<100; ++i)
    {
        std::vector<double> v1(10);
        for(std::size_t i=0; i<10; ++i)
            v1.at(i) = randreal(mt);

        std::vector<double> v2(10);
        for(std::size_t i=0; i<10; ++i)
            v2.at(i) = randreal(mt);

        const VectorDd vec1(v1);
        const VectorDd vec2(v2);

        double dot_product = 0e0;
        for(std::size_t i=0; i<10; ++i)
            dot_product += v1.at(i) * v2.at(i);

        BOOST_CHECK_CLOSE_FRACTION(dot_prod(vec1, vec2), dot_product, tolerance);
    }
}

BOOST_AUTO_TEST_CASE(VectorNd_length)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    for(auto i=0; i<100; ++i)
    {
        std::array<double, 10> v1;
        for(std::size_t i=0; i<10; ++i)
            v1.at(i) = randreal(mt);

        const VectorDd vec1(v1);

        double lensq = 0e0;
        for(std::size_t i=0; i<10; ++i)
            lensq += v1.at(i) * v1.at(i);

        BOOST_CHECK_CLOSE_FRACTION(len_square(vec1), lensq, tolerance);
        BOOST_CHECK_CLOSE_FRACTION(length(vec1), std::sqrt(lensq), tolerance);

        BOOST_CHECK_CLOSE_FRACTION(len_square(vec1), dot_prod(vec1, vec1), tolerance);
    }
}


// Boost.uBLAS
//
// Copyright (c) 2018 Fady Essam
// Copyright (c) 2018 Stefan Seefeld
//
// 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)

#ifndef boost_numeric_ublas_opencl_transpose_hpp_
#define boost_numeric_ublas_opencl_transpose_hpp_

#include <boost/numeric/ublas/opencl/library.hpp>
#include <boost/numeric/ublas/opencl/vector.hpp>
#include <boost/numeric/ublas/opencl/matrix.hpp>

// Kernel for transposition of various data types
#define OPENCL_TRANSPOSITION_KERNEL(DATA_TYPE)	\
"__kernel void transpose(__global "  #DATA_TYPE "* in, __global " #DATA_TYPE "* result, unsigned int width, unsigned int height) \n"                       \
"{ \n"								        \
"  unsigned int column_index = get_global_id(0); \n"			\
"  unsigned int row_index = get_global_id(1); \n"			\
"  if (column_index < width && row_index < height) \n"			\
"  { \n"							      	\
"    unsigned int index_in = column_index + width * row_index; \n"	\
"    unsigned int index_result = row_index + height * column_index; \n"	\
"    result[index_result] = in[index_in]; \n"				\
"  } \n"								\
"} \n"

namespace boost { namespace numeric { namespace ublas { namespace opencl {

template<class T, class L1, class L2>
typename std::enable_if<is_numeric<T>::value>::type
change_layout(ublas::matrix<T, L1, opencl::storage> const &m,
	      ublas::matrix<T, L2, opencl::storage> &result,
	      compute::command_queue& queue)
{
  assert(m.size1() == result.size1() && m.size2() == result.size2());
  assert(m.device() == result.device() && m.device() == queue.get_device());
  assert(!(std::is_same<L1, L2>::value));
  char const *kernel;
  if (std::is_same<T, float>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(float);
  else if (std::is_same<T, double>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(double);
  else if (std::is_same<T, std::complex<float>>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(float2);
  else if (std::is_same<T, std::complex<double>>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(double2);
  size_t len = strlen(kernel);
  cl_int err;
  cl_context c_context = queue.get_context().get();
  cl_program program = clCreateProgramWithSource(c_context, 1, &kernel, &len, &err);
  clBuildProgram(program, 1, &queue.get_device().get(), NULL, NULL, NULL);
  cl_kernel c_kernel = clCreateKernel(program, "transpose", &err);
  size_t width = std::is_same < L1, ublas::basic_row_major<>>::value ? m.size2() : m.size1();
  size_t height = std::is_same < L1, ublas::basic_row_major<>>::value ? m.size1() : m.size2();
  size_t global_size[2] = { width , height };
  clSetKernelArg(c_kernel, 0, sizeof(T*), &m.begin().get_buffer().get());
  clSetKernelArg(c_kernel, 1, sizeof(T*), &result.begin().get_buffer().get());
  clSetKernelArg(c_kernel, 2, sizeof(unsigned int), &width);
  clSetKernelArg(c_kernel, 3, sizeof(unsigned int), &height);
  cl_command_queue c_queue = queue.get();
  cl_event event = NULL;
  clEnqueueNDRangeKernel(c_queue, c_kernel, 2, NULL, global_size, NULL, 0, NULL, &event);
  clWaitForEvents(1, &event);
}

template<class T, class L1, class L2, class A>
typename std::enable_if<is_numeric<T>::value>::type
change_layout(ublas::matrix<T, L1, A> const &m,
	      ublas::matrix<T, L2, A> &result,
	      compute::command_queue& queue)
{
  ublas::matrix<T, L1, opencl::storage> mdev(m, queue);
  ublas::matrix<T, L2, opencl::storage> rdev(result.size1(), result.size2(), queue.get_context());
  change_layout(mdev, rdev, queue);
  rdev.to_host(result, queue);
}

template<class T, class L>
typename std::enable_if<is_numeric<T>::value>::type
trans(ublas::matrix<T, L, opencl::storage> const &m,
      ublas::matrix<T, L, opencl::storage> &result,
      compute::command_queue& queue)
{
  assert(m.size1() == result.size2() && m.size2() == result.size1());
  assert(m.device() == result.device() && m.device() == queue.get_device());
  char const *kernel;
  if (std::is_same<T, float>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(float);
  else if (std::is_same<T, double>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(double);
  else if (std::is_same<T, std::complex<float>>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(float2);
  else if (std::is_same<T, std::complex<double>>::value)
    kernel = OPENCL_TRANSPOSITION_KERNEL(double2);
  size_t len = strlen(kernel);
  cl_int err;
  cl_context c_context = queue.get_context().get();
  cl_program program = clCreateProgramWithSource(c_context, 1, &kernel, &len, &err);
  clBuildProgram(program, 1, &queue.get_device().get(), NULL, NULL, NULL);
  cl_kernel c_kernel = clCreateKernel(program, "transpose", &err);
  size_t width = std::is_same <L, ublas::basic_row_major<>>::value ? m.size2() : m.size1();
  size_t height = std::is_same <L, ublas::basic_row_major<>>::value ? m.size1() : m.size2();
  size_t global_size[2] = { width , height };
  clSetKernelArg(c_kernel, 0, sizeof(T*), &m.begin().get_buffer().get());
  clSetKernelArg(c_kernel, 1, sizeof(T*), &result.begin().get_buffer().get());
  clSetKernelArg(c_kernel, 2, sizeof(unsigned int), &width);
  clSetKernelArg(c_kernel, 3, sizeof(unsigned int), &height);
  cl_command_queue c_queue = queue.get();
  cl_event event = NULL;
  clEnqueueNDRangeKernel(c_queue, c_kernel, 2, NULL, global_size, NULL, 0, NULL, &event);
  clWaitForEvents(1, &event);
}

template<class T, class L, class A>
typename std::enable_if<is_numeric<T>::value>::type
trans(ublas::matrix<T, L, A> const &m,
      ublas::matrix<T, L, A> &result,
      compute::command_queue& queue)
{
  ublas::matrix<T, L, opencl::storage> mdev(m, queue);
  ublas::matrix<T, L, opencl::storage> rdev(result.size1(), result.size2(), queue.get_context());
  trans(mdev, rdev, queue);
  rdev.to_host(result, queue);
}

template<class T, class L, class A>
typename std::enable_if<is_numeric<T>::value, ublas::matrix<T, L, A>>::type
trans(ublas::matrix<T, L, A>& m, compute::command_queue& queue)
{
  ublas::matrix<T, L, A> result(m.size2(), m.size1());
  trans(m, result, queue);
  return result;
}

}}}}

#endif


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

void matrixCondition()
{
    //https://www.youtube.com/watch?v=GDYmWtWWtNQ
    //https://stackoverflow.com/questions/33575478/how-can-you-find-the-condition-number-in-eigen/33577450
    //https://math.stackexchange.com/questions/2817630/why-is-the-condition-number-of-a-matrix-given-by-these-eigenvalues

}

int main()
{

}


// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_TEST_LINEARELLIPTIC_SWIPDG_HH
#define DUNE_HDD_TEST_LINEARELLIPTIC_SWIPDG_HH

#include <algorithm>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/localization-study.hh>

#include <dune/gdt/products/l2.hh>
#include <dune/gdt/products/h1.hh>
#include <dune/gdt/products/elliptic.hh>
#include <dune/gdt/playground/spaces/finitevolume/default.hh>
#include <dune/gdt/discretefunction/default.hh>

#include <dune/hdd/linearelliptic/discretizations/swipdg.hh>
#include <dune/hdd/linearelliptic/estimators/swipdg.hh>
#include <dune/hdd/linearelliptic/testcases/ESV2007.hh>
#include <dune/hdd/linearelliptic/testcases/OS2014.hh>

#include "linearelliptic.hh"
#include "linearelliptic-swipdg-expectations.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Tests {
namespace internal {

template< class TestCaseType, int polOrder, GDT::ChooseSpaceBackend space_backend, Stuff::LA::ChooseBackend la_backend >
class DiscretizationSWIPDG
{
  typedef typename TestCaseType::GridType GridType;
  typedef typename TestCaseType::RangeFieldType RangeFieldType;
  static const unsigned int dimRange = TestCaseType::dimRange;
public:
  typedef Discretizations::SWIPDG
      < GridType, Stuff::Grid::ChooseLayer::level, RangeFieldType, dimRange, polOrder, space_backend, la_backend > Type;
  typedef Estimators::SWIPDG
      < typename Type::AnsatzSpaceType, typename Type::VectorType, typename Type::ProblemType, GridType > EstimatorType;
}; // class DiscretizationSWIPDG

} // namespace internal

template< class TestCaseType,
          int polOrder = 1,
#if HAVE_DUNE_FEM
          GDT::ChooseSpaceBackend space_backend = GDT::ChooseSpaceBackend::fem,
#else
# error No space backend available!
#endif
          Stuff::LA::ChooseBackend la_backend = Stuff::LA::default_sparse_backend >
class SWIPDGStudy
  : public EocStudyBase< TestCaseType,
                         typename internal::DiscretizationSWIPDG< TestCaseType,
                                                                  polOrder,
                                                                  space_backend,
                                                                  la_backend >::Type >
  , public Stuff::Common::LocalizationStudy
{
  typedef EocStudyBase< TestCaseType,
                        typename internal::DiscretizationSWIPDG< TestCaseType,
                                                                 polOrder,
                                                                 space_backend,
                                                                 la_backend >::Type > StudyBaseType;
  typedef Stuff::Common::LocalizationStudy                                            LocalizationBaseType;
  typedef SWIPDGStudy< TestCaseType, polOrder, space_backend, la_backend >            ThisType;

  typedef typename StudyBaseType::DiscretizationType DiscretizationType;
  typedef typename internal::DiscretizationSWIPDG< TestCaseType, polOrder, space_backend, la_backend >::EstimatorType
                                                     EstimatorType;
  typedef typename StudyBaseType::GridViewType GridViewType;
  typedef typename StudyBaseType::FunctionType FunctionType;
  typedef typename StudyBaseType::VectorType   VectorType;

public:
  SWIPDGStudy(const TestCaseType& test_case,
              const std::vector< std::string > only_these_norms = {},
              const std::vector< std::string > only_these_local_norms = {},
              const std::string visualize_prefix = "")
    : StudyBaseType(test_case, only_these_norms, visualize_prefix)
    , LocalizationBaseType(only_these_local_norms)
  {}

  virtual ~SWIPDGStudy() {}

  virtual std::string identifier() const override final
  {
    return DiscretizationType::static_id() + " (polorder " + Stuff::Common::toString(polOrder) + ")";
  }

  virtual size_t expected_rate(const std::string type) const override final
  {
    // If you get an undefined reference here from the linker you are missing the appropriate
    // specialization of BlockSWIPDGStudyExpectations!
    // For a new TestCaseType you have to add a specialization in a separate object file
    // (see linearelliptic-block-swipdg-expectations_os2014_2daluconform.cxx for example) and adjust the
    // CMakeLists.txt accordingly. For a new polOrder add
    //     template class BlockSWIPDGStudyExpectations< TestCasesType, polOrder >;
    // in the appropriate (existing) object file and implement a specialization for this polOrder, if needed!
    //
    // Oh: and do not forget to add
    //   'extern template class BlockSWIPDGStudyExpectations< ... >'
    // to each test source using these results!
    return SWIPDGStudyExpectations< TestCaseType, polOrder >::rate(this->test_case_, type);
  } // ... expected_rate(...)

  virtual std::vector< double > expected_results(const std::string type) const override final
  {
    // If you get an undefined reference here from the linker you are missing the appropriate
    // specialization of BlockSWIPDGStudyExpectations!
    // For a new TestCaseType you have to add a specialization in a separate object file
    // (see linearelliptic-block-swipdg-expectations_os2014_2daluconform.cxx for example) and adjust the
    // CMakeLists.txt accordingly. For a new polOrder add
    //     template class BlockSWIPDGStudyExpectations< TestCasesType, polOrder >;
    // in the appropriate (existing) object file and implement a specialization for this polOrder, if needed!
    //
    // Oh: and do not forget to add
    //   'extern template class BlockSWIPDGStudyExpectations< ... >'
    // to each test source using these results!
    return SWIPDGStudyExpectations< TestCaseType, polOrder >::results(this->test_case_, type);
  } // ... expected_results(...)

  virtual Stuff::LA::CommonDenseVector< double > compute_reference_indicators() const
  {
    typedef typename TestCaseType::ProblemType ProblemType;
    typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
    typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;
    typedef typename DiffusionFactorType::RangeFieldType RangeFieldType;
    assert(!this->test_case_.problem().diffusion_factor()->parametric());
    assert(!this->test_case_.problem().diffusion_tensor()->parametric());
    const auto diffusion_factor = this->test_case_.problem().diffusion_factor()->affine_part();
    const auto diffusion_tensor = this->test_case_.problem().diffusion_tensor()->affine_part();

    // get current solution
    assert(this->current_refinement_ <= this->num_refinements());
    if (this->last_computed_refinement_ != this->current_refinement_)
      const_cast< ThisType& >(*this).compute_on_current_refinement();
    assert(this->last_computed_refinement_ == this->current_refinement_);
    assert(this->current_solution_vector_);
    typedef typename StudyBaseType::DiscreteFunctionType      DiscreteFunctionType;
    typedef typename StudyBaseType::ConstDiscreteFunctionType ConstDiscreteFunctionType;
    const ConstDiscreteFunctionType current_solution(*(this->reference_discretization_->ansatz_space()),
                                                     *this->current_solution_vector_,
                                                     "current solution");
    // compute error
    if (this->test_case_.provides_exact_solution()) {
      DUNE_THROW(Stuff::Exceptions::you_have_to_implement_this, "Felix!");
    } else {
      if (!this->reference_solution_computed_)
        const_cast< ThisType& >(*this).compute_reference_solution();
      assert(this->reference_discretization_);
      assert(this->reference_solution_vector_);
      // get reference solution
      const ConstDiscreteFunctionType reference_solution(*(this->reference_discretization_->ansatz_space()),
                                                         *this->reference_solution_vector_,
                                                         "reference solution");
      // define error norm
      const auto difference = reference_solution - current_solution;
      typedef typename ConstDiscreteFunctionType::DifferenceType DifferenceType;
      GDT::Products::EllipticLocalizable< GridViewType, DiffusionFactorType,
                                          DifferenceType, DifferenceType, RangeFieldType,
                                          DiffusionTensorType >
          local_energy_norm(this->test_case_.reference_grid_view(),
                            difference,
                            difference,
                            *diffusion_factor,
                            *diffusion_tensor,
                            Estimators::internal::SWIPDG::over_integrate);
      local_energy_norm.prepare();
      // prepare
      const auto& current_grid_view = this->current_discretization_->grid_view();
      const int current_level = current_grid_view.template begin< 0 >()->level();
      Stuff::LA::CommonDenseVector< double > error_indicators(boost::numeric_cast< size_t >(current_grid_view.indexSet().size(0)),
                                                              0.0);
      std::vector< size_t > fine_entities_per_coarse_entity(error_indicators.size(), 0);
      RangeFieldType energy_error_squared = 0.0;
      // walk the reference grid
      auto reference_grid_view = this->test_case_.reference_grid_view();
      const auto entity_it_end = reference_grid_view.template end< 0 >();
      for (auto entity_it = reference_grid_view.template begin< 0 >();
           entity_it != entity_it_end;
           ++entity_it) {
        const auto& entity = *entity_it;
        int fine_level = entity.level();
        typename GridViewType::template Codim< 0 >::EntityPointer father_entity_ptr(entity);
        // find father entity in coarse grid view
        assert(fine_level >= current_level);
        while (fine_level > current_level) {
          assert(father_entity_ptr->hasFather());
          father_entity_ptr = father_entity_ptr->father();
          fine_level = father_entity_ptr.level();
        }
        assert(fine_level == current_level);
        const auto& father_entity = *father_entity_ptr;
        assert(current_grid_view.indexSet().contains(father_entity));
        const size_t index = current_grid_view.indexSet().index(father_entity);
        ++fine_entities_per_coarse_entity[index];
        const RangeFieldType local_energy_error_squared = local_energy_norm.compute_locally(entity);
        energy_error_squared += local_energy_error_squared;
        error_indicators[index] += local_energy_error_squared;
      } // walk the reference grid
      // average
      for (size_t ii = 0; ii < error_indicators.size(); ++ii)
        error_indicators[ii] /= (energy_error_squared * fine_entities_per_coarse_entity[ii]);
      if (!this->visualize_prefix_.empty())
        visualize_indicators(current_grid_view,
                             error_indicators,
                             "energy",
                             this->visualize_prefix_ + "_indicators_energy_error_"
                                + DSC::toString(this->current_refinement_));
      return error_indicators;
    }
  } // ... compute_reference_indicators(...)

  virtual std::vector< std::string > provided_indicators() const
  {
    return EstimatorType::available_local();
  }

  virtual Stuff::LA::CommonDenseVector< double > compute_indicators(const std::string type) const
  {
    // get current solution
    assert(this->current_refinement_ <= this->num_refinements());
    if (this->last_computed_refinement_ != this->current_refinement_) {
      const_cast< ThisType& >(*this).compute_on_current_refinement();
    }
    assert(this->current_solution_vector_on_level_);
    auto indicators = EstimatorType::estimate_local(*this->current_discretization_->ansatz_space(),
                                                    *this->current_solution_vector_on_level_,
                                                    this->test_case_.problem(),
                                                    type);
    if (!this->visualize_prefix_.empty())
      visualize_indicators(this->current_discretization_->grid_view(),
                           indicators,
                           type,
                           this->visualize_prefix_ + "_indicators_" + type + "_"
                           + DSC::toString(this->current_refinement_));
    return indicators;
  } // ... compute_indicators(...)

  std::map< std::string, std::vector< double > > run_eoc(std::ostream& out)
  {
    return StudyBaseType::run(out);
  }

  void run_localization(std::ostream& out)
  {
    LocalizationBaseType::run(out);
  }

private:
  virtual std::vector< std::string > available_norms() const override final
  {
    return {"L2", "H1_semi", "energy"};
  }

  virtual double compute_norm(const GridViewType& grid_view,
                              const FunctionType& function,
                              const std::string type) const override final
  {
    using namespace GDT;
    typedef typename TestCaseType::ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
    typedef typename TestCaseType::ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;
    if (type == "L2") {
      return Products::L2< GridViewType >(grid_view).induced_norm(function);
    } else if (type == "H1_semi") {
      return Products::H1Semi< GridViewType >(grid_view).induced_norm(function);
    } else if (type == "energy") {
      const auto& diffusion_factor = *(this->test_case_.problem().diffusion_factor());
      assert(!diffusion_factor.parametric());
      assert(diffusion_factor.has_affine_part());
      const auto& diffusion_tensor = *(this->test_case_.problem().diffusion_tensor());
      assert(!diffusion_tensor.parametric());
      assert(diffusion_tensor.has_affine_part());
      Products::Elliptic< GridViewType, DiffusionFactorType, double, DiffusionTensorType >
          elliptic_product(grid_view, *diffusion_factor.affine_part(), *diffusion_tensor.affine_part());
      return elliptic_product.induced_norm(function);
    } else
      DUNE_THROW(Stuff::Exceptions::wrong_input_given, "Wrong type '" << type << "' requested!");
  } // ... compute_norm(...)

  virtual std::vector< std::string > available_estimators() const override final
  {
    auto ret = EstimatorType::available();
    if (std::find(ret.begin(), ret.end(), "eta_ESV2007") != ret.end())
      ret.push_back("eff_ESV2007");
    if (std::find(ret.begin(), ret.end(), "eta_ESV2007_alt") != ret.end())
      ret.push_back("eff_ESV2007_alt");
    return ret;
  }

  virtual double estimate(const VectorType& vector, const std::string type) const override final
  {
    if (type == "eff_ESV2007")
      return estimate(vector, "eta_ESV2007") / const_cast< ThisType& >(*this).current_error_norm("energy");
    else if (type == "eff_ESV2007_alt")
      return estimate(vector, "eta_ESV2007_alt") / const_cast< ThisType& >(*this).current_error_norm("energy");
    else {
      assert(this->current_discretization_);
      return EstimatorType::estimate(*this->current_discretization_->ansatz_space(),
                                     vector,
                                     this->test_case_.problem(),
                                     type);
    }
  } // ... estimate(...)

  template< class GV, class VV >
  void visualize_indicators(const GV& grid_view,
                            const VV& vector,
                            const std::string name,
                            const std::string filename) const
  {
    typedef GDT::Spaces::FiniteVolume::Default< GV, typename VV::ScalarType, 1 > FVSpaceType;
    const FVSpaceType fv_space(grid_view);
    GDT::ConstDiscreteFunction< FVSpaceType, VV > discrete_function(fv_space, vector, name);
    discrete_function.visualize(filename);
  } // ... visualize_indicators(...)
}; // class SWIPDGStudy

} // namespace Tests
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_TEST_LINEARELLIPTIC_SWIPDG_HH


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    timeublas.cpp
 * @brief   Tests to help determine which way of accomplishing something with Eigen is faster
 * @author  Richard Roberts
 * @date    Sep 18, 2010
 */

#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/format.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/foreach.hpp>

#include <gtsam/base/Matrix.h>

#include <iostream>
#include <vector>
#include <utility>

using namespace std;
//namespace ublas = boost::numeric::ublas;
//using namespace Eigen;
using boost::timer;
using boost::format;
using namespace boost::lambda;

static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0));
//typedef ublas::matrix<double> matrix;
//typedef ublas::matrix_range<matrix> matrix_range;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
typedef Eigen::Block<matrix> matrix_block;

//using ublas::range;
//using ublas::triangular_matrix;

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

  if(true) {
    cout << "\nTiming matrix_block:" << endl;

    // We use volatile here to make these appear to the optimizing compiler as
    // if their values are only known at run-time.
    volatile size_t m=500;
    volatile size_t n=300;
    volatile size_t nReps = 1000;
    assert(m > n);
    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rni(boost::mt19937(), boost::uniform_int<size_t>(0,m-1));
    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rnj(boost::mt19937(), boost::uniform_int<size_t>(0,n-1));
    matrix mat(m,n);
    matrix_block full = mat.block(0, 0, m, n);
    matrix_block top = mat.block(0, 0, n, n);
    matrix_block block = mat.block(m/4, n/4, m-m/2, n-n/2);

    cout << format("  Basic: %1%x%2%\n") % m % n;
    cout << format("  Full:  mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n;
    cout << format("  Top:   mat(%1%:%2%, %3%:%4%)\n") % 0 % n % 0 % n;
    cout << format("  Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4);
    cout << endl;

    {
      timer tim;
      double basicTime, fullTime, topTime, blockTime;

      cout << "Row-major matrix, row-major assignment:" << endl;

      // Do a few initial assignments to let any cache effects stabilize
      for(size_t rep=0; rep<1000; ++rep)
        for(size_t i=0; i<(size_t)mat.rows(); ++i)
          for(size_t j=0; j<(size_t)mat.cols(); ++j)
            mat(i,j) = rng();

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t i=0; i<(size_t)mat.rows(); ++i)
          for(size_t j=0; j<(size_t)mat.cols(); ++j)
            mat(i,j) = rng();
      basicTime = tim.elapsed();
      cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t i=0; i<(size_t)full.rows(); ++i)
          for(size_t j=0; j<(size_t)full.cols(); ++j)
            full(i,j) = rng();
      fullTime = tim.elapsed();
      cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t i=0; i<(size_t)top.rows(); ++i)
          for(size_t j=0; j<(size_t)top.cols(); ++j)
            top(i,j) = rng();
      topTime = tim.elapsed();
      cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t i=0; i<(size_t)block.rows(); ++i)
          for(size_t j=0; j<(size_t)block.cols(); ++j)
            block(i,j) = rng();
      blockTime = tim.elapsed();
      cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;

      cout << endl;
    }

    {
      timer tim;
      double basicTime, fullTime, topTime, blockTime;

      cout << "Row-major matrix, column-major assignment:" << endl;

      // Do a few initial assignments to let any cache effects stabilize
      for(size_t rep=0; rep<1000; ++rep)
        for(size_t j=0; j<(size_t)mat.cols(); ++j)
          for(size_t i=0; i<(size_t)mat.rows(); ++i)
            mat(i,j) = rng();

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t j=0; j<(size_t)mat.cols(); ++j)
          for(size_t i=0; i<(size_t)mat.rows(); ++i)
            mat(i,j) = rng();
      basicTime = tim.elapsed();
      cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t j=0; j<(size_t)full.cols(); ++j)
          for(size_t i=0; i<(size_t)full.rows(); ++i)
            full(i,j) = rng();
      fullTime = tim.elapsed();
      cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t j=0; j<(size_t)top.cols(); ++j)
          for(size_t i=0; i<(size_t)top.rows(); ++i)
            top(i,j) = rng();
      topTime = tim.elapsed();
      cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;

      tim.restart();
      for(size_t rep=0; rep<nReps; ++rep)
        for(size_t j=0; j<(size_t)block.cols(); ++j)
          for(size_t i=0; i<(size_t)block.rows(); ++i)
            block(i,j) = rng();
      blockTime = tim.elapsed();
      cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;

      cout << endl;
    }

    {
      timer tim;
      double basicTime, fullTime, topTime, blockTime;
      typedef pair<size_t,size_t> ij_t;
      vector<ij_t> ijs(100000);

      cout << "Row-major matrix, random assignment:" << endl;

      // Do a few initial assignments to let any cache effects stabilize
      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
      for(size_t rep=0; rep<1000; ++rep)
        BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); }

      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
      for(size_t rep=0; rep<1000; ++rep)
        BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); }
      basicTime = tim.elapsed();
      cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl;

      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
      for(size_t rep=0; rep<1000; ++rep)
        BOOST_FOREACH(const ij_t& ij, ijs) { full(ij.first, ij.second) = rng(); }
      fullTime = tim.elapsed();
      cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl;

      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj()));
      for(size_t rep=0; rep<1000; ++rep)
        BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); }
      topTime = tim.elapsed();
      cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl;

      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols()));
      for(size_t rep=0; rep<1000; ++rep)
        BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); }
      blockTime = tim.elapsed();
      cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl;

      cout << endl;
    }
  }

//  if(true) {
//    cout << "\nTesting square triangular matrices:" << endl;
//
////    typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular;
////    typedef ublas::matrix<double, ublas::column_major> matrix;
//    typedef MatrixXd matrix; // default col major
//
////    triangular tri(5,5);
//
//    matrix mat(5,5);
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//
//    tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
//    cout << "  Assigned from triangular adapter: " << tri << endl;
//
//    cout << "  Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl;
//
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//    mat = tri;
//    cout << "  Assign matrix from triangular: " << mat << endl;
//
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//    (ublas::triangular_adaptor<matrix, ublas::upper>(mat)) = tri;
//    cout << "  Assign triangular adaptor from triangular: " << mat << endl;
//  }

//  {
//    cout << "\nTesting wide triangular matrices:" << endl;
//
//    typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular;
//    typedef ublas::matrix<double, ublas::column_major> matrix;
//
//    triangular tri(5,7);
//
//    matrix mat(5,7);
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//
//    tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
//    cout << "  Assigned from triangular adapter: " << tri << endl;
//
//    cout << "  Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl;
//
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//    mat = tri;
//    cout << "  Assign matrix from triangular: " << mat << endl;
//
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = rng();
//    mat = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
//    cout << "  Assign matrix from triangular adaptor of self: " << mat << endl;
//  }

//  {
//    cout << "\nTesting subvectors:" << endl;
//
//    typedef MatrixXd matrix;
//    matrix mat(4,4);
//
//    for(size_t j=0; j<(size_t)mat.cols(); ++j)
//      for(size_t i=0; i<(size_t)mat.rows(); ++i)
//        mat(i,j) = i*mat.rows() + j;
//    cout << "  mat = " << mat;
//
//    cout << "  vec(1:4, 2:2) = " << mat.block(1,2, ), ublas::range(1,4), ublas::range(2,2));
//
//  }

  return 0;

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2017, Oracle and/or its affiliates.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP

#include <algorithm>
#include <vector>

#include <boost/range.hpp>
#include <boost/mpl/assert.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/iterators/segment_iterator.hpp>

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/expand.hpp>

#include <boost/geometry/algorithms/detail/check_iterator_range.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>
#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/multirange_geometry.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_geometry.hpp>
#include <boost/geometry/algorithms/detail/relate/less.hpp>

#include <boost/geometry/algorithms/dispatch/disjoint.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

template <typename MultiPoint1, typename MultiPoint2>
class multipoint_multipoint
{
private:
    template <typename Iterator>
    class unary_disjoint_predicate
        : detail::relate::less
    {
    private:
        typedef detail::relate::less base_type;

    public:
        unary_disjoint_predicate(Iterator first, Iterator last)
            : base_type(), m_first(first), m_last(last)
        {}

        template <typename Point>
        inline bool apply(Point const& point) const
        {
            return !std::binary_search(m_first,
                                       m_last,
                                       point,
                                       static_cast<base_type const&>(*this));
        }

    private:
        Iterator m_first, m_last;
    };

public:
    static inline bool apply(MultiPoint1 const& multipoint1,
                             MultiPoint2 const& multipoint2)
    {
        BOOST_GEOMETRY_ASSERT( boost::size(multipoint1) <= boost::size(multipoint2) );

        typedef typename boost::range_value<MultiPoint1>::type point1_type;

        std::vector<point1_type> points1(boost::begin(multipoint1),
                                         boost::end(multipoint1));

        std::sort(points1.begin(), points1.end(), detail::relate::less());

        typedef unary_disjoint_predicate
            <
                typename std::vector<point1_type>::const_iterator
            > predicate_type;

        return check_iterator_range
            <
                predicate_type
            >::apply(boost::begin(multipoint2),
                     boost::end(multipoint2),
                     predicate_type(points1.begin(), points1.end()));
    }
};

template <typename MultiPoint, typename Linear>
class multipoint_linear
{
private:
    struct expand_box_point
    {
        template <typename Box, typename Point>
        static inline void apply(Box& total, Point const& point)
        {
            geometry::expand(total, point);
        }
    };

    template <typename EnvelopeStrategy>
    struct expand_box_segment
    {
        explicit expand_box_segment(EnvelopeStrategy const& strategy)
            : m_strategy(strategy)
        {}

        template <typename Box, typename Segment>
        inline void apply(Box& total, Segment const& segment) const
        {
            geometry::expand(total,
                             geometry::return_envelope<Box>(segment, m_strategy));
        }

        EnvelopeStrategy const& m_strategy;
    };

    struct overlaps_box_point
    {
        template <typename Box, typename Point>
        static inline bool apply(Box const& box, Point const& point)
        {
            // The default strategy is enough in this case
            return ! detail::disjoint::disjoint_point_box(point, box);
        }
    };

    template <typename DisjointStrategy>
    struct overlaps_box_segment
    {
        explicit overlaps_box_segment(DisjointStrategy const& strategy)
            : m_strategy(strategy)
        {}

        template <typename Box, typename Segment>
        inline bool apply(Box const& box, Segment const& segment) const
        {
            return ! dispatch::disjoint<Segment, Box>::apply(segment, box, m_strategy);
        }

        DisjointStrategy const& m_strategy;
    };

    template <typename PtSegStrategy>
    class item_visitor_type
    {
    public:
        item_visitor_type(PtSegStrategy const& strategy)
            : m_intersection_found(false)
            , m_strategy(strategy)
        {}

        template <typename Item1, typename Item2>
        inline bool apply(Item1 const& item1, Item2 const& item2)
        {
            if (! m_intersection_found
                && ! dispatch::disjoint<Item1, Item2>::apply(item1, item2, m_strategy))
            {
                m_intersection_found = true;
                return false;
            }
            return true;
        }

        inline bool intersection_found() const { return m_intersection_found; }

    private:
        bool m_intersection_found;
        PtSegStrategy const& m_strategy;
    };
    // structs for partition -- end

    class segment_range
    {
    public:
        typedef geometry::segment_iterator<Linear const> const_iterator;
        typedef const_iterator iterator;

        segment_range(Linear const& linear)
            : m_linear(linear)
        {}

        const_iterator begin() const
        {
            return geometry::segments_begin(m_linear);
        }

        const_iterator end() const
        {
            return geometry::segments_end(m_linear);
        }

    private:
        Linear const& m_linear;
    };

public:
    template <typename Strategy>
    static inline bool apply(MultiPoint const& multipoint, Linear const& linear, Strategy const& strategy)
    {
        item_visitor_type<Strategy> visitor(strategy);

        typedef typename Strategy::envelope_strategy_type envelope_strategy_type;
        typedef typename Strategy::disjoint_strategy_type disjoint_strategy_type;

        // TODO: disjoint Segment/Box may be called in partition multiple times
        // possibly for non-cartesian segments which could be slow. We should consider
        // passing a range of bounding boxes of segments after calculating them once.
        // Alternatively instead of a range of segments a range of Segment/Envelope pairs
        // should be passed, where envelope would be lazily calculated when needed the first time
        geometry::partition
            <
                geometry::model::box<typename point_type<MultiPoint>::type>
            >::apply(multipoint, segment_range(linear), visitor,
                     expand_box_point(),
                     overlaps_box_point(),
                     expand_box_segment<envelope_strategy_type>(strategy.get_envelope_strategy()),
                     overlaps_box_segment<disjoint_strategy_type>(strategy.get_disjoint_strategy()));

        return ! visitor.intersection_found();
    }

    template <typename Strategy>
    static inline bool apply(Linear const& linear, MultiPoint const& multipoint, Strategy const& strategy)
    {
        return apply(multipoint, linear, strategy);
    }
};

template <typename MultiPoint, typename SingleGeometry>
class multi_point_single_geometry
{
public:
    template <typename Strategy>
    static inline bool apply(MultiPoint const& multi_point, SingleGeometry const& single_geometry, Strategy const& strategy)
    {
        typedef typename point_type<MultiPoint>::type point1_type;
        typedef typename point_type<SingleGeometry>::type point2_type;
        typedef model::box<point2_type> box2_type;

        box2_type box2;
        geometry::envelope(single_geometry, box2, strategy.get_envelope_strategy());
        geometry::detail::expand_by_epsilon(box2);

        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            // The default strategy is enough for Point/Box
            if (! detail::disjoint::disjoint_point_box(*it, box2)
                && ! dispatch::disjoint<point1_type, SingleGeometry>::apply(*it, single_geometry, strategy))
            {
                return false;
            }
        }

        return true;
    }

    template <typename Strategy>
    static inline bool apply(SingleGeometry const& single_geometry, MultiPoint const& multi_point, Strategy const& strategy)
    {
        return apply(multi_point, single_geometry, strategy);
    }
};

template <typename MultiPoint, typename MultiGeometry>
class multi_point_multi_geometry
{
private:
    struct expand_box_point
    {
        template <typename Box, typename Point>
        static inline void apply(Box& total, Point const& point)
        {
            geometry::expand(total, point);
        }
    };

    struct expand_box_box_pair
    {
        template <typename Box, typename BoxPair>
        inline void apply(Box& total, BoxPair const& box_pair) const
        {
            geometry::expand(total, box_pair.first);
        }
    };

    struct overlaps_box_point
    {
        template <typename Box, typename Point>
        static inline bool apply(Box const& box, Point const& point)
        {
            // The default strategy is enough for Point/Box
            return ! detail::disjoint::disjoint_point_box(point, box);
        }
    };

    struct overlaps_box_box_pair
    {
        template <typename Box, typename BoxPair>
        inline bool apply(Box const& box, BoxPair const& box_pair) const
        {
            // The default strategy is enough for Box/Box
            return ! detail::disjoint::disjoint_box_box(box_pair.first, box);
        }
    };

    template <typename PtSegStrategy>
    class item_visitor_type
    {
    public:
        item_visitor_type(MultiGeometry const& multi_geometry,
                          PtSegStrategy const& strategy)
            : m_intersection_found(false)
            , m_multi_geometry(multi_geometry)
            , m_strategy(strategy)
        {}

        template <typename Point, typename BoxPair>
        inline bool apply(Point const& point, BoxPair const& box_pair)
        {
            typedef typename boost::range_value<MultiGeometry>::type single_type;

            // The default strategy is enough for Point/Box
            if (! m_intersection_found
                && ! detail::disjoint::disjoint_point_box(point, box_pair.first)
                && ! dispatch::disjoint<Point, single_type>::apply(point, range::at(m_multi_geometry, box_pair.second), m_strategy))
            {
                m_intersection_found = true;
                return false;
            }
            return true;
        }

        inline bool intersection_found() const { return m_intersection_found; }

    private:
        bool m_intersection_found;
        MultiGeometry const& m_multi_geometry;
        PtSegStrategy const& m_strategy;
    };
    // structs for partition -- end

public:
    template <typename Strategy>
    static inline bool apply(MultiPoint const& multi_point, MultiGeometry const& multi_geometry, Strategy const& strategy)
    {
        typedef typename point_type<MultiPoint>::type point1_type;
        typedef typename point_type<MultiGeometry>::type point2_type;
        typedef model::box<point1_type> box1_type;
        typedef model::box<point2_type> box2_type;
        typedef std::pair<box2_type, std::size_t> box_pair_type;

        typename Strategy::envelope_strategy_type const
            envelope_strategy = strategy.get_envelope_strategy();

        std::size_t count2 = boost::size(multi_geometry);
        std::vector<box_pair_type> boxes(count2);
        for (std::size_t i = 0 ; i < count2 ; ++i)
        {
            geometry::envelope(range::at(multi_geometry, i), boxes[i].first, envelope_strategy);
            geometry::detail::expand_by_epsilon(boxes[i].first);
            boxes[i].second = i;
        }

        item_visitor_type<Strategy> visitor(multi_geometry, strategy);

        geometry::partition
            <
                box1_type
            >::apply(multi_point, boxes, visitor,
                     expand_box_point(),
                     overlaps_box_point(),
                     expand_box_box_pair(),
                     overlaps_box_box_pair());

        return ! visitor.intersection_found();
    }

    template <typename Strategy>
    static inline bool apply(MultiGeometry const& multi_geometry, MultiPoint const& multi_point, Strategy const& strategy)
    {
        return apply(multi_point, multi_geometry, strategy);
    }
};

template <typename MultiPoint, typename Areal, typename Tag = typename tag<Areal>::type>
struct multipoint_areal
    : multi_point_single_geometry<MultiPoint, Areal>
{};

template <typename MultiPoint, typename Areal>
struct multipoint_areal<MultiPoint, Areal, multi_polygon_tag>
    : multi_point_multi_geometry<MultiPoint, Areal>
{};

}} // namespace detail::disjoint
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename MultiPoint, std::size_t DimensionCount>
struct disjoint
    <
        Point, MultiPoint, DimensionCount, point_tag, multi_point_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Point>
{};

template <typename MultiPoint, typename Segment, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Segment, DimensionCount, multi_point_tag, segment_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Segment>
{};

template <typename MultiPoint, typename Box, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Box, DimensionCount, multi_point_tag, box_tag, false
    > : detail::disjoint::multirange_constant_size_geometry<MultiPoint, Box>
{};

template
<
    typename MultiPoint1,
    typename MultiPoint2,
    std::size_t DimensionCount
>
struct disjoint
    <
        MultiPoint1, MultiPoint2, DimensionCount,
        multi_point_tag, multi_point_tag, false
    >
{
    template <typename Strategy>
    static inline bool apply(MultiPoint1 const& multipoint1,
                             MultiPoint2 const& multipoint2,
                             Strategy const& )
    {
        if ( boost::size(multipoint2) < boost::size(multipoint1) )
        {
            return detail::disjoint::multipoint_multipoint
                <
                    MultiPoint2, MultiPoint1
                >::apply(multipoint2, multipoint1);
        }

        return detail::disjoint::multipoint_multipoint
            <
                MultiPoint1, MultiPoint2
            >::apply(multipoint1, multipoint2);
   }
};

template <typename Linear, typename MultiPoint, std::size_t DimensionCount>
struct disjoint
    <
        Linear, MultiPoint, DimensionCount, linear_tag, multi_point_tag, false
    > : detail::disjoint::multipoint_linear<MultiPoint, Linear>
{};

template <typename MultiPoint, typename Linear, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Linear, DimensionCount, multi_point_tag, linear_tag, false
    > : detail::disjoint::multipoint_linear<MultiPoint, Linear>
{};

template <typename Areal, typename MultiPoint, std::size_t DimensionCount>
struct disjoint
    <
        Areal, MultiPoint, DimensionCount, areal_tag, multi_point_tag, false
    > : detail::disjoint::multipoint_areal<MultiPoint, Areal>
{};

template <typename MultiPoint, typename Areal, std::size_t DimensionCount>
struct disjoint
    <
        MultiPoint, Areal, DimensionCount, multi_point_tag, areal_tag, false
    > : detail::disjoint::multipoint_areal<MultiPoint, Areal>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_MULTIPOINT_GEOMETRY_HPP