// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "ColorMapOptimization.h"

#include <Eigen/Dense>

#include <Core/Utility/Console.h>
#include <Core/Geometry/Image.h>
#include <Core/Geometry/RGBDImage.h>
#include <Core/Geometry/TriangleMesh.h>
#include <Core/Camera/PinholeCameraTrajectory.h>
#include <IO/ClassIO/TriangleMeshIO.h>
#include <Core/Utility/Eigen.h>

namespace three {

namespace {

class ImageWarpingField {

public:
    ImageWarpingField (int width, int height, int number_of_vertical_anchors) {
        InitializeWarpingFields(width, height, number_of_vertical_anchors);
    }

    void InitializeWarpingFields(int width, int height,
            int number_of_vertical_anchors) {
        anchor_h_ = number_of_vertical_anchors;
        anchor_step_ = double(height) / (anchor_h_ - 1);
        anchor_w_ = int(std::ceil(double(width) / anchor_step_) + 1);
        flow_ = Eigen::VectorXd(anchor_w_ * anchor_h_ * 2);
        for (int i = 0; i <= (anchor_w_ - 1); i++) {
            for (int j = 0; j <= (anchor_h_ - 1); j++) {
                int baseidx = (i + j * anchor_w_) * 2;
                flow_(baseidx) = i * anchor_step_;
                flow_(baseidx + 1) = j * anchor_step_;
            }
        }
    }

    Eigen::Vector2d QueryFlow(int i, int j) const {
        int baseidx = (i + j * anchor_w_) * 2;
        // exceptional case: quried anchor index is out of pre-defined space
        if (baseidx < 0 || baseidx > anchor_w_ * anchor_h_ * 2)
            return Eigen::Vector2d(0.0, 0.0);
        else
            return Eigen::Vector2d(flow_(baseidx), flow_(baseidx + 1));
    }

    Eigen::Vector2d GetImageWarpingField(double u, double v) const {
        int i = (int)(u / anchor_step_);
        int j = (int)(v / anchor_step_);
        double p = (u - i * anchor_step_) / anchor_step_;
        double q = (v - j * anchor_step_) / anchor_step_;
        return (1 - p) * (1 - q) * QueryFlow(i, j)
            + (1 - p) * (q)* QueryFlow(i, j + 1)
            + (p)* (1 - q) * QueryFlow(i + 1, j)
            + (p)* (q)* QueryFlow(i + 1, j + 1);
    }

public:
    Eigen::VectorXd flow_;
    int anchor_w_, anchor_h_;
    double anchor_step_;
};

const double IMAGE_BOUNDARY_MARGIN = 10;

inline std::tuple<float, float, float> Project3DPointAndGetUVDepth(
        const Eigen::Vector3d X,
        const PinholeCameraTrajectory& camera, int camid)
{
    std::pair<double, double> f = camera.intrinsic_.GetFocalLength();
    std::pair<double, double> p = camera.intrinsic_.GetPrincipalPoint();
    Eigen::Vector4d Vt = camera.extrinsic_[camid] *
            Eigen::Vector4d(X(0), X(1), X(2), 1);
    float u = float((Vt(0) * f.first) / Vt(2) + p.first);
    float v = float((Vt(1) * f.second) / Vt(2) + p.second);
    float z = float(Vt(2));
    return std::make_tuple(u, v, z);
}

std::tuple<std::vector<std::vector<int>>, std::vector<std::vector<int>>>
        MakeVertexAndImageVisibility(const TriangleMesh& mesh,
        const std::vector<RGBDImage>& images_rgbd,
        const std::vector<Image>& images_mask,
        const PinholeCameraTrajectory& camera,
        const ColorMapOptmizationOption& option)
{
    auto n_camera = camera.extrinsic_.size();
    auto n_vertex = mesh.vertices_.size();
    std::vector<std::vector<int>> visiblity_vertex_to_image;
    std::vector<std::vector<int>> visiblity_image_to_vertex;
    visiblity_vertex_to_image.resize(n_vertex);
    visiblity_image_to_vertex.resize(n_camera);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int c = 0; c < n_camera; c++) {
        int viscnt = 0;
        for (int vertex_id = 0; vertex_id < n_vertex; vertex_id++) {
            Eigen::Vector3d X = mesh.vertices_[vertex_id];
            float u, v, d;
            std::tie(u, v, d) = Project3DPointAndGetUVDepth(X, camera, c);
            int u_d = int(round(u)), v_d = int(round(v));
            if (d < 0.0 || !images_rgbd[c].depth_.TestImageBoundary(u_d, v_d))
                continue;
            float d_sensor = *PointerAt<float>(images_rgbd[c].depth_, u_d, v_d);
            if (d_sensor > option.maximum_allowable_depth_)
                continue;
            if (*PointerAt<unsigned char>(images_mask[c], u_d, v_d) == 255)
                continue;
            if (std::fabs(d - d_sensor) <
                    option.depth_threshold_for_visiblity_check_) {
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    visiblity_vertex_to_image[vertex_id].push_back(c);
                    visiblity_image_to_vertex[c].push_back(vertex_id);
                    viscnt++;
                }
            }
        }
        PrintDebug("[cam %d] %.5f percents are visible\n",
                c, double(viscnt) / n_vertex * 100); fflush(stdout);
    }
    return std::move(std::make_tuple(
            visiblity_vertex_to_image, visiblity_image_to_vertex));
}

std::vector<ImageWarpingField> MakeWarpingFields(
        const std::vector<std::shared_ptr<Image>>& images,
        const ColorMapOptmizationOption& option)
{
    std::vector<ImageWarpingField> fields;
    for (auto i = 0; i < images.size(); i++) {
        int width = images[i]->width_;
        int height = images[i]->height_;
        fields.push_back(ImageWarpingField(width, height,
                option.number_of_vertical_anchors_));
    }
    return std::move(fields);
}

template<typename T>
std::tuple<bool, T> QueryImageIntensity(
        const Image& img, const Eigen::Vector3d& V,
        const PinholeCameraTrajectory& camera, int camid, int ch = -1)
{
    float u, v, depth;
    std::tie(u, v, depth) = Project3DPointAndGetUVDepth(V, camera, camid);
    if (img.TestImageBoundary(u, v, IMAGE_BOUNDARY_MARGIN)) {
        int u_round = int(round(u));
        int v_round = int(round(v));
        if (ch == -1) {
            return std::make_tuple(true,
                    *PointerAt<T>(img, u_round, v_round));
        } else {
            return std::make_tuple(true,
                    *PointerAt<T>(img, u_round, v_round, ch));
        }
    } else {
        return std::make_tuple(false, 0);
    }
}

template<typename T>
std::tuple<bool, T> QueryImageIntensity(
        const Image& img, const ImageWarpingField& field,
        const Eigen::Vector3d& V,
        const PinholeCameraTrajectory& camera, int camid, int ch = -1)
{
    float u, v, depth;
    std::tie(u, v, depth) = Project3DPointAndGetUVDepth(V, camera, camid);
    if (img.TestImageBoundary(u, v, IMAGE_BOUNDARY_MARGIN)) {
        Eigen::Vector2d uv_shift = field.GetImageWarpingField(u, v);
        if (img.TestImageBoundary(uv_shift(0), uv_shift(1),
                IMAGE_BOUNDARY_MARGIN)) {
            int u_shift = int(round(uv_shift(0)));
            int v_shift = int(round(uv_shift(1)));
            if (ch == -1) {
                return std::make_tuple(true,
                        *PointerAt<T>(img, u_shift, v_shift));
            } else {
                return std::make_tuple(true,
                        *PointerAt<T>(img, u_shift, v_shift, ch));
            }
        }
    }
    return std::make_tuple(false, 0);
}

void SetProxyIntensityForVertex(const TriangleMesh& mesh,
        const std::vector<std::shared_ptr<Image>>& images_gray,
        const std::vector<ImageWarpingField>& warping_field,
        const PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image,
        std::vector<double>& proxy_intensity)
{
    auto n_vertex = mesh.vertices_.size();
    proxy_intensity.resize(n_vertex);

#pragma omp parallel for schedule(static)
    for (auto i = 0; i < n_vertex; i++) {
        proxy_intensity[i] = 0.0;
        float sum = 0.0;
        for (auto iter = 0; iter < visiblity_vertex_to_image[i].size();
                iter++) {
            int j = visiblity_vertex_to_image[i][iter];
            float gray;
            bool valid = false;
            std::tie(valid, gray) = QueryImageIntensity<float>(
                    *images_gray[j], warping_field[j],
                    mesh.vertices_[i], camera, j);
            if (valid) {
                sum += 1.0;
                proxy_intensity[i] += gray;
            }
        }
        if (sum > 0) {
            proxy_intensity[i] /= sum;
        }
    }
}

void SetProxyIntensityForVertex(const TriangleMesh& mesh,
        const std::vector<std::shared_ptr<Image>>& images_gray,
        const PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image,
        std::vector<double>& proxy_intensity)
{
    auto n_vertex = mesh.vertices_.size();
    proxy_intensity.resize(n_vertex);

#pragma omp parallel for num_threads( 8 )
    for (auto i = 0; i < n_vertex; i++) {
        proxy_intensity[i] = 0.0;
        float sum = 0.0;
        for (auto iter = 0; iter < visiblity_vertex_to_image[i].size();
                iter++) {
            int j = visiblity_vertex_to_image[i][iter];
            float gray;
            bool valid = false;
            std::tie(valid, gray) = QueryImageIntensity<float>(
                    *images_gray[j], mesh.vertices_[i], camera, j);
            if (valid) {
                sum += 1.0;
                proxy_intensity[i] += gray;
            }
        }
        if (sum > 0) {
            proxy_intensity[i] /= sum;
        }
    }
}

void OptimizeImageCoorNonrigid(
        const TriangleMesh& mesh,
        const std::vector<std::shared_ptr<Image>>& images_gray,
        const std::vector<std::shared_ptr<Image>>& images_dx,
        const std::vector<std::shared_ptr<Image>>& images_dy,
        std::vector<ImageWarpingField>& warping_fields,
        const std::vector<ImageWarpingField>& warping_fields_init,
        PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image,
        const std::vector<std::vector<int>>& visiblity_image_to_vertex,
        std::vector<double>& proxy_intensity,
        const ColorMapOptmizationOption& option)
{
    auto n_vertex = mesh.vertices_.size();
    auto n_camera = camera.extrinsic_.size();
    Eigen::Matrix4d intr = Eigen::Matrix4d::Zero();
    intr.block<3,3>(0,0) = camera.intrinsic_.intrinsic_matrix_;
    intr(3, 3) = 1.0;
    double fx = intr(0, 0);
    double fy = intr(1, 1);
    SetProxyIntensityForVertex(mesh, images_gray, warping_fields, camera,
            visiblity_vertex_to_image, proxy_intensity);
    for (int itr = 0; itr < option.maximum_iteration_; itr++) {
        PrintDebug("[Iteration %04d] ", itr+1);
        double residual = 0.0;
        double residual_reg = 0.0;
        int total_num_ = 0;
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int i = 0; i < n_camera; i++) {
            int nonrigidval = warping_fields[i].anchor_w_ *
                    warping_fields[i].anchor_h_ * 2;
            Eigen::MatrixXd JJ = Eigen::MatrixXd::Zero(
                    6 + nonrigidval, 6 + nonrigidval);
            Eigen::VectorXd Jb = Eigen::VectorXd::Zero(6 + nonrigidval);
            double rr = 0.0;
            double rr_reg = 0.0;
            int this_num = 0;
            Eigen::Matrix4d pose;
            pose = camera.extrinsic_[i];
            double anchor_step = warping_fields[i].anchor_step_;
            int anchor_w = warping_fields[i].anchor_w_;
            for (auto iter = 0; iter < visiblity_image_to_vertex[i].size();
                    iter++) {
                int j = visiblity_image_to_vertex[i][iter];
                Eigen::Vector3d V = mesh.vertices_[j];
                Eigen::Vector4d G = pose * Eigen::Vector4d(V(0), V(1), V(2), 1);
                Eigen::Vector4d L = intr * G;
                double u = L(0) / L(2);
                double v = L(1) / L(2);
                int ii = (int)(u / anchor_step);
                int jj = (int)(v / anchor_step);
                double p = (u - ii * anchor_step) / anchor_step;
                double q = (v - jj * anchor_step) / anchor_step;
                Eigen::Vector2d grids[4] = {
                    warping_fields[i].QueryFlow(ii, jj),
                    warping_fields[i].QueryFlow(ii, jj + 1),
                    warping_fields[i].QueryFlow(ii + 1, jj),
                    warping_fields[i].QueryFlow(ii + 1, jj + 1),
                };
                Eigen::Vector2d uuvv = (1 - p) * (1 - q) * grids[0]
                    + (1 - p) * (q)* grids[1]
                    + (p)* (1 - q) * grids[2]
                    + (p)* (q)* grids[3];
                double uu = uuvv(0);
                double vv = uuvv(1);
                if (!images_gray[i]->TestImageBoundary(uu, vv,
                        IMAGE_BOUNDARY_MARGIN))
                    continue;
                bool valid; double gray, dIdfx, dIdfy;
                std::tie(valid, gray) = images_gray[i]->FloatValueAt(uu, vv);
                std::tie(valid, dIdfx) = images_dx[i]->FloatValueAt(uu, vv);
                std::tie(valid, dIdfy) = images_dy[i]->FloatValueAt(uu, vv);
                Eigen::Vector2d dIdf(dIdfx, dIdfy);
                Eigen::Vector2d dfdx = ((grids[2] - grids[0]) * (1 - q) +
                        (grids[3] - grids[1]) * q) / anchor_step;
                Eigen::Vector2d dfdy = ((grids[1] - grids[0]) * (1 - p) +
                        (grids[3] - grids[2]) * p) / anchor_step;
                double dIdx = dIdf.dot(dfdx);
                double dIdy = dIdf.dot(dfdy);
                double invz = 1. / G(2);
                double v0 = dIdx * fx * invz;
                double v1 = dIdy * fy * invz;
                double v2 = -(v0 * G(0) + v1 * G(1)) * invz;
                double C[6 + 8];
                C[0] = -G(2) * v1 + G(1) * v2;
                C[1] = G(2) * v0 - G(0) * v2;
                C[2] = -G(1) * v0 + G(0) * v1;
                C[3] = v0;
                C[4] = v1;
                C[5] = v2;
                C[6] = dIdf(0) * (1 - p) * (1 - q);
                C[7] = dIdf(1) * (1 - p) * (1 - q);
                C[8] = dIdf(0) * (1 - p) * (q);
                C[9] = dIdf(1) * (1 - p) * (q);
                C[10] = dIdf(0) * (p)* (1 - q);
                C[11] = dIdf(1) * (p)* (1 - q);
                C[12] = dIdf(0) * (p)* (q);
                C[13] = dIdf(1) * (p)* (q);
                int idx[6 + 8];
                idx[0] = 0;
                idx[1] = 1;
                idx[2] = 2;
                idx[3] = 3;
                idx[4] = 4;
                idx[5] = 5;
                idx[6] = 6 + (ii + jj * anchor_w) * 2;
                idx[7] = 6 + (ii + jj * anchor_w) * 2 + 1;
                idx[8] = 6 + (ii + (jj + 1) * anchor_w) * 2;
                idx[9] = 6 + (ii + (jj + 1) * anchor_w) * 2 + 1;
                idx[10] = 6 + ((ii + 1) + jj * anchor_w) * 2;
                idx[11] = 6 + ((ii + 1) + jj * anchor_w) * 2 + 1;
                idx[12] = 6 + ((ii + 1) + (jj + 1) * anchor_w) * 2;
                idx[13] = 6 + ((ii + 1) + (jj + 1) * anchor_w) * 2 + 1;
                for (int x = 0; x < 14; x++) {
                    for (int y = 0; y < 14; y++) {
                        JJ(idx[x], idx[y]) += C[x] * C[y];
                    }
                }
                double r = (proxy_intensity[j] - gray);
                for (int x = 0; x < 14; x++) {
                    Jb(idx[x]) -= r * C[x];
                }
                rr += r * r;
                this_num++;
            }
            if (this_num == 0)
                continue;
            double weight = option.non_rigid_anchor_point_weight_
                    * this_num / n_vertex;
            for (int j = 0; j < nonrigidval; j++) {
                double r = weight * (warping_fields[i].flow_(j) -
                        warping_fields_init[i].flow_(j));
                JJ(6 + j, 6 + j) += weight * weight;
                Jb(6 + j) += weight * r;
                rr_reg += r * r;
            }
#ifdef _OPENMP
#pragma omp critical
#endif
            {
                bool success = false;
                Eigen::VectorXd result;
                std::tie(success, result) = SolveLinearSystem(JJ, -Jb, false);
                Eigen::Affine3d aff_mat;
                aff_mat.linear() = (Eigen::Matrix3d)
                        Eigen::AngleAxisd(result(2),Eigen::Vector3d::UnitZ())
                        * Eigen::AngleAxisd(result(1),Eigen::Vector3d::UnitY())
                        * Eigen::AngleAxisd(result(0),Eigen::Vector3d::UnitX());
                aff_mat.translation() =
                        Eigen::Vector3d(result(3), result(4), result(5));
                pose = aff_mat.matrix() * pose;
                for (int j = 0; j < nonrigidval; j++) {
                    warping_fields[i].flow_(j) += result(6 + j);
                }
            }
            camera.extrinsic_[i] = pose;
            residual += rr;
            residual_reg += rr_reg;
            total_num_ += this_num;
        }
        PrintDebug("Residual error : %.6f, reg : %.6f\n",
                residual, residual_reg);
        SetProxyIntensityForVertex(mesh, images_gray, warping_fields, camera,
                visiblity_vertex_to_image, proxy_intensity);
    }
}

void OptimizeImageCoorRigid(
        const TriangleMesh& mesh,
        const std::vector<std::shared_ptr<Image>>& images_gray,
        const std::vector<std::shared_ptr<Image>>& images_dx,
        const std::vector<std::shared_ptr<Image>>& images_dy,
        PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image,
        const std::vector<std::vector<int>>& visiblity_image_to_vertex,
        std::vector<double>& proxy_intensity,
        const ColorMapOptmizationOption& option)
{
    int total_num_ = 0;
    auto n_camera = camera.extrinsic_.size();
    Eigen::Matrix4d intr = Eigen::Matrix4d::Zero();
    intr.block<3,3>(0,0) = camera.intrinsic_.intrinsic_matrix_;
    intr(3, 3) = 1.0;
    double fx = intr(0, 0);
    double fy = intr(1, 1);
    SetProxyIntensityForVertex(mesh, images_gray, camera,
            visiblity_vertex_to_image, proxy_intensity);
    for (int itr = 0; itr < option.maximum_iteration_; itr++) {
        PrintDebug("[Iteration %04d] ", itr+1);
        double residual = 0.0;
        total_num_ = 0;
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int i = 0; i < n_camera; i++) {
            Eigen::MatrixXd JJ(6, 6);
            Eigen::VectorXd Jb(6);
            JJ.setZero();
            Jb.setZero();
            double rr = 0.0;
            int this_num = 0;
            Eigen::Matrix4d pose;
            pose = camera.extrinsic_[i];
            for (auto iter = 0; iter < visiblity_image_to_vertex[i].size();
                    iter++) {
                int j = visiblity_image_to_vertex[i][iter];
                Eigen::Vector3d V = mesh.vertices_[j];
                Eigen::Vector4d G = pose * Eigen::Vector4d(V(0), V(1), V(2), 1);
                Eigen::Vector4d L = intr * G;
                double u = L(0) / L(2);
                double v = L(1) / L(2);
                if (!images_gray[i]->TestImageBoundary(u, v,
                        IMAGE_BOUNDARY_MARGIN))
                    continue;
                bool valid; double gray, dIdx, dIdy;
                std::tie(valid, gray) = images_gray[i]->FloatValueAt(u, v);
                std::tie(valid, dIdx) = images_dx[i]->FloatValueAt(u, v);
                std::tie(valid, dIdy) = images_dy[i]->FloatValueAt(u, v);
                if (gray == -1.0)
                    continue;
                double invz = 1. / G(2);
                double v0 = dIdx * fx * invz;
                double v1 = dIdy * fy * invz;
                double v2 = -(v0 * G(0) + v1 * G(1)) * invz;
                double C[6];
                C[0] = (-G(2) * v1 + G(1) * v2);
                C[1] = (G(2) * v0 - G(0) * v2);
                C[2] = (-G(1) * v0 + G(0) * v1);
                C[3] = v0;
                C[4] = v1;
                C[5] = v2;
                for (int x = 0; x < 6; x++) {
                    for (int y = 0; y < 6; y++) {
                        JJ(x, y) += C[x] * C[y];
                    }
                }
                double r = (proxy_intensity[j] - gray);
                for (int x = 0; x < 6; x++) {
                    Jb(x) -= r * C[x];
                }
                rr += r * r;
                this_num++;
            }
            Eigen::VectorXd result(6);
            result = -JJ.inverse() * Jb;
            Eigen::Affine3d aff_mat;
            aff_mat.linear() = (Eigen::Matrix3d)
                    Eigen::AngleAxisd(result(2), Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(result(1), Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(result(0), Eigen::Vector3d::UnitX());
            aff_mat.translation() =
                    Eigen::Vector3d(result(3), result(4), result(5));
            pose = aff_mat.matrix() * pose;
            camera.extrinsic_[i] = pose;
#ifdef _OPENMP
#pragma omp critical
#endif
            {
                residual += rr;
                total_num_ += this_num;
            }
        }
        PrintDebug("Residual error : %.6f (avg : %.6f)\n",
                residual, residual / total_num_);
        SetProxyIntensityForVertex(mesh, images_gray, camera,
                visiblity_vertex_to_image, proxy_intensity);
    }
}

void SetGeometryColorAverage(TriangleMesh& mesh,
        const std::vector<RGBDImage>& images_rgbd,
        const std::vector<ImageWarpingField>& warping_fields,
        const PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image)
{
    auto n_vertex = mesh.vertices_.size();
    mesh.vertex_colors_.clear();
    mesh.vertex_colors_.resize(n_vertex);
#pragma omp parallel for schedule(static)
    for (int i = 0; i < n_vertex; i++) {
        mesh.vertex_colors_[i] = Eigen::Vector3d::Zero();
        double sum = 0.0;
        for (auto iter = 0; iter < visiblity_vertex_to_image[i].size();
                iter++) {
            int j = visiblity_vertex_to_image[i][iter];
            unsigned char r_temp, g_temp, b_temp;
            bool valid = false;
            std::tie(valid, r_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, warping_fields[j],
                    mesh.vertices_[i], camera, j, 0);
            std::tie(valid, g_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, warping_fields[j],
                    mesh.vertices_[i], camera, j, 1);
            std::tie(valid, b_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, warping_fields[j],
                    mesh.vertices_[i], camera, j, 2);
            float r = (float)r_temp / 255.0f;
            float g = (float)g_temp / 255.0f;
            float b = (float)b_temp / 255.0f;
            if (valid) {
                mesh.vertex_colors_[i] += Eigen::Vector3d(r, g, b);
                sum += 1.0;
            }
        }
        if (sum > 0.0) {
            mesh.vertex_colors_[i] /= sum;
        }
    }
}

void SetGeometryColorAverage(TriangleMesh& mesh,
        const std::vector<RGBDImage>& images_rgbd,
        const PinholeCameraTrajectory& camera,
        const std::vector<std::vector<int>>& visiblity_vertex_to_image)
{
    auto n_vertex = mesh.vertices_.size();
    mesh.vertex_colors_.clear();
    mesh.vertex_colors_.resize(n_vertex);
#pragma omp parallel for schedule(static)
    for (int i = 0; i < n_vertex; i++) {
        mesh.vertex_colors_[i] = Eigen::Vector3d::Zero();
        double sum = 0.0;
        for (auto iter = 0; iter < visiblity_vertex_to_image[i].size();
                iter++) {
            int j = visiblity_vertex_to_image[i][iter];
            unsigned char r_temp, g_temp, b_temp;
            bool valid = false;
            std::tie(valid, r_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, mesh.vertices_[i], camera, j, 0);
            std::tie(valid, g_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, mesh.vertices_[i], camera, j, 1);
            std::tie(valid, b_temp) = QueryImageIntensity<unsigned char>(
                    images_rgbd[j].color_, mesh.vertices_[i], camera, j, 2);
            float r = (float)r_temp / 255.0f;
            float g = (float)g_temp / 255.0f;
            float b = (float)b_temp / 255.0f;
            if (valid) {
                mesh.vertex_colors_[i] += Eigen::Vector3d(r, g, b);
                sum += 1.0;
            }
        }
        if (sum > 0.0) {
            mesh.vertex_colors_[i] /= sum;
        }
    }
}

std::tuple<std::vector<std::shared_ptr<Image>>,
        std::vector<std::shared_ptr<Image>>,
        std::vector<std::shared_ptr<Image>>> MakeGradientImages(
        const std::vector<RGBDImage>& images_rgbd)
{
    auto n_images = images_rgbd.size();
    std::vector<std::shared_ptr<Image>> images_gray;
    std::vector<std::shared_ptr<Image>> images_dx;
    std::vector<std::shared_ptr<Image>> images_dy;
    for (int i=0; i<n_images; i++) {
        auto gray_image = CreateFloatImageFromImage(images_rgbd[i].color_);
        auto gray_image_filtered = FilterImage(*gray_image,
                Image::FilterType::Gaussian3);
        images_gray.push_back(gray_image_filtered);
        images_dx.push_back(FilterImage(*gray_image_filtered,
                Image::FilterType::Sobel3Dx));
        images_dy.push_back(FilterImage(*gray_image_filtered,
                Image::FilterType::Sobel3Dy));
    }
    return std::move(std::make_tuple(images_gray, images_dx, images_dy));
}

std::vector<Image> MakeDepthMasks(
        const std::vector<RGBDImage>& images_rgbd,
        const ColorMapOptmizationOption& option)
{
    auto n_images = images_rgbd.size();
    std::vector<Image> images_mask;
    for (int i=0; i<n_images; i++) {
        PrintDebug("[MakeDepthMasks] Image %d/%d\n", i, n_images);
        int width = images_rgbd[i].depth_.width_;
        int height = images_rgbd[i].depth_.height_;
        auto depth_image = CreateFloatImageFromImage(images_rgbd[i].depth_);
        auto depth_image_gradient_dx = FilterImage(*depth_image,
                Image::FilterType::Sobel3Dx);
        auto depth_image_gradient_dy = FilterImage(*depth_image,
                Image::FilterType::Sobel3Dy);
        auto mask = std::make_shared<Image>();
        mask->PrepareImage(width, height, 1, 1);
        for (int v=0; v<height; v++) {
            for (int u=0; u<width; u++) {
                double dx = *PointerAt<float>(*depth_image_gradient_dx, u, v);
                double dy = *PointerAt<float>(*depth_image_gradient_dy, u, v);
                double mag = sqrt(dx * dx + dy * dy);
                if (mag > option.depth_threshold_for_discontinuity_check_) {
                    *PointerAt<unsigned char>(*mask, u, v) = 255;
                } else {
                    *PointerAt<unsigned char>(*mask, u, v) = 0;
                }
            }
        }
        auto mask_dilated = DilateImage(*mask,
                option.half_dilation_kernel_size_for_discontinuity_map_);
        images_mask.push_back(*mask_dilated);
    }
    return std::move(images_mask);
}

}    // unnamed namespace

void ColorMapOptimization(TriangleMesh& mesh,
        const std::vector<RGBDImage>& images_rgbd,
        PinholeCameraTrajectory& camera,
        const ColorMapOptmizationOption& option
        /* = ColorMapOptmizationOption()*/)
{
    PrintDebug("[ColorMapOptimization]\n");
    std::vector<std::shared_ptr<Image>> images_gray;
    std::vector<std::shared_ptr<Image>> images_dx;
    std::vector<std::shared_ptr<Image>> images_dy;
    std::tie(images_gray, images_dx, images_dy) =
            MakeGradientImages(images_rgbd);

    PrintDebug("[ColorMapOptimization] :: MakingMasks\n");
    auto images_mask = MakeDepthMasks(images_rgbd, option);

    PrintDebug("[ColorMapOptimization] :: VisibilityCheck\n");
    std::vector<std::vector<int>> visiblity_vertex_to_image;
    std::vector<std::vector<int>> visiblity_image_to_vertex;
    std::tie(visiblity_vertex_to_image, visiblity_image_to_vertex) =
            MakeVertexAndImageVisibility(mesh, images_rgbd,
            images_mask, camera, option);

    std::vector<double> proxy_intensity;
    if (option.non_rigid_camera_coordinate_) {
        PrintDebug("[ColorMapOptimization] :: Non-Rigid Optimization\n");
        auto warping_uv_ = MakeWarpingFields(images_gray, option);
        auto warping_uv_init_ = MakeWarpingFields(images_gray, option);
        OptimizeImageCoorNonrigid(mesh, images_gray,
                images_dx, images_dy, warping_uv_, warping_uv_init_, camera,
                visiblity_vertex_to_image, visiblity_image_to_vertex,
                proxy_intensity, option);
        SetGeometryColorAverage(mesh, images_rgbd, warping_uv_, camera,
                visiblity_vertex_to_image);
    } else {
        PrintDebug("[ColorMapOptimization] :: Rigid Optimization\n");
        OptimizeImageCoorRigid(mesh, images_gray, images_dx, images_dy, camera,
                visiblity_vertex_to_image, visiblity_image_to_vertex,
                proxy_intensity, option);
        SetGeometryColorAverage(mesh, images_rgbd, camera,
                visiblity_vertex_to_image);
    }
}

}    // namespace three


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_MEMORY_META_ALIGN_ON_HPP_INCLUDED
#define BOOST_SIMD_SDK_MEMORY_META_ALIGN_ON_HPP_INCLUDED

#include <cstddef>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/simd/sdk/memory/parameters.hpp>
#include <boost/simd/sdk/memory/meta/is_power_of_2.hpp>

namespace boost { namespace simd {  namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Compute an aligned value of an integral constant on a power of 2 boundary.
  // Documentation: align_on_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t V, std::size_t N = BOOST_SIMD_CONFIG_ALIGNMENT>
  struct align_on_c
       : boost::mpl::integral_c<std::size_t, (V+N-1) & ~(N-1) >
  {
    //==========================================================================
    /*
     * Alignment done on a non-power of two boundary
     */
    //==========================================================================
    BOOST_MPL_ASSERT_MSG
    ( (meta::is_power_of_2_c<N>::value)
    , BOOST_SIMD_INVALID_ALIGNMENT_VALUE
    , (boost::mpl::int_<N>)
    );
  };

  //////////////////////////////////////////////////////////////////////////////
  // Compute an aligned value of an Integral Constant on a power of 2 boundary.
  // Documentation: align_on.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class V, class N = boost::mpl::size_t<BOOST_SIMD_CONFIG_ALIGNMENT> >
  struct align_on
       :  boost::mpl::
          integral_c< typename V::value_type
                    , typename V::value_type(align_on_c < std::size_t(V::value)
                                                        , std::size_t(N::value)
                                                        >::value
                                            )
                    >
  {};
} } }

#endif


#include <cstdlib>
#include <fstream>
#include <iostream>
#include <list>
#include <string>

#include <Eigen/Core>

#include <cppmath/matlab/io.hpp>

using namespace std;
using namespace Eigen;
using namespace cppmath;

int main()
{
    const string FNAME = "cppmath.mat";
    cout << "Read MAT-file: " << FNAME << endl;

    // Open file
    // ---------
    ifstream ifs;
    ifs.open( FNAME.c_str(), ifstream::in | ifstream::binary );
    if( !ifs || ifs.bad() )
    {
        cout << "Could not open file!" << endl;
        return EXIT_FAILURE;
    }

    // Read file info
    // --------------
    matlab::FileInfo fileInfo;
    if( !matlab::MatReader::readHeader( &fileInfo, ifs ) )
    {
        cout << "Could read header!" << endl;
        return EXIT_FAILURE;
    }
    if( !fileInfo.isMatFile )
    {
        cout << "No MAT-file!" << endl;
        return EXIT_FAILURE;
    }
    cout << endl; // FIXME(cpieloth): Missing endl in logger methods.
    cout << "Description: " << fileInfo.description << endl;

    // Retrieve elements
    // -----------------
    list< matlab::ElementInfo > elements;
    if( !matlab::MatReader::retrieveDataElements( &elements, ifs, fileInfo ) )
    {
        cout << "Error while retrieving data elements!" << endl;
        return EXIT_FAILURE;
    }
    cout << endl; // FIXME(cpieloth): Missing endl in logger methods.
    cout << "Element count: " << elements.size() << endl;

    // Read elements
    // -------------
    cout << "Elements:" << endl;
    for( list< matlab::ElementInfo >::const_iterator it = elements.begin(); it != elements.end(); ++it )
    {
        cout << "Element name: " << it->arrayName << endl;
        if( it->dataType != matlab::DataTypes::miMATRIX )
        {
            cout << "No matrix, skipping element!" << endl;
            continue;
        }

        const bool isComplex = matlab::ArrayFlags::isComplex( it->arrayFlags );
        const bool isDouble = matlab::ArrayFlags::getArrayType( it->arrayFlags ) == matlab::ArrayTypes::mxDOUBLE_CLASS;
        if( !isDouble || isComplex )
        {
            cout << endl; // FIXME(cpieloth): Missing endl in logger methods.
            cout << "Data type not supported, skipping element!" << endl;
            continue;
        }

        Eigen::MatrixXd matrixDbl;
        if( !matlab::MatReader::readMatrixDouble( &matrixDbl, *it, ifs, fileInfo ) )
        {
            cout << endl; // FIXME(cpieloth): Missing endl in logger methods.
            cout << "Error on reading matrix, skipping element!" << endl;
            continue;
        }
        cout << "Matrix:\n" << matrixDbl << endl;
    }

    return EXIT_SUCCESS;
}


// Copyright (c) 2012-2015 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "script/int_serialization.h"
#include "script/script.h"
#include "script/script_num.h"
#include "scriptnum10.h"

#include <boost/test/unit_test.hpp>

#include "big_int.h"

using namespace std;
using bsv::bint;

namespace
{
    constexpr auto min64 = numeric_limits<int64_t>::min();
    constexpr auto max64 = numeric_limits<int64_t>::max();

    vector<int64_t> test_data{min64, -1, 0, 1, max64};
}

BOOST_AUTO_TEST_SUITE(scriptnum_tests)

BOOST_AUTO_TEST_CASE(construction)
{
    using script_data = vector<uint8_t>;
    using test_args = tuple<script_data, size_t, bool>;
    vector<test_args> valid_constructions = {
        {{}, 0, false},
        {{}, 0, true},
        {{}, 1, false},
        {{}, 1, true},
        {{1}, 1, false},
        {{1}, 1, true},
        {{1}, 2, false},
        {{1}, 2, true},
        {{1, 2, 3}, 4, false},
        {{1, 2, 3}, 4, true},
        {{1, 2, 3, 4}, 4, false},
        {{1, 2, 3, 4}, 4, true},
        {{1, 2, 3, 4}, 5, false},
        {{1, 2, 3, 4}, 5, true},
        {{1, 2, 3, 4, 5}, 5, false},
        {{2, 2, 3, 4, 5}, 5, true},
        {{1, 2, 3, 4, 5}, 6, false},
        {{1, 2, 3, 4, 5}, 6, true},
    };

    for(const auto [v, max_size, big_int] : valid_constructions)
    {
        try
        {
            CScriptNum actual{v, false, max_size, big_int};
        }
        catch(...)
        {
            BOOST_FAIL("should not throw");
        }
    }

    vector<test_args> invalid_constructions = {
        {{1}, 0, false},
        {{1}, 0, true},
        {{1, 2, 3, 4}, 3, false},
        {{1, 2, 3, 4}, 3, true},
        {{1, 2, 3, 4, 5}, 4, false},
        {{1, 2, 3, 4, 5}, 4, true},
    };
    for(const auto [v, max_size, big_int] : invalid_constructions)
    {
        try
        {
            CScriptNum actual{v, false, max_size, big_int};
            BOOST_FAIL("should throw");
        }
        catch(...)
        {
        }
    }
}

BOOST_AUTO_TEST_CASE(insertion_op)
{
    for(const int64_t n : test_data)
    {
        CScriptNum a{n};
        ostringstream actual;
        actual << a;

        ostringstream expected;
        expected << n;
        BOOST_CHECK_EQUAL(expected.str(), actual.str());
    }

    for(const int64_t n : test_data)
    {
        CScriptNum a{bint{n}};
        ostringstream actual;
        actual << a;

        ostringstream expected;
        expected << n;
        BOOST_CHECK_EQUAL(expected.str(), actual.str());
    }
}

BOOST_AUTO_TEST_CASE(equality)
{
    for(const int64_t n : test_data)
    {
        CScriptNum a{n};
        CScriptNum b{n};
        BOOST_CHECK_EQUAL(a, a);
        BOOST_CHECK_EQUAL(a, b);
        BOOST_CHECK_EQUAL(b, a);
    }

    for(const int64_t n : test_data)
    {
        bint bn{n};
        bn *= bint{10}; // *10 so we are testing outside of range of int64_t
        CScriptNum a{bn};
        CScriptNum b{bn};
        BOOST_CHECK_EQUAL(a, a);
        BOOST_CHECK_EQUAL(a, b);
        BOOST_CHECK_EQUAL(b, a);
    }
}

BOOST_AUTO_TEST_CASE(less)
{
    vector<pair<int64_t, int64_t>> test_data{
        {min64, -1}, {-1, 0}, {0, 1}, {min64, max64}, {1, max64}};
    for(const auto& [n, m] : test_data)
    {
        CScriptNum a{n};
        CScriptNum b{m};
        BOOST_CHECK_LT(a, b);
        BOOST_CHECK_LE(a, a);
        BOOST_CHECK_GE(a, a);
        BOOST_CHECK_GT(b, a);
    }

    for(const auto& [n, m] : test_data)
    {
        // n *= 10; // *10 so we are testing outside of range of int64_t
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        BOOST_CHECK_LT(a, b);
        BOOST_CHECK_LE(a, a);
        BOOST_CHECK_GE(a, a);
        BOOST_CHECK_GT(b, a);
    }

    for(const auto& [n, m] : test_data)
    {
        CScriptNum a{n};
        CScriptNum b{bint{m}};
        BOOST_CHECK_LT(a, b);
        BOOST_CHECK_LE(a, a);
        BOOST_CHECK_GE(a, a);
        BOOST_CHECK_GT(b, a);
    }
}

BOOST_AUTO_TEST_CASE(addition)
{
    const vector<tuple<int64_t, int64_t, int64_t>> test_data{
        {-1, 0, -1},
        {0, -1, -1},
        {-1, 1, 0},
        {1, -1, 0},
        {0, 1, 1},
        {1, 0, 1},
        {min64 + 1, -1, min64},
        {max64 - 1, +1, max64}};

    for(const auto& [n, m, o] : test_data)
    {
        // little int + little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a + b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int + big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a + b);
    }
}

BOOST_AUTO_TEST_CASE(subtraction)
{
    const vector<tuple<int64_t, int64_t, int64_t>> test_data{
        {0, 1, -1},
        {-1, 0, -1},
        {1, 1, 0},
        {-1, -1, 0},
        {2, 1, 1},
        {0, -1, 1},
        {min64 + 1, 1, min64},
        {max64 - 1, -1, max64}};

    for(const auto& [n, m, o] : test_data)
    {
        // little int - little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a - b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int - big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a - b);
    }
}

BOOST_AUTO_TEST_CASE(multiplication)
{
    // clang-format off
    const vector<tuple<int64_t, int64_t, int64_t>> test_data
    {
        {1, -1, -1},
        {-1, 1, -1},
        {0, 1, 0},
        {1, 0, 0},
        {1, 1, 1},
        {-1, -1, 1},
        {min64, 1, min64},
        {min64 + 1, -1, max64},
        {max64, 1, max64},
        {max64, -1, min64 + 1}
    };
    // clang-format on

    for(const auto& [n, m, o] : test_data)
    {
        // little int * little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a * b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int * big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a * b);
    }
}

BOOST_AUTO_TEST_CASE(division)
{
    const vector<tuple<int64_t, int64_t, int64_t>> test_data{
        {1, -1, -1},
        {-1, 1, -1},
        {0, 1, 0},
        {1, 1, 1},
        {-1, -1, 1},
        {min64, 1, min64},
        {min64 + 1, -1, max64},
        {max64, 1, max64},
        {max64, -1, min64 + 1}};

    for(const auto& [n, m, o] : test_data)
    {
        // little int / little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a / b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int / big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a / b);
    }
}

BOOST_AUTO_TEST_CASE(modular)
{
    const vector<tuple<int64_t, int64_t, int64_t>> test_data{{-3, -2, -1},
                                                             {1, 1, 0},
                                                             {-1, -1, 0},
                                                             {1, -1, 0},
                                                             {-1, 1, 0},
                                                             {3, 2, 1}};

    for(const auto& [n, m, o] : test_data)
    {
        // little int % little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a % b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int % big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a % b);
    }
}

BOOST_AUTO_TEST_CASE(and_)
{
    const vector<tuple<int64_t, int64_t, int64_t>> test_data{
        {0x0, 0x0, 0x0},
        {0xffffffffffffffff, 0x0, 0x0},
        {0x0, 0xffffffffffffffff, 0x0},
        {0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff},
        {0x555555555555555, 0xaaaaaaaaaaaaaaa, 0x0},
        {0xaaaaaaaaaaaaaaa, 0x555555555555555, 0x0},
    };

    for(const auto& [n, m, o] : test_data)
    {
        // little int & little int
        CScriptNum a{n};
        CScriptNum b{m};
        CScriptNum c{o};
        BOOST_CHECK_EQUAL(c, a & b);
    }

    for(const auto& [n, m, o] : test_data)
    {
        // big int & big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        CScriptNum c{bint{o}};
        BOOST_CHECK_EQUAL(c, a & b);
    }
}

BOOST_AUTO_TEST_CASE(negation)
{
    // clang-format off
    const vector<pair<int64_t, int64_t>> test_data
    {
        {0, 0},
        {1, -1},
        {-1, 1},
        {max64, min64+1}
    };
    // clang-format off

    for(const auto [n, m] : test_data)
    {
        // little int & little int
        CScriptNum a{n};
        CScriptNum b{m};
        BOOST_CHECK_EQUAL(b, -a);
    }

    for(const auto [n, m] : test_data)
    {
        // big int & big int
        CScriptNum a{bint{n}};
        CScriptNum b{bint{m}};
        BOOST_CHECK_EQUAL(b, -a);
    }
}

BOOST_AUTO_TEST_CASE(getint)
{
    constexpr int min_int{numeric_limits<int>::min()};
    constexpr int max_int{numeric_limits<int>::max()};

    const bint max64{max_int};
    CScriptNum max{max64 + 1};
    BOOST_CHECK_EQUAL(max_int, max.getint());

    const bint min64{min_int};
    CScriptNum min{min64 - 1};
    BOOST_CHECK_EQUAL(min_int, min.getint());
}

BOOST_AUTO_TEST_CASE(to_size_t_limited)
{
    //constexpr size_t size_t_min{std::numeric_limits<size_t>::min()}; // This causes compiler error ons MSVC when invoking CScriptNum constructor due to narrowing conversion
    static_assert(std::numeric_limits<size_t>::min() == 0);
    constexpr int64_t size_t_min{ 0 };

    constexpr size_t size_t_max{static_cast<size_t>(std::numeric_limits<int32_t>::max()) };

    BOOST_CHECK_EQUAL(size_t_min, CScriptNum{size_t_min}.to_size_t_limited());
    BOOST_CHECK_EQUAL(1, CScriptNum{1}.to_size_t_limited());

    BOOST_CHECK_EQUAL(size_t_min, CScriptNum{bint{size_t_min}}.to_size_t_limited());
    BOOST_CHECK_EQUAL(1, CScriptNum{bint{1}}.to_size_t_limited());
    BOOST_CHECK_EQUAL(size_t_max, CScriptNum{bint{size_t_max}}.to_size_t_limited());
}

// clang-format off
/** A selection of numbers that do not trigger int64_t overflow
 *  when added/subtracted. */
static const int64_t values[] = {0,
                                 1,
                                 -2,
                                 127,
                                 128,
                                 -255,
                                 256,
                                 (1LL << 15) - 1,
                                 -(1LL << 16),
                                 (1LL << 24) - 1,
                                 (1LL << 31),
                                 1 - (1LL << 32),
                                 1LL << 40};

static const int64_t offsets[] = {1,      0x79,   0x80,   0x81,   0xFF,
                                  0x7FFF, 0x8000, 0xFFFF, 0x10000};

static bool verify(const CScriptNum10 &bignum, const CScriptNum &scriptnum) {
    return bignum.getvch() == scriptnum.getvch() &&
           bignum.getint() == scriptnum.getint();
}

static void CheckCreateVch(const int64_t &num) {
    CScriptNum10 bignum(num);
    CScriptNum scriptnum(num);
    BOOST_CHECK(verify(bignum, scriptnum));

    std::vector<uint8_t> vch = bignum.getvch();

    CScriptNum10 bignum2(bignum.getvch(), false);
    vch = scriptnum.getvch();
    CScriptNum scriptnum2(scriptnum.getvch(), false);
    BOOST_CHECK(verify(bignum2, scriptnum2));

    CScriptNum10 bignum3(scriptnum2.getvch(), false);
    CScriptNum scriptnum3(bignum2.getvch(), false);
    BOOST_CHECK(verify(bignum3, scriptnum3));
}

static void CheckCreateInt(const int64_t &num) {
    CScriptNum10 bignum(num);
    CScriptNum scriptnum(num);
    BOOST_CHECK(verify(bignum, scriptnum));
    BOOST_CHECK(
        verify(CScriptNum10(bignum.getint()), CScriptNum(scriptnum.getint())));
    BOOST_CHECK(
        verify(CScriptNum10(scriptnum.getint()), CScriptNum(bignum.getint())));
    BOOST_CHECK(verify(CScriptNum10(CScriptNum10(scriptnum.getint()).getint()),
                       CScriptNum(CScriptNum(bignum.getint()).getint())));
}

static void CheckAdd(const int64_t &num1, const int64_t &num2) {
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);
    CScriptNum10 bignum3(num1);
    CScriptNum10 bignum4(num1);
    CScriptNum scriptnum3(num1);
    CScriptNum scriptnum4(num1);

    // int64_t overflow is undefined.
    bool invalid =
        (((num2 > 0) &&
          (num1 > (std::numeric_limits<int64_t>::max() - num2))) ||
         ((num2 < 0) && (num1 < (std::numeric_limits<int64_t>::min() - num2))));
    if (!invalid) {
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum1 + scriptnum2));
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum1 + scriptnum2));
        BOOST_CHECK(verify(bignum1 + bignum2, scriptnum2 + scriptnum1));
    }
}

static void CheckNegate(const int64_t &num) {
    const CScriptNum10 bignum(num);
    const CScriptNum scriptnum(num);

    // -INT64_MIN is undefined
    if (num != std::numeric_limits<int64_t>::min())
        BOOST_CHECK(verify(-bignum, -scriptnum));
}

static void CheckSubtract(const int64_t &num1, const int64_t &num2) {
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);
    bool invalid = false;

    // int64_t overflow is undefined.
    invalid =
        ((num2 > 0 && num1 < std::numeric_limits<int64_t>::min() + num2) ||
         (num2 < 0 && num1 > std::numeric_limits<int64_t>::max() + num2));
    if (!invalid) {
        BOOST_CHECK(verify(bignum1 - bignum2, scriptnum1 - scriptnum2));
        BOOST_CHECK(verify(bignum1 - bignum2, scriptnum1 - scriptnum2));
    }

    invalid =
        ((num1 > 0 && num2 < std::numeric_limits<int64_t>::min() + num1) ||
         (num1 < 0 && num2 > std::numeric_limits<int64_t>::max() + num1));
    if (!invalid) {
        BOOST_CHECK(verify(bignum2 - bignum1, scriptnum2 - scriptnum1));
        BOOST_CHECK(verify(bignum2 - bignum1, scriptnum2 - scriptnum1));
    }
}

static void CheckCompare(const int64_t &num1, const int64_t &num2) {
    const CScriptNum10 bignum1(num1);
    const CScriptNum10 bignum2(num2);
    const CScriptNum scriptnum1(num1);
    const CScriptNum scriptnum2(num2);

    BOOST_CHECK((bignum1 == bignum1) == (scriptnum1 == scriptnum1));
    BOOST_CHECK((bignum1 != bignum1) == (scriptnum1 != scriptnum1));
    BOOST_CHECK((bignum1 < bignum1) == (scriptnum1 < scriptnum1));
    BOOST_CHECK((bignum1 > bignum1) == (scriptnum1 > scriptnum1));
    BOOST_CHECK((bignum1 >= bignum1) == (scriptnum1 >= scriptnum1));
    BOOST_CHECK((bignum1 <= bignum1) == (scriptnum1 <= scriptnum1));

    BOOST_CHECK((bignum1 < bignum1) == (scriptnum1 < num1));
    BOOST_CHECK((bignum1 > bignum1) == (scriptnum1 > num1));
    BOOST_CHECK((bignum1 >= bignum1) == (scriptnum1 >= num1));
    BOOST_CHECK((bignum1 <= bignum1) == (scriptnum1 <= num1));

    BOOST_CHECK((bignum1 == bignum2) == (scriptnum1 == scriptnum2));
    BOOST_CHECK((bignum1 != bignum2) == (scriptnum1 != scriptnum2));
    BOOST_CHECK((bignum1 < bignum2) == (scriptnum1 < scriptnum2));
    BOOST_CHECK((bignum1 > bignum2) == (scriptnum1 > scriptnum2));
    BOOST_CHECK((bignum1 >= bignum2) == (scriptnum1 >= scriptnum2));
    BOOST_CHECK((bignum1 <= bignum2) == (scriptnum1 <= scriptnum2));

    BOOST_CHECK((bignum1 < bignum2) == (scriptnum1 < num2));
    BOOST_CHECK((bignum1 > bignum2) == (scriptnum1 > num2));
    BOOST_CHECK((bignum1 >= bignum2) == (scriptnum1 >= num2));
    BOOST_CHECK((bignum1 <= bignum2) == (scriptnum1 <= num2));
}

static void RunCreate(const int64_t &num) {
    CheckCreateInt(num);
    CScriptNum scriptnum(num);
    if (scriptnum.getvch().size() <= CScriptNum::MAXIMUM_ELEMENT_SIZE) {
        CheckCreateVch(num);
    } else {
        BOOST_CHECK_THROW(CheckCreateVch(num), scriptnum10_error);
    }
}

static void RunOperators(const int64_t &num1, const int64_t &num2) {
    CheckAdd(num1, num2);
    CheckSubtract(num1, num2);
    CheckNegate(num1);
    CheckCompare(num1, num2);
}

BOOST_AUTO_TEST_CASE(creation) {
    for (size_t i = 0; i < sizeof(values) / sizeof(values[0]); ++i) {
        for (size_t j = 0; j < sizeof(offsets) / sizeof(offsets[0]); ++j) {
            RunCreate(values[i]);
            RunCreate(values[i] + offsets[j]);
            RunCreate(values[i] - offsets[j]);
        }
    }
}

BOOST_AUTO_TEST_CASE(operators) {
    for (size_t i = 0; i < sizeof(values) / sizeof(values[0]); ++i) {
        for (size_t j = 0; j < sizeof(offsets) / sizeof(offsets[0]); ++j) {
            RunOperators(values[i], values[i]);
            RunOperators(values[i], -values[i]);
            RunOperators(values[i], values[j]);
            RunOperators(values[i], -values[j]);
            RunOperators(values[i] + values[j], values[j]);
            RunOperators(values[i] + values[j], -values[j]);
            RunOperators(values[i] - values[j], values[j]);
            RunOperators(values[i] - values[j], -values[j]);
            RunOperators(values[i] + values[j], values[i] + values[j]);
            RunOperators(values[i] + values[j], values[i] - values[j]);
            RunOperators(values[i] - values[j], values[i] + values[j]);
            RunOperators(values[i] - values[j], values[i] - values[j]);
        }
    }
}

static void CheckMinimalyEncode(std::vector<uint8_t> data,
                                const std::vector<uint8_t> &expected) {
    bool alreadyEncoded = bsv::IsMinimallyEncoded(data, data.size());
    bool hasEncoded = bsv::MinimallyEncode(data);
    BOOST_CHECK_EQUAL(hasEncoded, !alreadyEncoded);
    BOOST_CHECK(data == expected);
}

BOOST_AUTO_TEST_CASE(minimize_encoding_test) {
    CheckMinimalyEncode({}, {});

    // Check that positive and negative zeros encode to nothing.
    std::vector<uint8_t> zero, negZero;
    for (size_t i = 0; i < MAX_SCRIPT_ELEMENT_SIZE_BEFORE_GENESIS; i++) {
        zero.push_back(0x00);
        CheckMinimalyEncode(zero, {});

        negZero.push_back(0x80);
        CheckMinimalyEncode(negZero, {});

        // prepare for next round.
        negZero[negZero.size() - 1] = 0x00;
    }

    // Keep one leading zero when sign bit is used.
    std::vector<uint8_t> n{0x80, 0x00}, negn{0x80, 0x80};
    std::vector<uint8_t> npadded = n, negnpadded = negn;
    for (size_t i = 0; i < MAX_SCRIPT_ELEMENT_SIZE_BEFORE_GENESIS; i++) {
        CheckMinimalyEncode(npadded, n);
        npadded.push_back(0x00);

        CheckMinimalyEncode(negnpadded, negn);
        negnpadded[negnpadded.size() - 1] = 0x00;
        negnpadded.push_back(0x80);
    }

    // Mege leading byte when sign bit isn't used.
    std::vector<uint8_t> k{0x7f}, negk{0xff};
    std::vector<uint8_t> kpadded = k, negkpadded = negk;
    for (size_t i = 0; i < MAX_SCRIPT_ELEMENT_SIZE_BEFORE_GENESIS; i++) {
        CheckMinimalyEncode(kpadded, k);
        kpadded.push_back(0x00);

        CheckMinimalyEncode(negkpadded, negk);
        negkpadded[negkpadded.size() - 1] &= 0x7f;
        negkpadded.push_back(0x80);
    }
}
// clang-format on

BOOST_AUTO_TEST_SUITE_END()


/* statistic_tests.hpp header file
 *
 * Copyright Jens Maurer 2000
 * 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)
 *
 * $Id$
 *
 */

#ifndef STATISTIC_TESTS_HPP
#define STATISTIC_TESTS_HPP

#include <stdexcept>
#include <iterator>
#include <vector>
#include <boost/limits.hpp>
#include <algorithm>
#include <cmath>

#include <boost/config.hpp>
#include <boost/bind.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>

#include "integrate.hpp"

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
namespace std
{
  inline double pow(double a, double b) { return ::pow(a,b); }
  inline double ceil(double x) { return ::ceil(x); }
} // namespace std
#endif

template<class T>
inline T fac(int k)
{
  T result = 1;
  for(T i = 2; i <= k; ++i)
    result *= i;
  return result;
}

template<class T>
T binomial(int n, int k)
{
  if(k < n/2)
    k = n-k;
  T result = 1;
  for(int i = k+1; i<= n; ++i)
    result *= i;
  return result / fac<T>(n-k);
}

template<class T>
T stirling2(int n, int m)
{
  T sum = 0;
  for(int k = 0; k <= m; ++k)
    sum += binomial<T>(m, k) * std::pow(double(k), n) *
      ( (m-k)%2 == 0 ? 1 : -1);
  return sum / fac<T>(m);
}

/*
 * Experiments which create an empirical distribution in classes,
 * suitable for the chi-square test.
 */
// std::floor(gen() * classes)

class experiment_base
{
public:
  experiment_base(int cls) : _classes(cls) { }
  unsigned int classes() const { return _classes; }
protected:
  unsigned int _classes;
};

class equidistribution_experiment : public experiment_base
{
public:
  explicit equidistribution_experiment(unsigned int classes)
    : experiment_base(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    assert((f.min)() == 0 &&
           static_cast<unsigned int>((f.max)()) == classes()-1);
    for(int i = 0; i < n; ++i)
      count(f());
  }
  double probability(int /*i*/) const { return 1.0/classes(); }
};

// two-dimensional equidistribution experiment
class equidistribution_2d_experiment : public equidistribution_experiment
{
public:
  explicit equidistribution_2d_experiment(unsigned int classes)
    : equidistribution_experiment(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    unsigned int range = (f.max)()+1;
    assert((f.min)() == 0 && range*range == classes());
    for(int i = 0; i < n; ++i) {
      int y1 = f();
      int y2 = f();
      count(y1 + range * y2);
    }
  }
};

// distribution experiment: assume a probability density and
// count events so that an equidistribution results.
class distribution_experiment : public equidistribution_experiment
{
public:
  template<class Distribution>
  distribution_experiment(Distribution dist , unsigned int classes)
    : equidistribution_experiment(classes), limit(classes)
  {
    for(unsigned int i = 0; i < classes-1; ++i)
      limit[i] = quantile(dist, (i+1)*0.05);
    limit[classes-1] = std::numeric_limits<double>::infinity();
    if(limit[classes-1] < (std::numeric_limits<double>::max)())
      limit[classes-1] = (std::numeric_limits<double>::max)();
#if 0
    std::cout << __PRETTY_FUNCTION__ << ": ";
    for(unsigned int i = 0; i < classes; ++i)
      std::cout << limit[i] << " ";
    std::cout << std::endl;
#endif
  }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    for(int i = 0; i < n; ++i) {
      limits_type::const_iterator it =
        std::lower_bound(limit.begin(), limit.end(), f());
      count(it-limit.begin());
    }
  }
private:
  typedef std::vector<double> limits_type;
  limits_type limit;
};

template<bool up, bool is_float>
struct runs_direction_helper
{
  template<class T>
  static T init(T)
  {
    return (std::numeric_limits<T>::max)();
  }
};

template<>
struct runs_direction_helper<true, true>
{
  template<class T>
  static T init(T)
  {
    return -(std::numeric_limits<T>::max)();
  }
};

template<>
struct runs_direction_helper<true, false>
{
  template<class T>
  static T init(T)
  {
    return (std::numeric_limits<T>::min)();
  }
};

// runs-up/runs-down experiment
template<bool up>
class runs_experiment : public experiment_base
{
public:
  explicit runs_experiment(unsigned int classes) : experiment_base(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    typedef typename NumberGenerator::result_type result_type;
    result_type init =
      runs_direction_helper<
        up,
        !std::numeric_limits<result_type>::is_integer
      >::init(result_type());
    result_type previous = init;
    unsigned int length = 0;
    for(int i = 0; i < n; ++i) {
      result_type val = f();
      if(up ? previous <= val : previous >= val) {
        previous = val;
        ++length;
      } else {
        count((std::min)(length, classes())-1);
        length = 0;
        previous = init;
        // don't use this value, so that runs are independent
      }
    }
  }
  double probability(unsigned int r) const
  {
    if(r == classes()-1)
      return 1.0/fac<double>(classes());
    else
      return static_cast<double>(r+1)/fac<double>(r+2);
  }
};

// gap length experiment
class gap_experiment : public experiment_base
{
public:
  template<class Dist>
  gap_experiment(unsigned int classes, const Dist & dist, double alpha, double beta)
    : experiment_base(classes), alpha(alpha), beta(beta), low(quantile(dist, alpha)), high(quantile(dist, beta)) {}

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    typedef typename NumberGenerator::result_type result_type;
    unsigned int length = 0;
    for(int i = 0; i < n; ) {
      result_type value = f();
      if(value < low || value > high)
        ++length;
      else {
        count((std::min)(length, classes()-1));
        length = 0;
        ++i;
      }
    }
  }
  double probability(unsigned int r) const
  {
    double p = beta-alpha;
    if(r == classes()-1)
      return std::pow(1-p, static_cast<double>(r));
    else
      return p * std::pow(1-p, static_cast<double>(r));
  }
private:
  double alpha, beta;
  double low, high;
};

// poker experiment
class poker_experiment : public experiment_base
{
public:
  poker_experiment(unsigned int d, unsigned int k)
    : experiment_base(k), range(d)
  {
    assert(range > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(range-1));
    std::vector<result_type> v(classes());
    for(int i = 0; i < n; ++i) {
      for(unsigned int j = 0; j < classes(); ++j)
        v[j] = f();
      std::sort(v.begin(), v.end());
      result_type prev = v[0];
      int r = 1;     // count different values in v
      for(unsigned int i = 1; i < classes(); ++i) {
        if(prev != v[i]) {
          prev = v[i];
          ++r;
        }
      }
      count(r-1);
    }
  }

  double probability(unsigned int r) const
  {
    ++r;       // transform to 1 <= r <= 5
    double result = range;
    for(unsigned int i = 1; i < r; ++i)
      result *= range-i;
    return result / std::pow(range, static_cast<double>(classes())) *
      stirling2<double>(classes(), r);
  }
private:
  unsigned int range;
};

// coupon collector experiment
class coupon_collector_experiment : public experiment_base
{
public:
  coupon_collector_experiment(unsigned int d, unsigned int cls)
    : experiment_base(cls), d(d)
  {
    assert(d > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(d-1));
    std::vector<bool> occurs(d);
    for(int i = 0; i < n; ++i) {
      occurs.assign(d, false);
      unsigned int r = 0;            // length of current sequence
      int q = 0;                     // number of non-duplicates in current set
      for(;;) {
        result_type val = f();
        ++r;
        if(!occurs[val]) {       // new set element
          occurs[val] = true;
          ++q;
          if(q == d)
            break;     // one complete set
        }
      }
      count((std::min)(r-d, classes()-1));
    }
  }
  double probability(unsigned int r) const
  {
    if(r == classes()-1)
      return 1-fac<double>(d)/
        std::pow(static_cast<double>(d), static_cast<double>(d+classes()-2)) *
        stirling2<double>(d+classes()-2, d);
    else
      return fac<double>(d)/
        std::pow(static_cast<double>(d), static_cast<double>(d+r)) *
        stirling2<double>(d+r-1, d-1);
  }
private:
  int d;
};

// permutation test
class permutation_experiment : public equidistribution_experiment
{
public:
  permutation_experiment(unsigned int t)
    : equidistribution_experiment(fac<int>(t)), t(t)
  {
    assert(t > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    std::vector<result_type> v(t);
    for(int i = 0; i < n; ++i) {
      for(int j = 0; j < t; ++j) {
        v[j] = f();
      }
      int x = 0;
      for(int r = t-1; r > 0; r--) {
        typename std::vector<result_type>::iterator it =
          std::max_element(v.begin(), v.begin()+r+1);
        x = (r+1)*x + (it-v.begin());
        std::iter_swap(it, v.begin()+r);
      }
      count(x);
    }
  }
private:
  int t;
};

// birthday spacing experiment test
class birthday_spacing_experiment : public experiment_base
{
public:
  birthday_spacing_experiment(unsigned int d, int n, int m)
    : experiment_base(d), n(n), m(m)
  {
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n_total) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(m-1));

    for(int j = 0; j < n_total; j++) {
      std::vector<result_type> v(n);
      std::generate_n(v.begin(), n, f);
      std::sort(v.begin(), v.end());
      std::vector<result_type> spacing(n);
      for(int i = 0; i < n-1; i++)
        spacing[i] = v[i+1]-v[i];
      spacing[n-1] = v[0] + m - v[n-1];
      std::sort(spacing.begin(), spacing.end());
      unsigned int k = 0;
      for(int i = 0; i < n-1; ++i) {
        if(spacing[i] == spacing[i+1])
          ++k;
      }
      count((std::min)(k, classes()-1));
    }
  }

  double probability(unsigned int r) const
  {
    assert(classes() == 4);
    assert(m == (1<<25));
    assert(n == 512);
    static const double prob[] = { 0.368801577, 0.369035243, 0.183471182,
                                   0.078691997 };
    return prob[r];
  }
private:
  int n, m;
};
/*
 * Misc. helper functions.
 */

template<class Float>
struct distribution_function
{
  typedef Float result_type;
  typedef Float argument_type;
  typedef Float first_argument_type;
  typedef Float second_argument_type;
};

// computes P(K_n <= t) or P(t1 <= K_n <= t2).  See Knuth, 3.3.1
class kolmogorov_smirnov_probability : public distribution_function<double>
{
public:
  kolmogorov_smirnov_probability(int n)
    : approx(n > 50), n(n), sqrt_n(std::sqrt(double(n)))
  {
    if(!approx)
      n_n = std::pow(static_cast<double>(n), n);
  }

  double cdf(double t) const
  {
    if(approx) {
      return 1-std::exp(-2*t*t)*(1-2.0/3.0*t/sqrt_n);
    } else {
      t *= sqrt_n;
      double sum = 0;
      for(int k = static_cast<int>(std::ceil(t)); k <= n; k++)
        sum += binomial<double>(n, k) * std::pow(k-t, k) *
          std::pow(t+n-k, n-k-1);
      return 1 - t/n_n * sum;
    }
  }
  //double operator()(double t1, double t2) const
  //{ return operator()(t2) - operator()(t1); }

private:
  bool approx;
  int n;
  double sqrt_n;
  double n_n;
};

inline double cdf(const kolmogorov_smirnov_probability& dist, double val)
{
  return dist.cdf(val);
}

inline double quantile(const kolmogorov_smirnov_probability& dist, double val)
{
    return invert_monotone_inc(boost::bind(&cdf, dist, _1), val, 0.0, 1000.0);
}

/*
 * Experiments for generators with continuous distribution functions
 */
class kolmogorov_experiment
{
public:
  kolmogorov_experiment(int n) : n(n), ksp(n) { }
  template<class NumberGenerator, class Distribution>
  double run(NumberGenerator & gen, Distribution distrib) const
  {
    const int m = n;
    typedef std::vector<double> saved_temp;
    saved_temp a(m,1.0), b(m,0);
    std::vector<int> c(m,0);
    for(int i = 0; i < n; ++i) {
      double val = static_cast<double>(gen());
      double y = cdf(distrib, val);
      int k = static_cast<int>(std::floor(m*y));
      if(k >= m)
        --k;    // should not happen
      a[k] = (std::min)(a[k], y);
      b[k] = (std::max)(b[k], y);
      ++c[k];
    }
    double kplus = 0, kminus = 0;
    int j = 0;
    for(int k = 0; k < m; ++k) {
      if(c[k] > 0) {
        kminus = (std::max)(kminus, a[k]-j/static_cast<double>(n));
        j += c[k];
        kplus = (std::max)(kplus, j/static_cast<double>(n) - b[k]);
      }
    }
    kplus *= std::sqrt(double(n));
    kminus *= std::sqrt(double(n));
    // std::cout << "k+ " << kplus << "   k- " << kminus << std::endl;
    return kplus;
  }
  double probability(double x) const
  {
    return cdf(ksp, x);
  }
private:
  int n;
  kolmogorov_smirnov_probability ksp;
};

struct power_distribution
{
  power_distribution(double t) : t(t) {}
  double t;
};

double cdf(const power_distribution& dist, double val)
{
  return std::pow(val, dist.t);
}

// maximum-of-t test (KS-based)
template<class UniformRandomNumberGenerator>
class maximum_experiment
{
public:
  typedef UniformRandomNumberGenerator base_type;
  maximum_experiment(base_type & f, int n, int t) : f(f), ke(n), t(t)
  { }

  double operator()() const
  {
    generator gen(f, t);
    return ke.run(gen, power_distribution(t));
  }

private:
  struct generator {
    generator(base_type & f, int t) : f(f, boost::uniform_01<>()), t(t) { }
    double operator()()
    {
      double mx = f();
      for(int i = 1; i < t; ++i)
        mx = (std::max)(mx, f());
      return mx;
    }
  private:
    boost::variate_generator<base_type&, boost::uniform_01<> > f;
    int t;
  };
  base_type & f;
  kolmogorov_experiment ke;
  int t;
};

// compute a chi-square value for the distribution approximation error
template<class ForwardIterator, class UnaryFunction>
typename UnaryFunction::result_type
chi_square_value(ForwardIterator first, ForwardIterator last,
                 UnaryFunction probability)
{
  typedef std::iterator_traits<ForwardIterator> iter_traits;
  typedef typename iter_traits::value_type counter_type;
  typedef typename UnaryFunction::result_type result_type;
  unsigned int classes = std::distance(first, last);
  result_type sum = 0;
  counter_type n = 0;
  for(unsigned int i = 0; i < classes; ++first, ++i) {
    counter_type count = *first;
    n += count;
    sum += (count/probability(i)) * count;  // avoid overflow
  }
#if 0
  for(unsigned int i = 0; i < classes; ++i) {
    // std::cout << (n*probability(i)) << " ";
    if(n * probability(i) < 5)
      std::cerr << "Not enough test runs for slot " << i
                << " p=" << probability(i) << ", n=" << n
                << std::endl;
  }
#endif
  // std::cout << std::endl;
  // throw std::invalid_argument("not enough test runs");

  return sum/n - n;
}
template<class RandomAccessContainer>
class generic_counter
{
public:
  explicit generic_counter(unsigned int classes) : container(classes, 0) { }
  void operator()(int i)
  {
    assert(i >= 0);
    assert(static_cast<unsigned int>(i) < container.size());
    ++container[i];
  }
  typename RandomAccessContainer::const_iterator begin() const
  { return container.begin(); }
  typename RandomAccessContainer::const_iterator end() const
  { return container.end(); }

private:
  RandomAccessContainer container;
};

// chi_square test
template<class Experiment, class Generator>
double run_experiment(const Experiment & experiment, Generator & gen, int n)
{
  generic_counter<std::vector<int> > v(experiment.classes());
  experiment.run(gen, v, n);
  return chi_square_value(v.begin(), v.end(),
                          boost::bind(&Experiment::probability,
                                       experiment, boost::placeholders::_1));
}

// chi_square test
template<class Experiment, class Generator>
double run_experiment(const Experiment & experiment, const Generator & gen, int n)
{
  generic_counter<std::vector<int> > v(experiment.classes());
  experiment.run(gen, v, n);
  return chi_square_value(v.begin(), v.end(),
                          boost::bind(&Experiment::probability,
                                       experiment, boost::placeholders::_1));
}

// number generator with experiment results (for nesting)
template<class Experiment, class Generator>
class experiment_generator_t
{
public:
  experiment_generator_t(const Experiment & exper, Generator & gen, int n)
    : experiment(exper), generator(gen), n(n) { }
  double operator()() const { return run_experiment(experiment, generator, n); }
private:
  const Experiment & experiment;
  Generator & generator;
  int n;
};

template<class Experiment, class Generator>
experiment_generator_t<Experiment, Generator>
experiment_generator(const Experiment & e, Generator & gen, int n)
{
  return experiment_generator_t<Experiment, Generator>(e, gen, n);
}

template<class Experiment, class Generator, class Distribution>
class ks_experiment_generator_t
{
public:
  ks_experiment_generator_t(const Experiment & exper, Generator & gen,
                            const Distribution & distrib)
    : experiment(exper), generator(gen), distribution(distrib) { }
  double operator()() const { return experiment.run(generator, distribution); }
private:
  const Experiment & experiment;
  Generator & generator;
  Distribution distribution;
};

template<class Experiment, class Generator, class Distribution>
ks_experiment_generator_t<Experiment, Generator, Distribution>
ks_experiment_generator(const Experiment & e, Generator & gen,
                        const Distribution & distrib)
{
  return ks_experiment_generator_t<Experiment, Generator, Distribution>
    (e, gen, distrib);
}

#endif /* STATISTIC_TESTS_HPP */


// Boost.Polygon library voronoi_diagram_test.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#include <boost/core/lightweight_test.hpp>
#include <boost/polygon/voronoi_diagram.hpp>
#include <boost/polygon/voronoi_geometry_type.hpp>

using namespace boost::polygon;

typedef voronoi_cell<double> voronoi_cell_type;
typedef voronoi_vertex<double> voronoi_vertex_type;
typedef voronoi_edge<double> voronoi_edge_type;
typedef voronoi_diagram<double> voronoi_diagram_type;

void voronoi_cell_test()
{
  voronoi_cell_type cell(1, SOURCE_CATEGORY_INITIAL_SEGMENT);
  cell.color(27);
  BOOST_TEST(!cell.contains_point());
  BOOST_TEST(cell.contains_segment());
  BOOST_TEST(cell.is_degenerate());
  BOOST_TEST(cell.source_index() == 1);
  BOOST_TEST(cell.source_category() == SOURCE_CATEGORY_INITIAL_SEGMENT);
  BOOST_TEST(cell.incident_edge() == NULL);
  BOOST_TEST(cell.color() == 27);

  voronoi_edge_type edge(true, true);
  cell.incident_edge(&edge);
  BOOST_TEST(!cell.is_degenerate());
  BOOST_TEST(cell.incident_edge() == &edge);
}

void voronoi_vertex_test()
{
  voronoi_vertex_type vertex(1, 2);
  vertex.color(27);
  BOOST_TEST(vertex.is_degenerate());
  BOOST_TEST(vertex.x() == 1);
  BOOST_TEST(vertex.y() == 2);
  BOOST_TEST(vertex.incident_edge() == NULL);
  BOOST_TEST(vertex.color() == 27);

  voronoi_edge_type edge(true, true);
  vertex.incident_edge(&edge);
  BOOST_TEST(!vertex.is_degenerate());
  BOOST_TEST(vertex.incident_edge() == &edge);
}

void voronoi_edge_test()
{
  voronoi_edge_type edge1(false, false);
  edge1.color(13);
  BOOST_TEST(!edge1.is_primary());
  BOOST_TEST(edge1.is_secondary());
  BOOST_TEST(!edge1.is_linear());
  BOOST_TEST(edge1.is_curved());
  BOOST_TEST(!edge1.is_finite());
  BOOST_TEST(edge1.is_infinite());
  BOOST_TEST(edge1.color() == 13);

  voronoi_edge_type edge2(true, true);
  edge2.color(14);
  BOOST_TEST(edge2.is_primary());
  BOOST_TEST(!edge2.is_secondary());
  BOOST_TEST(edge2.is_linear());
  BOOST_TEST(!edge2.is_curved());
  BOOST_TEST(!edge2.is_finite());
  BOOST_TEST(edge2.is_infinite());
  BOOST_TEST(edge2.color() == 14);

  edge1.twin(&edge2);
  edge2.twin(&edge1);
  BOOST_TEST(edge1.twin() == &edge2);
  BOOST_TEST(edge2.twin() == &edge1);

  edge1.next(&edge2);
  edge1.prev(&edge2);
  edge2.next(&edge1);
  edge2.prev(&edge1);
  BOOST_TEST(edge1.next() == &edge2);
  BOOST_TEST(edge1.prev() == &edge2);
  BOOST_TEST(edge1.rot_next() == &edge1);
  BOOST_TEST(edge1.rot_prev() == &edge1);

  voronoi_cell_type cell(1, SOURCE_CATEGORY_INITIAL_SEGMENT);
  edge1.cell(&cell);
  BOOST_TEST(edge1.cell() == &cell);

  voronoi_vertex_type vertex0(1, 2);
  edge1.vertex0(&vertex0);
  BOOST_TEST(edge1.vertex0() == &vertex0);
  BOOST_TEST(edge2.vertex1() == &vertex0);

  voronoi_vertex_type vertex1(2, 1);
  edge2.vertex0(&vertex1);
  BOOST_TEST(edge1.vertex1() == &vertex1);
  BOOST_TEST(edge2.vertex0() == &vertex1);

  BOOST_TEST(edge1.is_finite());
  BOOST_TEST(edge2.is_finite());
}

void voronoi_diagram_test()
{
  voronoi_diagram_type vd;
  BOOST_TEST(vd.num_cells() == 0);
  BOOST_TEST(vd.num_vertices() == 0);
  BOOST_TEST(vd.num_edges() == 0);
  vd.clear();
}

int main()
{
    voronoi_cell_test();
    voronoi_vertex_test();
    voronoi_edge_test();
    voronoi_diagram_test();
    return boost::report_errors();
}


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cfloat>

// Boost
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

// Eigen
#include <Eigen/Dense>

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
  /** \brief Empty constructor. */
  Mesh() {

  }

  /** \brief Reading an off file and returning the vertices x, y, z coordinates and the
   * face indices.
   * \param[in] filepath path to the OFF file
   * \param[out] mesh read mesh with vertices and faces
   * \return success
   */
  static bool from_off(const std::string filepath, Mesh& mesh) {

    std::ifstream* file = new std::ifstream(filepath.c_str());
    std::string line;
    std::stringstream ss;
    int line_nb = 0;

    std::getline(*file, line);
    ++line_nb;

    if (line != "off" && line != "OFF") {
      std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
      return false;
    }

    size_t n_edges;
    std::getline(*file, line);
    ++line_nb;

    int n_vertices;
    int n_faces;
    ss << line;
    ss >> n_vertices;
    ss >> n_faces;
    ss >> n_edges;

    for (size_t v = 0; v < n_vertices; ++v) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      Eigen::Vector3f vertex;
      ss << line;
      ss >> vertex(0);
      ss >> vertex(1);
      ss >> vertex(2);

      mesh.add_vertex(vertex);
    }

    size_t n;
    for (size_t f = 0; f < n_faces; ++f) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      size_t n;
      ss << line;
      ss >> n;

      if(n != 3) {
        std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
        return false;
      }

      Eigen::Vector3i face;
      ss >> face(0);
      ss >> face(1);
      ss >> face(2);

      mesh.add_face(face);
    }

    if (n_vertices != mesh.num_vertices()) {
      std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
      return false;
    }

    if (n_faces != mesh.num_faces()) {
      std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
      return false;
    }

    file->close();
    delete file;

    return true;
  }

  /** \brief Write mesh to OFF file.
   * \param[in] filepath path to OFF file to write
   * \return success
   */
  bool to_off(const std::string filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(out)) {
      return false;
    }

    (*out) << "OFF" << std::endl;
    (*out) << this->num_vertices() << " " << this->num_faces() << " 0" << std::endl;

    for (unsigned int v = 0; v < this->num_vertices(); v++) {
      (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
    }

    for (unsigned int f = 0; f < this->num_faces(); f++) {
      (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Add a vertex.
   * \param[in] vertex vertex to add
   */
  void add_vertex(Eigen::Vector3f& vertex) {
    this->vertices.push_back(vertex);
  }

  /** \brief Get the number of vertices.
   * \return number of vertices
   */
  int num_vertices() {
    return static_cast<int>(this->vertices.size());
  }

  /** \brief Add a face.
   * \param[in] face face to add
   */
  void add_face(Eigen::Vector3i& face) {
    this->faces.push_back(face);
  }

  /** \brief Get the number of faces.
   * \return number of faces
   */
  int num_faces() {
    return static_cast<int>(this->faces.size());
  }

  /** \brief Translate the mesh.
   * \param[in] translation translation vector
   */
  void translate(const Eigen::Vector3f& translation) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) += translation(i);
      }
    }
  }

  /** \brief Scale the mesh.
   * \param[in] scale scale vector
   */
  void scale(const Eigen::Vector3f& scale) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) *= scale(i);
      }
    }
  }

private:

  /** \brief Vertices as (x,y,z)-vectors. */
  std::vector<Eigen::Vector3f> vertices;

  /** \brief Faces as list of vertex indices. */
  std::vector<Eigen::Vector3i> faces;
};

int main(int argc, char** argv) {
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
      ("input",  boost::program_options::value<std::string>(), "path to input OFF file");

  boost::program_options::positional_options_description positionals;
  positionals.add("input", 1);

  boost::program_options::variables_map parameters;
  boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(positionals).run(), parameters);
  boost::program_options::notify(parameters);

  if (parameters.find("help") != parameters.end()) {
    std::cout << desc << std::endl;
    return 1;
  }

  boost::filesystem::path input(parameters["input"].as<std::string>());
  if (!boost::filesystem::is_regular_file(input)) {
    std::cout << "Input file does not exist." << std::endl;
    return 1;
  }

  Mesh mesh;
  Mesh::from_off(input.string(), mesh);

  std::cout << "Read " << input << "." << std::endl;
  std::cout << mesh.num_vertices() << " vertices and " << mesh.num_faces() << " faces." << std::endl;

  return 0;
}


#if HAVE_UNISTD_H
#include <unistd.h>
#else
#if !defined(__MTA__)
#include <getopt.h>
#endif
#endif

#include <stdio.h>
#include <sys/stat.h>
#include <math.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <limits>

#include <Grappa.hpp>
#include <GlobalAllocator.hpp>
#include <Cache.hpp>
#include <Collective.hpp>
#include <Delegate.hpp>
#include <FileIO.hpp>
#include <Array.hpp>
#include <ParallelLoop.hpp>
#include <Metrics.hpp>
#include <AsyncDelegate.hpp>

#include "npb_intsort.h"

using namespace Grappa;

/// Grappa implementation of a paper and pencil sorting benchmark.
/// Does a bucket sort of a bunch of 64-bit integers, beginning and ending in a Grappa global array.

/// Parameter for how large the file I/O buffer is
static const size_t BUFSIZE = 1L<<22;

// little helper for iterating over things numerous enough to need to be buffered
#define for_buffered(i, n, start, end, nbuf) \
  for (size_t i=start, n=nbuf; i<end && (n = std::min(nbuf, end-i)); i+=nbuf)

static void printHelp(const char * exe);
static void parseOptions(int argc, char ** argv);

typedef boost::mt19937_64 engine_t;
typedef boost::uniform_int<uint64_t> dist_t;
typedef boost::variate_generator<engine_t&,dist_t> gen_t;

/// Quick and dirty growable bucket that is padded to take up a full Grappa block so
/// that malloc'ing a global array round-robins buckets to nodes to keep it as even
/// as possible (for small numbers of buckets this is especially important)
struct bucket_t {
  uint64_t * v;
  size_t nelems;
  size_t maxelems;
  bucket_t(): v(NULL), nelems(0) {}
  ~bucket_t() { delete [] v; }
  void reserve(size_t nelems) {
    maxelems = nelems;
    if (v != NULL) delete [] v;
    v = new uint64_t[nelems];
    if (!v) {
      LOG(ERROR) << "Unable to allocate bucket of " << nelems << " elements."; exit(1);
    }
  }
  const uint64_t& operator[](size_t i) const { return v[i]; }
  uint64_t& operator[](size_t i) {
    CHECK( i < maxelems ) << "i = " << i << ", maxelems: " << maxelems;
    return v[i];
  }
  void append(uint64_t val) {
    v[nelems] = val;
    nelems++;
    CHECK(nelems <= maxelems);
  }
  size_t size() { return nelems; }
} GRAPPA_BLOCK_ALIGNED;

////////////
// Globals
////////////

// global const for one sort
int scale;
int log2buckets;
int log2maxkey;
int64_t nelems;
int nbuckets;

// local version on each node, which then gets local copy of global state
std::vector<size_t> counts;
std::vector<size_t> offsets;

// not for sort itself
uint64_t maxkey;
bool read_from_disk;
bool generate;
bool write_to_disk;
bool do_sort;

#define LOBITS (log2maxkey - log2buckets)

inline uint64_t next_random() {
  // continue using same generator with multiple calls (to not repeat numbers)
  // but start at different seed on each node so we don't get overlap
  static engine_t engine(12345L*Grappa::mycore());
  static gen_t gen(engine, dist_t(0, maxkey));
  return gen();
}

template< typename T >
inline void print_array(const char * name, std::vector<T> v) {
  std::stringstream ss; ss << name << ": [";
  for (size_t i=0; i<v.size(); i++) ss << " " << v[i];
  ss << " ]"; VLOG(1) << ss.str();
}
template< typename T >
inline void print_array(const char * name, GlobalAddress<T> base, size_t nelem) {
  std::stringstream ss; ss << name << ": [";
  for (size_t i=0; i<nelem; i++) ss << " " << delegate::read(base+i);
  ss << " ]"; VLOG(1) << ss.str();
}

int ui64cmp(const void * a, const void * b) {
  uint64_t * aa = (uint64_t*)a;
  uint64_t * bb = (uint64_t*)b;
  if (*aa < *bb) return -1;
  else if (*aa > *bb) return 1;
  else return 0;
}

void bucket_sort(GlobalAddress<uint64_t> array, size_t nelems, size_t nbuckets) {
  double t, sort_time, histogram_time, allreduce_time, scatter_time, local_sort_scatter_time, put_back_time;

  GlobalAddress<bucket_t> bucketlist = Grappa::global_alloc<bucket_t>(nbuckets);

#ifdef DEBUG
  for (size_t i=0; i<nbuckets; i++) {
    GlobalAddress<bucket_t> bi = bucketlist+i;
    VLOG(1) << "bucket[" << i << "] on Core " << bi.core() << ", offset = " << bi.pointer() - bucketlist.localize(bi.core()) << ", ptr = " << bi.pointer();
  }
#endif

  sort_time = Grappa::walltime();

  // initialize globals and histogram counts
  // { setup_counts f(array, nbuckets, bucketlist); fork_join_custom(&f); }
  on_all_cores([nbuckets]{
    counts.resize(nbuckets);
    offsets.resize(nbuckets);
    for (size_t i=0; i<nbuckets; i++) {
      counts[i] = 0;
    }
  });

  t = Grappa::walltime();

  // do local bucket counts
  // forall_local<uint64_t,histogram>(array, nelems);
  forall(array, nelems, [](int64_t i, uint64_t& v){
    size_t b = v >> LOBITS;
    counts[b]++;
  });

  histogram_time = Grappa::walltime() - t;
  LOG(INFO) << "histogram_time: " << histogram_time;
  t = Grappa::walltime();

  // allreduce everyone's counts & compute global offsets (prefix sum)
  // { aggregate_counts f; fork_join_custom(&f); }
  on_all_cores([nbuckets]{
    CHECK_EQ(nbuckets, counts.size());
    // all nodes get total counts put into their counts array
    allreduce_inplace<size_t,collective_add>(&counts[0], counts.size());

    VLOG(1) << "after allreduce_inplace (just in case)";

    // everyone computes prefix sum over buckets locally
    offsets[0] = 0;
    for (size_t i=1; i<offsets.size(); i++) {
      offsets[i] = offsets[i-1] + counts[i-1];
    }
  });

  allreduce_time = Grappa::walltime() - t;
  LOG(INFO) << "allreduce_time: " << allreduce_time;

  // allocate space in buckets
  VLOG(3) << "allocating space...";
  // forall_local<bucket_t,init_buckets>(bucketlist, nbuckets);
  forall(bucketlist, nbuckets, [](int64_t id, bucket_t& bucket){
    // (global malloc doesn't call constructors)
    new (&bucket) bucket_t();
    bucket.reserve(counts[id]);
  });

  VLOG(3) << "scattering...";
      t = Grappa::walltime();

  // scatter into buckets
  // forall_local<uint64_t,scatter>(array, nelems);
  forall(array, nelems, [bucketlist](int64_t s, int64_t n, uint64_t * first){
    size_t nbuckets = counts.size();

    for (int i=0; i<n; i++) {
      auto v = first[i];
      size_t b = v >> LOBITS;
      CHECK( b < nbuckets ) << "bucket id = " << b << ", nbuckets = " << nbuckets;
      // ff_delegate<bucket_t,uint64_t,ff_append>(bucketlist+b, v);
      auto destb = bucketlist+b;
      delegate::call<async>(destb.core(), [destb,v]{
        destb.pointer()->append(v);
      });
    }
  });

  scatter_time = Grappa::walltime() - t;
  LOG(INFO) << "scatter_time: " << scatter_time;
  t = Grappa::walltime();

  // sort buckets locally
  // forall_local<bucket_t,sort_bucket>(bucketlist, nbuckets);
  /// Do some kind of local serial sort of a bucket
  forall(bucketlist, nbuckets, [](int64_t bucket_id, bucket_t& bucket){
    if (bucket.size() == 0) return;
    qsort(&bucket[0], bucket.size(), sizeof(uint64_t), &ui64cmp);
  });

  local_sort_scatter_time = Grappa::walltime() - t;
  LOG(INFO) << "local_sort_time: " << local_sort_scatter_time;
  t = Grappa::walltime();

  // redistribute buckets back into global array
  // forall_local<bucket_t,put_back_bucket>(bucketlist, nbuckets);
  /// Redistribute sorted buckets back into global array
  forall(bucketlist, nbuckets, [array](int64_t b, bucket_t& bucket) {
    const size_t NBUF = BUFSIZE / sizeof(uint64_t);
    DCHECK( b < counts.size() );

    // TODO: shouldn't need to buffer this, but a bug of some sort is currently forcing us to limit the number of outstanding messages
    for_buffered(i, n, 0, bucket.size(), NBUF) {
      Incoherent<uint64_t>::WO c(array+offsets[b]+i, n, &bucket[i]);
      c.block_until_released();
    }
    //Incoherent<uint64_t>::WO c(array+offsets[b], bucket->size(), &(*bucket)[0]);
    //c.block_until_released();
    VLOG(3) << "bucket[" << b << "] release successful";
  });

  put_back_time = Grappa::walltime() - t;
  LOG(INFO) << "put_back_time: " << put_back_time;

  sort_time = Grappa::walltime() - sort_time;
  LOG(INFO) << "total_sort_time: " << sort_time;
}

int main(int argc, char* argv[]) {
  Grappa::init(&argc, &argv);

  parseOptions(argc, argv);

  Grappa::run([]{
    call_on_all_cores([]{ Metrics::reset(); });

    double t, rand_time;

    LOG(INFO) << "### Sort Benchmark ###";
    LOG(INFO) << "nelems = (1 << " << scale << ") = " << nelems << " (" << ((double)nelems)*sizeof(uint64_t)/(1L<<30) << " GB)";
    LOG(INFO) << "nbuckets = (1 << " << log2buckets << ") = " << nbuckets;
    LOG(INFO) << "maxkey = (1 << " << log2maxkey << ") - 1 = " << maxkey;

    //LOG(INFO) << "iobufsize_mb = " << (double)BUFSIZE/(1L<<20);
    LOG(INFO) << "block_size = " << block_size;

    GlobalAddress<uint64_t> array = Grappa::global_alloc<uint64_t>(nelems);

    char dirname[256]; sprintf(dirname, "/scratch/hdfs/sort/uniform.%d.%d", scale, log2maxkey);
    Grappa::File f(dirname, true);

    if (generate || write_to_disk || (read_from_disk && !fs::exists(dirname))) {
      LOG(INFO) << "generating...";
      t = Grappa::walltime();

      // fill vector with random 64-bit integers
      // forall_local<uint64_t,set_random>(array, nelems);
      forall(array, nelems, [](int64_t i, uint64_t& e){ e = next_random(); });

      rand_time = Grappa::walltime() - t;
      LOG(INFO) << "fill_random_time: " << rand_time;

      //print_array("generated array", array, nelems);
    }

    if (write_to_disk || (read_from_disk && !fs::exists(dirname))) {
      if (fs::exists(dirname)) fs::remove_all(dirname);

      Grappa::save_array(f, true, array, nelems);
    }

    if (read_from_disk) {
      Grappa::memset(array, (uint64_t)0, nelems);
      Grappa::read_array(f, array, nelems);
      //print_array("read array", array, nelems);
    }

    /////////
    // sort
    /////////
    if (do_sort) {
      bucket_sort(array, nelems, nbuckets);

      // verify
      size_t jump = nelems/17;
      uint64_t prev;
      for (size_t i=0; i<nelems; i+=jump) {
        prev = 0;
        for (size_t j=1; j<64 && (i+j)<nelems; j++) {
          uint64_t curr = delegate::read(array+i+j);
          CHECK( curr >= prev ) << "verify failed: prev = " << prev << ", curr = " << curr;
          prev = curr;
        }
      }
    }

    Metrics::merge_and_print();
  });
  Grappa::finalize();
}

static void printHelp(const char * exe) {
  printf("Usage: %s [options]\nOptions:\n", exe);
  printf("  --help,h   Prints this help message displaying command-line options\n");
  printf("  --scale,s  Number of keys to be sorted: 2^SCALE keys.\n");
  printf("  --log2buckets,b  Number of buckets will be 2^log2buckets.\n");
  printf("  --log2maxkey,k   Maximum value of random numbers, range will be [0,2^log2maxkey)");
  printf("  --class,c   NAS Parallel Benchmark Class (problem size) (W, S, A, B, C, or D)");
  printf("  --gen,g   Generate random numbers and save to file only (no sort).\n");
  printf("  --write,w   Save array to file.\n");
  printf("  --read,r  Read from file rather than generating.\n");
  printf("  --nosort  Skip sort.\n");
  exit(0);
}

static void parseOptions(int argc, char ** argv) {
  struct option long_opts[] = {
    {"help", no_argument, 0, 'h'},
    {"scale", required_argument, 0, 's'},
    {"log2buckets", required_argument, 0, 'b'},
    {"log2maxkey", required_argument, 0, 'k'},
    {"class", required_argument, 0, 'c'},
    {"gen", no_argument, 0, 'g'},
    {"read", no_argument, 0, 'r'},
    {"write", no_argument, 0, 'w'},
    {"nosort", no_argument, 0, 'n'}
  };

  // defaults
  generate = true;
  read_from_disk = false;
  write_to_disk = false;
  do_sort = true;
  scale = 8;

  // at least 2*num_nodes buckets by default
  nbuckets = 2;
  log2buckets = 1;
  Core nodes = Grappa::cores();
  while (nbuckets < nodes) {
    nbuckets <<= 1;
    log2buckets++;
  }

  log2maxkey = 64;

  int c = 0;
  while (c != -1) {
    int option_index = 0;
    npb_class cls;
    c = getopt_long(argc, argv, "hs:b:", long_opts, &option_index);
    switch (c) {
      case 'h':
        printHelp(argv[0]);
        exit(0);
      case 's':
        scale = atoi(optarg);
        break;
      case 'b':
        log2buckets = atoi(optarg);
        break;
      case 'k':
        log2maxkey = atoi(optarg);
        break;
      case 'c':
        cls = get_npb_class(optarg[0]);
        log2maxkey = MAX_KEY_LOG2[cls];
        log2buckets = NBUCKET_LOG2[cls];
        scale = NKEY_LOG2[cls];
        break;
      case 'g':
        generate = true;
        break;
      case 'w':
        write_to_disk = true;
        break;
      case 'r':
        read_from_disk = true;
        break;
      case 'n':
        do_sort = false;
    }
  }
  nelems = 1L << scale;
  nbuckets = 1L << log2buckets;
  maxkey = (log2maxkey == 64) ? (std::numeric_limits<uint64_t>::max()) : (1ul << log2maxkey) - 1;
}


// -*- mode:c++; c-style:k&r; c-basic-offset:4; indent-tabs-mode: nil; -*-
// vi:set et cin sw=4 cino=>se0n0f0{0}0^0\:0=sl1g0hspst0+sc3C0/0(0u0U0w0m0:

#ifndef __RAND_HPP__
#define __RAND_HPP__

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

//using namespace boost;

class random_gen {
private:
   boost::mt19937 rng;
   boost::uniform_01<boost::mt19937> gen;

public:
   random_gen(const uint32_t id, const uint32_t seed) throw();

   // return a natural number selected u.a.r. from [0..max)
   int random_range(int max) throw();

   // return a real number selected u.a.r. from [0..max)
   double random_range_double(double max) throw();

   uint32_t get_id() const throw();
private:
   uint32_t id;
};

#endif // __RAND_HPP__